**Meet the editor**

Dr. Andon V. Topalov received the MSc degree in Control Engineering from the Faculty of Information Systems, Technologies, and Automation at Moscow State University of Civil Engineering (MGGU) in 1979. He then received his PhD degree in Control Engineering from the Department of Automation and Remote Control at Moscow State Mining University (MGSU),

Moscow, in 1984. From 1985 to 1986, he was a Research Fellow in the Research Institute for Electronic Equipment, ZZU AD, Plovdiv, Bulgaria. In 1986, he joined the Department of Control Systems, Technical University of Sofia at the Plovdiv campus, where he is presently a Full Professor. He has held long-term visiting Professor/Scholar positions at various institutions in South Korea, Turkey, Mexico, Greece, Belgium, UK, and Germany. And he has coauthored one book and authored or coauthored more than 80 research papers in conference proceedings and journals. His current research interests are in the fields of intelligent control and robotics.

Contents

**Preface IX** 

**Part 1 Visual Perception, Mapping, Robot Localization** 

Chapter 2 **Development of an Autonomous Visual Perception** 

Chapter 3 **Three-Dimensional Environment Modeling Based** 

**by Using Omnidirectional Camera 51** 

Chapter 5 **Vision Based Obstacle Avoidance Techniques 83**  Mehmet Serdar Guzel and Robert Bicker

Chapter 6 **Non-Rigid Obstacle Avoidance for Mobile Robots 109** 

Chapter 4 **Mobile Robot Position Determination 69**  Farouk Azizi and Nasser Houshangi

Junghee Park and Jeong S. Choi

Chapter 7 **Path Planning of Mobile Robot in** 

**Part 2 Path Planning and Motion Planning 125** 

**Relative Velocity Coordinates 127**  Yang Chen, Jianda Han and Liying Yang

Chapter 8 **Reachable Sets for Simple Models of Car Motion 147**  Andrey Fedotov, Valerii Patsko and Varvara Turova

Chapter 1 **3D Visual Information for Dynamic Objects Detection and Tracking During Mobile Robot Navigation 3**  D.-L. Almanza-Ojeda and M.-A. Ibarra-Manzano

**System for Robots Using Object-Based Visual Attention 25** 

Yuanlong Yu, George K. I. Mann and Raymond G. Gosine

**on Structure from Motion with Point and Line Features** 

Ryosuke Kawanishi, Atsushi Yamashita and Toru Kaneko

**and Obstacle Avoidance 1** 

### Contents

#### **Preface XIII**

	- **Part 2 Path Planning and Motion Planning 125**

X Contents



Contents VII

Chapter 20 **Feedback Equivalence and Control of Mobile Robots Through**

G.P. Moustris, K.M. Deliparaschos and S.G. Tzafestas

**Localization for a Gas Cutting Robot 427**  KiSung You, HwangRyol Ryu and Chintae Choi

**a Scalable FPGA Architecture 401**

Chapter 21 **Control Architecture Design and** 

	- **Part 4 Methods for Control 347**

Chapter 20 **Feedback Equivalence and Control of Mobile Robots Through a Scalable FPGA Architecture 401**  G.P. Moustris, K.M. Deliparaschos and S.G. Tzafestas

#### Chapter 21 **Control Architecture Design and Localization for a Gas Cutting Robot 427**  KiSung You, HwangRyol Ryu and Chintae Choi

VI Contents

Chapter 9 **Neural Networks Based Path Planning** 

Valeri Kroumov and Jianli Yu

**Part 3 Mobile Robots Navigation 233** 

Chapter 12 **Fictitious Fuzzy-Magnet Concept in Solving Mobile–Robot Target Navigation,** 

Eugene Kagan and Irad Ben-Gal

Krzysztof Okarma and Piotr Lech

Chapter 18 **An Embedded Type-2 Fuzzy Controller** 

Chapter 16 **Tracking Control for Reliable Outdoor** 

Seung-Hun Kim

**Part 4 Methods for Control 347** 

Srđan T. Mitrović and Željko M. Ðurović

Chapter 10 **Path Searching Algorithms of Multiple Robot** 

**and Navigation of Mobile Robots 173** 

**System Applying in Chinese Chess Game 191** Jr-Hung Guo, Kuo-Lan Su and Sheng-Ven Shiau

Chapter 11 **Motion Planning for Mobile Robots Via Sampling-Based Model Predictive Optimization 211**  Damion D. Dunlap, Charmane V. Caldwell, Emmanuel G. Collins, Jr. and Oscar Chuy

**Obstacle Avoidance and Garaging Problems 235** 

Chapter 13 **Reliable Long-Term Navigation in Indoor Environments 261** Mattias Wahde, David Sandberg and Krister Wolff

Chapter 14 **Fuzzy Logic Based Navigation of Mobile Robots 287**  Amur S. Al Yahmedi and Muhammed A. Fatmi

Chapter 15 **Navigation of Quantum-Controlled Mobile Robots 311** 

**Navigation Using Curb Detection 327** 

Chapter 17 **Statistical Video Based Control of Mobile Robots 349** 

**for a Mobile Robot Application 365**  Leonardo Leottau and Miguel Melgarejo

Chapter 19 **LQR Control Methods for Trajectory Execution in Omnidirectional Mobile Robots 385**  Luis F. Lupián and Josué R. Rabadán-Martin

Preface

and writer Josef Čapek - as its actual originator.

The term *robotics* was coined by the science fiction writer Isaac Asimov in 1942, in his short story *Runaround* (*Astounding Science Fiction*) to denote the technological area addressing the design, construction, operation, and application of robots. The word *robot* itself was first introduced to the public by the Czech play writer Karel Čapek in his play *R.U.R.* (*Rossum's Universal Robots*), published in 1920, and comes from the Czech word *robota,* which means "work". Karel Čapek named his brother - the painter

There is no single correct definition of *robot* which seems to satisfy everyone. In fact, many people have their own definition. According to the *Collins Cobuild Dictionary* "A robot is a machine which is programmed to move and perform certain tasks automatically". *Oxford English Dictionary* defines *robot* as "A machine capable of carrying out a complex series of actions automatically, especially one programmable by a computer" or (especially in science fiction) "a machine resembling a human being

Various techniques have emerged to develop the science of robotics and robots. Mobile robots are the focus of a great deal of current research. Mobile robotics is a young multidisciplinary field involving knowledge from many areas - electrical, electronics and mechanical engineering, computer, cognitive, and social sciences. Being engaged in the design of automated systems, it lies at the intersection of artificial intelligence, computational vision, and robotics. Mobile robots have the capability to move around in their environment and are not fixed to one physical location. Contemporary mobile robots are living up to their name more and more. They are no longer regarded only as mechanisms attached to controls, but represent a combination of manipulative, perceptive, communicative, and cognitive abilities. Nowadays, mobile robots are characterized by an increased autonomy and must negotiate the reality of moving, sensing, and reasoning out their environment. Thanks to the numerous researchers sharing their goals, visions, and results with the community,

The book *Recent Advances in Mobile Robotics* addresses the topic by integrating contributions from many researchers around the globe. It emphasizes the computational methods of programming mobile robots, rather than the methods of

and able to replicate certain human movements and functions automatically".

mobile robotics is becoming a very rich and stimulating field.

### Preface

The term *robotics* was coined by the science fiction writer Isaac Asimov in 1942, in his short story *Runaround* (*Astounding Science Fiction*) to denote the technological area addressing the design, construction, operation, and application of robots. The word *robot* itself was first introduced to the public by the Czech play writer Karel Čapek in his play *R.U.R.* (*Rossum's Universal Robots*), published in 1920, and comes from the Czech word *robota,* which means "work". Karel Čapek named his brother - the painter and writer Josef Čapek - as its actual originator.

There is no single correct definition of *robot* which seems to satisfy everyone. In fact, many people have their own definition. According to the *Collins Cobuild Dictionary* "A robot is a machine which is programmed to move and perform certain tasks automatically". *Oxford English Dictionary* defines *robot* as "A machine capable of carrying out a complex series of actions automatically, especially one programmable by a computer" or (especially in science fiction) "a machine resembling a human being and able to replicate certain human movements and functions automatically".

Various techniques have emerged to develop the science of robotics and robots. Mobile robots are the focus of a great deal of current research. Mobile robotics is a young multidisciplinary field involving knowledge from many areas - electrical, electronics and mechanical engineering, computer, cognitive, and social sciences. Being engaged in the design of automated systems, it lies at the intersection of artificial intelligence, computational vision, and robotics. Mobile robots have the capability to move around in their environment and are not fixed to one physical location. Contemporary mobile robots are living up to their name more and more. They are no longer regarded only as mechanisms attached to controls, but represent a combination of manipulative, perceptive, communicative, and cognitive abilities. Nowadays, mobile robots are characterized by an increased autonomy and must negotiate the reality of moving, sensing, and reasoning out their environment. Thanks to the numerous researchers sharing their goals, visions, and results with the community, mobile robotics is becoming a very rich and stimulating field.

The book *Recent Advances in Mobile Robotics* addresses the topic by integrating contributions from many researchers around the globe. It emphasizes the computational methods of programming mobile robots, rather than the methods of

#### XIV Preface

constructing the hardware. Its content reflects different complementary aspects of theory and practice, which have recently taken place. The book consists of 21 chapters which are organized into four major parts:


The book editor is grateful to the authors for their excellent work and interesting contributions. Thanks are also due to the prestigious publisher for their editorial assistance and excellent technical arrangement of the book. We believe that the content of this book will serve as a valuable handbook to those who work in the research and development of mobile robots.

October, 2011

**Andon V. Topalov** Control Systems Department Technical University – Sofia, campus in Plovdiv, Plovdiv, Bulgaria

## **Part 1**

## **Visual Perception, Mapping, Robot Localization and Obstacle Avoidance**

**0**

**1**

*Mexico*

**3D Visual Information for Dynamic**

D.-L. Almanza-Ojeda\* and M.-A. Ibarra-Manzano

**Mobile Robot Navigation**

*University of Guanajuato, Salamanca, Guanajuato*

**Objects Detection and Tracking During**

*Digital Signal Processing Laboratory, Electronics Department; DICIS,*

An autonomous mobile robot that navigates in outdoor environments requires functional and decisional routines enabling it to supervise the estimation and the performance of all its movements for carrying out an envisaged trajectory. At this end, a robot is usually equipped with several high-performance sensors. However, we are often interested in less complex and low-cost sensors that could provide enough information to detect in real-time when the trajectory is free of dynamic obstacles. In this context, our strategy was focused on visual sensors, particulary on stereo vision since this provides the depth coordinate for allowing a better perception of the environment. Visual perception for robot mobile navigation is a complex function that requires the presence of "salience" or "evident" patrons to identify something that "breaks" the continuous tendency of data. Usually, interesting points or segments are used for evaluating patrons in position, velocity, appearance or other characteristics that allows us forming groups (Lookingbill et al., 2007), (Talukder & Matthies, 2004). Whereas complete feature vectors are more expressive for explaining objects, here we use 3D feature points for proposing a strategy computationally less demanding conserving

This chapter presents a strategy for detecting and tracking dynamic objects using a stereo-vision system mounted on a mobile robot. First, a set of interesting points are extracted from the left image. A disparity map, provided by a real-time stereo vision algorithm implemented on FPGA, gives the 3D position of each point. In addition, velocity magnitude and orientation are obtained to characterize the set of points on the space *R*6. Groups of dynamic 2D points are formed using the *a contrario* clustering technique in the 4D space and then evaluated on their depth value yielding groups of dynamic 3D-points. Each one of these groups is initialized by a convex contour with the velocity and orientation of the points given a first estimation of the dynamic object position and velocity. Then an active contour defines a more detailed silhouette of the object based on the intensity and depth value inside of the contour. It is well known that active contour techniques require a highly dense computations. Therefore, in order to reduce the time of processing a fixed number of iterations is used at each frame, so the convergence of the object real limits will be incrementally achieved along

the main objective of the work: detect and track moving objects in real time.

\*Part of this work was developed when authors were with LAAS-CNRS, Toulouse, France

**1. Introduction**

### **3D Visual Information for Dynamic Objects Detection and Tracking During Mobile Robot Navigation**

D.-L. Almanza-Ojeda\* and M.-A. Ibarra-Manzano *Digital Signal Processing Laboratory, Electronics Department; DICIS, University of Guanajuato, Salamanca, Guanajuato Mexico*

#### **1. Introduction**

An autonomous mobile robot that navigates in outdoor environments requires functional and decisional routines enabling it to supervise the estimation and the performance of all its movements for carrying out an envisaged trajectory. At this end, a robot is usually equipped with several high-performance sensors. However, we are often interested in less complex and low-cost sensors that could provide enough information to detect in real-time when the trajectory is free of dynamic obstacles. In this context, our strategy was focused on visual sensors, particulary on stereo vision since this provides the depth coordinate for allowing a better perception of the environment. Visual perception for robot mobile navigation is a complex function that requires the presence of "salience" or "evident" patrons to identify something that "breaks" the continuous tendency of data. Usually, interesting points or segments are used for evaluating patrons in position, velocity, appearance or other characteristics that allows us forming groups (Lookingbill et al., 2007), (Talukder & Matthies, 2004). Whereas complete feature vectors are more expressive for explaining objects, here we use 3D feature points for proposing a strategy computationally less demanding conserving the main objective of the work: detect and track moving objects in real time.

This chapter presents a strategy for detecting and tracking dynamic objects using a stereo-vision system mounted on a mobile robot. First, a set of interesting points are extracted from the left image. A disparity map, provided by a real-time stereo vision algorithm implemented on FPGA, gives the 3D position of each point. In addition, velocity magnitude and orientation are obtained to characterize the set of points on the space *R*6. Groups of dynamic 2D points are formed using the *a contrario* clustering technique in the 4D space and then evaluated on their depth value yielding groups of dynamic 3D-points. Each one of these groups is initialized by a convex contour with the velocity and orientation of the points given a first estimation of the dynamic object position and velocity. Then an active contour defines a more detailed silhouette of the object based on the intensity and depth value inside of the contour. It is well known that active contour techniques require a highly dense computations. Therefore, in order to reduce the time of processing a fixed number of iterations is used at each frame, so the convergence of the object real limits will be incrementally achieved along

<sup>\*</sup>Part of this work was developed when authors were with LAAS-CNRS, Toulouse, France

processed by a stereo vision algorithm designed on FPGA that calculates the disparity map which provides the depth at each point of the input image. Thus, we use a combination of the 3D and 2D representation of feature points for grouping them according to similar distribution function in position and velocity. Moreover, these groups of points permit us to initialize an active contour for obtaining the object boundary. Therefore, the contour initialization of the

<sup>5</sup> 3D Visual Information for Dynamic Objects

A block diagram of the proposed generalized algorithm is depicted in figure 1. A stereo vision module for FPGA based on Census Transform provides the disparity map using left and right images. At the same time, a selection-tracking process of feature points is carried out on a loop. However, rather than considering both stereo vision images during interest point selection, we will consider only left image for obtaining these 2D feature points. Further, each feature point will be associated with a depth value given by the disparity map. At this point it is necessary to point out that clustering task is performed until a small number of images have been processed. According to this, clustering module will receive a spatial-temporal set of points since each feature location and velocity have been accumulated through the time. The clustering method that we use is the *a contrario* method proposed by Veit et. al. (Veit et al., 2007). This clustering method consist in grouping the interesting points which have a "coherent" movement along a short number of consecutive images. Here the term coherent refers to movements that follow a similar and constant magnitude and direction described by

Fig. 1. Global strategy of object detection and identification using a stereo vision system

follow the non-rigid objects that have more complicated movement, like pedestrians.

Once dynamic group of points have been detected by the clustering process, from this group of points it is possible to delimit a region and an irregular contour in the image with the aim of characterizing a probable dynamic object. Thus, by doing an analysis of intensity inside of this contour, we propose to match the provided region along the image sequence in order to

This work provides two distinct contributions: (1) the addition of depth information for performing the clustering of 3D set of dynamic points instead of only 2D as in the *a contrario* clustering technique (Veit et al., 2007), (2) the use of dynamic groups for initializing

detected points is fundamental in order to properly perform the shape recovering.

the probability density function of the points under evaluation.

Detection and Tracking During Mobile Robot Navigation

mounted on a mobile robot

several frames. A simple and predefined knowledge about the most usual dynamic objects found in urban environments are used to label the growing regions as a rigid or non-rigid object, essentially cars and people. Experiments on detection and tracking of vehicles and people, as well as during occlusion situations with a mobile robot in real world scenarios are presented and discussed.

#### **2. Related works**

The issue of moving object detection has been largely studied by the robotic and computer vision community. Proposed strategies use mainly a combination of active and passive sensors mounted on the mobile robot like laser (Vu & Aycard, 2009) with cameras (Katz et al., 2008), or infrared cameras (Matthies et al., 1998), just to name a few. However, a multi-sensor system requires to solve the problem of fusing the data from different sources which often requires more complex estimation cases. Indeed, more information could be acquired using several sensors but these systems are expensive and complex. To overcome those constraints, the proposed solution consist in using one or more cameras as the only source of information in the system (Talukder & Matthies, 2004), (Williamson, 1998). Essentially vision sensors provide enough information for localization and mapping (Sola et al., 2007) or for describing static and moving objects on the environment (Klappstein et al., 2008).

The stereo-vision is one of the most used techniques for reconstructing the 3D (depth) information of a scene from two images, called left and right. This information is acquired from two cameras separated by a previously established distance. The disparity map is a representation that contains the depth information of the scene. It is well known that dense stereo-vision delivers more complete information than sparse stereo-vision but this is a high-processing cost technique which enables to perform in real time using and ordinary computer system. We use a stereo-vision technique in order to detect moving objects but implemented on a re-configurable architecture that maximizes the efficiency of the system. In the last decade, several works have proposed the development of high-performance architectures to solve the stereo-vision problem i.e. digital signal processing (DSP), field programmable gate arrays (FPGA) or application-specific integrated circuits (ASIC). The ASIC devices are one of the most complicated and expensive solutions, however they afford the best conditions for developing a final commercial system (Woodfill et al., 2006). On the other hand, FPGA have allowed the creation of hardware designs in standard, high-volume parts, thereby amortizing the cost of mask sets and significantly reducing time-to-market for hardware solutions. However, engineering cost and design time for FPGA-based solutions still remain significantly higher than software-based solutions. Designers must frequently iterate the design process in order to achieve system performance requirements and simultaneously minimize the required size of the FPGA. Each iteration of this process takes hours or days to be completed (Schmit et al., 2000). Even if designing with FPGAs is faster than designing ASICs, it has a finite resource capacity which demands clever strategies for adapting versatile real-time systems (Masrani & MacLean, 2006).

#### **2.1 Overall strategy for 3D dynamic object detection from a mobile robot**

A seminal work of this strategy was presented in (Almanza-Ojeda et al., 2010) and (Almanza-Ojeda et al., 2011). Whereas the former proposes a monocamera strategy (Almanza-Ojeda et al., 2010), and the latter the fusion with the information provided by inertial (IMU) (Almanza-Ojeda et al., 2011), here we propose an extension of this strategy to a stereo vision images provided by a bank stereo mounted on the robot. The stereo images are 2 Will-be-set-by-IN-TECH

several frames. A simple and predefined knowledge about the most usual dynamic objects found in urban environments are used to label the growing regions as a rigid or non-rigid object, essentially cars and people. Experiments on detection and tracking of vehicles and people, as well as during occlusion situations with a mobile robot in real world scenarios are

The issue of moving object detection has been largely studied by the robotic and computer vision community. Proposed strategies use mainly a combination of active and passive sensors mounted on the mobile robot like laser (Vu & Aycard, 2009) with cameras (Katz et al., 2008), or infrared cameras (Matthies et al., 1998), just to name a few. However, a multi-sensor system requires to solve the problem of fusing the data from different sources which often requires more complex estimation cases. Indeed, more information could be acquired using several sensors but these systems are expensive and complex. To overcome those constraints, the proposed solution consist in using one or more cameras as the only source of information in the system (Talukder & Matthies, 2004), (Williamson, 1998). Essentially vision sensors provide enough information for localization and mapping (Sola et al., 2007) or for describing

The stereo-vision is one of the most used techniques for reconstructing the 3D (depth) information of a scene from two images, called left and right. This information is acquired from two cameras separated by a previously established distance. The disparity map is a representation that contains the depth information of the scene. It is well known that dense stereo-vision delivers more complete information than sparse stereo-vision but this is a high-processing cost technique which enables to perform in real time using and ordinary computer system. We use a stereo-vision technique in order to detect moving objects but implemented on a re-configurable architecture that maximizes the efficiency of the system. In the last decade, several works have proposed the development of high-performance architectures to solve the stereo-vision problem i.e. digital signal processing (DSP), field programmable gate arrays (FPGA) or application-specific integrated circuits (ASIC). The ASIC devices are one of the most complicated and expensive solutions, however they afford the best conditions for developing a final commercial system (Woodfill et al., 2006). On the other hand, FPGA have allowed the creation of hardware designs in standard, high-volume parts, thereby amortizing the cost of mask sets and significantly reducing time-to-market for hardware solutions. However, engineering cost and design time for FPGA-based solutions still remain significantly higher than software-based solutions. Designers must frequently iterate the design process in order to achieve system performance requirements and simultaneously minimize the required size of the FPGA. Each iteration of this process takes hours or days to be completed (Schmit et al., 2000). Even if designing with FPGAs is faster than designing ASICs, it has a finite resource capacity which demands clever strategies for adapting versatile

static and moving objects on the environment (Klappstein et al., 2008).

real-time systems (Masrani & MacLean, 2006).

**2.1 Overall strategy for 3D dynamic object detection from a mobile robot**

A seminal work of this strategy was presented in (Almanza-Ojeda et al., 2010) and (Almanza-Ojeda et al., 2011). Whereas the former proposes a monocamera strategy (Almanza-Ojeda et al., 2010), and the latter the fusion with the information provided by inertial (IMU) (Almanza-Ojeda et al., 2011), here we propose an extension of this strategy to a stereo vision images provided by a bank stereo mounted on the robot. The stereo images are

presented and discussed.

**2. Related works**

processed by a stereo vision algorithm designed on FPGA that calculates the disparity map which provides the depth at each point of the input image. Thus, we use a combination of the 3D and 2D representation of feature points for grouping them according to similar distribution function in position and velocity. Moreover, these groups of points permit us to initialize an active contour for obtaining the object boundary. Therefore, the contour initialization of the detected points is fundamental in order to properly perform the shape recovering.

A block diagram of the proposed generalized algorithm is depicted in figure 1. A stereo vision module for FPGA based on Census Transform provides the disparity map using left and right images. At the same time, a selection-tracking process of feature points is carried out on a loop. However, rather than considering both stereo vision images during interest point selection, we will consider only left image for obtaining these 2D feature points. Further, each feature point will be associated with a depth value given by the disparity map. At this point it is necessary to point out that clustering task is performed until a small number of images have been processed. According to this, clustering module will receive a spatial-temporal set of points since each feature location and velocity have been accumulated through the time. The clustering method that we use is the *a contrario* method proposed by Veit et. al. (Veit et al., 2007). This clustering method consist in grouping the interesting points which have a "coherent" movement along a short number of consecutive images. Here the term coherent refers to movements that follow a similar and constant magnitude and direction described by the probability density function of the points under evaluation.

Fig. 1. Global strategy of object detection and identification using a stereo vision system mounted on a mobile robot

Once dynamic group of points have been detected by the clustering process, from this group of points it is possible to delimit a region and an irregular contour in the image with the aim of characterizing a probable dynamic object. Thus, by doing an analysis of intensity inside of this contour, we propose to match the provided region along the image sequence in order to follow the non-rigid objects that have more complicated movement, like pedestrians.

This work provides two distinct contributions: (1) the addition of depth information for performing the clustering of 3D set of dynamic points instead of only 2D as in the *a contrario* clustering technique (Veit et al., 2007), (2) the use of dynamic groups for initializing

Fig. 2. Block diagram of the stereo vision algorithm (Ibarra-Manzano & Almanza-Ojeda,

median spatial filter was found more convenient.

Detection and Tracking During Mobile Robot Navigation

**3.1 Disparity map acquired in real time**

which is called the correlation process allows us to obtain a disparity measure. The disparity measure comes from the similarity maximization function in the same epipolar line for the two Census images. The similarity evaluation is based on the binary comparison between two bit chains calculated by the Census Transform. The correlation process is carried out two times, (left to right then right to left) with the aim of reducing the disparity error and thus complementing the process. Once both disparity measures have been obtained, the disparity measure validation (right to left and left to right) consists of comparing both disparity values and obtaining the absolute difference between them. Before delivering a final disparity image, a novel filtering process is needed for improving its quality. In this final stage, the use of a

<sup>7</sup> 3D Visual Information for Dynamic Objects

The final architecture for executing the stereo vision algorithm based on the Census Transform was developed using the level design flow RTL (Ibarra-Manzano, Devy, Boizard, Lacroix & Fourniols, 2009). The architecture was codified in VHDL language using Quartus II workspace and ModelSim. Finally, it was synthesized for an EP2C35F672C6 device contained in the Cyclone IV family of Altera. Table 1 lays out some of the configuration parameters used during the disparity map computation exploited during the dynamic object detection and tracking approach. We would like to highlight that disparity image is computed through the time with a high performance of 30 image per second although detection and tracking

2011)

an irregular contour active that temporally recovers actual object boundary. The second contribution was preliminary discussed in (Almanza-Ojeda et al., 2011) where the same active contour was initialized for representing object region, however that only uses a snake based on contour information, so nothing is doing with features inside of the region.

The structure of the chapter is as follows. In section 3, we describe the stereo vision module and its performance. Then section 4.1 details interest 2D point selection and how the 3D points are obtained using the disparity map. The grouping technique of the interest points based on the *a contrario* clustering is explained in section 4.2. We present in section 5 object shape recovering using active contours. Section 6 contains our experimental results from indoor an outdoor environments. Finally, we end up with conclusions in section 7.

#### **3. Stereo vision module**

We use a disparity map calculated using the Census Transform algorithm (Zabih & Woodfill, 1994) implemented on a programmable device, in this case a FPGA (Field Programmable Gate Array). The architecture of the Census transform algorithm was developed by (Ibarra-Manzano, Devy, Boizard, Lacroix & Fourniols, 2009) in which left and right images acquired from the stereo vision bank are processed for generating up to 325 dense disparity maps of 640 × 480 pixels per second. It is important to point out that most of the vision-based systems do not require high video-frame rates because usually they are implemented on computers or embedded platforms which are not FPGA-based. As this is also our case, we have adapted the disparity map generation to the real-time application required by our system by tuning some configuration parameters in the architecture. A block diagram of the stereo vision algorithm is shown in figure 2. In the following, we will describe in general the calculation of the disparity map based on the Census transform. However, the architectural implementation on the FPGA is a problem that has not dealt with in this work, all these details will be found in (Ibarra-Manzano, Devy, Boizard, Lacroix & Fourniols, 2009), (Ibarra-Manzano & Almanza-Ojeda, 2011).

In the stereo vision algorithm each of the images (right and left) are processed independently in parallel. The process begins with the rectification and correction of the distortion for each image in order to decrease the size of the search of points to a single dimension during disparity calculation. This strategy is known as epipolar restriction in which, once the main axes of the cameras have been aligned in parallel, founding the displacement of the position between the two pixels (one per camera) is reduced to search in each aligned line. That is, if any pair of pixels is visible in both cameras and assuming they are the projection of a single point in the scene, then both pixels must be aligned on the same epipolar line (Ibarra-Manzano, Almanza-Ojeda, Devy, Boizard & Fourniols, 2009). Therefore under this condition, an object location in the scene is reduced to a horizontal translation. Furthermore, the use of the epipolar restriction allows to reduce the complexity and the size of the final architecture.

Next, rectified and corrected images are filtered using an arithmetic mean filter. Once input images have been filtered, they are used to calculate the Census Transform as depicted in the figure 2. This transform is a non-parametric measure used during the matching process for measuring similarities and obtaining the correspondence between the points into the left and right images. A neighborhood of pixels is used for establishing the relationships among them. From the Census Transform, two images are obtained referred to as *ICl* and *ICr* which represent the left and right Census images. Two pixels extracted from the Census images (one for each image) are compared using the Hamming distance. This comparison 4 Will-be-set-by-IN-TECH

an irregular contour active that temporally recovers actual object boundary. The second contribution was preliminary discussed in (Almanza-Ojeda et al., 2011) where the same active contour was initialized for representing object region, however that only uses a snake based

The structure of the chapter is as follows. In section 3, we describe the stereo vision module and its performance. Then section 4.1 details interest 2D point selection and how the 3D points are obtained using the disparity map. The grouping technique of the interest points based on the *a contrario* clustering is explained in section 4.2. We present in section 5 object shape recovering using active contours. Section 6 contains our experimental results from indoor an

We use a disparity map calculated using the Census Transform algorithm (Zabih & Woodfill, 1994) implemented on a programmable device, in this case a FPGA (Field Programmable Gate Array). The architecture of the Census transform algorithm was developed by (Ibarra-Manzano, Devy, Boizard, Lacroix & Fourniols, 2009) in which left and right images acquired from the stereo vision bank are processed for generating up to 325 dense disparity maps of 640 × 480 pixels per second. It is important to point out that most of the vision-based systems do not require high video-frame rates because usually they are implemented on computers or embedded platforms which are not FPGA-based. As this is also our case, we have adapted the disparity map generation to the real-time application required by our system by tuning some configuration parameters in the architecture. A block diagram of the stereo vision algorithm is shown in figure 2. In the following, we will describe in general the calculation of the disparity map based on the Census transform. However, the architectural implementation on the FPGA is a problem that has not dealt with in this work, all these details will be found in (Ibarra-Manzano, Devy, Boizard, Lacroix & Fourniols,

In the stereo vision algorithm each of the images (right and left) are processed independently in parallel. The process begins with the rectification and correction of the distortion for each image in order to decrease the size of the search of points to a single dimension during disparity calculation. This strategy is known as epipolar restriction in which, once the main axes of the cameras have been aligned in parallel, founding the displacement of the position between the two pixels (one per camera) is reduced to search in each aligned line. That is, if any pair of pixels is visible in both cameras and assuming they are the projection of a single point in the scene, then both pixels must be aligned on the same epipolar line (Ibarra-Manzano, Almanza-Ojeda, Devy, Boizard & Fourniols, 2009). Therefore under this condition, an object location in the scene is reduced to a horizontal translation. Furthermore, the use of the epipolar restriction allows to reduce the complexity and the size of the final

Next, rectified and corrected images are filtered using an arithmetic mean filter. Once input images have been filtered, they are used to calculate the Census Transform as depicted in the figure 2. This transform is a non-parametric measure used during the matching process for measuring similarities and obtaining the correspondence between the points into the left and right images. A neighborhood of pixels is used for establishing the relationships among them. From the Census Transform, two images are obtained referred to as *ICl* and *ICr* which represent the left and right Census images. Two pixels extracted from the Census images (one for each image) are compared using the Hamming distance. This comparison

on contour information, so nothing is doing with features inside of the region.

outdoor environments. Finally, we end up with conclusions in section 7.

**3. Stereo vision module**

architecture.

2009), (Ibarra-Manzano & Almanza-Ojeda, 2011).

Fig. 2. Block diagram of the stereo vision algorithm (Ibarra-Manzano & Almanza-Ojeda, 2011)

which is called the correlation process allows us to obtain a disparity measure. The disparity measure comes from the similarity maximization function in the same epipolar line for the two Census images. The similarity evaluation is based on the binary comparison between two bit chains calculated by the Census Transform. The correlation process is carried out two times, (left to right then right to left) with the aim of reducing the disparity error and thus complementing the process. Once both disparity measures have been obtained, the disparity measure validation (right to left and left to right) consists of comparing both disparity values and obtaining the absolute difference between them. Before delivering a final disparity image, a novel filtering process is needed for improving its quality. In this final stage, the use of a median spatial filter was found more convenient.

#### **3.1 Disparity map acquired in real time**

The final architecture for executing the stereo vision algorithm based on the Census Transform was developed using the level design flow RTL (Ibarra-Manzano, Devy, Boizard, Lacroix & Fourniols, 2009). The architecture was codified in VHDL language using Quartus II workspace and ModelSim. Finally, it was synthesized for an EP2C35F672C6 device contained in the Cyclone IV family of Altera. Table 1 lays out some of the configuration parameters used during the disparity map computation exploited during the dynamic object detection and tracking approach. We would like to highlight that disparity image is computed through the time with a high performance of 30 image per second although detection and tracking

sight in the right camera. Therefore, the dense disparity map shown in figure 4 (c) does not display a cohesive depth value of the vehicle due to mismatches in the information between both images. Nevertheless, we highlight all the different depths represented by the gray color value in the highway that gradually turns darker until the black color which represents an

<sup>9</sup> 3D Visual Information for Dynamic Objects

(a) Left image (b) Right image (c) Result from FPGA

Fig. 4. Stereo images acquired from a vehicle in the highway: a) left image b) right image and

When dealing with vision-based approaches, the problem of processing a large quantity of information requires that the system resources be sophisticated and expensive if we want to get a real time performance. As we work with left and right images from the stereo vision bank, processing both images yields to more information about the scene but has also more computation requirements. To overcome this problem, we consider to deal with a sparse set of features resulted by the analysis of the left image that represents the most significant and salience feature points in all the image. We use this sparse proposition because it is important to distribute time of processing among some others essential tasks of the strategy, therefore feature selection process must expend minimal time. For the feature point selection, we use the Shi-Tomasi approach (Shi & Tomasi, 1994). This approach consists in providing the most representative points based on image gradient analysis, i.e. corners, borders, and all the regions with high contrast change. Figure 5 depicts both image gradients in horizontal and vertical directions of an input image in gray color level, from which *N* best features are selected for describing image content. In particular, this input image has high content of information, therefore, a larger number of points have to be selected in accordance with image-density information. Furthermore, the number *N* is restricted by the time of processing required for reaching real-time performance. According to this, the number of points does not have to exceed *N* = 180. Once 2D's interesting points have been selected on the left image and the disparity map for the stereo-images computed, obtaining the 3D characterization of the points is straightforward. Each 2D point is associated with a corresponding depth value

provided by the disparity image to conform the 3D point representation.

Until now, we have obtain a set of 2D features on the left image at time *t* and their corresponding 3D characterization. For each feature, displacement vectors are computed through the time by using the Kanade-Lucas-Tomasi tracker, referred to as the KLT

implementation

infinity depth.

c) the disparity map.

**4.1 Active 3D point selection**

**4. Moving features perception and grouping**

Detection and Tracking During Mobile Robot Navigation

Parameter Value Image size 640 × 480 Window size 7 × 7 Disparity max 64 Performance 40 Latency (*μs*) 206 Area 6, 977 Memory size 109 Kb

approach does not reach this performance. More technical details about the implementation are discussed in section 6.

Table 1. Configuration parameters of the Stereo vision architecture.

The architecture was tested for different navigational scenes using a stereo vision bank, first mounted in a mobile robot and then in a vehicle. In the following, we will describe the obtained results for two operational environments. Figure 3 shows the left and right images acquired from the bank stereo and the associated disparity image delivered by the stereo vision algorithm. Dense disparity image depicts the disparity value in gray color levels in figure 3 (c). By examining this last image, we can determine that if the object is close to the stereo vision bank that means a big disparity value, so it corresponds to a light gray level. Otherwise, if the object is far from the stereo vision bank, the disparity value is low, which corresponds to a dark gray level. In this way, we observe that the gray color which represents the road in the resulting images gradually changes from the light to dark gray level. We point out the right side of the image, where we can see the different tones of gray level corresponding to the building. Since large of the building is located at different depths with respect to the stereo vision bank, in the disparity map corresponding gray color value is assigned from lighter to darker gray tones.

Fig. 3. Stereo images acquired from a mobile robot during outdoor navigation: a) left image b) right image and c) the disparity map.

implementation

In the second test (see figure 4), the stereo vision bank is mounted on a vehicle that is driven on a highway. This experimental test results in a difficult situation because the vehicle is driven at high-speed during the test. Furthermore, this urban condition requires a big distance between both cameras in the stereo vision bank with the aim of augmenting the field of view. A consequence of having a big distance between the two cameras is represented in figure 4 (a) and (b) in which, while the left image show a car that overtakes our vehicle, this car is out of 6 Will-be-set-by-IN-TECH

approach does not reach this performance. More technical details about the implementation

Parameter Value Image size 640 × 480 Window size 7 × 7 Disparity max 64 Performance 40 Latency (*μs*) 206 Area 6, 977 Memory size 109 Kb

The architecture was tested for different navigational scenes using a stereo vision bank, first mounted in a mobile robot and then in a vehicle. In the following, we will describe the obtained results for two operational environments. Figure 3 shows the left and right images acquired from the bank stereo and the associated disparity image delivered by the stereo vision algorithm. Dense disparity image depicts the disparity value in gray color levels in figure 3 (c). By examining this last image, we can determine that if the object is close to the stereo vision bank that means a big disparity value, so it corresponds to a light gray level. Otherwise, if the object is far from the stereo vision bank, the disparity value is low, which corresponds to a dark gray level. In this way, we observe that the gray color which represents the road in the resulting images gradually changes from the light to dark gray level. We point out the right side of the image, where we can see the different tones of gray level corresponding to the building. Since large of the building is located at different depths with respect to the stereo vision bank, in the disparity map corresponding gray color value is

(a) Left image (b) Right image (c) Result from FPGA

Fig. 3. Stereo images acquired from a mobile robot during outdoor navigation: a) left image

In the second test (see figure 4), the stereo vision bank is mounted on a vehicle that is driven on a highway. This experimental test results in a difficult situation because the vehicle is driven at high-speed during the test. Furthermore, this urban condition requires a big distance between both cameras in the stereo vision bank with the aim of augmenting the field of view. A consequence of having a big distance between the two cameras is represented in figure 4 (a) and (b) in which, while the left image show a car that overtakes our vehicle, this car is out of

implementation

Table 1. Configuration parameters of the Stereo vision architecture.

assigned from lighter to darker gray tones.

b) right image and c) the disparity map.

are discussed in section 6.

sight in the right camera. Therefore, the dense disparity map shown in figure 4 (c) does not display a cohesive depth value of the vehicle due to mismatches in the information between both images. Nevertheless, we highlight all the different depths represented by the gray color value in the highway that gradually turns darker until the black color which represents an infinity depth.

Fig. 4. Stereo images acquired from a vehicle in the highway: a) left image b) right image and c) the disparity map.

implementation

#### **4. Moving features perception and grouping**

#### **4.1 Active 3D point selection**

When dealing with vision-based approaches, the problem of processing a large quantity of information requires that the system resources be sophisticated and expensive if we want to get a real time performance. As we work with left and right images from the stereo vision bank, processing both images yields to more information about the scene but has also more computation requirements. To overcome this problem, we consider to deal with a sparse set of features resulted by the analysis of the left image that represents the most significant and salience feature points in all the image. We use this sparse proposition because it is important to distribute time of processing among some others essential tasks of the strategy, therefore feature selection process must expend minimal time. For the feature point selection, we use the Shi-Tomasi approach (Shi & Tomasi, 1994). This approach consists in providing the most representative points based on image gradient analysis, i.e. corners, borders, and all the regions with high contrast change. Figure 5 depicts both image gradients in horizontal and vertical directions of an input image in gray color level, from which *N* best features are selected for describing image content. In particular, this input image has high content of information, therefore, a larger number of points have to be selected in accordance with image-density information. Furthermore, the number *N* is restricted by the time of processing required for reaching real-time performance. According to this, the number of points does not have to exceed *N* = 180. Once 2D's interesting points have been selected on the left image and the disparity map for the stereo-images computed, obtaining the 3D characterization of the points is straightforward. Each 2D point is associated with a corresponding depth value provided by the disparity image to conform the 3D point representation.

Until now, we have obtain a set of 2D features on the left image at time *t* and their corresponding 3D characterization. For each feature, displacement vectors are computed through the time by using the Kanade-Lucas-Tomasi tracker, referred to as the KLT

In the following, we explain the a *contrario* method used for clustering position and velocity

<sup>11</sup> 3D Visual Information for Dynamic Objects

(a) First image of the tracking (b) Last image of the tracking (c) Accumulate position

Fig. 6. *N* feature points initially detected at image a) are tracked through 4 images. Image b)

At this point, we have a distributed set of 2D feature points characterized by their position and velocity that have been tracked through a short number of consecutive images. Alternatively, we associated each feature with their corresponding depth position which allows us to manage with a 3D-data set. Following with the global diagram in figure 1, clustering of feature points is the next task to carry out. In order to do that, we use the *a contrario* clustering method proposed in (Veit et al., 2007). This algorithm is based on the Gestalt theory that establishes which groups could be formed based on one or several common characteristics of their elements. In accord to this statement, the *a contrario* clustering technique identifies one group as meaningful if all their elements show a different distribution than an established background random model. Contrary to most clustering techniques, neither initial number of clusters is required nor parameter have to be tuned. These characteristics result very favorable in an unknown environment context where the number of resulted clusters have not been

In this section we summarize some important concepts of the *a contrario* clustering method, used to group feature points. A detailed description of the method derivation is available

As we mentioned above, a distribution model for the background has to be defined for comparing with the associated distribution of the set of points, here referred to as *V*(*x*, *y*, *v*, *θ*) in *R*4. In this work, we use the background model proposed in (Veit et al., 2007) which establishes a random organization of the observations. Therefore, background model elements are independent identically distributed (*iid*) and follow a distribution *p*. The *iid* nature of random model components proposes an organization with non coherent motion

Next, given the input vector *V*(*x*, *y*, *v*, *θ*) from the KLT process (section 4.1), the first objective is to evaluate which elements in *V* shows a particular distribution contrary to the established distribution *p* of the background model (that explains "*a contrario*" name). To overcome

in (Desolneux et al., 2008), (Desolneux et al., 2003), (Cao et al., 2007).

displays the last image during the tracking task. Image c) depicts all the accumulate

positions for the N initial points detected calculated by the tracking process.

through 4 images

of the points that possibly describe a mobile object.

Detection and Tracking During Mobile Robot Navigation

**4.2 Clustering of 3D points cloud**

**4.3** *A contrario* **algorithm description**

predefined.

present.

(a) Image gradients

(b) N best features selected

Fig. 5. Image processing for the best interest point selection. a) Gradients in vertical and horizontal direction of the above input image. b) Green points represent the *N* best interesting features resulted by the gradient image analysis.

technique (Shi & Tomasi, 1994), (Lucas & Kanade, 1981). These displacement vectors are used to calculate feature velocities. We are interested in the accumulation of previous position and velocity of the points in order to establish a trail of motion. An example of the accumulation point positions for *N* initial features detected appears in figure 6. Note that, while most of the points are distributed in all the image, a small set of points horizontally aligned can be remarked on the left side of the figure c). These points represent the front part of the vehicle that enters into the field of view of our robot. Once a short number of images have been processed, the accumulated vector displacements and positions of the feature points are evaluated in order to find significant patterns of motion that possibly represent dynamic objects in the scene. A new feature selection task is carried out, as indicated in the green block of figure 1. Further, in the case that any dynamic group of points is found, this information will initialize in the second stage of our strategy, that is the object characterization (right rectangular box in figure 1).

In the following, we explain the a *contrario* method used for clustering position and velocity of the points that possibly describe a mobile object.

8 Will-be-set-by-IN-TECH

(a) Image gradients

(b) N best features selected

technique (Shi & Tomasi, 1994), (Lucas & Kanade, 1981). These displacement vectors are used to calculate feature velocities. We are interested in the accumulation of previous position and velocity of the points in order to establish a trail of motion. An example of the accumulation point positions for *N* initial features detected appears in figure 6. Note that, while most of the points are distributed in all the image, a small set of points horizontally aligned can be remarked on the left side of the figure c). These points represent the front part of the vehicle that enters into the field of view of our robot. Once a short number of images have been processed, the accumulated vector displacements and positions of the feature points are evaluated in order to find significant patterns of motion that possibly represent dynamic objects in the scene. A new feature selection task is carried out, as indicated in the green block of figure 1. Further, in the case that any dynamic group of points is found, this information will initialize in the second stage of our strategy, that is the object characterization (right

Fig. 5. Image processing for the best interest point selection. a) Gradients in vertical and horizontal direction of the above input image. b) Green points represent the *N* best

interesting features resulted by the gradient image analysis.

rectangular box in figure 1).

(a) First image of the tracking (b) Last image of the tracking (c) Accumulate position through 4 images

Fig. 6. *N* feature points initially detected at image a) are tracked through 4 images. Image b) displays the last image during the tracking task. Image c) depicts all the accumulate positions for the N initial points detected calculated by the tracking process.

#### **4.2 Clustering of 3D points cloud**

At this point, we have a distributed set of 2D feature points characterized by their position and velocity that have been tracked through a short number of consecutive images. Alternatively, we associated each feature with their corresponding depth position which allows us to manage with a 3D-data set. Following with the global diagram in figure 1, clustering of feature points is the next task to carry out. In order to do that, we use the *a contrario* clustering method proposed in (Veit et al., 2007). This algorithm is based on the Gestalt theory that establishes which groups could be formed based on one or several common characteristics of their elements. In accord to this statement, the *a contrario* clustering technique identifies one group as meaningful if all their elements show a different distribution than an established background random model. Contrary to most clustering techniques, neither initial number of clusters is required nor parameter have to be tuned. These characteristics result very favorable in an unknown environment context where the number of resulted clusters have not been predefined.

In this section we summarize some important concepts of the *a contrario* clustering method, used to group feature points. A detailed description of the method derivation is available in (Desolneux et al., 2008), (Desolneux et al., 2003), (Cao et al., 2007).

#### **4.3** *A contrario* **algorithm description**

As we mentioned above, a distribution model for the background has to be defined for comparing with the associated distribution of the set of points, here referred to as *V*(*x*, *y*, *v*, *θ*) in *R*4. In this work, we use the background model proposed in (Veit et al., 2007) which establishes a random organization of the observations. Therefore, background model elements are independent identically distributed (*iid*) and follow a distribution *p*. The *iid* nature of random model components proposes an organization with non coherent motion present.

Next, given the input vector *V*(*x*, *y*, *v*, *θ*) from the KLT process (section 4.1), the first objective is to evaluate which elements in *V* shows a particular distribution contrary to the established distribution *p* of the background model (that explains "*a contrario*" name). To overcome

*NFA* (*G*) <sup>=</sup> *<sup>N</sup>*<sup>2</sup> · |H| min

Detection and Tracking During Mobile Robot Navigation

these four distributions. A group *G* is said to be meaningful if *NFA* (*G*) ≤ 1.

*N*� = *N* − 2, number of elements in *G*<sup>1</sup> and *G*2, respectively *n*�

the point location in the image and *z* denotes the disparity value.

*NFAG* (*G*1, *<sup>G</sup>*2) <sup>=</sup> *<sup>N</sup>*<sup>4</sup> · |H|

term T which represents the accumulated trinomial law.

b).

**4.4 Depth validation**

*X* ∈ *G*, *H* ∈ H, *G* ⊂ *HX*

In this equation *N* represents the number of elements in vector *V*, |H| is the cardinality of regions and *n* is the elements in group test *G*. The term which appears in the minimum function is the accumulated binomial law, this represents the probability that at least *n* points including *X* are inside the region test centered in *X* (*HX*). Distribution *p* consist of four independent distributions, one for each dimension data. Point positions and velocity orientation follow a uniform distribution because object moving position and direction is arbitrary. On the other hand, velocity magnitude distribution is obtained directly of the empirically histogram of the observed data. So that, joint distribution p will be the product of

<sup>13</sup> 3D Visual Information for Dynamic Objects

Furthermore two sibling meaningful groups in the binary tree could be belong to the same moving object, then a second evaluation for all the meaningful groups is calculated by Eq. 2.To obtain this new measure, we reuse region group information (dimensions and probability) and just a new region that contains both test groups *G*<sup>1</sup> and *G*<sup>2</sup> is calculated. New terms are

Both mesures 1 and 2 represent the significance of groups in binary tree. Final clusters are found by exploring all the binary tree and comparing to see if it is more significant to have two moving objects *G*<sup>1</sup> and *G*<sup>2</sup> or to fusion it in a group *G*. Mathematically, *NFA*(*G*) < *NFAG*(*G*1, *G*2) where *G*<sup>1</sup> ∪ *G*<sup>2</sup> ⊂ *G*. A descriptive result is provided in figure 8. Here blue points correspond to those that could be dynamic but without a well defined motion, so they are associated with the background. On the other hand, green points represent the group in *G* which shows a different distribution than the random background model. Notice that graph c) displays polar velocity considering magnitude in X-axis and orientation in Y-axis, from there, green point positions are not together because they have orientations among 5◦ and 355◦ Further, these points correspond to the vehicle entering on the right side of the image

As previously mentioned, the 3D characterization of points is achieved using the depth value described in the disparity map for each two dimensional feature point. From previous section, a set of dynamic points have been obtained using the clustering process, however this process was performed considering uniquely 2D points. Thanks to the disparity map, we know the depth of each feature in dynamic group, so it is possible to perform a second evaluation looking for similarity in their depths. This depth evaluation is computed in a formally analogous way to that of the *a contrario* clustering. However in this case, the number of regions for testing is considerably reduced since the group of points is already detected in a defined region, so it is only need the evaluation of similar depths of the points around that region avoiding the best region search. Additionally, there is not an associated velocity in Z-axis direction which reduces from hyper-rectangle regions to 3D boxes. Namely *x* and *y* denote

<sup>2</sup> <sup>T</sup> *N*� , *n*� <sup>1</sup>, *n*�

*B* (*N* − 1, *n* − 1, *p* (*HX*)) (1)

<sup>2</sup> = *n*<sup>1</sup> − 1 and *n*�

2, *p*1, *p*<sup>2</sup>

<sup>2</sup> = *n*<sup>2</sup> − 1, and

(2)

Fig. 7. Best fit region search. An initial set of points is selected and tracked in an indoor environment displayed in image a). The accumulated locations and velocities of the points are analyzed in order to find the region of points with *NFA* (*G*) ≤ 1.

the problem of point by point evaluation, *V*(*x*, *y*, *v*, *θ*) is divided in testing groups with different size of elements using a single linkage method. This method constructs a binary tree where each node represents a candidate group *G*. Once a different group of points have been established, these will be evaluate using a set of given regions represented by H. This set of regions is formed by a different size of hyper-rectangles that will be used to test the distribution of each data group in *G*. An example of groups distribution evaluation is depicted in figure 7. Each region *H* ∈ H is centered at each element *X* ∈ *G* to find the region *HX* that contains all the elements in *G* and at the same time this region has to minimize the probability of the background model distribution. This procedure requires that sizes of hyper-rectangles be in function of data range, in our experiments we use 20 different sizes by dimension. The measure of meaningfulness (called Number of False Alarms *NFA* in referenced work) is given by eq 1.

10 Will-be-set-by-IN-TECH

(a) Point detection

(b) Position graph (c) Polar velocity graph

the problem of point by point evaluation, *V*(*x*, *y*, *v*, *θ*) is divided in testing groups with different size of elements using a single linkage method. This method constructs a binary tree where each node represents a candidate group *G*. Once a different group of points have been established, these will be evaluate using a set of given regions represented by H. This set of regions is formed by a different size of hyper-rectangles that will be used to test the distribution of each data group in *G*. An example of groups distribution evaluation is depicted in figure 7. Each region *H* ∈ H is centered at each element *X* ∈ *G* to find the region *HX* that contains all the elements in *G* and at the same time this region has to minimize the probability of the background model distribution. This procedure requires that sizes of hyper-rectangles be in function of data range, in our experiments we use 20 different sizes by dimension. The measure of meaningfulness (called Number of False Alarms *NFA* in referenced work) is given

Fig. 7. Best fit region search. An initial set of points is selected and tracked in an indoor environment displayed in image a). The accumulated locations and velocities of the points

are analyzed in order to find the region of points with *NFA* (*G*) ≤ 1.

by eq 1.

$$NFA\left(G\right) = N^2 \cdot \left|\mathcal{H}\right| \min\_{\begin{subarray}{c} X \in \mathcal{G}, \\ H \in \mathcal{H}, \\ G \subset H\_X \end{subarray}} B\left(N-1, n-1, p\left(H\_X\right)\right) \tag{1}$$

In this equation *N* represents the number of elements in vector *V*, |H| is the cardinality of regions and *n* is the elements in group test *G*. The term which appears in the minimum function is the accumulated binomial law, this represents the probability that at least *n* points including *X* are inside the region test centered in *X* (*HX*). Distribution *p* consist of four independent distributions, one for each dimension data. Point positions and velocity orientation follow a uniform distribution because object moving position and direction is arbitrary. On the other hand, velocity magnitude distribution is obtained directly of the empirically histogram of the observed data. So that, joint distribution p will be the product of these four distributions. A group *G* is said to be meaningful if *NFA* (*G*) ≤ 1.

Furthermore two sibling meaningful groups in the binary tree could be belong to the same moving object, then a second evaluation for all the meaningful groups is calculated by Eq. 2.To obtain this new measure, we reuse region group information (dimensions and probability) and just a new region that contains both test groups *G*<sup>1</sup> and *G*<sup>2</sup> is calculated. New terms are *N*� = *N* − 2, number of elements in *G*<sup>1</sup> and *G*2, respectively *n*� <sup>2</sup> = *n*<sup>1</sup> − 1 and *n*� <sup>2</sup> = *n*<sup>2</sup> − 1, and term T which represents the accumulated trinomial law.

$$NFA\_G\left(G\_1, G\_2\right) = N^4 \cdot \left|\mathcal{H}\right|^2 \mathcal{T}\left(N', n\_1', n\_2', p\_1, p\_2\right) \tag{2}$$

Both mesures 1 and 2 represent the significance of groups in binary tree. Final clusters are found by exploring all the binary tree and comparing to see if it is more significant to have two moving objects *G*<sup>1</sup> and *G*<sup>2</sup> or to fusion it in a group *G*. Mathematically, *NFA*(*G*) < *NFAG*(*G*1, *G*2) where *G*<sup>1</sup> ∪ *G*<sup>2</sup> ⊂ *G*. A descriptive result is provided in figure 8. Here blue points correspond to those that could be dynamic but without a well defined motion, so they are associated with the background. On the other hand, green points represent the group in *G* which shows a different distribution than the random background model. Notice that graph c) displays polar velocity considering magnitude in X-axis and orientation in Y-axis, from there, green point positions are not together because they have orientations among 5◦ and 355◦ Further, these points correspond to the vehicle entering on the right side of the image b).

#### **4.4 Depth validation**

As previously mentioned, the 3D characterization of points is achieved using the depth value described in the disparity map for each two dimensional feature point. From previous section, a set of dynamic points have been obtained using the clustering process, however this process was performed considering uniquely 2D points. Thanks to the disparity map, we know the depth of each feature in dynamic group, so it is possible to perform a second evaluation looking for similarity in their depths. This depth evaluation is computed in a formally analogous way to that of the *a contrario* clustering. However in this case, the number of regions for testing is considerably reduced since the group of points is already detected in a defined region, so it is only need the evaluation of similar depths of the points around that region avoiding the best region search. Additionally, there is not an associated velocity in Z-axis direction which reduces from hyper-rectangle regions to 3D boxes. Namely *x* and *y* denote the point location in the image and *z* denotes the disparity value.

with feature points, here we describe a strategy for recovering the deformable shape of the objects through the time by considering others features like intensity image gradient inside of contour. Therefore, through this section we will describe fundamental details of the active

<sup>15</sup> 3D Visual Information for Dynamic Objects

The results obtained in section 4.2 allow the definition of an initial irregular contour that contains totally or partially our interest object. To this end, we take an outer location of points in the detected group for initializing an active contour which delimits the object on the image. An example is displayed in figure 9 1: in image (a) a set of points dynamic (in blue) is detected on which correspond to the person in motion on this sequence. From this set of points we have selected those illustrated in color magenta to describe the object contour depicted in yellow. Due to the fact these points are the most farthest from the center then they are the most representative and closest to the object frontiers. For the results show in image 9(a), we have 8 points on the curve and we use for each 4 control points. The value of 4 control points is fixed for each point on the frontier in order to introduce the corners or discontinuities in the curve (Marin-Hernandez, 2004). Initial contour will allow us to obtain a deformable model by a bounded potential shown in figure 9(b). The zone occupied by the object (represented in red color) is separated from the background. The most intens tones inside of the object are used to best adapt initial contour to the actual object silhouette. Therefore, a more detailed object shape is achieved by analyzing internal and external energy in a bounding box that contains

(b) energy functional

contours theory, and the object tracking procedure by means of the active contours.

**5.1 Active contour initialization**

Detection and Tracking During Mobile Robot Navigation

the initial contour.

object

**5.1.1 Parametric active contours.**

CAVIAR project/IST 2001 37540

to minimize the energy expressed by:

(a) Initial contour of a non-rigid

Fig. 9. Test performed with real images acquired by a fixed camera in an indoor

environment. a)Initial contour derived from points further away from the center b) Energy functional that concentrates internal and external energy considering the initial contour of a).

At this point, it is necessary to represent the initial contour in 9 by a parametric curve *u*(*τ*) = (*x*(*τ*), *y*(*τ*)), *τ* ∈ [0, 1], with *u*(0) = *u*(1). this contour is deformed through the time domain

<sup>1</sup> This image sequence was downloaded from the web site (Fisher, 2011) provided by EC Funded

Fig. 8. Clustering results after processing 5 consecutive images. a)First image used for feature selection, b) Last image used for obtaining the trails of the points. A unique dynamic group of points is detected (green points) for whose graph c) depicts their position and graph d) their velocity and orientation.

In the next section, we explain how an irregular contour is initialized using the groups of points calculated by the *a contrario* clustering, therefore used for passing from a dynamic set of points to actual object boundary.

#### **5. Object characterization by an active contour**

Active models have been widely used in image processing applications, in particular for recovering shapes and the tracking of moving objects (Li et al., 2010), (Paragios & Deriche, 2005). An active contour or snake is a curve which minimizes energy from restrictive external and internal forces in the image, typically calculated from edges, gradient, among others. Essentially, a snake is not thought of to solve the problem of automatic search for prominent contours of the image, but rather for recovering the contour of a form, from an initial position proposed by other mechanisms. That is to say, if the initial contour is relatively close to the solution (for example a contour defined manually by an operator or obtained through any other method), the contour evolves up until minimizing the function of energy defined from the internal and external forces.

One of the main objectives of this work is to track all mobile objects detected without any prior knowledge about object type that one follows. In a real and dynamic context, we expect to find rigid and non rigid mobile objects ie. vehicles, persons. We have shown in section 4.2 the good performance of the *a contrario* method for finding initial sets of dynamic points which correspond to the mobile objects in the scene. However, whereas clustering process deals with feature points, here we describe a strategy for recovering the deformable shape of the objects through the time by considering others features like intensity image gradient inside of contour. Therefore, through this section we will describe fundamental details of the active contours theory, and the object tracking procedure by means of the active contours.

#### **5.1 Active contour initialization**

12 Will-be-set-by-IN-TECH

(a) Initial image (b) Final image

(c) Position points (d) polar-velocity points

Fig. 8. Clustering results after processing 5 consecutive images. a)First image used for feature selection, b) Last image used for obtaining the trails of the points. A unique dynamic group of points is detected (green points) for whose graph c) depicts their position and graph d)

In the next section, we explain how an irregular contour is initialized using the groups of points calculated by the *a contrario* clustering, therefore used for passing from a dynamic set

Active models have been widely used in image processing applications, in particular for recovering shapes and the tracking of moving objects (Li et al., 2010), (Paragios & Deriche, 2005). An active contour or snake is a curve which minimizes energy from restrictive external and internal forces in the image, typically calculated from edges, gradient, among others. Essentially, a snake is not thought of to solve the problem of automatic search for prominent contours of the image, but rather for recovering the contour of a form, from an initial position proposed by other mechanisms. That is to say, if the initial contour is relatively close to the solution (for example a contour defined manually by an operator or obtained through any other method), the contour evolves up until minimizing the function of energy defined from

One of the main objectives of this work is to track all mobile objects detected without any prior knowledge about object type that one follows. In a real and dynamic context, we expect to find rigid and non rigid mobile objects ie. vehicles, persons. We have shown in section 4.2 the good performance of the *a contrario* method for finding initial sets of dynamic points which correspond to the mobile objects in the scene. However, whereas clustering process deals

their velocity and orientation.

of points to actual object boundary.

the internal and external forces.

**5. Object characterization by an active contour**

The results obtained in section 4.2 allow the definition of an initial irregular contour that contains totally or partially our interest object. To this end, we take an outer location of points in the detected group for initializing an active contour which delimits the object on the image. An example is displayed in figure 9 1: in image (a) a set of points dynamic (in blue) is detected on which correspond to the person in motion on this sequence. From this set of points we have selected those illustrated in color magenta to describe the object contour depicted in yellow. Due to the fact these points are the most farthest from the center then they are the most representative and closest to the object frontiers. For the results show in image 9(a), we have 8 points on the curve and we use for each 4 control points. The value of 4 control points is fixed for each point on the frontier in order to introduce the corners or discontinuities in the curve (Marin-Hernandez, 2004). Initial contour will allow us to obtain a deformable model by a bounded potential shown in figure 9(b). The zone occupied by the object (represented in red color) is separated from the background. The most intens tones inside of the object are used to best adapt initial contour to the actual object silhouette. Therefore, a more detailed object shape is achieved by analyzing internal and external energy in a bounding box that contains the initial contour.

Fig. 9. Test performed with real images acquired by a fixed camera in an indoor environment. a)Initial contour derived from points further away from the center b) Energy functional that concentrates internal and external energy considering the initial contour of a).

#### **5.1.1 Parametric active contours.**

At this point, it is necessary to represent the initial contour in 9 by a parametric curve *u*(*τ*) = (*x*(*τ*), *y*(*τ*)), *τ* ∈ [0, 1], with *u*(0) = *u*(1). this contour is deformed through the time domain to minimize the energy expressed by:

<sup>1</sup> This image sequence was downloaded from the web site (Fisher, 2011) provided by EC Funded CAVIAR project/IST 2001 37540

(a) Initial snake at

(e) Initial mask at image

**5.3 Object contour validation**

(b) 30 iterations at

executing 30 iterations per image in order to find the actual object shape.

statistical validation for object contour refining can be written as:

*M* =

Fig. 10. Results of the detection and tracking of a non-rigid dynamic object, in this case a pedestrian on the same sequence of figure 9. Here, we have processed 5 consecutive images,

filter prediction. We always consider a velocity constant model and the vector state is obtained from model object displacement. However, in some cases it is first necessary to tune the initial parameters of the filter because there may exist different types of object movements producing

<sup>17</sup> 3D Visual Information for Dynamic Objects

The difficulties rising from our proposed strategy points out the sensitivity of the active contours to small discontinuities on the object edges with low-contrast because the energy is configured over the entire image. To overcome these difficulties we evaluate the disparity inside of the region corresponding with the binary mask (that will be presented in the next section figure 15). This evaluation consists in ordering in an ascendant way all the disparity values inside the region designed by the binary mask then we uniquely consider the median of the values. In particular, we consider a valid point of the object region if its disparity value is located up to the 4th percentil in the ordered list (here referred to as 4*p*). It follows that our

<sup>1</sup> *depth* <sup>≥</sup> <sup>4</sup>*<sup>p</sup>*

where M represents the binary mask. It is important to remark that the depth value is the inverse value of the disparity therefore in this case the fact of rejecting points located on the first four percentiles represents that these values have a lower disparity value, that is, disparity values of the background points are expecting to be low. This constrain allow us to develop a last validity evaluation for obtaining a most detailed and accurated representation of the object shape. The next section describes our experimental results and presents how the disparity map plays a fundamental role in increasing the efficiency and improve the obtained

(c) Image *t*<sup>0</sup> + 2 (d) Image *t*<sup>0</sup> + 4

<sup>0</sup> *otherwise* (7)

(f) Mask at image *t*<sup>0</sup> (g) Mask at image *t*<sup>0</sup> + 2 (h) Mask at image *t*<sup>0</sup> + 4

image *t*<sup>0</sup>

Detection and Tracking During Mobile Robot Navigation

an undesired acceleration of the predicted position.

image *t*<sup>0</sup>

*t*0

results.

$$E\_{\rm snake} = \int\_0^1 \left[ E\_{\rm int}(\mu(\tau)) + E\_{\rm ext}(\mu(\tau)) \right] d\tau \tag{3}$$

where *Eint* is expressed by two main terms, the first one refers to the elasticity and the second the flexibility, given:

$$E\_{\rm int} = \mathfrak{a} \int\_0^1 |u\_{\tau}(\tau)|^2 d\tau + \mathcal{J} \int\_0^1 |u\_{\tau\tau}(\tau)|^2 d\tau \tag{4}$$

*τ* and *ττ* indexes in the term *u*(*τ*) implies respectively first and second order of derivation. By returning to the equation 3 for defining the term *Eext* or the energy of the image (Sekhar et al., 2008), as the field of potential *P*:

$$E\_{\rm ext} = \int\_0^1 P(u(\tau))d\tau\tag{5}$$

The potential includes different terms defined from image proprieties like edges, lines, etc. Edges energy is obtained by computing the magnitude of the gradient intensity |∇*I*|. Without a good initialization, the energy of the edges will not be enough to locate the objects on noisy or low contrast images. Therefore an additional potential of the regions is added to the edge energy. Generally, the potential of the regions is defined by the mean (*μ*) and the variance (*σ*2) of the pixels intensity in the region. However, other constraints could be added like the object velocities, or other statistics derived from region characteristics (Brox et al., 2010). Because of the real-time constraints, we calculate only some statistics in the region that describe the object, such as the main properties to its implementation in correspondence to the next image. The following section describes the proposed strategy about shape recovering and object tracking using active contours.

#### **5.2 Incremental silhouette definition**

We have tested a method based on the work of Chan and Vese (Chan & Vese, 2001) in order to find the silhouette of the object. In this work, the authors give a region of initialization which may contain total or partially the object. The analysis of this initial contour will allow evaluation of the conditions of the energy minimization inside and outside of the contour. An example of a shape recovering of a person in an indoor environment using the Chan and Vese method is shown in figure 10. First row of images display the contour evolution. In the second row the region inside of the contour for each corresponding above image is illustrated. These images are given as input to the process for recovering real object contour. Furthermore, white regions on these images are labeled as occupied locations which avoid the detection of new interesting points inside of it through the time. According to this, new feature selection (detailed in section 4.1) will look for unoccupied locations in the image allowing the detection of incoming objects.

Once a partial contour at image *t* in figure 10 has been obtained by minimizing eq. 3, we estimate its position at image *t* + 1 for starting a new convergence process. The prediction of the region on the next image is always given by the Kalman filter. The vector state of our object is expressed as:

$$\mathbf{x}\_0 = \begin{bmatrix} \bar{\mathbf{x}}\_{\prime} \bar{\mathbf{y}}\_{\prime} \bar{\mathbf{v}}\_{\mathcal{X}} \ \bar{v}\_{\mathcal{Y}} \end{bmatrix}^T \tag{6}$$

Namely, *x*¯, *y*¯ denote the barycenter location and *v*¯*x*, *v*¯*<sup>y</sup>* the means of velocity vector in *X* and *Y* direction respectively. Figure 11 illustrates an example of the filter prediction. The barycenter position and the partial converged contour are located in accordance with Kalman 14 Will-be-set-by-IN-TECH

where *Eint* is expressed by two main terms, the first one refers to the elasticity and the second

<sup>2</sup>*dτ* + *β*

*τ* and *ττ* indexes in the term *u*(*τ*) implies respectively first and second order of derivation. By returning to the equation 3 for defining the term *Eext* or the energy of the image (Sekhar

> 1 0

The potential includes different terms defined from image proprieties like edges, lines, etc. Edges energy is obtained by computing the magnitude of the gradient intensity |∇*I*|. Without a good initialization, the energy of the edges will not be enough to locate the objects on noisy or low contrast images. Therefore an additional potential of the regions is added to the edge energy. Generally, the potential of the regions is defined by the mean (*μ*) and the variance (*σ*2) of the pixels intensity in the region. However, other constraints could be added like the object velocities, or other statistics derived from region characteristics (Brox et al., 2010). Because of the real-time constraints, we calculate only some statistics in the region that describe the object, such as the main properties to its implementation in correspondence to the next image. The following section describes the proposed strategy about shape recovering

We have tested a method based on the work of Chan and Vese (Chan & Vese, 2001) in order to find the silhouette of the object. In this work, the authors give a region of initialization which may contain total or partially the object. The analysis of this initial contour will allow evaluation of the conditions of the energy minimization inside and outside of the contour. An example of a shape recovering of a person in an indoor environment using the Chan and Vese method is shown in figure 10. First row of images display the contour evolution. In the second row the region inside of the contour for each corresponding above image is illustrated. These images are given as input to the process for recovering real object contour. Furthermore, white regions on these images are labeled as occupied locations which avoid the detection of new interesting points inside of it through the time. According to this, new feature selection (detailed in section 4.1) will look for unoccupied locations in the image allowing the detection

Once a partial contour at image *t* in figure 10 has been obtained by minimizing eq. 3, we estimate its position at image *t* + 1 for starting a new convergence process. The prediction of the region on the next image is always given by the Kalman filter. The vector state of our

*x*¯, *y*¯, *v*¯*x*, *v*¯*<sup>y</sup>*

Namely, *x*¯, *y*¯ denote the barycenter location and *v*¯*x*, *v*¯*<sup>y</sup>* the means of velocity vector in *X* and *Y* direction respectively. Figure 11 illustrates an example of the filter prediction. The barycenter position and the partial converged contour are located in accordance with Kalman

**x**<sup>0</sup> =

 1 0


[*Eint*(*u*(*τ*)) + *Eext*(*u*(*τ*))] *dτ* (3)

*P*(*u*(*τ*))*dτ* (5)

*<sup>T</sup>* (6)

<sup>2</sup>*dτ* (4)

*Esnake* =

*Eint* = *α*

the flexibility, given:

et al., 2008), as the field of potential *P*:

and object tracking using active contours.

**5.2 Incremental silhouette definition**

of incoming objects.

object is expressed as:

 1 0

 1 0


*Eext* =

(a) Initial snake at image *t*<sup>0</sup> (b) 30 iterations at image *t*<sup>0</sup> (c) Image *t*<sup>0</sup> + 2 (d) Image *t*<sup>0</sup> + 4

(e) Initial mask at image *t*0 (f) Mask at image *t*<sup>0</sup> (g) Mask at image *t*<sup>0</sup> + 2 (h) Mask at image *t*<sup>0</sup> + 4

Fig. 10. Results of the detection and tracking of a non-rigid dynamic object, in this case a pedestrian on the same sequence of figure 9. Here, we have processed 5 consecutive images, executing 30 iterations per image in order to find the actual object shape.

filter prediction. We always consider a velocity constant model and the vector state is obtained from model object displacement. However, in some cases it is first necessary to tune the initial parameters of the filter because there may exist different types of object movements producing an undesired acceleration of the predicted position.

#### **5.3 Object contour validation**

The difficulties rising from our proposed strategy points out the sensitivity of the active contours to small discontinuities on the object edges with low-contrast because the energy is configured over the entire image. To overcome these difficulties we evaluate the disparity inside of the region corresponding with the binary mask (that will be presented in the next section figure 15). This evaluation consists in ordering in an ascendant way all the disparity values inside the region designed by the binary mask then we uniquely consider the median of the values. In particular, we consider a valid point of the object region if its disparity value is located up to the 4th percentil in the ordered list (here referred to as 4*p*). It follows that our statistical validation for object contour refining can be written as:

$$M = \begin{cases} 1 & \text{depth} \ge 4p \\ 0 & \text{otherwise} \end{cases} \tag{7}$$

where M represents the binary mask. It is important to remark that the depth value is the inverse value of the disparity therefore in this case the fact of rejecting points located on the first four percentiles represents that these values have a lower disparity value, that is, disparity values of the background points are expecting to be low. This constrain allow us to develop a last validity evaluation for obtaining a most detailed and accurated representation of the object shape. The next section describes our experimental results and presents how the disparity map plays a fundamental role in increasing the efficiency and improve the obtained results.

(a) Image 79 (b) Image 80 (c) Image 81

<sup>19</sup> 3D Visual Information for Dynamic Objects

Detection and Tracking During Mobile Robot Navigation

(d) Image 82 (e) Image 83 (f) Image 84

(g) Image 85 (h) Image 86 (i) Image 87

(j) Image 88 (k) Image 89 (l) Image 90

Fig. 12. Detection and tracking of a non-rigid dynamic object along 12 consecutive images. In

point of image, however it can only be perceived by our strategy at the moment of it is closer to our robot position. Whereas the first experiment was performed under ideal conditions of controlled illumination provided by the indoor environment here this conditions do not hold. This fact notably avoids the computation of a cohesive representation of the energy functional as the one illustrated in figure 9(b). As a consequence, there is only one object at each image of

each image, 30 iterations are used in order to find the object silhouette.

(a) Object silhouette (b) Estimation for the next image

Fig. 11. Motion estimation using Kalman filter. a) The barycenter location at image *t* is used for predict its position at the next image. b) The irregular contour is centered at the predicted barycenter position, this will be used as initialization region in incoming images.

#### **6. Experimental results**

We evaluate the performance of the proposed strategy for detecting and tracking dynamic objects by carrying out experiments using both secure and mounted cameras on autonomous vehicles. Since, using one secure camera, vibrations, egomotion, among other typical problems of mounted cameras are neglected, first we do an experiment under this controlled condition to verify that our algorithm detects moving objects in real images. Moreover, we have observed that the total convergence of energy equation is computationally demanding. To overcome this constraint, it was necessary to propose that convergence task works uniquely a small number of iterations and re-starting the process at next image from previous results. In our case, we see a significant improvement in the efficiency of computation by using this, even when the most similar contour to actual object shape is found after some iterations.

#### **6.1 Fixed camera case**

Figure 12 shows 10 consecutive images in which a person appears in the field of view of one security camera on a Commercial Center. These resulting images refer to the case where the number of iterations for converging the active contour at each image is set to 30. The initial contour obtained from the cluster of dynamic points was displayed in figure 9. The effect of the Kalman filter estimation permit us to achieve a more detailed shape than the initial contour frame by frame. We remark that even if initial contour does not contain all the object, the actual shape is achieved after processing 5 images. It is important to point out that resulting object bounds are almost the same as the real thanks to the high-contrast generated between the person and the background.

#### **6.2 Experiments during mobile robot navigation: rigid objects detection**

A second experiment was performed in an outdoor environment where the robot accomplishes a linear trajectory of navigation at low-speed (about 6 m/s). In our experiments, we only use the stereo-vision bank mounted on the mobile robot. The robot cameras provide images with resolution of 640 × 480 pixels. The egomotion derived from the robot displacements is neglected by verifying similar depth values inside of the region containing the dynamic group of points. Moreover, dynamic groups detected on the road shows different depth values, so they are rejected to define an initial contour. Figure 13 illustrates the detection and tracking of a white vehicle. Note that this vehicle comes from the imaginary epipolar

(a) Image 79 (b) Image 80 (c) Image 81

16 Will-be-set-by-IN-TECH

(a) Object silhouette (b) Estimation for the next image

Fig. 11. Motion estimation using Kalman filter. a) The barycenter location at image *t* is used for predict its position at the next image. b) The irregular contour is centered at the predicted

We evaluate the performance of the proposed strategy for detecting and tracking dynamic objects by carrying out experiments using both secure and mounted cameras on autonomous vehicles. Since, using one secure camera, vibrations, egomotion, among other typical problems of mounted cameras are neglected, first we do an experiment under this controlled condition to verify that our algorithm detects moving objects in real images. Moreover, we have observed that the total convergence of energy equation is computationally demanding. To overcome this constraint, it was necessary to propose that convergence task works uniquely a small number of iterations and re-starting the process at next image from previous results. In our case, we see a significant improvement in the efficiency of computation by using this, even when the most similar contour to actual object shape is found after some iterations.

Figure 12 shows 10 consecutive images in which a person appears in the field of view of one security camera on a Commercial Center. These resulting images refer to the case where the number of iterations for converging the active contour at each image is set to 30. The initial contour obtained from the cluster of dynamic points was displayed in figure 9. The effect of the Kalman filter estimation permit us to achieve a more detailed shape than the initial contour frame by frame. We remark that even if initial contour does not contain all the object, the actual shape is achieved after processing 5 images. It is important to point out that resulting object bounds are almost the same as the real thanks to the high-contrast generated

A second experiment was performed in an outdoor environment where the robot accomplishes a linear trajectory of navigation at low-speed (about 6 m/s). In our experiments, we only use the stereo-vision bank mounted on the mobile robot. The robot cameras provide images with resolution of 640 × 480 pixels. The egomotion derived from the robot displacements is neglected by verifying similar depth values inside of the region containing the dynamic group of points. Moreover, dynamic groups detected on the road shows different depth values, so they are rejected to define an initial contour. Figure 13 illustrates the detection and tracking of a white vehicle. Note that this vehicle comes from the imaginary epipolar

**6.2 Experiments during mobile robot navigation: rigid objects detection**

barycenter position, this will be used as initialization region in incoming images.

**6. Experimental results**

**6.1 Fixed camera case**

between the person and the background.

(d) Image 82 (e) Image 83 (f) Image 84

(g) Image 85 (h) Image 86 (i) Image 87

```
(j) Image 88 (k) Image 89 (l) Image 90
```
Fig. 12. Detection and tracking of a non-rigid dynamic object along 12 consecutive images. In each image, 30 iterations are used in order to find the object silhouette.

point of image, however it can only be perceived by our strategy at the moment of it is closer to our robot position. Whereas the first experiment was performed under ideal conditions of controlled illumination provided by the indoor environment here this conditions do not hold. This fact notably avoids the computation of a cohesive representation of the energy functional as the one illustrated in figure 9(b). As a consequence, there is only one object at each image of

(a) (b) (c)

<sup>21</sup> 3D Visual Information for Dynamic Objects

Detection and Tracking During Mobile Robot Navigation

(d) (e) (f)

(g) (h) (i)

(j) (k) (l)

(m) (n) (o)

Fig. 15. Images on the left column show the contour obtained by only considering the intensity inside of the region. The middle column isolates the object contour representation and its corresponding disparity values. Images on the right evaluate the depth of the target

to boil down extra regions included in the object contour.

figure 13 but its contour is represented by two separated regions that have similar energy level inside. Furthermore, we analyze the disparity map for obtaining the statistics of disparity values in both regions as mentioned in section 5.3

(a) (b) (c) (d)

(e) (f) (g) (h)

Fig. 13. Experimental results in an outdoor environment during robot navigation. Images show the detection and tracking of a rigid object.

#### **6.3 Experiments during mobile robot navigation: non rigid objects detection**

In the third experiment, we considered a mobile robot moving in an outdoor environment again but in this case the robot finds a dynamic non-rigid object during its trajectory. The left image of figure 14 displays the frame in which the dynamic object was detected and initialized as a irregular contour, middle image shows the disparity map and right image the respective initial mask. In practice detecting non-rigid objects is more complicated than the previous experiment because a person walking has different motions in his legs than his shoulder or his head. Because of improvements, we use in this experiment 40 iterations per image for converging the active contour. Figure 15 illustrates some resulted images of the tracking performed by our proposed strategy. In this experiment, we found that almost all the person could be covered with the active contour. However by examining the left column

Fig. 14. a) Initial contour derived from the dynamic group of points. b) The corresponding disparity map of the image, c) the initial mask used by the active contour.

18 Will-be-set-by-IN-TECH

figure 13 but its contour is represented by two separated regions that have similar energy level inside. Furthermore, we analyze the disparity map for obtaining the statistics of disparity

(a) (b) (c) (d)

(e) (f) (g) (h)

In the third experiment, we considered a mobile robot moving in an outdoor environment again but in this case the robot finds a dynamic non-rigid object during its trajectory. The left image of figure 14 displays the frame in which the dynamic object was detected and initialized as a irregular contour, middle image shows the disparity map and right image the respective initial mask. In practice detecting non-rigid objects is more complicated than the previous experiment because a person walking has different motions in his legs than his shoulder or his head. Because of improvements, we use in this experiment 40 iterations per image for converging the active contour. Figure 15 illustrates some resulted images of the tracking performed by our proposed strategy. In this experiment, we found that almost all the person could be covered with the active contour. However by examining the left column

(a) Initial detection (b) disparity map (c) Initial mask

Fig. 14. a) Initial contour derived from the dynamic group of points. b) The corresponding

disparity map of the image, c) the initial mask used by the active contour.

Fig. 13. Experimental results in an outdoor environment during robot navigation. Images

**6.3 Experiments during mobile robot navigation: non rigid objects detection**

values in both regions as mentioned in section 5.3

show the detection and tracking of a rigid object.

(d) (e) (f)

(g) (h) (i)

(j) (k) (l)

Fig. 15. Images on the left column show the contour obtained by only considering the intensity inside of the region. The middle column isolates the object contour representation and its corresponding disparity values. Images on the right evaluate the depth of the target to boil down extra regions included in the object contour.

Almanza-Ojeda, D. L., Devy, M. & Herbulot, A. (2011). Active method for mobile object

<sup>23</sup> 3D Visual Information for Dynamic Objects

Brox, T., Rousson, M., Deriche, R. & Weickert, J. (2010). Colour, texture, and motion in level set based segmentation and tracking, *Image Vision Comput.* 28(3): 376–390. Cao, F., Delon, J., Desolneux, A., Musé, P. & Sur, F. (2007). A unified framework for detecting

Chan, T. & Vese, L. (2001). Active contours without edges, *Transactions on Image Processing,*

Desolneux, A., Moisan, L. & Morel, J.-M. (2003). A grouping principle and four applications, *IEEE Transactions on Pattern Analysis and Machine Intelligence* 25(4): 508–513. Desolneux, A., Moisan, L. & Morel, J.-M. (2008). *From Gestalt Theory to Image Analysis A*

Ibarra-Manzano, M.-A. & Almanza-Ojeda, D.-L. (2011). *Advances in Stereo Vision*, InTech,

Ibarra-Manzano, M., Almanza-Ojeda, D.-L., Devy, M., Boizard, J.-L. & Fourniols, J.-Y. (2009).

Ibarra-Manzano, M., Devy, M., Boizard, J.-L., Lacroix, P. & Fourniols, J.-Y. (2009). An

Katz, R., Douillard, B., Nieto, J. & Nebot, E. (2008). A self-supervised architecture for

Klappstein, J., Vaudrey, T., Rabe, C., Wedel, A. & Klette, R. (2008). Moving object segmentation

Li, C., Xu, C., Gui, C. & Fox, M. D. (2010). Distance regularized level set evolution and its application to image segmentation, *IEEE Trans. Image Process.* 19(12): 3243–3254. Lookingbill, A., Lieb, D. & Thrun, S. (2007). *Autonomous Navigation in Dynamic Environments*,

Lucas, B. D. & Kanade, T. (1981). An iterative image registration technique with an application to stereo vision, *Proceedings of DARPA Image Understanding Workshop*, pp. 121–130. Marin-Hernandez, A. (2004). *Vision dynamique pour la navigation d'un robot mobile.*, PhD thesis,

Masrani, D. & MacLean, W. (2006). A Real-Time large disparity range Stereo-System using

chapter High-Speed Architecture Based on FPGA for a Stereo-Vision Algorithm,

Stereo vision algorithm implementation in fpga using census transform for effective resource optimization, *Digital System Design, Architectures, Methods and Tools, 2009.*

efficient reconfigurable architecture to implement dense stereo vision algorithm using high-level synthesis, *2009 International Conference on Field Programmable Logic*

moving obstacles classification, *IEEE/RSJ International Conference on Intelligent Robots*

using optical flow and depth information, *PSIVT '09: Proceedings of the 3rd Pacific Rim Symposium on Advances in Image and Video Technology*, Springer-Verlag, Berlin,

Vol. 35 of *Springer Tracts in Advanced Robotics*, Springer Berlin / Heidelberg, pp. 29–44.

FPGAs, *Computer Vision Systems, 2006 ICVS '06. IEEE International Conference on*, p. 13.

*Probabilistic Approach*, Vol. 34, Springer Berlin / Heidelberg.

URL: *http://groups.inf.ed.ac.uk/vision/CAVIAR/CAVIARDATA1/*

*12th Euromicro Conference on*, pp. 799 –805.

*and Systems, IROS 2008*, pp. 155–160.

Heidelberg, pp. 611–623.

INPT-LAAS-CNRS.

*and Applications*, Prague, Czech Republic, pp. 444–447.

10.1007/978-3-642-19539-6\_18.

Detection and Tracking During Mobile Robot Navigation

*Vision* 27(2): 91–119.

*IEEE* 10(2): 266–277.

Fisher, R. (2011).

pp. 71-88.

URL: *http://dx.doi.org/10.1007/978-3-642-19539-6\_18*

detection from an embedded camera, based on a contrario clustering, *in* J. A. Cetto, J.-L. Ferrier & J. Filipe (eds), *Informatics in Control, Automation and Robotics*, Vol. 89 of *Lecture Notes in Electrical Engineering*, Springer Berlin Heidelberg, pp. 267–280.

groups and application to shape recognition, *Journal of Mathematical Imaging and*

of figure 15, we can figure out that part of the background was also included in our object region. To overcome this problem we performed the same statistical calculation of disparity values inside of the region-object as described in section 5.3. By examining the middle column of figure 15, we can figure out that similar depth values are concentrated in actual object contour. We can see that additional zones to this contour that represents the tree and other locations of the background will be rejected using the disparity map since they provide lower values of disparity, that is they are farther away than our interesting moving object.

The proposed algorithm was coded on C/C++ and TCL, however disparity maps are computed for the FPGA by means of an architecture codified in VHDL. The Cyclone IV card calculates 30 disparity maps per second. After several tests, we measure that our algorithm runs around 1 or 1.5 Hz depending of the nature of the environment. From this, note that the disparity is available at higher frequency than our algorithm performance, however we comment that until now the goal of the experiment was to provide an algorithm for detecting and tracking moving objects.

#### **7. Conclusions**

In this project, we considered the problem of dynamic object detection from a mobile robot in indoor/outdoor environments of navigation. Specially, the proposed strategy uses only visual-information provided by a stereo-vision bank mounted on the mobile robot. The speed of the robot during navigation is established low to avoid disturbance on the velocity data due to robot ego-motion. Experimental results allow us to realize that the proposed strategy performs a correct and total detection of the rigid and non-rigid objects and it is able to tracking them along the image sequence. Motivated by these results future contributions to this project consist in decreasing the time of computation. Nevertheless, we make some assumptions by avoiding excessive or unnecessary computations ie. the number of selected feature points, number of iterations during the active contour processing, our global algorithm is not able to perform in real time (at least 10 Hz). Significant improvements could be obtained by an emigration of all our algorithm design to embedded architectures like GPU or FPGA devices. Furthermore these kinds of devices provide a high portability towards robotics or autonomous vehicle platforms.

We also comment the difficulties rising from the disparity map constructed by the stereo vision module in which a cohesive and accurate representation of the actual scene have to be improved. To this end, future works consider the addition of a strategy for rejecting "spikes" in the disparity map cased by stereo mismatches.

#### **8. Acknowledgments**

This work was partially funded by the CONACyT with the project entitled "Diseno y optimizacion de una arquitectura para la clasificacion de objetos en tiempo real por color y textura basada en FPGA". Authors would like to thank Cyril Roussillon for help provided with the robot.

#### **9. References**

Almanza-Ojeda, D., Devy, M. & Herbulot, A. (2010). Visual-based detection and tracking of dynamic obstacles from a mobile robot, *In Proceedings of the 7th International Conference on Informatics in Control, Automation and Robotics (ICINCO 2010)*, Madeira, Portugal.

Almanza-Ojeda, D. L., Devy, M. & Herbulot, A. (2011). Active method for mobile object detection from an embedded camera, based on a contrario clustering, *in* J. A. Cetto, J.-L. Ferrier & J. Filipe (eds), *Informatics in Control, Automation and Robotics*, Vol. 89 of *Lecture Notes in Electrical Engineering*, Springer Berlin Heidelberg, pp. 267–280. 10.1007/978-3-642-19539-6\_18.

URL: *http://dx.doi.org/10.1007/978-3-642-19539-6\_18*


20 Will-be-set-by-IN-TECH

of figure 15, we can figure out that part of the background was also included in our object region. To overcome this problem we performed the same statistical calculation of disparity values inside of the region-object as described in section 5.3. By examining the middle column of figure 15, we can figure out that similar depth values are concentrated in actual object contour. We can see that additional zones to this contour that represents the tree and other locations of the background will be rejected using the disparity map since they provide lower

The proposed algorithm was coded on C/C++ and TCL, however disparity maps are computed for the FPGA by means of an architecture codified in VHDL. The Cyclone IV card calculates 30 disparity maps per second. After several tests, we measure that our algorithm runs around 1 or 1.5 Hz depending of the nature of the environment. From this, note that the disparity is available at higher frequency than our algorithm performance, however we comment that until now the goal of the experiment was to provide an algorithm for detecting

In this project, we considered the problem of dynamic object detection from a mobile robot in indoor/outdoor environments of navigation. Specially, the proposed strategy uses only visual-information provided by a stereo-vision bank mounted on the mobile robot. The speed of the robot during navigation is established low to avoid disturbance on the velocity data due to robot ego-motion. Experimental results allow us to realize that the proposed strategy performs a correct and total detection of the rigid and non-rigid objects and it is able to tracking them along the image sequence. Motivated by these results future contributions to this project consist in decreasing the time of computation. Nevertheless, we make some assumptions by avoiding excessive or unnecessary computations ie. the number of selected feature points, number of iterations during the active contour processing, our global algorithm is not able to perform in real time (at least 10 Hz). Significant improvements could be obtained by an emigration of all our algorithm design to embedded architectures like GPU or FPGA devices. Furthermore these kinds of devices provide a high portability towards robotics or

We also comment the difficulties rising from the disparity map constructed by the stereo vision module in which a cohesive and accurate representation of the actual scene have to be improved. To this end, future works consider the addition of a strategy for rejecting "spikes"

This work was partially funded by the CONACyT with the project entitled "Diseno y optimizacion de una arquitectura para la clasificacion de objetos en tiempo real por color y textura basada en FPGA". Authors would like to thank Cyril Roussillon for help provided

Almanza-Ojeda, D., Devy, M. & Herbulot, A. (2010). Visual-based detection and tracking

of dynamic obstacles from a mobile robot, *In Proceedings of the 7th International Conference on Informatics in Control, Automation and Robotics (ICINCO 2010)*, Madeira,

values of disparity, that is they are farther away than our interesting moving object.

and tracking moving objects.

autonomous vehicle platforms.

**8. Acknowledgments**

Portugal.

with the robot.

**9. References**

in the disparity map cased by stereo mismatches.

**7. Conclusions**

URL: *http://groups.inf.ed.ac.uk/vision/CAVIAR/CAVIARDATA1/*


**1. Introduction**

visual perception.

Unlike the traditional robotic systems in which the perceptual behaviors are manually designed by programmers for a given task and environment, autonomous perception of the world is one of the challenging issues in the cognitive robotics. It is known that the *selective attention* mechanism serves to link the processes of perception, action and learning (Grossberg, 2007; Tipper et al., 1998). It endows humans with the *cognitive capability* that allows them to *learn* and *think* about how to perceive the environment autonomously. This visual attention based autonomous perception mechanism involves two aspects: *conscious aspect* that directs perception based on the current task and learned knowledge, and *unconscious aspect* that directs perception in the case of facing an unexpected or unusual situation. The *top-down attention* mechanism (Wolfe, 1994) is responsible for the conscious aspect whereas the *bottom-up attention* mechanism (Treisman & Gelade, 1980) corresponds to the unconscious aspect. This paper therefore discusses about how to build an artificial system of autonomous

*Faculty of Engineering and Applied Science, Memorial University of Newfoundland* 

**Development of an Autonomous Visual** 

**Perception System for Robots Using** 

Yuanlong Yu, George K. I. Mann and Raymond G. Gosine

**Object-Based Visual Attention** 

*St. John's, NL,* 

*Canada* 

**2**

Three fundamental problems are addressed in this paper. The first problem is about pre-attentive segmentation for object-based attention. It is known that attentional selection is either *space-based* or *object-based* (Scholl, 2001). The space-based theory holds that attention is allocated to a spatial location (Posner et al., 1980). The object-based theory, however, posits that some pre-attentive processes serve to segment the field into discrete objects, followed by the attention that deals with one object at a time (Duncan, 1984). This paper proposes that object-based attention has the following three advantages in terms of computations: 1) Object-based attention is more robust than space-based attention since the attentional activation at the object level is estimated by accumulating contributions of all components within that object, 2) attending to an exact object can provide more useful information (e.g., shape and size) to produce the appropriate actions than attending to a spatial location, and 3) the discrete objects obtained by pre-attentive segmentation are required in the case that a global feature (e.g., shape) is selected to guide the top-down attention. Thus this paper adopts

Although a few object-based visual attention models have been proposed, such as (Sun, 2008; Sun & Fisher, 2003), developing a pre-attentive segmentation algorithm is still a challenging issue as it is a unsupervised process. This issue includes three types of challenges: 1) The

the object-based visual attention theory (Duncan, 1984; Scholl, 2001).


### **Development of an Autonomous Visual Perception System for Robots Using Object-Based Visual Attention**

Yuanlong Yu, George K. I. Mann and Raymond G. Gosine *Faculty of Engineering and Applied Science, Memorial University of Newfoundland St. John's, NL, Canada* 

#### **1. Introduction**

22 Will-be-set-by-IN-TECH

24 Recent Advances in Mobile Robotics

Matthies, L., Litwin, T., Owens, K., Rankin, A., Murphy, K., Coombs, D., Gilsinn, J., Hong,

Paragios, N. & Deriche, R. (2005). Geodesic active regions and level set methods for motion estimation and tracking, *Computer Vision and Image Understanding* 97(3): 259 – 282.

Sekhar, S. C., Aguet, F., Romain, S., Thévenaz, P. & Unser, M. (2008). Parametric

*Proceedings of the 16th European Signal Processing Conference, (EUSIPCO2008)* . Shi, J. & Tomasi, C. (1994). Good features to track, *proceedings of the IEEE Conference on Computer*

Sola, J., Monin, A. & Devy, M. (2007). BiCamSLAM: two times mono is more than stereo, *IEEE*

Talukder, A. & Matthies, L. (2004). Real-time detection of moving objects from moving

Veit, T., Cao, F. & Bouthemy, P. (2007). Space-time a contrario clustering for detecting coherent

Vu, T. & Aycard, O. (2009). Laser-based detection and tracking moving objects using

Williamson, T. (1998). *A High-Performance Stereo Vision System for Obstacle Detection*, PhD thesis,

Woodfill, J., Gordon, G., Jurasek, D., Brown, T. & Buck, R. (2006). The tyzx DeepSea g2 vision

Zabih, R. & Woodfill, J. (1994). Non-parametric local transforms for computing visual

b-spline snakes on distance maps—application to segmentation of histology images,

*International Conference on Robotics Automation (ICRA2007), Rome, Italy*, pp. 4795–4800.

vehicles using dense stereo and optical flow, *proceedings of the International Conference*

motion, *IEEE International Conference on Robotics and Automation, (ICRA07)*, Roma,

data-driven markov chain monte carlo, *IEEE International Conference on Robotics*

system, ATaskable, embedded stereo camera, *Computer Vision and Pattern Recognition*

correspondence, *ECCV '94: Proceedings of the Third European Conference on Computer Vision*, Vol. II, Springer-Verlag New York, Inc., Secaucus, NJ, USA, pp. 151–158.

URL: *http://www.sciencedirect.com/science/article/pii/S1077314204001213* Schmit, H. H., Cadambi, S., Moe, M. & Goldstein, S. C. (2000). Pipeline reconfigurable fpgas,

*Journal of VLSI Signal Processing Systems* 24(2-3): 129–146.

*on Intelligent Robots and Systems (IROS2004)*, pp. 3718–3725.

Robotics Institute, Carnegie Mellon University, Pittsburgh, PA.

*Workshop, 2006. CVPRW '06. Conference on*, p. 126.

*Vision and Pattern Recognition*, pp. 593–600.

*Automation (ICRA2009), Kobe, Japan*.

*Conference* pp. 658–670.

Italy, pp. 33–39.

T., Legowik, S., Nashman, M. & Yoshimi, B. (1998). Performance evaluation of ugv obstacle detection with ccd/flir stereo vision and ladar, *ISIC/CIRA/ISAS Joint*

> Unlike the traditional robotic systems in which the perceptual behaviors are manually designed by programmers for a given task and environment, autonomous perception of the world is one of the challenging issues in the cognitive robotics. It is known that the *selective attention* mechanism serves to link the processes of perception, action and learning (Grossberg, 2007; Tipper et al., 1998). It endows humans with the *cognitive capability* that allows them to *learn* and *think* about how to perceive the environment autonomously. This visual attention based autonomous perception mechanism involves two aspects: *conscious aspect* that directs perception based on the current task and learned knowledge, and *unconscious aspect* that directs perception in the case of facing an unexpected or unusual situation. The *top-down attention* mechanism (Wolfe, 1994) is responsible for the conscious aspect whereas the *bottom-up attention* mechanism (Treisman & Gelade, 1980) corresponds to the unconscious aspect. This paper therefore discusses about how to build an artificial system of autonomous visual perception.

> Three fundamental problems are addressed in this paper. The first problem is about pre-attentive segmentation for object-based attention. It is known that attentional selection is either *space-based* or *object-based* (Scholl, 2001). The space-based theory holds that attention is allocated to a spatial location (Posner et al., 1980). The object-based theory, however, posits that some pre-attentive processes serve to segment the field into discrete objects, followed by the attention that deals with one object at a time (Duncan, 1984). This paper proposes that object-based attention has the following three advantages in terms of computations: 1) Object-based attention is more robust than space-based attention since the attentional activation at the object level is estimated by accumulating contributions of all components within that object, 2) attending to an exact object can provide more useful information (e.g., shape and size) to produce the appropriate actions than attending to a spatial location, and 3) the discrete objects obtained by pre-attentive segmentation are required in the case that a global feature (e.g., shape) is selected to guide the top-down attention. Thus this paper adopts the object-based visual attention theory (Duncan, 1984; Scholl, 2001).

> Although a few object-based visual attention models have been proposed, such as (Sun, 2008; Sun & Fisher, 2003), developing a pre-attentive segmentation algorithm is still a challenging issue as it is a unsupervised process. This issue includes three types of challenges: 1) The

association between the attended object at the last time and predicted task-relevant object at the current time. In other words, it tells the robot what should be perceived at each time. Thus its objective is to deduce the task-relevant object given the task in the attentional selection stage. The *LTM object representation* embodies the properties of an object. It has two objectives: 1) Directing the top-down biasing given the task-relevant object and 2) directing the post-attentive perception and action selection. The third issue is about how to build their structure in order to realize the objectives of these two representations. This paper employs the *connectionist approach* to model both representations as the self-organization can be more effectively achieved by using the cluster-based structure, although some symbolic approaches (Navalpakkam & Itti, 2005) have been proposed for task representations. The last issue is about how to learn both representations through the duration from an infant robot to a mature one. It indicates that a dynamic, constructive learning algorithm is required to achieve the self-organization, such as generation of new patterns and re-organization of existing patterns. Since this paper focuses on the perception process, only the learning of LTM

<sup>27</sup> Development of an Autonomous Visual Perception

The remainder of this paper is organized as follows. Some related work of modeling visual attention and its applications in robotic perception are reviewed in section 2. The framework of the proposed autonomous visual perception system is given in section 3. Three stages of this proposed system are presented in section 4, section 5 and section 6 respectively. Experimental

There are mainly four psychological theories of visual attention, which are the basis of computational modeling. Feature integration theory (FIT) (Treisman & Gelade, 1980) is widely used for explaining the space-based bottom-up attention. The FIT asserts that the visual scene is initially coded along a variety of feature dimensions, then attention competition performs in a location-based serial fashion by combining all features spatially, and focal attention finally provides a way to integrate the initially separated features into a whole object. Guided search model (GSM) (Wolfe, 1994) was further proposed to model the space-based top-down attention mechanism in conjunction with bottom-up attention. The GSM posits that the top-down request for a given feature will activate the locations that might contain the given feature. Unlike FIT and GSM, the biased competition (BC) hypothesis (Desimone & Duncan, 1995) asserts that attentional selection, regardless of being space-based or object-based, is a biased competition process. Competition is biased in part by the bottom-up mechanism that favors a local inhomogeneity in the spatial and temporal context and in part by the top-down mechanism that favors items relative to the current task. By extending the BC hypothesis, the IC hypothesis (Duncan, 1998; Duncan et al., 1997) was further presented to explain the object-based attention mechanism. The IC hypothesis holds that any property of an object can be used as a task-relevant feature to guide the top-down attention and the whole object can

be attended once the task-relevant feature successfully captures the attention.

A variety of computational models of space-based attention for computer vision have been proposed. A space-based bottom-up attention model was first built in (Itti et al., 1998). The surprise mechanism (Itti & Baldi, 2009; Maier & Steinbach, 2010) was further proposed to model the bottom-up attention in terms of both spatial and temporal context. Itti's model was further extended in (Navalpakkam & Itti, 2005) by modeling the top-down attention mechanism. One contribution of Navalpakkam's model is the symbolic knowledge

object representations is presented.

System for Robots Using Object-Based Visual Attention

results are finally given in section 7.

**2. Related work**

ability to automatically determine the number of segments (termed as *self-determination*), 2) the computational efficiency, and 3) the robustness to noise. Although K-labeling methods (e.g., normalized cut (Shi & Malik, 2000)) can provide the accuracy and robustness, they are ineffective and inefficient when the number of segments is unknown. In contrast, recent split-and-merge methods (e.g., irregular pyramid based segmentation (Sharon et al., 2006)) are capable of determining the number of segments and computationally efficient, whereas they are not robust to noise. This paper proposes a new pre-attentive segmentation algorithm based on the irregular pyramid technique in order to achieve the self-determination and robustness as well as keep the balance between the accuracy and efficiency.

The second problem is about how to model the attentional selection, i.e., model the cognitive capability of *thinking* about what should be perceived. Compared with the well-developed bottom-up attention models (Itti & Baldi, 2009; Itti et al., 1998), modeling the top-down attention is far from being well-studied. The top-down attention consists of two components: 1) Deduction of task-relevant object given the task and 2) top-down biasing that guides the focus of attention (FOA) to the task-relevant object. Although some top-down methods have been proposed, such as (Navalpakkam & Itti, 2005), several challenging issues require further concerns. Since the first component is greatly dependent on the knowledge representation, it will be discussed in the next paragraph. Regarding the second component, the first issue is about the effectiveness of top-down biasing. The main factor that decays the effectiveness is that the task-relevant object shares some features with the distracters. It indicates that the top-down biasing method should include a mechanism to make sure that the task-relevant object can be discriminated from distracters. The second issue is about the computational efficiency based on the fact that the attention is a fast process to select an object of interest from the image input. Thus it is reasonable to use some low-level features rather than high-level features (e.g., the iconic representation (Rao & Ballard, 1995a)) for top-down biasing. The third one is the adaptivity to automatically determine which feature(s) is used for top-down biasing such that the requirement of manually re-selecting the features for different tasks and environment is eliminated. This paper attempts to address the above issues by using the integrated competition (IC) hypothesis (Duncan et al., 1997) since it not only summarizes a theory of the top-down attention, which can lead to a computational model with effectiveness, efficiency and adaptivity, but also integrates the object-based attention theory. Furthermore, it is known that bottom-up attention and top-down attention work together to decide the attentional selection, but how to combine them is another challenging issue due to the multi-modality of bottom-up saliency and top-down biases. A promising approach to this issue is setting up a unified scale at which they can be combined.

The third problem is about the cognitive capability of autonomously *learning* the knowledge that is used to guide the conscious perceptual behavior. According to the psychological concept, the memory used to store this type of knowledge is called long-term memory (LTM). Regarding this problem, the following four issues are addressed in this paper. The first issue is about the unit of knowledge representations. Object-based vision theory (Duncan, 1984; Scholl, 2001) indicates that a general way of organizing the visual scene is to parcel it into discrete objects, on which perception, action and learning perform. In other words, the internal attentional representations are in the form of objects. Therefore objects are used as the units of the learned knowledge. The second issue is what types of knowledge should be modeled for guiding the conscious perceptual behavior. According to the requirements of the attention mechanism, this paper proposes that the knowledge mainly includes LTM task representations and LTM object representations. The *LTM task representation* embodies the association between the attended object at the last time and predicted task-relevant object at the current time. In other words, it tells the robot what should be perceived at each time. Thus its objective is to deduce the task-relevant object given the task in the attentional selection stage. The *LTM object representation* embodies the properties of an object. It has two objectives: 1) Directing the top-down biasing given the task-relevant object and 2) directing the post-attentive perception and action selection. The third issue is about how to build their structure in order to realize the objectives of these two representations. This paper employs the *connectionist approach* to model both representations as the self-organization can be more effectively achieved by using the cluster-based structure, although some symbolic approaches (Navalpakkam & Itti, 2005) have been proposed for task representations. The last issue is about how to learn both representations through the duration from an infant robot to a mature one. It indicates that a dynamic, constructive learning algorithm is required to achieve the self-organization, such as generation of new patterns and re-organization of existing patterns. Since this paper focuses on the perception process, only the learning of LTM object representations is presented.

The remainder of this paper is organized as follows. Some related work of modeling visual attention and its applications in robotic perception are reviewed in section 2. The framework of the proposed autonomous visual perception system is given in section 3. Three stages of this proposed system are presented in section 4, section 5 and section 6 respectively. Experimental results are finally given in section 7.

#### **2. Related work**

2 Will-be-set-by-IN-TECH

ability to automatically determine the number of segments (termed as *self-determination*), 2) the computational efficiency, and 3) the robustness to noise. Although K-labeling methods (e.g., normalized cut (Shi & Malik, 2000)) can provide the accuracy and robustness, they are ineffective and inefficient when the number of segments is unknown. In contrast, recent split-and-merge methods (e.g., irregular pyramid based segmentation (Sharon et al., 2006)) are capable of determining the number of segments and computationally efficient, whereas they are not robust to noise. This paper proposes a new pre-attentive segmentation algorithm based on the irregular pyramid technique in order to achieve the self-determination and

The second problem is about how to model the attentional selection, i.e., model the cognitive capability of *thinking* about what should be perceived. Compared with the well-developed bottom-up attention models (Itti & Baldi, 2009; Itti et al., 1998), modeling the top-down attention is far from being well-studied. The top-down attention consists of two components: 1) Deduction of task-relevant object given the task and 2) top-down biasing that guides the focus of attention (FOA) to the task-relevant object. Although some top-down methods have been proposed, such as (Navalpakkam & Itti, 2005), several challenging issues require further concerns. Since the first component is greatly dependent on the knowledge representation, it will be discussed in the next paragraph. Regarding the second component, the first issue is about the effectiveness of top-down biasing. The main factor that decays the effectiveness is that the task-relevant object shares some features with the distracters. It indicates that the top-down biasing method should include a mechanism to make sure that the task-relevant object can be discriminated from distracters. The second issue is about the computational efficiency based on the fact that the attention is a fast process to select an object of interest from the image input. Thus it is reasonable to use some low-level features rather than high-level features (e.g., the iconic representation (Rao & Ballard, 1995a)) for top-down biasing. The third one is the adaptivity to automatically determine which feature(s) is used for top-down biasing such that the requirement of manually re-selecting the features for different tasks and environment is eliminated. This paper attempts to address the above issues by using the integrated competition (IC) hypothesis (Duncan et al., 1997) since it not only summarizes a theory of the top-down attention, which can lead to a computational model with effectiveness, efficiency and adaptivity, but also integrates the object-based attention theory. Furthermore, it is known that bottom-up attention and top-down attention work together to decide the attentional selection, but how to combine them is another challenging issue due to the multi-modality of bottom-up saliency and top-down biases. A promising approach to this

robustness as well as keep the balance between the accuracy and efficiency.

issue is setting up a unified scale at which they can be combined.

The third problem is about the cognitive capability of autonomously *learning* the knowledge that is used to guide the conscious perceptual behavior. According to the psychological concept, the memory used to store this type of knowledge is called long-term memory (LTM). Regarding this problem, the following four issues are addressed in this paper. The first issue is about the unit of knowledge representations. Object-based vision theory (Duncan, 1984; Scholl, 2001) indicates that a general way of organizing the visual scene is to parcel it into discrete objects, on which perception, action and learning perform. In other words, the internal attentional representations are in the form of objects. Therefore objects are used as the units of the learned knowledge. The second issue is what types of knowledge should be modeled for guiding the conscious perceptual behavior. According to the requirements of the attention mechanism, this paper proposes that the knowledge mainly includes LTM task representations and LTM object representations. The *LTM task representation* embodies the There are mainly four psychological theories of visual attention, which are the basis of computational modeling. Feature integration theory (FIT) (Treisman & Gelade, 1980) is widely used for explaining the space-based bottom-up attention. The FIT asserts that the visual scene is initially coded along a variety of feature dimensions, then attention competition performs in a location-based serial fashion by combining all features spatially, and focal attention finally provides a way to integrate the initially separated features into a whole object. Guided search model (GSM) (Wolfe, 1994) was further proposed to model the space-based top-down attention mechanism in conjunction with bottom-up attention. The GSM posits that the top-down request for a given feature will activate the locations that might contain the given feature. Unlike FIT and GSM, the biased competition (BC) hypothesis (Desimone & Duncan, 1995) asserts that attentional selection, regardless of being space-based or object-based, is a biased competition process. Competition is biased in part by the bottom-up mechanism that favors a local inhomogeneity in the spatial and temporal context and in part by the top-down mechanism that favors items relative to the current task. By extending the BC hypothesis, the IC hypothesis (Duncan, 1998; Duncan et al., 1997) was further presented to explain the object-based attention mechanism. The IC hypothesis holds that any property of an object can be used as a task-relevant feature to guide the top-down attention and the whole object can be attended once the task-relevant feature successfully captures the attention.

A variety of computational models of space-based attention for computer vision have been proposed. A space-based bottom-up attention model was first built in (Itti et al., 1998). The surprise mechanism (Itti & Baldi, 2009; Maier & Steinbach, 2010) was further proposed to model the bottom-up attention in terms of both spatial and temporal context. Itti's model was further extended in (Navalpakkam & Itti, 2005) by modeling the top-down attention mechanism. One contribution of Navalpakkam's model is the symbolic knowledge

Fig. 1. The framework of the proposed autonomous visual perception system for robots.

attention.

to model the conscious aspect of the autonomous perception. This module is modeled based on the IC hypothesis and consists of four steps. *Step 1* is the deduction of the task-relevant object from the corresponding LTM task representation given the task. *Step 2* is the deduction of the task-relevant feature dimension(s) from the corresponding LTM object representation given the task-relevant object. *Step 3* is to build the attentional template(s) in working memory (WM) by recalling the task-relevant feature(s) from LTM. *Step 4* is to estimate a probabilistic location-based top-down bias map by comparing attentional template(s) with corresponding pre-attentive feature(s). The obtained top-down biases and bottom-up saliency are combined in a probabilistic manner to yield a location-based attentional activation map. By combining location-based attentional activation within each proto-object, a proto-object based attentional activation map is finally achieved, based on which the most active proto-object is selected for

<sup>29</sup> Development of an Autonomous Visual Perception

System for Robots Using Object-Based Visual Attention

*Stage 3*: The main objective of the *post-attentive perception stage* is to interpret the attended object in more detail. The detailed interpretation aims to produce the appropriate action and learn the corresponding LTM object representation at the current time as well as to guide the top-down attention at the next time. This paper introduces four modules in this stage. The first module is perceptual completion processing. Since an object is always composed of several parts, this module is required to perceive the complete region of the attended object post-attentively. In the following text, the term *attended object* is used to represent one or all of the proto-objects in the complete region being attended. The second module is the extraction of post-attentive features that are a type of representation of the attended object in WM and used for the following two modules. The third module is object recognition. It functions as a decision unit that determines to which LTM object representation and/or to which instance of that representation the attended object belongs. The fourth module is learning of LTM

representations that are used to deduce the task-relevant entities for top-down attention. The other contribution is the multi-scale object representations that are used to bias attentional selection. However, this top-down biasing method might be ineffective in the case that environment contains distracters which share one or some features with the target. Another model that selectively tunes the visual processing networks by a top-down hierarchy of winner-take-all processes was also proposed in (Tsotsos et al., 1995). Some template matching methods such as (Rao et al., 2002), and neural networks based methods, such as (Baluja & Pomerleau, 1997; Hoya, 2004), were also presented for modeling top-down biasing. Recently an interesting computational method that models attention as a Bayesian inference process was reported in (Chikkerur et al., 2010). Some space-based attention model for robots was further proposed in (Belardinelli & Pirri, 2006; Belardinelli et al., 2006; Frintrop, 2005) by integrating both bottom-up and top-down attention.

Above computational models direct attention to a spatial location rather than a perceptual object. An alternative, which draws attention to an object, has been proposed by (Sun & Fisher, 2003). It presents a computational method for grouping-based saliency and a hierarchical framework for attentional selection at different perceptual levels (e.g. a point, a region or an object). Since the pre-attentive segmentation is manually achieved in the original work, Sun's model was further improved in (Sun, 2008) by integrating an automatic segmentation algorithm. Some object-based visual attention models (Aziz et al., 2006; Orabona et al., 2005) have also been presented. However, the top-down attention is not fully achieved in these existing object-based models, e.g., how to get the task-relevant feature is not realized.

Visual attention has been applied in several robotic tasks, such as object recognition (Walther et al., 2004), object tracking (Frintrop & Kessel, 2009), simultaneous localization and mapping (SLAM) (Frintrop & Jensfelt, 2008) and exploration of unknown environment (Carbone et al., 2008). A few general visual perception models (Backer et al., 2001; Breazeal et al., 2001) are also presented by using visual attention. Furthermore, some research (Grossberg, 2005; 2007) has proposed that the adaptive resonance theory (ART) (Carpenter & Grossberg, 2003) can predict the functional link between attention and processes of consciousness, learning, expectation, resonance and synchrony.

#### **3. Framework of the proposed system**

The proposed autonomous visual perception system involves three successive stages: pre-attentive processing, attentional selection and post-attentive perception. Fig. 1 illustrates the framework of this proposed system.

*Stage 1*: The *pre-attentive processing stage* includes two successive steps. The first step is the extraction of pre-attentive features at multiple scales (e.g., nine scales for a 640 × 480 image). The second step is the pre-attentive segmentation that divides the scene into proto-objects in an unsupervised manner. The *proto-objects* can be defined as uniform regions such that the pixels in the same region are similar. The obtained proto-objects are the fundamental units of attentional selection.

*Stage 2*: The *attentional selection stage* involves four modules: bottom-up attention, top-down attention, a combination of bottom-up saliency and top-down biases, as well as estimation of proto-object based attentional activation. The bottom-up attention module aims to model the unconscious aspect of the autonomous perception. This module generates a probabilistic location-based bottom-up saliency map. This map shows the conspicuousness of a location compared with others in terms of pre-attentive features. The top-down attention module aims 4 Will-be-set-by-IN-TECH

representations that are used to deduce the task-relevant entities for top-down attention. The other contribution is the multi-scale object representations that are used to bias attentional selection. However, this top-down biasing method might be ineffective in the case that environment contains distracters which share one or some features with the target. Another model that selectively tunes the visual processing networks by a top-down hierarchy of winner-take-all processes was also proposed in (Tsotsos et al., 1995). Some template matching methods such as (Rao et al., 2002), and neural networks based methods, such as (Baluja & Pomerleau, 1997; Hoya, 2004), were also presented for modeling top-down biasing. Recently an interesting computational method that models attention as a Bayesian inference process was reported in (Chikkerur et al., 2010). Some space-based attention model for robots was further proposed in (Belardinelli & Pirri, 2006; Belardinelli et al., 2006; Frintrop, 2005) by

Above computational models direct attention to a spatial location rather than a perceptual object. An alternative, which draws attention to an object, has been proposed by (Sun & Fisher, 2003). It presents a computational method for grouping-based saliency and a hierarchical framework for attentional selection at different perceptual levels (e.g. a point, a region or an object). Since the pre-attentive segmentation is manually achieved in the original work, Sun's model was further improved in (Sun, 2008) by integrating an automatic segmentation algorithm. Some object-based visual attention models (Aziz et al., 2006; Orabona et al., 2005) have also been presented. However, the top-down attention is not fully achieved in these

existing object-based models, e.g., how to get the task-relevant feature is not realized.

Visual attention has been applied in several robotic tasks, such as object recognition (Walther et al., 2004), object tracking (Frintrop & Kessel, 2009), simultaneous localization and mapping (SLAM) (Frintrop & Jensfelt, 2008) and exploration of unknown environment (Carbone et al., 2008). A few general visual perception models (Backer et al., 2001; Breazeal et al., 2001) are also presented by using visual attention. Furthermore, some research (Grossberg, 2005; 2007) has proposed that the adaptive resonance theory (ART) (Carpenter & Grossberg, 2003) can predict the functional link between attention and processes of consciousness, learning, expectation,

The proposed autonomous visual perception system involves three successive stages: pre-attentive processing, attentional selection and post-attentive perception. Fig. 1 illustrates

*Stage 1*: The *pre-attentive processing stage* includes two successive steps. The first step is the extraction of pre-attentive features at multiple scales (e.g., nine scales for a 640 × 480 image). The second step is the pre-attentive segmentation that divides the scene into proto-objects in an unsupervised manner. The *proto-objects* can be defined as uniform regions such that the pixels in the same region are similar. The obtained proto-objects are the fundamental units of

*Stage 2*: The *attentional selection stage* involves four modules: bottom-up attention, top-down attention, a combination of bottom-up saliency and top-down biases, as well as estimation of proto-object based attentional activation. The bottom-up attention module aims to model the unconscious aspect of the autonomous perception. This module generates a probabilistic location-based bottom-up saliency map. This map shows the conspicuousness of a location compared with others in terms of pre-attentive features. The top-down attention module aims

integrating both bottom-up and top-down attention.

resonance and synchrony.

attentional selection.

**3. Framework of the proposed system**

the framework of this proposed system.

Fig. 1. The framework of the proposed autonomous visual perception system for robots.

to model the conscious aspect of the autonomous perception. This module is modeled based on the IC hypothesis and consists of four steps. *Step 1* is the deduction of the task-relevant object from the corresponding LTM task representation given the task. *Step 2* is the deduction of the task-relevant feature dimension(s) from the corresponding LTM object representation given the task-relevant object. *Step 3* is to build the attentional template(s) in working memory (WM) by recalling the task-relevant feature(s) from LTM. *Step 4* is to estimate a probabilistic location-based top-down bias map by comparing attentional template(s) with corresponding pre-attentive feature(s). The obtained top-down biases and bottom-up saliency are combined in a probabilistic manner to yield a location-based attentional activation map. By combining location-based attentional activation within each proto-object, a proto-object based attentional activation map is finally achieved, based on which the most active proto-object is selected for attention.

*Stage 3*: The main objective of the *post-attentive perception stage* is to interpret the attended object in more detail. The detailed interpretation aims to produce the appropriate action and learn the corresponding LTM object representation at the current time as well as to guide the top-down attention at the next time. This paper introduces four modules in this stage. The first module is perceptual completion processing. Since an object is always composed of several parts, this module is required to perceive the complete region of the attended object post-attentively. In the following text, the term *attended object* is used to represent one or all of the proto-objects in the complete region being attended. The second module is the extraction of post-attentive features that are a type of representation of the attended object in WM and used for the following two modules. The third module is object recognition. It functions as a decision unit that determines to which LTM object representation and/or to which instance of that representation the attended object belongs. The fourth module is learning of LTM

the pre-attentive segmentation is modeled as a hierarchical accumulation procedure, in which each level of the irregular pyramid is built by accumulating similar local nodes at the level below. The final proto-objects emerge during this hierarchical accumulation process as they are represented by single nodes at some levels. This accumulation process consists of four

<sup>31</sup> Development of an Autonomous Visual Perception

System for Robots Using Object-Based Visual Attention

Fig. 3. An illustration of the hierarchical accumulation process in the pre-attentive segmentation. This process is shown from bottom to top. In the left figure, this process is represented by vertices and each circle represents a vertex. In the right figure, this process is represented by image pixels and each block represents an image pixel. The color of each vertex and block represents the feature value. It can be seen that the image is partitioned into three irregular regions once the accumulation process is finished.

*Procedure 1* is decimation. A set of surviving nodes (i.e., parent nodes) is selected from the son level to build the parent level. This procedure is constrained by the following two rules (Meer, 1989): 1) Any two neighbor son nodes cannot both survive to the parent level and 2) any son node must have at least one parent node. Instead of the *random values* used in the stochastic pyramid decimation (SPD) algorithm (Jolion, 2003; Meer, 1989), this paper proposes a new recursive similarity-driven algorithm (i.e., the first extension), in which a son node will survive if it has the maximum *similarity* among its neighbors with the constraints of the aforementioned rules. The advantage is the improved segmentation performance since the nodes that can greatly represent their neighbors deterministically survive. As the second extension, *Bhattacharyya distance* (Bhattacharyya, 1943) is used to estimate the similarity between nodes at the same level (i.e., the strength of intra-level edges). One advantage is that the similarity measure is approximately scale-invariant during the accumulation process since Bhattacharyya distance takes into account the correlations of the data. The other advantage is

In *procedure 2*, the strength of inter-level edges is estimated. Each son node and its parent nodes are linked by inter-level edges. The strength of these edges is estimated in proportion to the corresponding intra-level strength at the son level by using the method in (Sharon et al.,

*Procedure 3* aims to estimate the aggregate features and covariances of each parent node based

The purpose of *procedure 4* is to search for neighbors of each parent node and simultaneously estimate the strength of intra-level edges at the parent level. As the third extension, a new

on the strength of inter-level edges by using the method in (Sharon et al., 2000).

that the probabilistic measure can improve the robustness to noise.

procedures.

2000).

object representations. This module aims to develop the corresponding LTM representation of the attended object. The probabilistic neural network (PNN) is used to build the LTM object representation. Meanwhile, a constructive learning algorithm is also proposed. Note that the LTM task representation is another important module in the post-attentive perception stage. Its learning requires the perception-action training pairs, but this paper focuses on the perception process rather than the action selection process. So this module will be discussed in the future work.

#### **4. Pre-attentive processing**

#### **4.1 Extraction of pre-attentive features**

Fig. 2. Pre-attentive features at the original scale. (a) Intensity. (b) Red-green. (c) Blue-yellow. (d) Contour. (e) - (h) Orientation energy in direction 0◦, 45◦, 90◦ and 135◦ respectively. Brightness represents the energy value.

By using the method in Itti's model (Itti et al., 1998), pre-attentive features are extracted at multiple scales in the following dimensions: intensity **F***int*, red-green **F***rg*, blue-yellow **F***by*, orientation energy **F***o<sup>θ</sup>* with *θ* ∈ {0◦, 45◦, 90◦, 135◦}, and contour **F***ct*. Symbol **F** is used to denote pre-attentive features.

Given 8-bit RGB color components **r**, **g** and **b** of the input image, intensity and color pairs at the original scale are extracted as : **F***int* = (**r** + **g** + **b**)/3, **F***rg* = **R** − **G**, **F***by* = **B** − **Y**, where **R** = **r** − (**g** + **b**)/2, **G** = **g** − (**r** + **b**)/2, **B** = **b** − (**r** + **g**)/2, and **Y** = (**r** + **g**)/2 − |**r** − **g**|/2 − **b**. Gaussian pyramid (Burt & Adelson, 1983) is used to create the multi-scale intensity and color pairs. The multi-scale orientation energy is extracted using the Gabor pyramid (Greenspan et al., 1994). The contour feature **F***ct*(*s*) is approximately estimated by applying a pixel-wise maximum operator over four orientations of orientation energy: **<sup>F</sup>***ct*(*s*) = max*θ*∈{0◦,45◦,90◦,135◦} **<sup>F</sup>***o<sup>θ</sup>* (*s*), where *<sup>s</sup>* denotes the spatial scale. Examples of the extracted pre-attentive features have been shown in Fig. 2.

#### **4.2 Pre-attentive segmentation**

This paper proposes a pre-attentive segmentation algorithm by extending the irregular pyramid techniques (Montanvert et al., 1991; Sharon et al., 2000; 2006). As shown in Fig. 3, 6 Will-be-set-by-IN-TECH

object representations. This module aims to develop the corresponding LTM representation of the attended object. The probabilistic neural network (PNN) is used to build the LTM object representation. Meanwhile, a constructive learning algorithm is also proposed. Note that the LTM task representation is another important module in the post-attentive perception stage. Its learning requires the perception-action training pairs, but this paper focuses on the perception process rather than the action selection process. So this module will be discussed

(a) (b) (c) (d)

(e) (f) (g) (h)

Fig. 2. Pre-attentive features at the original scale. (a) Intensity. (b) Red-green. (c) Blue-yellow. (d) Contour. (e) - (h) Orientation energy in direction 0◦, 45◦, 90◦ and 135◦ respectively.

By using the method in Itti's model (Itti et al., 1998), pre-attentive features are extracted at multiple scales in the following dimensions: intensity **F***int*, red-green **F***rg*, blue-yellow **F***by*, orientation energy **F***o<sup>θ</sup>* with *θ* ∈ {0◦, 45◦, 90◦, 135◦}, and contour **F***ct*. Symbol **F** is used to

Given 8-bit RGB color components **r**, **g** and **b** of the input image, intensity and color pairs at the original scale are extracted as : **F***int* = (**r** + **g** + **b**)/3, **F***rg* = **R** − **G**, **F***by* = **B** − **Y**, where **R** = **r** − (**g** + **b**)/2, **G** = **g** − (**r** + **b**)/2, **B** = **b** − (**r** + **g**)/2, and **Y** = (**r** + **g**)/2 − |**r** − **g**|/2 − **b**. Gaussian pyramid (Burt & Adelson, 1983) is used to create the multi-scale intensity and color pairs. The multi-scale orientation energy is extracted using the Gabor pyramid (Greenspan et al., 1994). The contour feature **F***ct*(*s*) is approximately estimated by applying a pixel-wise maximum operator over four orientations of orientation energy: **<sup>F</sup>***ct*(*s*) = max*θ*∈{0◦,45◦,90◦,135◦} **<sup>F</sup>***o<sup>θ</sup>* (*s*), where *<sup>s</sup>* denotes the spatial scale. Examples of the

This paper proposes a pre-attentive segmentation algorithm by extending the irregular pyramid techniques (Montanvert et al., 1991; Sharon et al., 2000; 2006). As shown in Fig. 3,

in the future work.

**4. Pre-attentive processing**

**4.1 Extraction of pre-attentive features**

Brightness represents the energy value.

extracted pre-attentive features have been shown in Fig. 2.

denote pre-attentive features.

**4.2 Pre-attentive segmentation**

the pre-attentive segmentation is modeled as a hierarchical accumulation procedure, in which each level of the irregular pyramid is built by accumulating similar local nodes at the level below. The final proto-objects emerge during this hierarchical accumulation process as they are represented by single nodes at some levels. This accumulation process consists of four procedures.

Fig. 3. An illustration of the hierarchical accumulation process in the pre-attentive segmentation. This process is shown from bottom to top. In the left figure, this process is represented by vertices and each circle represents a vertex. In the right figure, this process is represented by image pixels and each block represents an image pixel. The color of each vertex and block represents the feature value. It can be seen that the image is partitioned into three irregular regions once the accumulation process is finished.

*Procedure 1* is decimation. A set of surviving nodes (i.e., parent nodes) is selected from the son level to build the parent level. This procedure is constrained by the following two rules (Meer, 1989): 1) Any two neighbor son nodes cannot both survive to the parent level and 2) any son node must have at least one parent node. Instead of the *random values* used in the stochastic pyramid decimation (SPD) algorithm (Jolion, 2003; Meer, 1989), this paper proposes a new recursive similarity-driven algorithm (i.e., the first extension), in which a son node will survive if it has the maximum *similarity* among its neighbors with the constraints of the aforementioned rules. The advantage is the improved segmentation performance since the nodes that can greatly represent their neighbors deterministically survive. As the second extension, *Bhattacharyya distance* (Bhattacharyya, 1943) is used to estimate the similarity between nodes at the same level (i.e., the strength of intra-level edges). One advantage is that the similarity measure is approximately scale-invariant during the accumulation process since Bhattacharyya distance takes into account the correlations of the data. The other advantage is that the probabilistic measure can improve the robustness to noise.

In *procedure 2*, the strength of inter-level edges is estimated. Each son node and its parent nodes are linked by inter-level edges. The strength of these edges is estimated in proportion to the corresponding intra-level strength at the son level by using the method in (Sharon et al., 2000).

*Procedure 3* aims to estimate the aggregate features and covariances of each parent node based on the strength of inter-level edges by using the method in (Sharon et al., 2000).

The purpose of *procedure 4* is to search for neighbors of each parent node and simultaneously estimate the strength of intra-level edges at the parent level. As the third extension, a new

addition to yield a location-based conspicuity map of that feature dimension:

*sc*=2

*s c*+4 *ss*=*sc*+3

<sup>33</sup> Development of an Autonomous Visual Perception

where N is the normalization operator, is across-scale addition, consisting of interpolation of each normalized center-surround difference to the working scale and point-by-point

All conspicuity maps are point-by-point added together to yield a location-based bottom-up

Given the following two assumptions: 1) the selection process guided by the space-based bottom-up attention is a random event, and 2) the sample space of this random event is composed of all spatial locations in the image, the salience of a spatial location can be used to represent the degree of belief that bottom-up attention selects that location. Therefore, the probability of a spatial location **r***<sup>i</sup>* being attended by the bottom-up attention mechanism can

*pbu*(**r***i*) = *Sbu*(**r***i*)

where *pbu*(**r***i*) denotes the probability of a spatial location **r***<sup>i</sup>* being attended by the bottom-up

The *task-relevant object* can be defined as an object whose occurrence is expected by the task. Consistent with the autonomous mental development (AMD) paradigm (Weng et al., 2001), this paper proposes that actions include external actions that operate effectors and internal actions that predict the next possible attentional state (i.e., attentional prediction). Since the proposed perception system is object-based, the attentional prediction can be seen as the task-relevant object. Thus this paper models the *LTM task representation* as the association between attentional states and attentional prediction and uses it to deduce the task-relevant

It can be further proposed that the LTM task representation can be modeled by using a first-order discrete Markov process (FDMP). The FDMP can be expressed as *p*(*at*+1|*at*), where *at* denotes the attentional state at time *t* and *at*+<sup>1</sup> denotes the attentional prediction for time *t* + 1. This definition means that the probability of each attentional prediction for the next time can be estimated given the attentional state at the current time. The discrete attentional states

According to the IC hypothesis, it is required to deduce the task-relevant feature from the task-relevant object. This paper defines the *task-relevant feature* as a property that can discriminate the object from others. Although several autonomous factors (e.g., rewards obtained from learning) could be used, this paper uses the *conspicuity* quantity since it

∑**r***i*� *Sbu*(**r***i*�)

N **F**� *<sup>f</sup>*(*sc*,*ss*) 

*<sup>f</sup>* denotes a location-based conspicuity map.

*by*) + <sup>1</sup> <sup>4</sup> ∑ *θ* **F***s oθ*  (2)

(3)

, (4)

1 6 4

**F***s <sup>f</sup>* = N

System for Robots Using Object-Based Visual Attention

**S***bu* = N

 **F***s ct* + **<sup>F</sup>***<sup>s</sup> int* + 1 2 (**F***<sup>s</sup> rg* + **<sup>F</sup>***<sup>s</sup>*

attention, and the denominator ∑**r***i*� *Sbu*(**r***i*�) is the normalizing constant.

**5.2.1 LTM task representations and task-relevant objects**

is composed of LTM object representations.

**5.2.2 Task-relevant feature**

addition, *<sup>f</sup>* ∈ {*int*,*rg*, *by*, *<sup>o</sup><sup>θ</sup>* , *ct*}, and **<sup>F</sup>***<sup>s</sup>*

saliency map **S***bu*:

be estimated as:

**5.2 Top-down attention**

object.

Fig. 4. Results of pre-attentive segmentation. (a)-(d) Original images, where (d) includes salt and pepper noise (noise density:0.1, patch size: 5 × 5 pixels). (e)-(h) Segmentation results. Each color represents one proto-object in these results.

neighbor search method is proposed by considering not only the graphic constraints but also the similarity constraints. A candidate node is selected as a neighbor of a center node if the similarity between them is above a predefined threshold. Since the similarity measure is scale-invariant, a fixed value of the threshold can be used for most pyramidal levels. The advantage of this method is the improved segmentation performance since the connections between nodes that are located at places with great transition are deterministically cut.

In the case that no neighbors are found for a node, it is labeled as a new proto-object. The construction of the full pyramid is finished once all nodes at a level have no neighbors. The membership of each node at the base level to each proto-object is iteratively calculated from the top pyramidal level to the base level. According to the membership, each node at the base level is finally labeled. The results of the pre-attentive segmentation are shown in Fig. 4.

#### **5. Attentional selection**

#### **5.1 Bottom-up attention**

The proposed bottom-up attention module is developed by extending Itti's model (Itti et al., 1998). Center-surround differences in terms of pre-attentive features are first calculated to simulate the competition in the spatial context:

$$\mathbf{F}'\_f(\mathbf{s}\_\mathbf{c}, \mathbf{s}\_\mathbf{s}) = |\mathbf{F}\_f(\mathbf{s}\_\mathbf{c}) \ominus \mathbf{F}\_f(\mathbf{s}\_\mathbf{s})| \tag{1}$$

where � denotes across-scale subtraction, consisting of interpolation of each feature at the surround scale to the center scale and point-by-point difference, *sc* = {2, 3, 4} and *ss* = *sc* + *δ* with *δ* = {3, 4} represent the center scales and surround scales respectively, *f* ∈ {*int*,*rg*, *by*, *o<sup>θ</sup>* , *ct*} with *θ* ∈ {0◦, 45◦, 90◦, 135◦}, and *F*� *<sup>f</sup>*(*sc*,*ss*) denotes a center-surround difference map.

These center-surround differences in terms of the same feature dimension are then normalized and combined at scale 2, termed as *working scale* and denoted as *swk*, using across-scale 8 Will-be-set-by-IN-TECH

(a) (b) (c) (d)

(e) (f) (g) (h)

Fig. 4. Results of pre-attentive segmentation. (a)-(d) Original images, where (d) includes salt and pepper noise (noise density:0.1, patch size: 5 × 5 pixels). (e)-(h) Segmentation results.

neighbor search method is proposed by considering not only the graphic constraints but also the similarity constraints. A candidate node is selected as a neighbor of a center node if the similarity between them is above a predefined threshold. Since the similarity measure is scale-invariant, a fixed value of the threshold can be used for most pyramidal levels. The advantage of this method is the improved segmentation performance since the connections between nodes that are located at places with great transition are deterministically cut. In the case that no neighbors are found for a node, it is labeled as a new proto-object. The construction of the full pyramid is finished once all nodes at a level have no neighbors. The membership of each node at the base level to each proto-object is iteratively calculated from the top pyramidal level to the base level. According to the membership, each node at the base level is finally labeled. The results of the pre-attentive segmentation are shown in Fig. 4.

The proposed bottom-up attention module is developed by extending Itti's model (Itti et al., 1998). Center-surround differences in terms of pre-attentive features are first calculated to

where � denotes across-scale subtraction, consisting of interpolation of each feature at the surround scale to the center scale and point-by-point difference, *sc* = {2, 3, 4} and *ss* = *sc* + *δ* with *δ* = {3, 4} represent the center scales and surround scales respectively,

These center-surround differences in terms of the same feature dimension are then normalized and combined at scale 2, termed as *working scale* and denoted as *swk*, using across-scale

*<sup>f</sup>*(*sc*,*ss*) = |**F***f*(*sc*) � **F***f*(*ss*)| (1)

*<sup>f</sup>*(*sc*,*ss*) denotes a center-surround

Each color represents one proto-object in these results.

**5. Attentional selection**

simulate the competition in the spatial context:

**F**�

*f* ∈ {*int*,*rg*, *by*, *o<sup>θ</sup>* , *ct*} with *θ* ∈ {0◦, 45◦, 90◦, 135◦}, and *F*�

**5.1 Bottom-up attention**

difference map.

addition to yield a location-based conspicuity map of that feature dimension:

$$\mathbf{F}\_f^s = \mathcal{N}\left(\frac{1}{6} \bigoplus\_{s\_\iota=2}^4 \bigoplus\_{s\_\iota=s\_\iota+3}^{s\_\iota+4} \mathcal{N}\left(\mathbf{F}\_f'(s\_\iota, s\_s)\right)\right) \tag{2}$$

where N is the normalization operator, is across-scale addition, consisting of interpolation of each normalized center-surround difference to the working scale and point-by-point addition, *<sup>f</sup>* ∈ {*int*,*rg*, *by*, *<sup>o</sup><sup>θ</sup>* , *ct*}, and **<sup>F</sup>***<sup>s</sup> <sup>f</sup>* denotes a location-based conspicuity map.

All conspicuity maps are point-by-point added together to yield a location-based bottom-up saliency map **S***bu*:

$$\mathbf{S}\_{b\mu} = \mathcal{N}\left(\mathbf{F}\_{ct}^{\mathrm{s}} + \mathbf{F}\_{int}^{\mathrm{s}} + \frac{1}{2}(\mathbf{F}\_{r\mathrm{g}}^{\mathrm{s}} + \mathbf{F}\_{by}^{\mathrm{s}}) + \frac{1}{4}\sum\_{\theta} \mathbf{F}\_{o\_{\theta}}^{\mathrm{s}}\right) \tag{3}$$

Given the following two assumptions: 1) the selection process guided by the space-based bottom-up attention is a random event, and 2) the sample space of this random event is composed of all spatial locations in the image, the salience of a spatial location can be used to represent the degree of belief that bottom-up attention selects that location. Therefore, the probability of a spatial location **r***<sup>i</sup>* being attended by the bottom-up attention mechanism can be estimated as:

$$p\_{bu}(\mathbf{r}\_i) = \frac{S\_{bu}(\mathbf{r}\_i)}{\sum\_{\mathbf{r}\_{l'}} S\_{bu}(\mathbf{r}\_{l'})} \,\prime \tag{4}$$

where *pbu*(**r***i*) denotes the probability of a spatial location **r***<sup>i</sup>* being attended by the bottom-up attention, and the denominator ∑**r***i*� *Sbu*(**r***i*�) is the normalizing constant.

#### **5.2 Top-down attention**

#### **5.2.1 LTM task representations and task-relevant objects**

The *task-relevant object* can be defined as an object whose occurrence is expected by the task. Consistent with the autonomous mental development (AMD) paradigm (Weng et al., 2001), this paper proposes that actions include external actions that operate effectors and internal actions that predict the next possible attentional state (i.e., attentional prediction). Since the proposed perception system is object-based, the attentional prediction can be seen as the task-relevant object. Thus this paper models the *LTM task representation* as the association between attentional states and attentional prediction and uses it to deduce the task-relevant object.

It can be further proposed that the LTM task representation can be modeled by using a first-order discrete Markov process (FDMP). The FDMP can be expressed as *p*(*at*+1|*at*), where *at* denotes the attentional state at time *t* and *at*+<sup>1</sup> denotes the attentional prediction for time *t* + 1. This definition means that the probability of each attentional prediction for the next time can be estimated given the attentional state at the current time. The discrete attentional states is composed of LTM object representations.

#### **5.2.2 Task-relevant feature**

According to the IC hypothesis, it is required to deduce the task-relevant feature from the task-relevant object. This paper defines the *task-relevant feature* as a property that can discriminate the object from others. Although several autonomous factors (e.g., rewards obtained from learning) could be used, this paper uses the *conspicuity* quantity since it

aid each other: The salience descriptor guarantees that the task-relevant object can be effectively discriminated from distracters in terms of appearance, while the appearance descriptor can deal with the case that the task-relevant object and distracters have similar task-relevance values but different appearance values. The second advantage is efficiency. The computational complexity of (Rao & Ballard, 1995a) and our method can be approximated

<sup>35</sup> Development of an Autonomous Visual Perception

object representation, e.g., iconic representation (Rao & Ballard, 1995b) used in (Rao & Ballard,

number of one or a few pre-attentive features used in our method. Since *dh* � *<sup>d</sup>f ew*

the computation of our method is much cheaper. The third advantage is adaptability. As shown in (5), the task-relevant feature(s) can be autonomously deduced from the learned LTM representation such that the requirement of redesigning the representation of the task-relevant object for different tasks is eliminated. The fourth advantage is robustness. As shown in (6), the proposed method gives a bias toward the task-relevant object by using Bayes' rule, such that it is robust to work with noise, occlusion and a variety of viewpoints and illuminative

Assuming that bottom-up attention and top-down attention are two random events that are independent, the probability of an item being attended can be modeled as the probability of occurrence of either of these two events on that item. Thus, the probabilistic location-based attentional activation, denoted as *pattn*(**r***i*), can be obtained by combining bottom-up saliency

*<sup>f</sup>* }) <sup>−</sup> *pbu*(**r***i*) <sup>×</sup> *ptd*(**r***i*|{**F***<sup>t</sup>*

*<sup>f</sup>* }) if *wbu* = 0 and *wtd* = 1

*ptd*(**r***i*|**F***<sup>t</sup>*

*pattn*(**r***i*) = *pbu*(**r***i*) if *wbu* = 1 and *wtd* = 0

where *wbu* and *wtd* are two logic variables used as the conscious gating for bottom-up attention and top-down attention respectively and these two variables are set according to the task.

According to the IC hypothesis, it can be seen that a competitive advantage over an object is produced by directing attention to a spatial location in that object. Thus the probability of a proto-object being attended can be calculated using the *logic or* operator on the location-based probabilities. Furthermore, it can be assumed that two locations being attended are mutually exclusive according to the space-based attention theory (Posner et al., 1980). As a result, the probability of a proto-object **R***<sup>g</sup>* being attended, denoted as *pattn*(**R***g*), can be calculated as:

> *Ng* ∑ **r***i*∈**R***<sup>g</sup>*

where **R***<sup>g</sup>* denotes a proto-object, *Ng* denotes the number of pixels in **R***g*. The inclusion of 1/*Ng* is to eliminate the influence of the proto-object's size. The FOA is directed to the proto-object

*pattn*(**R***g*) = <sup>1</sup>

1995a), *dl* denotes the dimension number of a pre-attentive feature and *<sup>d</sup>f ew*

**5.3 Combination of bottom-up saliency and top-down biases**

System for Robots Using Object-Based Visual Attention

*<sup>f</sup> dl*) respectively, where *dh* denotes the dimension number of a high-level

*<sup>f</sup>* denotes the

*<sup>f</sup>* }) if *wbu* = 1 and *wtd* = 1

*<sup>f</sup>*), (8)

*<sup>f</sup> dl*,

, (7)

as <sup>O</sup>(*dh*) and <sup>O</sup>(*df ew*

and top-down biases:

*pattn*(**r***i*) = *pbu*(**r***i*) + *ptd*(**r***i*|{**F***<sup>t</sup>*

**5.4 Proto-object based attentional activation**

with maximal attentional activation.

*pattn*(**r***i*) = *ptd*(**r***i*|{**F***<sup>t</sup>*

effects.

⎧ ⎪⎪⎨

⎪⎪⎩

is one of the important intrinsic and innate properties of an object for measuring the discriminability. Through a training process that statistically encapsulates the conspicuity quantities obtained under different viewing conditions, a *salience descriptor* is achieved in the LTM object representation (See details in section 6.2 and section 6.3).

Therefore the salience descriptor is used to deduce the task-relevant feature by finding the feature dimension that has the greatest conspicuity. This deduction can be expressed as:

$$\mathbb{P}(f\_{rel}, j\_{rel}) = \arg\max\_{f \in \{\text{ct,int,rg,by,}\}} \max\_{j \in \{1,2,\dots,N\}} \frac{\overline{\mu}\_f^{s\_f}}{1 + \overline{\sigma}\_f^{s\_f}} \tag{5}$$

where *Nj* denotes the number of parts when *f* ∈ {*int*,*rg*, *by*, *oθ*} and *Nj* = 1 when *f* = *ct*, *μs*,*j <sup>f</sup>* and *<sup>σ</sup>s*,*<sup>j</sup> <sup>f</sup>* respectively denote the mean and STD of salience descriptors in terms of a feature *f* in the LTM representation of the task-relevant object, *frel* denotes the *task-relevant feature dimension*, and *jrel* denotes the index of the *task-relevant part*. The LTM object representation can be seen in section 6.3.

In the proposed system, the most task-relevant feature is first selected for guiding top-down attention. If the post-attentive recognition shows that the attended object is not the target, then the next task-relevant feature is joined. This process does not stop until the attended object is verified or all features are used.

#### **5.2.3 Attentional template**

Given the task-relevant feature dimension, its *appearance descriptor* in the LTM representation of the task-relevant object is used to build an attentional template in WM so as to estimate top-down biases. The attentional template is denoted as **F***<sup>t</sup> <sup>f</sup>* , where *f* ∈ {*ct*, *int*,*rg*, *by*, *oθ*}. The appearance descriptor will be presented in section 6.3.

#### **5.2.4 Estimation of top-down biases**

Bayesian inference is used to estimate the location-based top-down bias, which represents the probability of a spatial location being an instance of the task-relevant object. It can be generally expressed as:

$$p\_{td}(\mathbf{r}\_i|\mathbf{F}\_f^t) = \frac{p\_{td}(\mathbf{F}\_f^t|\mathbf{r}\_i) \times p\_{td}(\mathbf{r}\_i)}{\sum\_{\mathbf{r}\_{i'}} p\_{td}(\mathbf{F}\_f^t|\mathbf{r}\_{i'}) \times p\_{td}(\mathbf{r}\_{i'})},\tag{6}$$

where *ptd*(**r***i*) denotes the prior probability of a location **r***<sup>i</sup>* being attended by the top-down attention, *ptd*(**F***<sup>t</sup> <sup>f</sup>* <sup>|</sup>**r***i*) denotes the observation likelihood, *ptd*(**r***i*|**F***<sup>t</sup> <sup>f</sup>*) is the posterior probability of the location **r***<sup>i</sup>* being attended by the top-down attention given the attentional template **F***<sup>t</sup> f* . Assuming that the prior probability *ptd*(**r***i*) is a uniform distribution, Eq. (6) can be simplified into estimating the observation likelihood *ptd*(**F***<sup>t</sup> <sup>f</sup>* <sup>|</sup>**r***i*). The detailed estimation of *ptd*(**F***<sup>t</sup> <sup>f</sup>* |**r***i*) for each feature dimension, including contour, intensity, red-green, blue-yellow and orientations can be seen in our previous object-based visual attention (OVA) model (Yu et al., 2010).

#### **5.2.5 Discussion**

Compared with existing top-down attention methods, e.g., (Navalpakkam & Itti, 2005; Rao & Ballard, 1995a), the proposed method has four advantages. The first advantage is effectiveness due to the use of both salience and appearance descriptors. These two descriptors reciprocally 10 Will-be-set-by-IN-TECH

is one of the important intrinsic and innate properties of an object for measuring the discriminability. Through a training process that statistically encapsulates the conspicuity quantities obtained under different viewing conditions, a *salience descriptor* is achieved in the

Therefore the salience descriptor is used to deduce the task-relevant feature by finding the feature dimension that has the greatest conspicuity. This deduction can be expressed as:

where *Nj* denotes the number of parts when *f* ∈ {*int*,*rg*, *by*, *oθ*} and *Nj* = 1 when *f* = *ct*,

*f* in the LTM representation of the task-relevant object, *frel* denotes the *task-relevant feature dimension*, and *jrel* denotes the index of the *task-relevant part*. The LTM object representation

In the proposed system, the most task-relevant feature is first selected for guiding top-down attention. If the post-attentive recognition shows that the attended object is not the target, then the next task-relevant feature is joined. This process does not stop until the attended object is

Given the task-relevant feature dimension, its *appearance descriptor* in the LTM representation of the task-relevant object is used to build an attentional template in WM so as to estimate

Bayesian inference is used to estimate the location-based top-down bias, which represents the probability of a spatial location being an instance of the task-relevant object. It can be generally

<sup>∑</sup>**r***i*� *ptd*(**F***<sup>t</sup>*

where *ptd*(**r***i*) denotes the prior probability of a location **r***<sup>i</sup>* being attended by the top-down

of the location **r***<sup>i</sup>* being attended by the top-down attention given the attentional template **F***<sup>t</sup>*

Assuming that the prior probability *ptd*(**r***i*) is a uniform distribution, Eq. (6) can be simplified

each feature dimension, including contour, intensity, red-green, blue-yellow and orientations can be seen in our previous object-based visual attention (OVA) model (Yu et al., 2010).

Compared with existing top-down attention methods, e.g., (Navalpakkam & Itti, 2005; Rao & Ballard, 1995a), the proposed method has four advantages. The first advantage is effectiveness due to the use of both salience and appearance descriptors. These two descriptors reciprocally

*<sup>f</sup>* |**r***i*) × *ptd*(**r***i*)

*<sup>f</sup>* |**r***i*�) × *ptd*(**r***i*�)

*<sup>f</sup>* <sup>|</sup>**r***i*). The detailed estimation of *ptd*(**F***<sup>t</sup>*

*<sup>f</sup>*) = *ptd*(**F***<sup>t</sup>*

*<sup>f</sup>* <sup>|</sup>**r***i*) denotes the observation likelihood, *ptd*(**r***i*|**F***<sup>t</sup>*

*<sup>f</sup>* respectively denote the mean and STD of salience descriptors in terms of a feature

max *j*∈{1,2,...,*Nj*}

*μs*,*j f* <sup>1</sup> + *<sup>σ</sup>s*,*<sup>j</sup> f*

, (5)

*<sup>f</sup>* , where *f* ∈ {*ct*, *int*,*rg*, *by*, *oθ*}. The

, (6)

*<sup>f</sup>*) is the posterior probability

*f* .

*<sup>f</sup>* |**r***i*) for

LTM object representation (See details in section 6.2 and section 6.3).

(*frel*, *jrel*) = arg max *<sup>f</sup>*∈{*ct*,*int*,*rg*,*by*,*oθ*}

*μs*,*j*

*<sup>f</sup>* and *<sup>σ</sup>s*,*<sup>j</sup>*

expressed as:

attention, *ptd*(**F***<sup>t</sup>*

**5.2.5 Discussion**

can be seen in section 6.3.

**5.2.3 Attentional template**

verified or all features are used.

**5.2.4 Estimation of top-down biases**

top-down biases. The attentional template is denoted as **F***<sup>t</sup>*

*ptd*(**r***i*|**F***<sup>t</sup>*

appearance descriptor will be presented in section 6.3.

into estimating the observation likelihood *ptd*(**F***<sup>t</sup>*

aid each other: The salience descriptor guarantees that the task-relevant object can be effectively discriminated from distracters in terms of appearance, while the appearance descriptor can deal with the case that the task-relevant object and distracters have similar task-relevance values but different appearance values. The second advantage is efficiency. The computational complexity of (Rao & Ballard, 1995a) and our method can be approximated as <sup>O</sup>(*dh*) and <sup>O</sup>(*df ew <sup>f</sup> dl*) respectively, where *dh* denotes the dimension number of a high-level object representation, e.g., iconic representation (Rao & Ballard, 1995b) used in (Rao & Ballard, 1995a), *dl* denotes the dimension number of a pre-attentive feature and *<sup>d</sup>f ew <sup>f</sup>* denotes the

number of one or a few pre-attentive features used in our method. Since *dh* � *<sup>d</sup>f ew <sup>f</sup> dl*, the computation of our method is much cheaper. The third advantage is adaptability. As shown in (5), the task-relevant feature(s) can be autonomously deduced from the learned LTM representation such that the requirement of redesigning the representation of the task-relevant object for different tasks is eliminated. The fourth advantage is robustness. As shown in (6), the proposed method gives a bias toward the task-relevant object by using Bayes' rule, such that it is robust to work with noise, occlusion and a variety of viewpoints and illuminative effects.

#### **5.3 Combination of bottom-up saliency and top-down biases**

Assuming that bottom-up attention and top-down attention are two random events that are independent, the probability of an item being attended can be modeled as the probability of occurrence of either of these two events on that item. Thus, the probabilistic location-based attentional activation, denoted as *pattn*(**r***i*), can be obtained by combining bottom-up saliency and top-down biases:

$$\begin{cases} p\_{at \text{th}}(\mathbf{r}\_{i}) = p\_{bu}(\mathbf{r}\_{i}) + p\_{td}(\mathbf{r}\_{i}|\{\mathbf{F}\_{f}^{t}\}) - p\_{bu}(\mathbf{r}\_{i}) \times p\_{td}(\mathbf{r}\_{i}|\{\mathbf{F}\_{f}^{t}\}) & \text{if } w\_{bu} = 1 \text{ and } w\_{td} = 1\\ p\_{at \text{th}}(\mathbf{r}\_{i}) = p\_{bu}(\mathbf{r}\_{i}) & \text{if } w\_{bu} = 1 \text{ and } w\_{td} = 0 \text{ .}\\ p\_{at \text{th}}(\mathbf{r}\_{i}) = p\_{td}(\mathbf{r}\_{i}|\{\mathbf{F}\_{f}^{t}\}) & \text{if } w\_{bu} = 0 \text{ and } w\_{td} = 1 \end{cases} (7)$$

where *wbu* and *wtd* are two logic variables used as the conscious gating for bottom-up attention and top-down attention respectively and these two variables are set according to the task.

#### **5.4 Proto-object based attentional activation**

According to the IC hypothesis, it can be seen that a competitive advantage over an object is produced by directing attention to a spatial location in that object. Thus the probability of a proto-object being attended can be calculated using the *logic or* operator on the location-based probabilities. Furthermore, it can be assumed that two locations being attended are mutually exclusive according to the space-based attention theory (Posner et al., 1980). As a result, the probability of a proto-object **R***<sup>g</sup>* being attended, denoted as *pattn*(**R***g*), can be calculated as:

$$p\_{attn}(\mathbf{R}\_{\mathcal{S}}) = \frac{1}{N\_{\mathcal{S}}} \sum\_{\mathbf{r}\_{i} \in \mathbf{R}\_{\mathcal{S}}} p\_{td}(\mathbf{r}\_{i}|\mathbf{F}\_{f}^{t}),\tag{8}$$

where **R***<sup>g</sup>* denotes a proto-object, *Ng* denotes the number of pixels in **R***g*. The inclusion of 1/*Ng* is to eliminate the influence of the proto-object's size. The FOA is directed to the proto-object with maximal attentional activation.

Fig. 5. The flowchart of the post-attentive perception stage.

salience components at these control points, i.e.,

System for Robots Using Object-Based Visual Attention

*gb*(**r***cp*) =

a control point, i.e., **F**˜ *<sup>a</sup>*

*gb*(**r***cp*) = *F<sup>s</sup>*

**6.3.1 PNN of local coding**

i.e., **F**˜*<sup>s</sup>*

using the conspicuity value *F<sup>s</sup>*

*ct*(**r***cp*).

and *salience descriptors* (denoted as **O***<sup>s</sup>*

**6.3 Development of LTM object representations**

two steps. The first step is to extract control points, denoted as a set {**r***cp*}, of the attended object's contour by using the method in our previous work (Yu et al., 2010). That is, each control point is an entry in the set {**F**˜ *gb*}. The second step is to estimate the appearance and

<sup>37</sup> Development of an Autonomous Visual Perception

appearance component of an entry consists of spatial coordinates in the reference frame at

The LTM object representation also consists of the local coding (denoted as **O***lc*) and global coding (denoted as **O***gb*). Each coding also consists of *appearance descriptors* (denoted as **O***a*)

The PNN of a local coding **O***lc* (termed as a *local PNN*) includes three layers. The input layer receives the local post-attentive feature vector **F**˜*lc*. Each radial basis function (RBF) at the

*<sup>x</sup>***r***cp <sup>y</sup>***r***cp <sup>T</sup>*

**<sup>F</sup>**˜ *gb* <sup>=</sup> **<sup>F</sup>**˜ *<sup>a</sup>*

*ct*(**r***cp*) in terms of pre-attentive contour feature at a control point,

). The PNN (Specht, 1990) is used to build them.

*gb*(**r***cp*), **<sup>F</sup>**˜*<sup>s</sup>*

. The salience component of an entry is built by

*gb*(**r***cp*)

*<sup>T</sup>*

<sup>∀</sup>**r***cp* . The

#### **6. Post-attentive perception**

The flow chart of the post-attentive perception can be illustrated in Fig. 5. Four modules, as presented in section 3, are **interactive** during this stage.

#### **6.1 Perceptual completion processing**

This module works around the attended proto-object, denoted as **R**<sup>1</sup> *attn*, to achieve the complete object region. It consists of two steps. The first step is recognition of the attended proto-object. This step explores LTM object representations in order to determine to which LTM object representation the attended proto-object belongs by using the post-attentive features. The extraction of post-attentive features and the recognition algorithm will be presented in section 6.2 and section 6.4 respectively. The matched LTM object representation, denoted as **O***attn*, is then recalled from LTM.

The second step is completion processing:


These labeled proto-objects constitute the complete region of the attended object, which is denoted as a set {**R***attn*}.

#### **6.2 Extraction of post-attentive features**

*Post-attentive features* **F**˜ are estimated by using the statistics within the attended object. They consist of *global post-attentive features* **F**˜ *gb* and *local post-attentive features* **F**˜*lc*. Each **F**˜ consists of *appearance component* **F**˜ *<sup>a</sup>* and *salience component* **F**˜*<sup>s</sup>* .

#### **6.2.1 Local post-attentive features**

Each proto-object, denoted as **R***<sup>m</sup> attn*, in the complete region being attended (i.e., **<sup>R</sup>***<sup>m</sup> attn* ∈ {**R***attn*}) is the unit for estimating local post-attentive features. They can be estimated as a set that can be expressed as: **<sup>F</sup>**˜*lc* <sup>=</sup> **<sup>F</sup>**˜ *<sup>a</sup> lc*(**R***<sup>m</sup> attn*), **<sup>F</sup>**˜*<sup>s</sup> lc*(**R***<sup>m</sup> attn*) *<sup>T</sup>*

<sup>∀</sup>**R***<sup>m</sup> attn*∈{**R***attn*}. The appearance components in an entry **<sup>F</sup>**˜*lc*, denoted as **<sup>F</sup>**˜ *<sup>a</sup> lc* <sup>=</sup> {**F**˜ *<sup>a</sup> <sup>f</sup>* } with *f* ∈ {*int*,*rg*, *by*, *oθ*}, are estimated by using the mean *μ*˜ *a*,*m <sup>f</sup>* of **<sup>R</sup>***<sup>m</sup> attn* in terms of *<sup>f</sup>* , i.e., **<sup>F</sup>**˜ *<sup>a</sup> f*(**R***<sup>m</sup> attn*) = *μ*˜ *a*,*m <sup>f</sup>* .

The salience components, denoted as **F**˜*<sup>s</sup> lc* <sup>=</sup> {**F**˜*<sup>s</sup> <sup>f</sup>* } with *f* ∈ {*int*,*rg*, *by*, *oθ*}, can be estimated using the mean of conspicuity *μ*˜ *s*,*m <sup>f</sup>* of a **<sup>R</sup>***<sup>m</sup> attn* in terms of *<sup>f</sup>* , i.e., *<sup>F</sup>*˜*<sup>s</sup> f*(**R***<sup>m</sup> attn*) = *μ*˜ *s*,*m <sup>f</sup>* . The conspicuity quantity *F<sup>s</sup> <sup>f</sup>* in terms of *f* is calculated using (2).

#### **6.2.2 Global post-attentive features**

The global post-attentive feature **F**˜ *gb* is estimated after the complete region of the attended object, i.e., {**R***attn*}, is obtained. Since the active contour technique (Blake & Isard, 1998; MacCormick, 2000) is used to represent a contour in this paper, the estimation of **F**˜ *gb* includes 12 Will-be-set-by-IN-TECH

The flow chart of the post-attentive perception can be illustrated in Fig. 5. Four modules, as

complete object region. It consists of two steps. The first step is recognition of the attended proto-object. This step explores LTM object representations in order to determine to which LTM object representation the attended proto-object belongs by using the post-attentive features. The extraction of post-attentive features and the recognition algorithm will be presented in section 6.2 and section 6.4 respectively. The matched LTM object representation,

1. If the local coding of **O***attn* includes multiple parts, several candidate proto-objects, which

3. Each **R***<sup>n</sup>* is recognized using the local post-attentive features and the matched LTM object representation **O***attn*. If it is recognized as a part of **O***attn*, it will be labeled as a part of the

These labeled proto-objects constitute the complete region of the attended object, which is

*Post-attentive features* **F**˜ are estimated by using the statistics within the attended object. They consist of *global post-attentive features* **F**˜ *gb* and *local post-attentive features* **F**˜*lc*. Each **F**˜ consists of

{**R***attn*}) is the unit for estimating local post-attentive features. They can be estimated as a

*attn*), **<sup>F</sup>**˜*<sup>s</sup>*

*lc*(**R***<sup>m</sup> attn*) *<sup>T</sup>* <sup>∀</sup>**R***<sup>m</sup>*

*attn* in terms of *<sup>f</sup>* , i.e., **<sup>F</sup>**˜ *<sup>a</sup>*

*attn* in terms of *<sup>f</sup>* , i.e., *<sup>F</sup>*˜*<sup>s</sup>*

*lc*(**R***<sup>m</sup>*

*lc* <sup>=</sup> {**F**˜*<sup>s</sup>*

The global post-attentive feature **F**˜ *gb* is estimated after the complete region of the attended object, i.e., {**R***attn*}, is obtained. Since the active contour technique (Blake & Isard, 1998; MacCormick, 2000) is used to represent a contour in this paper, the estimation of **F**˜ *gb* includes

*<sup>f</sup>* of a **<sup>R</sup>***<sup>m</sup>*

*<sup>f</sup>* in terms of *f* is calculated using (2).

.

4. Continue *item 2* and *item 3* iteratively until all neighbors have been checked.

*attn*, are selected from the current scene. They are termed as *neighbors*

*attn*, in the complete region being attended (i.e., **<sup>R</sup>***<sup>m</sup>*

*lc* <sup>=</sup> {**F**˜ *<sup>a</sup>*

*attn*∈{**R***attn*}.

*<sup>f</sup>* } with *f* ∈ {*int*,*rg*, *by*, *oθ*}, can be estimated

*attn*) = *μ*˜

*f*(**R***<sup>m</sup>*

*f*(**R***<sup>m</sup>*

*<sup>f</sup>* } with *f* ∈ {*int*,*rg*, *by*, *oθ*},

*a*,*m <sup>f</sup>* .

*attn*) = *μ*˜

*s*,*m <sup>f</sup>* . The

*attn*, to achieve the

*attn* ∈

**6. Post-attentive perception**

**6.1 Perceptual completion processing**

denoted as **O***attn*, is then recalled from LTM. The second step is completion processing:

are spatially close to **R**<sup>1</sup>

denoted as a set {**R***attn*}.

**6.2 Extraction of post-attentive features**

**6.2.1 Local post-attentive features** Each proto-object, denoted as **R***<sup>m</sup>*

are estimated by using the mean *μ*˜

using the mean of conspicuity *μ*˜

**6.2.2 Global post-attentive features**

conspicuity quantity *F<sup>s</sup>*

The salience components, denoted as **F**˜*<sup>s</sup>*

set that can be expressed as:

*appearance component* **F**˜ *<sup>a</sup>* and *salience component* **F**˜*<sup>s</sup>*

**F**˜*lc*

The appearance components in an entry **<sup>F</sup>**˜*lc*, denoted as **<sup>F</sup>**˜ *<sup>a</sup>*

= **F**˜ *<sup>a</sup>*

*a*,*m <sup>f</sup>* of **<sup>R</sup>***<sup>m</sup>*

*s*,*m*

and denoted as a set {**R***n*}.

presented in section 3, are **interactive** during this stage.

2. The local post-attentive features are extracted in each **R***n*.

attended object. Otherwise, it will be eliminated.

This module works around the attended proto-object, denoted as **R**<sup>1</sup>

Fig. 5. The flowchart of the post-attentive perception stage.

two steps. The first step is to extract control points, denoted as a set {**r***cp*}, of the attended object's contour by using the method in our previous work (Yu et al., 2010). That is, each control point is an entry in the set {**F**˜ *gb*}. The second step is to estimate the appearance and salience components at these control points, i.e., **<sup>F</sup>**˜ *gb* <sup>=</sup> **<sup>F</sup>**˜ *<sup>a</sup> gb*(**r***cp*), **<sup>F</sup>**˜*<sup>s</sup> gb*(**r***cp*) *<sup>T</sup>* <sup>∀</sup>**r***cp* . The appearance component of an entry consists of spatial coordinates in the reference frame at a control point, i.e., **F**˜ *<sup>a</sup> gb*(**r***cp*) = *<sup>x</sup>***r***cp <sup>y</sup>***r***cp <sup>T</sup>* . The salience component of an entry is built by using the conspicuity value *F<sup>s</sup> ct*(**r***cp*) in terms of pre-attentive contour feature at a control point, i.e., **F**˜*<sup>s</sup> gb*(**r***cp*) = *F<sup>s</sup> ct*(**r***cp*).

#### **6.3 Development of LTM object representations**

The LTM object representation also consists of the local coding (denoted as **O***lc*) and global coding (denoted as **O***gb*). Each coding also consists of *appearance descriptors* (denoted as **O***a*) and *salience descriptors* (denoted as **O***<sup>s</sup>* ). The PNN (Specht, 1990) is used to build them.

#### **6.3.1 PNN of local coding**

The PNN of a local coding **O***lc* (termed as a *local PNN*) includes three layers. The input layer receives the local post-attentive feature vector **F**˜*lc*. Each radial basis function (RBF) at the

**Algorithm 1** Learning Routine of Local and Global Codings 1: Given a local or global training pattern (**F**˜*lc*, *k*) or (**F**˜ *gb*, *k*):

<sup>2</sup> + (**F**˜).

*<sup>j</sup>* + 1; *// Increment the occurrence number*

*<sup>j</sup>*� . *// Normalize weights π*

detecting a salient object and the other is detecting a task-relevant object.

2 /(*a<sup>k</sup>*

<sup>2</sup> ; *// Update the STD*

*<sup>j</sup>* + 1); *// Update the mean vector*

<sup>39</sup> Development of an Autonomous Visual Perception

Due to the page limitation, the object recognition module can be summarized as follows. It can be modeled at two levels. The first one is the object level. The purpose of this level is to recognize to which LTM object an attended pattern belongs. The second one is the part level or control point level. Recognition at this level is performed given an LTM object to which the attended pattern belongs. Thus, the purpose of this level is to recognize to which part in a local PNN or to which control point in a global PNN an attended pattern belongs. At each level, object recognition can generally be modeled as a decision unit by using Bayes' theorem. Assuming that the prior probability is equal for all LTM patterns at each level, the observation

This proposed autonomous visual perception system is tested in the task of object detection. The unconscious perception path (i.e., the bottom-up attention module) can be used to detect a salient object, such as a landmark, whereas the conscious perception path (i.e., the top-down attention module) can be used to detect the task-relevant object, i.e., the expected target. Thus the unconscious and conscious aspects are tested in two robotics tasks respectively: One is

The salient object is an unusual or unexpected object and the current task has no prediction about its occurrence. There are three objectives in this task. The first objective is to illustrate the unconscious capability of the proposed perception system. The second objective is to show the advantages of using object-based visual attention for perception by comparing it with the space-based visual attention methods. The third objective is to show the advantage of integrating the contour feature into the bottom-up competition module. The result is that an object that has a conspicuous shape compared with its neighbors can be detected. Two

*<sup>i</sup>* (**F**˜);

*<sup>j</sup>* + 1); *// Prepare for updating the STD*

*<sup>j</sup>* = 1; *// Set the initial mean, STD and occurrence number*

3: Recognize **F**˜ to obtain a recognition probability *p<sup>k</sup>*

System for Robots Using Object-Based Visual Attention

*<sup>j</sup>* <sup>+</sup> **<sup>F</sup>**˜)/(*a<sup>k</sup>*

*j*). 2]. − 1

*<sup>j</sup>* <sup>=</sup> <sup>σ</sup>*init*; *<sup>a</sup><sup>k</sup>*

likelihood can be seen as the posterior probability.

2: Set **F**˜ = **F**˜*lc* or **F**˜ = **F**˜ *gb*;

*<sup>j</sup>* = (*a<sup>k</sup> j* μ*k*

*<sup>j</sup>* = [*σ<sup>d</sup>*

*<sup>j</sup>* <sup>=</sup> *<sup>a</sup><sup>k</sup>*

*<sup>j</sup>* <sup>=</sup> *<sup>a</sup><sup>k</sup>*

**6.4 Object recognition**

**7. Experiments**

**7.1 Detecting a salient object**

*<sup>j</sup>*(**F**˜) <sup>≥</sup> *<sup>τ</sup>*<sup>1</sup> or <sup>≥</sup> *<sup>τ</sup>*<sup>2</sup> **then** 5: *// Update part j of object k*

> *ak j*(*σ<sup>k</sup> j* ). <sup>2</sup> + *a<sup>k</sup> j*(*μ<sup>k</sup> j*).

11: *// Create a new part i of object k* 12: Set *Nk* = *Nk* + 1; *i* = *Nk*;

*<sup>j</sup>* / <sup>∑</sup>*j*� *<sup>a</sup><sup>k</sup>*

*<sup>j</sup>* <sup>=</sup> **<sup>F</sup>**˜; <sup>σ</sup>*<sup>k</sup>*

*temp* <sup>−</sup> (*μ<sup>k</sup>*

4: **if** *p<sup>k</sup>*

7: μ*<sup>k</sup>*

8: *σ<sup>k</sup>*

9: *a<sup>k</sup>*

13: μ*<sup>k</sup>*

14: **end if** 15: <sup>∀</sup>*j*: *<sup>π</sup><sup>k</sup>*

10: **else**

6: *σtemp* =

hidden layer represents a part of the learned object and thereby this layer is called a *part layer*. The output layer is a probabilistic mixture of all parts belonging to the object and thereby this layer is called an *object layer*.

The probability distribution of a RBF at the part layer of the local PNN can be expressed as:

$$\begin{split} p\_{\vec{j}}^{k}(\tilde{\mathbf{F}}\_{lc}) &= \mathcal{G}(\tilde{\mathbf{F}}\_{lc}; \boldsymbol{\mu}\_{\vec{j}}^{k}, \boldsymbol{\Sigma}\_{\vec{j}}^{k}) \\ &= \frac{1}{(2\pi)^{\frac{d}{2}} |\boldsymbol{\Sigma}\_{\vec{j}}^{k}|^{\frac{1}{2}}} \exp\{-\frac{1}{2} (\tilde{\mathbf{F}}\_{lc} - \boldsymbol{\mu}\_{\vec{j}}^{k})^{T} (\boldsymbol{\Sigma}\_{\vec{j}}^{k})^{-1} (\tilde{\mathbf{F}}\_{lc} - \boldsymbol{\mu}\_{\vec{j}}^{k})\} \end{split} \tag{9}$$

where <sup>G</sup> denotes the Gaussian distribution, <sup>μ</sup>*<sup>k</sup> <sup>j</sup>* and **<sup>Σ</sup>***<sup>k</sup> <sup>j</sup>* denote the mean vector and covariance matrix of a RBF, *j* is the index of a part, *k* is the index of an object in LTM, and *d* is the dimension number of a local post-attentive feature **F**˜*lc*. Since all feature dimensions are assumed to be independent, **Σ***<sup>k</sup> <sup>j</sup>* is a diagonal matrix and standard deviation (STD) values of all feature dimensions of a RBF can constitute an STD vector σ*<sup>k</sup> j* .

The probabilistic mixture estimation *rk*(**F**˜*lc*) at the object layer can be expressed as:

$$r^k(\mathbf{F}\_{lc}) = \sum\_{i} \pi\_j^k p\_j^k(\mathbf{F}\_{lc})\_\prime \tag{10}$$

where *π<sup>k</sup> <sup>j</sup>* denotes the contribution of part *<sup>j</sup>* to object *<sup>k</sup>*, which holds <sup>∑</sup>*<sup>j</sup> <sup>π</sup><sup>k</sup> <sup>j</sup>* = 1.

#### **6.3.2 PNN of global coding**

The PNN for a global coding **O***gb* (termed as a *global PNN*) also includes three layers. The input layer receives the global post-attentive feature vector **F**˜ *gb*. Each node of the hidden layer is a control point along the contour and thereby this layer is called a *control point layer*. The output layer is a probabilistic combination of all control points belonging to the object and thereby this layer is called an *object layer*. The mathematical expression of the global PNN is similar to the local PNN.

#### **6.3.3 Learning of LTM object representations**

Since the number of nodes (i.e., the numbers of parts and control points) is unknown and might be dynamically changed during the training course, this paper proposes a dynamical learning algorithm by using both the maximum likelihood estimation (MLE) and a Bayes' classifier to update the local and global PNNs at each time. This proposed dynamical learning algorithm can be summarized as follows. The Bayes' classifier is used to classify the training pattern to an existing LTM pattern. If the training pattern can be classified to an existing LTM pattern at the part level in a local PNN or at the control point level in a global PNN, both appearance and salience descriptors of this existing LTM pattern are updated using MLE. Otherwise, a new LTM pattern is created. Two thresholds *τ*<sup>1</sup> and *τ*<sup>2</sup> are introduced to determine the minimum correct classification probability to an existing part and an existing control point respectively. Algorithm 1 shows the learning routine of global and local codings. In the algorithm, *a<sup>k</sup> <sup>j</sup>* denotes the occurrence number of an existing pattern indexed by *j* of object *k* and it is initialized by 0, *Nk* denotes the number of parts in the local PNN or control points in the global PNN of object *k*, .2 denotes the element-by-element square operator, and *σinit* is a predefined STD value when a new pattern is created.

#### **Algorithm 1** Learning Routine of Local and Global Codings

1: Given a local or global training pattern (**F**˜*lc*, *k*) or (**F**˜ *gb*, *k*): 2: Set **F**˜ = **F**˜*lc* or **F**˜ = **F**˜ *gb*; 3: Recognize **F**˜ to obtain a recognition probability *p<sup>k</sup> <sup>i</sup>* (**F**˜); 4: **if** *p<sup>k</sup> <sup>j</sup>*(**F**˜) <sup>≥</sup> *<sup>τ</sup>*<sup>1</sup> or <sup>≥</sup> *<sup>τ</sup>*<sup>2</sup> **then** 5: *// Update part j of object k* 6: *σtemp* = *ak j*(*σ<sup>k</sup> j* ). <sup>2</sup> + *a<sup>k</sup> j*(*μ<sup>k</sup> j*). <sup>2</sup> + (**F**˜). 2 /(*a<sup>k</sup> <sup>j</sup>* + 1); *// Prepare for updating the STD* 7: μ*<sup>k</sup> <sup>j</sup>* = (*a<sup>k</sup> j* μ*k <sup>j</sup>* <sup>+</sup> **<sup>F</sup>**˜)/(*a<sup>k</sup> <sup>j</sup>* + 1); *// Update the mean vector* 8: *σ<sup>k</sup> <sup>j</sup>* = [*σ<sup>d</sup> temp* <sup>−</sup> (*μ<sup>k</sup> j*). 2]. − 1 <sup>2</sup> ; *// Update the STD* 9: *a<sup>k</sup> <sup>j</sup>* <sup>=</sup> *<sup>a</sup><sup>k</sup> <sup>j</sup>* + 1; *// Increment the occurrence number* 10: **else** 11: *// Create a new part i of object k* 12: Set *Nk* = *Nk* + 1; *i* = *Nk*; 13: μ*<sup>k</sup> <sup>j</sup>* <sup>=</sup> **<sup>F</sup>**˜; <sup>σ</sup>*<sup>k</sup> <sup>j</sup>* <sup>=</sup> <sup>σ</sup>*init*; *<sup>a</sup><sup>k</sup> <sup>j</sup>* = 1; *// Set the initial mean, STD and occurrence number* 14: **end if** 15: <sup>∀</sup>*j*: *<sup>π</sup><sup>k</sup> <sup>j</sup>* <sup>=</sup> *<sup>a</sup><sup>k</sup> <sup>j</sup>* / <sup>∑</sup>*j*� *<sup>a</sup><sup>k</sup> <sup>j</sup>*� . *// Normalize weights π*

#### **6.4 Object recognition**

14 Will-be-set-by-IN-TECH

hidden layer represents a part of the learned object and thereby this layer is called a *part layer*. The output layer is a probabilistic mixture of all parts belonging to the object and thereby this

The probability distribution of a RBF at the part layer of the local PNN can be expressed as:

(**F**˜*lc* <sup>−</sup> <sup>μ</sup>*<sup>k</sup>*

*<sup>j</sup>* and **<sup>Σ</sup>***<sup>k</sup>*

matrix of a RBF, *j* is the index of a part, *k* is the index of an object in LTM, and *d* is the dimension number of a local post-attentive feature **F**˜*lc*. Since all feature dimensions are assumed to

> *i πk j pk*

The PNN for a global coding **O***gb* (termed as a *global PNN*) also includes three layers. The input layer receives the global post-attentive feature vector **F**˜ *gb*. Each node of the hidden layer is a control point along the contour and thereby this layer is called a *control point layer*. The output layer is a probabilistic combination of all control points belonging to the object and thereby this layer is called an *object layer*. The mathematical expression of the global PNN is

Since the number of nodes (i.e., the numbers of parts and control points) is unknown and might be dynamically changed during the training course, this paper proposes a dynamical learning algorithm by using both the maximum likelihood estimation (MLE) and a Bayes' classifier to update the local and global PNNs at each time. This proposed dynamical learning algorithm can be summarized as follows. The Bayes' classifier is used to classify the training pattern to an existing LTM pattern. If the training pattern can be classified to an existing LTM pattern at the part level in a local PNN or at the control point level in a global PNN, both appearance and salience descriptors of this existing LTM pattern are updated using MLE. Otherwise, a new LTM pattern is created. Two thresholds *τ*<sup>1</sup> and *τ*<sup>2</sup> are introduced to determine the minimum correct classification probability to an existing part and an existing control point respectively. Algorithm 1 shows the learning routine of global and local codings.

object *k* and it is initialized by 0, *Nk* denotes the number of parts in the local PNN or control points in the global PNN of object *k*, .2 denotes the element-by-element square operator, and

*<sup>j</sup>* denotes the occurrence number of an existing pattern indexed by *j* of

*j*)*T*(**Σ***<sup>k</sup>*

*<sup>j</sup>* is a diagonal matrix and standard deviation (STD) values of all feature

*j* .

*<sup>j</sup>*)−1(**F**˜*lc* <sup>−</sup> <sup>μ</sup>*<sup>k</sup>*

*<sup>j</sup>* denote the mean vector and covariance

*<sup>j</sup>*(**F**˜*lc*), (10)

*<sup>j</sup>* = 1.

*<sup>j</sup>*)} (9)

exp{−<sup>1</sup> 2

The probabilistic mixture estimation *rk*(**F**˜*lc*) at the object layer can be expressed as:

*<sup>r</sup>k*(**F**˜*lc*) = ∑

*<sup>j</sup>* denotes the contribution of part *<sup>j</sup>* to object *<sup>k</sup>*, which holds <sup>∑</sup>*<sup>j</sup> <sup>π</sup><sup>k</sup>*

*<sup>j</sup>* , **<sup>Σ</sup>***<sup>k</sup> j*)

layer is called an *object layer*.

be independent, **Σ***<sup>k</sup>*

**6.3.2 PNN of global coding**

similar to the local PNN.

In the algorithm, *a<sup>k</sup>*

**6.3.3 Learning of LTM object representations**

*σinit* is a predefined STD value when a new pattern is created.

where *π<sup>k</sup>*

*pk*

*<sup>j</sup>*(**F**˜*lc*) = <sup>G</sup>(**F**˜*lc*; <sup>μ</sup>*<sup>k</sup>*

dimensions of a RBF can constitute an STD vector σ*<sup>k</sup>*

where <sup>G</sup> denotes the Gaussian distribution, <sup>μ</sup>*<sup>k</sup>*

<sup>=</sup> <sup>1</sup> (2*π*) *d* <sup>2</sup> <sup>|</sup>**Σ***<sup>k</sup> j* | 1 2

> Due to the page limitation, the object recognition module can be summarized as follows. It can be modeled at two levels. The first one is the object level. The purpose of this level is to recognize to which LTM object an attended pattern belongs. The second one is the part level or control point level. Recognition at this level is performed given an LTM object to which the attended pattern belongs. Thus, the purpose of this level is to recognize to which part in a local PNN or to which control point in a global PNN an attended pattern belongs. At each level, object recognition can generally be modeled as a decision unit by using Bayes' theorem. Assuming that the prior probability is equal for all LTM patterns at each level, the observation likelihood can be seen as the posterior probability.

#### **7. Experiments**

This proposed autonomous visual perception system is tested in the task of object detection. The unconscious perception path (i.e., the bottom-up attention module) can be used to detect a salient object, such as a landmark, whereas the conscious perception path (i.e., the top-down attention module) can be used to detect the task-relevant object, i.e., the expected target. Thus the unconscious and conscious aspects are tested in two robotics tasks respectively: One is detecting a salient object and the other is detecting a task-relevant object.

#### **7.1 Detecting a salient object**

The salient object is an unusual or unexpected object and the current task has no prediction about its occurrence. There are three objectives in this task. The first objective is to illustrate the unconscious capability of the proposed perception system. The second objective is to show the advantages of using object-based visual attention for perception by comparing it with the space-based visual attention methods. The third objective is to show the advantage of integrating the contour feature into the bottom-up competition module. The result is that an object that has a conspicuous shape compared with its neighbors can be detected. Two

(a)

<sup>41</sup> Development of an Autonomous Visual Perception

System for Robots Using Object-Based Visual Attention

(b)

(c)

(d) ——————————————————————————————–

(e)

(f)

Fig. 6. Detection of a salient object, which is conspicuous to its neighbors in terms of colors. Each column represents a type of experimental setting. Column 1 is a typical setting. Column 2 is a noise setting of column 1. Column 3 is a different lighting setting with respect to column 1. Column 4 is a spatial transformation setting with respect to column 1. Row (a): Original input images. Row (b): Pre-attentive segmentation. Each color represents one proto-object. Row (c): Proto-object based attentional activation map. Row (d): The complete region being attended. Row (e): Detection results using Itti's model. The red rectangles highlight the attended location. Row (f): Detection results using Sun's model. The red circles

highlight the attended object.

experiments are shown in this section, including the detection of an object that is conspicuous in colors and in contour respectively.

#### **7.1.1 Experimental setup**

Artificial images are used in the experiments. The frame size of all images is 640 × 480 pixels. In order to show the robustness of the proposed perception system, these images are obtained using different settings, including noise, spatial transformation and changes of lighting. The noisy images are manually obtained by adding salt and pepper noise patches (noise density: 0.1 ∼ 0.15, patch size: 10 × 10 pixels ∼ 15 × 15 pixels) into original *r*, *g* and *b* color channels respectively. The experimental results are compared with the results of Itti's model (i.e., space-based bottom-up attention) (Itti et al., 1998) and Sun's model (i.e., object-based bottom-up attention) (Sun & Fisher, 2003).

#### **7.1.2 An object conspicuous in colors**

The first experiment is detecting an object that is conspicuous to its neighbors in terms of colors and all other features are approximately the same between the object and its neighbors. The experimental results are shown in Fig. 6. The salient object is the red ball in this experiment. Results of the proposed perception system are shown in Fig. 6(d), which indicate that this proposed perception system can detect the object that is conspicuous to its neighbors in terms of colors in different settings. Results of Itti's model and Sun's model are shown in Fig. 6(e) and Fig. 6(f) respectively. It can be seen that Itti's model fails to detect the salient object when noise is added to the image, as shown in column 2 in Fig. 6(e). This indicates that the proposed object-based visual perception system is more robust to noise than the space-based visual perception methods.

#### **7.1.3 An object conspicuous in contour**

The second experiment is detecting an object that is conspicuous to its neighbors in terms of contour and all other features are approximately the same between the object and its neighbors. The experimental results are shown in Fig. 7. In this experiment, the salient object is the triangle. Detection results of the proposed perception system are shown in Fig. 7(d), which indicate that the proposed perception system can detect the object that is conspicuous to its neighbors in terms of contour in different settings. Detection results of Itti's model and Sun's model are shown in Fig. 7(e) and Fig. 7(f) respectively. It can be seen that both Itti's model and Sun's model fail to detect the salient object when noise is added to the image, as shown in column 2 in Fig. 7(e) and Fig. 7(f) respectively. This experiment indicates that the proposed object-based visual perception system is capable of detecting the object conspicuous in terms of contour in different settings due to the inclusion of contour conspicuity in the proposed bottom-up attention module.

#### **7.2 Detecting a task-relevant object**

It is an important ability for robots to accurately detect a task-relevant object (i.e., target) in the cluttered environment. According to the proposed perception system, the detection procedure consists of two phases: a learning phase and a detection phase. The objective of the learning phase is to develop the LTM representation of the target. The objective of the detection phase is to detect the target by using the learned LTM representation of the target. The detection phase can be implemented as a two-stage process. The first stage is attentional selection: The 16 Will-be-set-by-IN-TECH

experiments are shown in this section, including the detection of an object that is conspicuous

Artificial images are used in the experiments. The frame size of all images is 640 × 480 pixels. In order to show the robustness of the proposed perception system, these images are obtained using different settings, including noise, spatial transformation and changes of lighting. The noisy images are manually obtained by adding salt and pepper noise patches (noise density: 0.1 ∼ 0.15, patch size: 10 × 10 pixels ∼ 15 × 15 pixels) into original *r*, *g* and *b* color channels respectively. The experimental results are compared with the results of Itti's model (i.e., space-based bottom-up attention) (Itti et al., 1998) and Sun's model (i.e.,

The first experiment is detecting an object that is conspicuous to its neighbors in terms of colors and all other features are approximately the same between the object and its neighbors. The experimental results are shown in Fig. 6. The salient object is the red ball in this experiment. Results of the proposed perception system are shown in Fig. 6(d), which indicate that this proposed perception system can detect the object that is conspicuous to its neighbors in terms of colors in different settings. Results of Itti's model and Sun's model are shown in Fig. 6(e) and Fig. 6(f) respectively. It can be seen that Itti's model fails to detect the salient object when noise is added to the image, as shown in column 2 in Fig. 6(e). This indicates that the proposed object-based visual perception system is more robust to noise than the

The second experiment is detecting an object that is conspicuous to its neighbors in terms of contour and all other features are approximately the same between the object and its neighbors. The experimental results are shown in Fig. 7. In this experiment, the salient object is the triangle. Detection results of the proposed perception system are shown in Fig. 7(d), which indicate that the proposed perception system can detect the object that is conspicuous to its neighbors in terms of contour in different settings. Detection results of Itti's model and Sun's model are shown in Fig. 7(e) and Fig. 7(f) respectively. It can be seen that both Itti's model and Sun's model fail to detect the salient object when noise is added to the image, as shown in column 2 in Fig. 7(e) and Fig. 7(f) respectively. This experiment indicates that the proposed object-based visual perception system is capable of detecting the object conspicuous in terms of contour in different settings due to the inclusion of contour conspicuity in the

It is an important ability for robots to accurately detect a task-relevant object (i.e., target) in the cluttered environment. According to the proposed perception system, the detection procedure consists of two phases: a learning phase and a detection phase. The objective of the learning phase is to develop the LTM representation of the target. The objective of the detection phase is to detect the target by using the learned LTM representation of the target. The detection phase can be implemented as a two-stage process. The first stage is attentional selection: The

in colors and in contour respectively.

**7.1.2 An object conspicuous in colors**

space-based visual perception methods.

**7.1.3 An object conspicuous in contour**

proposed bottom-up attention module.

**7.2 Detecting a task-relevant object**

object-based bottom-up attention) (Sun & Fisher, 2003).

**7.1.1 Experimental setup**

Fig. 6. Detection of a salient object, which is conspicuous to its neighbors in terms of colors. Each column represents a type of experimental setting. Column 1 is a typical setting. Column 2 is a noise setting of column 1. Column 3 is a different lighting setting with respect to column 1. Column 4 is a spatial transformation setting with respect to column 1. Row (a): Original input images. Row (b): Pre-attentive segmentation. Each color represents one proto-object. Row (c): Proto-object based attentional activation map. Row (d): The complete region being attended. Row (e): Detection results using Itti's model. The red rectangles highlight the attended location. Row (f): Detection results using Sun's model. The red circles highlight the attended object.

task-relevant feature(s) of the target is used to guide attentional selection through top-down biasing to obtain an attended object. The second stage is post-attentive recognition: The attended object is recognized using the target's LTM representation to check if it is the target. If not, another procedure of attentional selection is performed by using more task-relevant

<sup>43</sup> Development of an Autonomous Visual Perception

Two objects are used to test the proposed method of detecting a task-relevant object: a book and a human. Images and videos are obtained under different settings, including noise, transformation, lighting changes and occlusion. For training for the book, 20 images are used. For testing for the book, 50 images are used. The size of each image is 640 × 480 pixels. For detecting the human, three videos are obtained by a moving robot. Two different office environments have been used. Video 1 and video 2 are obtained in office scene 1 with low and high lighting conditions respectively. Video 3 is obtained in office scene 2. All three videos contain a total of 650 image frames, in which 20 image frames are selected from video 1 and video 2 for training and the rest of the 630 image frames are used for testing. The size of each frame in these videos is 1024 × 768 pixels. It is important to note that each test image includes not only a target but also various distracters. The noisy images are manually obtained by adding salt and pepper noise patches (noise density: 0.1, patch size: 5 × 5 pixels) into original

The results of the proposed method are compared with the results of Itti's model (Itti et al., 1998) (i.e., a space-based bottom-up attention model) and Navalpakkam's model (Navalpakkam & Itti, 2005) (i.e., a space-based top-down attention model) respectively.

The first task is to detect the book that has multiple parts. The learned LTM representation of the book is shown in Table 1, which has shown that the book has two parts and the blue-yellow feature in the first part can be deduced as the task-relevant feature dimension since the value *μs*/(1 + *σs*) of this feature is maximal. Detection results of the proposed perception system are shown in Fig. 8(d). It can be seen that the book is successfully detected. Results of Itti's model and Navalpakkam's model, as shown in Fig. 8(e) and Fig. 8(f) respectively, show that

The second task is to detect a human. Table 2 has shown that the human has two parts (including face and body) and the contour feature can be deduced as the task-relevant feature dimension since the value *μs*/(1 + *σs*) of this feature is maximal. Detection results of the proposed perception system are shown in Fig. 9(d). It can be seen that the human is successfully detected. Results of Itti's model and Navalpakkam's model, as shown in Fig. 9(e) and Fig. 9(f) respectively, show that these models fail to detect the target in most cases.

Performance of detecting task-relevant objects is evaluated using true positive rate (TPR) and

*TPR* = *TP*/*nP*, (11)

features.

**7.2.2 Task 1**

**7.2.3 Task 2**

**7.2.4 Performance evaluation**

**7.2.1 Experimental setup**

System for Robots Using Object-Based Visual Attention

*r*, *g* and *b* color channels respectively.

these models fail to detect the target in some cases.

false positive rate (FPR), which are calculated as:

(f)

Fig. 7. Detection of a salient object, which is conspicuous to its neighbors in terms of contour. Each column represents a type of experimental setting. Column 1 is a typical setting. Column 2 is a noise setting of column 1. Column 3 is a spatial transformation setting with respect to column 1. Row (a): Original input images. Row (b): Pre-attentive segmentation. Each color represents one proto-object. Row (c): Proto-object based attentional activation map. Row (d): The complete region being attended. Row (e): Detection results using Itti's model. The red rectangles highlight the attended location. Row (f): Detection results using Sun's model. The red circles highlight the attended object.

task-relevant feature(s) of the target is used to guide attentional selection through top-down biasing to obtain an attended object. The second stage is post-attentive recognition: The attended object is recognized using the target's LTM representation to check if it is the target. If not, another procedure of attentional selection is performed by using more task-relevant features.

#### **7.2.1 Experimental setup**

18 Will-be-set-by-IN-TECH

(a)

(b)

(c)

(d) ——————————————————————————————–

(e)

(f)

Fig. 7. Detection of a salient object, which is conspicuous to its neighbors in terms of contour.

Each column represents a type of experimental setting. Column 1 is a typical setting. Column 2 is a noise setting of column 1. Column 3 is a spatial transformation setting with respect to column 1. Row (a): Original input images. Row (b): Pre-attentive segmentation. Each color represents one proto-object. Row (c): Proto-object based attentional activation map. Row (d): The complete region being attended. Row (e): Detection results using Itti's model. The red rectangles highlight the attended location. Row (f): Detection results using

Sun's model. The red circles highlight the attended object.

Two objects are used to test the proposed method of detecting a task-relevant object: a book and a human. Images and videos are obtained under different settings, including noise, transformation, lighting changes and occlusion. For training for the book, 20 images are used. For testing for the book, 50 images are used. The size of each image is 640 × 480 pixels. For detecting the human, three videos are obtained by a moving robot. Two different office environments have been used. Video 1 and video 2 are obtained in office scene 1 with low and high lighting conditions respectively. Video 3 is obtained in office scene 2. All three videos contain a total of 650 image frames, in which 20 image frames are selected from video 1 and video 2 for training and the rest of the 630 image frames are used for testing. The size of each frame in these videos is 1024 × 768 pixels. It is important to note that each test image includes not only a target but also various distracters. The noisy images are manually obtained by adding salt and pepper noise patches (noise density: 0.1, patch size: 5 × 5 pixels) into original *r*, *g* and *b* color channels respectively.

The results of the proposed method are compared with the results of Itti's model (Itti et al., 1998) (i.e., a space-based bottom-up attention model) and Navalpakkam's model (Navalpakkam & Itti, 2005) (i.e., a space-based top-down attention model) respectively.

#### **7.2.2 Task 1**

The first task is to detect the book that has multiple parts. The learned LTM representation of the book is shown in Table 1, which has shown that the book has two parts and the blue-yellow feature in the first part can be deduced as the task-relevant feature dimension since the value *μs*/(1 + *σs*) of this feature is maximal. Detection results of the proposed perception system are shown in Fig. 8(d). It can be seen that the book is successfully detected. Results of Itti's model and Navalpakkam's model, as shown in Fig. 8(e) and Fig. 8(f) respectively, show that these models fail to detect the target in some cases.

#### **7.2.3 Task 2**

The second task is to detect a human. Table 2 has shown that the human has two parts (including face and body) and the contour feature can be deduced as the task-relevant feature dimension since the value *μs*/(1 + *σs*) of this feature is maximal. Detection results of the proposed perception system are shown in Fig. 9(d). It can be seen that the human is successfully detected. Results of Itti's model and Navalpakkam's model, as shown in Fig. 9(e) and Fig. 9(f) respectively, show that these models fail to detect the target in most cases.

#### **7.2.4 Performance evaluation**

Performance of detecting task-relevant objects is evaluated using true positive rate (TPR) and false positive rate (FPR), which are calculated as:

$$\text{TPR} = \text{TP}/n\text{P}\_i \tag{11}$$

(a)

<sup>45</sup> Development of an Autonomous Visual Perception

System for Robots Using Object-Based Visual Attention

(b)

(c)

(d) —————————————————————————————————————-

(e)

(f)

Fig. 8. Detection of the book. Each column represents a type of experimental setting. Column 1 is a typical setting. Column 2 is a noise setting of column 1. Column 3 is a spatial transformation (including translation and rotation) setting with respect to column 1. Column 4 is a different lighting setting with respect to column 1. Column 5 is an occlusion setting. Row (a): Original input images. Row (b): Pre-attentive segmentation. Each color represents one proto-object. Row (c): Proto-object based attentional activation map. Brightness represents the attentional activation value. Row (d): The complete region of the target. The red contour in the occlusion case represents the illusory contour (Lee & Nguyen, 2001), which shows the post-attentive perceptual completion effect. Row (e): Detection results using Itti's model. The red rectangle highlights the most salient location. Row (f): Detection results using Navalpakkam's model. The red rectangle highlights the most salient location.


Table 1. Learned LTM object representation of the book. *f* denotes a pre-attentive feature dimension. *j* denotes the index of a part. The definitions of *μa*, *σa*, *μ<sup>s</sup>* and *σ<sup>s</sup>* can be seen in section 5.2.2.


Table 2. Learned LTM object representation of the human. *f* denotes a pre-attentive feature dimension. *j* denotes the index of a part. The definitions of *μa*, *σa*, *μ<sup>s</sup>* and *σ<sup>s</sup>* can be seen in section 5.2.2.

$$FPR = FP/nN\_r \tag{12}$$

where *nP* and *nN* are numbers of positive and negative objects respectively in the testing image set, *TP* and *FP* are numbers of true positives and false positives. The positive object is the target to be detected and the negative objects are distracters in the scene.

Detection performance of the proposed perception system and other visual attention based methods is shown in Table 3. Note that "Naval's" represents Navalpakkam's method.

20 Will-be-set-by-IN-TECH

*f j μ<sup>a</sup> σ<sup>a</sup> μ<sup>s</sup> σ<sup>s</sup> μs*/(1 + *σs*) ct 1 - 75.0 19.7 3.6 int 1 106.6 5.8 27.9 14.5 1.8 rg 1 22.1 8.7 199.6 18.2 10.4 by 1 -108.0 9.1 215.6 8.7 **22.2** *o*0◦ 1 N/A N/A 41.8 9.8 3.9 *o*45◦ 1 N/A N/A 41.4 12.8 3.0 *o*90◦ 1 N/A N/A 34.7 16.3 2.0 *o*135◦ 1 N/A N/A 46.5 15.7 2.8 int 2 60.5 8.2 80.0 5.7 11.9 rg 2 0.4 4.3 18.3 6.4 2.5 by 2 120.8 6.7 194.7 8.1 21.4 *o*0◦ 2 N/A N/A 48.5 11.1 4.0 *o*45◦ 2 N/A N/A 53.8 9.9 4.9 *o*90◦ 2 N/A N/A 38.4 14.6 2.5 *o*135◦ 2 N/A N/A 59.4 20.3 2.8 Table 1. Learned LTM object representation of the book. *f* denotes a pre-attentive feature dimension. *j* denotes the index of a part. The definitions of *μa*, *σa*, *μ<sup>s</sup>* and *σ<sup>s</sup>* can be seen in

*f j μ<sup>a</sup> σ<sup>a</sup> μ<sup>s</sup> σ<sup>s</sup> μs*/(1 + *σs*) ct 1 - 68.3 6.9 **8.6** int 1 28.4 21.7 18.8 13.9 1.3 rg 1 -7.0 7.1 28.6 10.8 2.4 by 1 10.9 5.4 48.4 10.9 4.1 *o*0◦ 1 N/A N/A 33.4 6.7 4.3 *o*45◦ 1 N/A N/A 39.8 11.4 3.2 *o*90◦ 1 N/A N/A 37.4 6.1 5.3 *o*135◦ 1 N/A N/A 37.5 13.5 2.6 int 2 52.0 12.5 25.6 15.6 1.5 rg 2 -2.3 17.4 49.5 18.8 2.5 by 2 -29.3 6.9 60.4 22.3 2.6 *o*0◦ 2 N/A N/A 12.1 6.6 1.6 *o*45◦ 2 N/A N/A 16.5 8.3 1.8 *o*90◦ 2 N/A N/A 15.0 7.9 1.7 *o*135◦ 2 N/A N/A 17.2 8.1 1.9 Table 2. Learned LTM object representation of the human. *f* denotes a pre-attentive feature dimension. *j* denotes the index of a part. The definitions of *μa*, *σa*, *μ<sup>s</sup>* and *σ<sup>s</sup>* can be seen in

where *nP* and *nN* are numbers of positive and negative objects respectively in the testing image set, *TP* and *FP* are numbers of true positives and false positives. The positive object is

Detection performance of the proposed perception system and other visual attention based

methods is shown in Table 3. Note that "Naval's" represents Navalpakkam's method.

the target to be detected and the negative objects are distracters in the scene.

*FPR* = *FP*/*nN*, (12)

section 5.2.2.

section 5.2.2.

Fig. 8. Detection of the book. Each column represents a type of experimental setting. Column 1 is a typical setting. Column 2 is a noise setting of column 1. Column 3 is a spatial transformation (including translation and rotation) setting with respect to column 1. Column 4 is a different lighting setting with respect to column 1. Column 5 is an occlusion setting. Row (a): Original input images. Row (b): Pre-attentive segmentation. Each color represents one proto-object. Row (c): Proto-object based attentional activation map. Brightness represents the attentional activation value. Row (d): The complete region of the target. The red contour in the occlusion case represents the illusory contour (Lee & Nguyen, 2001), which shows the post-attentive perceptual completion effect. Row (e): Detection results using Itti's model. The red rectangle highlights the most salient location. Row (f): Detection results using Navalpakkam's model. The red rectangle highlights the most salient location.

Task Method *TP FP nP nN TPR* (%) *FPR* (%)

<sup>47</sup> Development of an Autonomous Visual Perception

Proposed 47 3 50 244 94.00 1.23 Itti's 16 34 50 244 32.00 13.93 Naval's 41 9 50 244 82.00 3.69

Proposed 581 49 630 30949 92.22 0.16 Itti's 5 625 630 30949 0.79 2.02 Naval's 36 594 630 30949 5.71 1.92

This paper has presented an autonomous visual perception system for robots using the object-based visual attention mechanism. This perception system provides the following four contributions. The first contribution is that the attentional selection stage supplies robots with the cognitive capability of knowing how to perceive the environment according to the current task and situation, such that this perception system is adaptive and general to any task and environment. The second contribution is the top-down attention method using the IC hypothesis. Since the task-relevant feature(s) are conspicuous, low-level and statistical, this top-down biasing method is more effective, efficient and robust than other methods. The third contribution is the PNN based LTM object representation. This LTM object representation can probabilistically embody various instances of that object, such that it is robust and discriminative for top-down attention and object recognition. The fourth contribution is the pre-attentive segmentation algorithm. This algorithm extends the irregular pyramid techniques by integrating a scale-invariant probabilistic similarity measure, a similarity-driven decimation method and a similarity-driven neighbor search method. It provides rapid and satisfactory results of pre-attentive segmentation for object-based visual attention. Based on these contributions, this perception system has been successfully tested in

The future work includes the integration of the bottom-up attention in the temporal context

Aziz, M. Z., Mertsching, B., Shafik, M. S. E.-N. & Stemmer, R. (2006). Evaluation of visual

Backer, G., Mertsching, B. & Bollmann, M. (2001). Data- and model-driven gaze control for

Baluja, S. & Pomerleau, D. (1997). Dynamic relevance: Vision-based focus of attention using

Belardinelli, A. & Pirri, F. (2006). A biologically plausible robot attention model, based on

Belardinelli, A., Pirri, F. & Carbone, A. (2006). Robot task-driven attention, *Proceedings of the International Symposium on Practical Cognitive Agents and Robots*, pp. 117–128.

attention models for robots, *Proceedings of the 4th IEEE Conference on Computer Vision*

an active-vision system, *IEEE Transactions on Pattern Analysis and Machine Intelligence*

the robotic task of object detection under different experimental settings.

and experiments of the combination of bottom-up and top-down attention.

artificial neural networks, *Artificial Intelligence* 97: 381–395.

space and time, *Cognitive Processing* 7(Supplement 5): 11–14.

1

System for Robots Using Object-Based Visual Attention

2

**8. Conclusion**

**9. References**

*Systems*, p. 20.

23(12): 1415–1429.

Table 3. Performance of detecting task-relevant objects.

Fig. 9. Detection of the human in the cluttered environment. Each column represents a type of experimental setting. Column 1 is a typical setting (from video 1). Column 2 is a noise setting of column 1. Column 3 is a scaling setting with respect to column 1 (from video 1). Column 4 is a rotation setting with respect to column 1 (from video 3). Column 5 is a different lighting setting with respect to column 1 (from video 2). Column 6 is an occlusion setting (from video 3). Row (a): Original input images. Row (b): Pre-attentive segmentation. Each color represents one proto-object. Row (c): Proto-object based attentional activation map. Brightness represents the attentional activation value. Row (d): The complete region of the target. The red contour in the occlusion case represents the illusory contour (Lee & Nguyen, 2001), which shows the post-attentive perceptual completion effect. Row (e): Detection results using Itti's model. The red rectangle highlights the most salient location. Row (f): Detection results using Navalpakkam's model. The red rectangle highlights the most salient location.


Table 3. Performance of detecting task-relevant objects.

#### **8. Conclusion**

22 Will-be-set-by-IN-TECH

(a)

(b)

(c)

(d) —————————————————————————————————————-

(e)

(f)

Fig. 9. Detection of the human in the cluttered environment. Each column represents a type of experimental setting. Column 1 is a typical setting (from video 1). Column 2 is a noise setting of column 1. Column 3 is a scaling setting with respect to column 1 (from video 1). Column 4 is a rotation setting with respect to column 1 (from video 3). Column 5 is a different lighting setting with respect to column 1 (from video 2). Column 6 is an occlusion setting (from video 3). Row (a): Original input images. Row (b): Pre-attentive segmentation. Each color represents one proto-object. Row (c): Proto-object based attentional activation map. Brightness represents the attentional activation value. Row (d): The complete region of the target. The red contour in the occlusion case represents the illusory contour (Lee & Nguyen, 2001), which shows the post-attentive perceptual completion effect. Row (e): Detection results using Itti's model. The red rectangle highlights the most salient location. Row (f): Detection results using Navalpakkam's model. The red rectangle highlights the

most salient location.

This paper has presented an autonomous visual perception system for robots using the object-based visual attention mechanism. This perception system provides the following four contributions. The first contribution is that the attentional selection stage supplies robots with the cognitive capability of knowing how to perceive the environment according to the current task and situation, such that this perception system is adaptive and general to any task and environment. The second contribution is the top-down attention method using the IC hypothesis. Since the task-relevant feature(s) are conspicuous, low-level and statistical, this top-down biasing method is more effective, efficient and robust than other methods. The third contribution is the PNN based LTM object representation. This LTM object representation can probabilistically embody various instances of that object, such that it is robust and discriminative for top-down attention and object recognition. The fourth contribution is the pre-attentive segmentation algorithm. This algorithm extends the irregular pyramid techniques by integrating a scale-invariant probabilistic similarity measure, a similarity-driven decimation method and a similarity-driven neighbor search method. It provides rapid and satisfactory results of pre-attentive segmentation for object-based visual attention. Based on these contributions, this perception system has been successfully tested in the robotic task of object detection under different experimental settings.

The future work includes the integration of the bottom-up attention in the temporal context and experiments of the combination of bottom-up and top-down attention.

#### **9. References**


Itti, L., Koch, C. & Niebur, E. (1998). A model of saliency-based visual attention for

<sup>49</sup> Development of an Autonomous Visual Perception

Jolion, J. M. (2003). Stochastic pyramid revisited, *Pattern Recognition Letters* 24(8): 1035–1042. Lee, T. S. & Nguyen, M. (2001). Dynamics of subjective contour formation in the early visual

MacCormick, J. (2000). *Probabilistic modelling and stochastic algorithms for visual localisation and tracking*, PhD thesis, Department of Engineering Science, University of Oxford. Maier, W. & Steinbach, E. (2010). A probabilistic appearance representation and its application

Meer, P. (1989). Stochastic image pyramids, *Computer Vision, Graphics, and Image Processing*

Montanvert, A., Meer, P. & Rosenfeld, A. (1991). Hierarchical image analysis using

Navalpakkam, V. & Itti, L. (2005). Modeling the influence of task on attention, *Vision Research*

Orabona, F., Metta, G. & Sandini, G. (2005). Object-based visual attention: a model for a

Posner, M. I., Snyder, C. R. R. & Davidson, B. J. (1980). Attention and the detection of signals,

Rao, R. P. N. & Ballard, D. H. (1995a). An active vision architecture based on iconic

Rao, R. P. N. & Ballard, D. H. (1995b). Object indexing using an iconic sparse distributed

Rao, R. P. N., Zelinsky, G. J., Hayhoe, M. M. & Ballard, D. H. (2002). Eye movements in iconic

Sharon, E., Brandt, A. & Basri, R. (2000). Fast multiscale image segmentation, *Proceedings of IEEE Conference on Computer Vision and Pattern Recognition (CVPR)*, pp. 70–77. Sharon, E., Galun, M., Sharon, D., Basri, R. & Brandt, A. (2006). Hierarchy and adaptivity in

Shi, J. & Malik, J. (2000). Normalized cuts and image segmentation, *IEEE Transactions on*

Sun, Y. (2008). A computer vision model for visual-object-based attention and eye movements,

Sun, Y. & Fisher, R. (2003). Object-based visual attention for computer vision, *Artificial*

Tipper, S. P., Howard, L. A. & Houghton, G. (1998). Action-based mechanisms of attention,

Treisman, A. M. & Gelade, G. (1980). A feature integration theory of attention, *Cognition*

*Journal of Experimental Psychology: General* 14(2): 160–174.

memory, *Proc. the 5th Intl. Conf. Computer Vision*, pp. 24–31.

Scholl, B. J. (2001). Objects and attention: the state of the art, *Cognition* 80(1-2): 1–46.

representations, *Artificial Intelligence* 78: 461–505.

visual search, *Vision Research* 42: 1447–1463.

segmenting visual scenes, *Nature* 442: 810–813.

*Intelligence* 146(1): 77–123.

*Psychology* 12(1–2): 507–545.

*Pattern Analysis and Machine Intelligence* 22(8): 888–905.

*Computer Vision and Image Understanding* 112(2): 126–142.

Specht, D. F. (1990). Probabilistic neural networks, *Neural Networks* 3(1): 109–118.

*Philosophical Transactions: Biological Sciences* 353(1373): 1385–1393.

20(11): 1254–1259.

System for Robots Using Object-Based Visual Attention

98(4): 1907–1911.

45(3): 269–294.

13(4): 307–316.

45(2): 205–231.

*Development* 2(4): 267–281.

*and Patter Recognition (CVPR)*, p. 89.

rapid scene analysis, *IEEE Transactions on Pattern Analysis and Machine Intelligence*

cortex, *Proceedings of the Natual Academy of Sciences of the United States of America*

to surprise detection in cognitive robots, *IEEE Transactions on Autonomous Mental*

irregular tessellations, *IEEE Transactions on Pattern Analysis and Machine Intelligence*

behaving robot, *Proceedings of the IEEE Computer Society Conference on Computer Vision*


24 Will-be-set-by-IN-TECH

Bhattacharyya, A. (1943). On a measure of divergence between two statistical populations

Blake, A. & Isard, M. (1998). *Active Contours: The Application of Techniques from Graphics, Vision,*

Breazeal, C., Edsinger, A., Fitzpatrick, P. & Scassellati, B. (2001). Active vision for sociable

Burt, P. J. & Adelson, E. H. (1983). The laplacian pyramid as a compact image code, *IEEE*

Carbone, A., Finzi, A. & Orlandini, A. (2008). Model-based control architecture for attentive

Carpenter, G. A. & Grossberg, S. (2003). Adaptive resonance theory, *in* M. A. Arbib (ed.), *The*

Chikkerur, S., Serre, T., Tan, C. & Poggio, T. (2010). What and where: A bayesian inference

Desimone, R. & Duncan, J. (1995). Neural mechanisms of selective visual attention, *Annual*

Duncan, J. (1984). Selective attention and the organization of visual information, *Journal of*

Duncan, J. (1998). Converging levels of analysis in the cognitive neuroscience of visual

Duncan, J., Humphreys, G. & Ward, R. (1997). Competitive brain activity in visual attention,

Frintrop, S. (2005). *VOCUS: A visual attention system for object detection and goal-directed search*,

Frintrop, S. & Jensfelt, P. (2008). Attentional landmarks and active gaze control for visual slam,

Frintrop, S. & Kessel, M. (2009). Most salient region tracking, *Proceedings of the IEEE International Conference on Robotics and Automation (ICRA)*, pp. 1869–1874. Greenspan, A. G., Belongie, S., Goodman, R., Perona, P., Rakshit, S. & Anderson, C. H. (1994).

Grossberg, S. (2005). Linking attention to learning, expectation, competition, and

Hoya, T. (2004). Notions of intuition and attention modeled by a hierarchically arranged

Itti, L. & Baldi, P. (2009). Bayesian surprise attracts human attention, *Visual Research*

Grossberg, S. (2007). Consciousness clears the mind, *Neural Networks* 20: 1040–1053.

Overcomplete steerable pyramid filters and rotation invariance, *Proceedings of IEEE*

consciousness, *in* L. Itti, G. Rees & J. Tsotsos (eds), *Neurobiology of Attention*, Elsevier,

generalized regression neural network, *IEEE Transactions on Systems, Man, and*

attention, *Philosophical Transactions of The Royal Society Lond B: Biological Sciences*

35: 99–109.

York, Secaucus, NJ.

MA, pp. 87–90.

353(1373): 1307–1317.

San Diego, CA, pp. 652–662.

49(10): 1295–1306.

*Humans* 31(5): 443–453.

*Transactions on Communications* 31(4): 532–540.

robots in rescue scenarios, *Autonomous Robots* 24(1): 87–120.

theory of attention, *Visual Research* 50(22): 2233–2247.

*Experimental Psychology: General* 113(4): 501–517.

*Current Opinion in Neurobiology* 7(2): 255–261.

*IEEE Transactions on Robotics* 24(5): 1054–1065.

*Cybernetics - Part B: Cybernetics* 34(1): 200–209.

*Computer Vision and Pattern Recognition*, pp. 222–228.

PhD thesis, University of Bonn, Germany.

*Reviews of Neuroscience* 18: 193–222.

defined by their probability distributions, *Bulletin of the Calcutta Mathematical Society*

*Control Theory and Statistics to Visual Tracking of Shapes in Motion*, Springer-Verlag New

robots, *IEEE Transactions on Systems, Man, and Cybernetics - Part A: Systems and*

*Handbook of Brain Theory and Neural Networks, Second Edition*, MIT Press, Cambridge,


**3** 

*Shizuoka University* 

*Japan* 

**Three-Dimensional Environment Modeling** 

Ryosuke Kawanishi, Atsushi Yamashita and Toru Kaneko

**Based on Structure from Motion with Point and** 

**Line Features by Using Omnidirectional Camera** 

Three-dimensional map is available for autonomous robot navigation (path planning, selflocalization and object recognition). In unknown environment, robots should measure

Three-dimensional measurement using image data makes it possible to construct an environment map (Davison, 2003). However, many environmental images are needed if we use a conventional camera having a limited field of view (Ishiguro et al., 1992). Then, an omnidirectional camera is available for wide-ranging measurement, because it has a panoramic field of view (Fig. 1). Many researchers showed that an omnidirectional camera is effective in measurement and recognition in environment (Bunschoten & Krose, 2003;

Fig. 1. Omnidirectional camera equipped with a hyperboloid mirror. The left figure shows

Our proposed method is based on structure from motion. Previous methods based on structure from motion often use feature points to estimate camera movement and measure environment (Rachmielowski et al., 2006; Kawanishi et al., 2009). However, many nontextured objects may exist in surrounding environments of mobile robots. It is hard to extract enough number of feature points from non-textured objects. Therefore, in an environment having non-textured objects, it is difficult to construct its map by using feature

environments and construct their maps by themselves.

Geyer & Daniilidis, 2003; Gluckman & Nayar, 1998).

Camera

Hyperboloid mirror

**1. Introduction** 

an acquired image.

points only.


## **Three-Dimensional Environment Modeling Based on Structure from Motion with Point and Line Features by Using Omnidirectional Camera**

Ryosuke Kawanishi, Atsushi Yamashita and Toru Kaneko *Shizuoka University Japan* 

#### **1. Introduction**

26 Will-be-set-by-IN-TECH

50 Recent Advances in Mobile Robotics

Tsotsos, J. K., Culhane, S. M., Wai, W. Y. K., Lai, Y., Davis, N. & Nuflo, F. (1995). Modelling visual attention via selective tuning, *Artificial Intelligence* 78: 282–299. Walther, D., Rutishauser, U., Koch, C. & Perona, P. (2004). On the usefulness of attention for

Weng, J., McClelland, J., Pentland, A., Sporns, O., Stockman, I. & Thelen, E. (2001).

Wolfe, J. M. (1994). Guided search 2.0: A revised model of visual search, *Psychonomic Bulletin*

Yu, Y., Mann, G. K. I. & Gosine, R. G. (2010). An object-based visual attention model for robotic

*ECCV*, pp. 96–103.

40(5): 1398–1412.

*and Review* 1(2): 202–238.

object recognition, *Workshop on Attention and Performance in Computational Vision at*

Autonomous mental development by robots and animals, *Science* 291(5504): 599–600.

applications, *IEEE Transactions on Systems, Man and Cybernetics, Part B: Cybernetics*

Three-dimensional map is available for autonomous robot navigation (path planning, selflocalization and object recognition). In unknown environment, robots should measure environments and construct their maps by themselves.

Three-dimensional measurement using image data makes it possible to construct an environment map (Davison, 2003). However, many environmental images are needed if we use a conventional camera having a limited field of view (Ishiguro et al., 1992). Then, an omnidirectional camera is available for wide-ranging measurement, because it has a panoramic field of view (Fig. 1). Many researchers showed that an omnidirectional camera is effective in measurement and recognition in environment (Bunschoten & Krose, 2003; Geyer & Daniilidis, 2003; Gluckman & Nayar, 1998).

Fig. 1. Omnidirectional camera equipped with a hyperboloid mirror. The left figure shows an acquired image.

Our proposed method is based on structure from motion. Previous methods based on structure from motion often use feature points to estimate camera movement and measure environment (Rachmielowski et al., 2006; Kawanishi et al., 2009). However, many nontextured objects may exist in surrounding environments of mobile robots. It is hard to extract enough number of feature points from non-textured objects. Therefore, in an environment having non-textured objects, it is difficult to construct its map by using feature points only.

Three-Dimensional Environment Modeling Based on Structure from

proposed method rejects such results.

following equations.

size, *f* is the focal length,

2 22

*α yx*

calibrated in advance.

three-dimensional environment model is constructed.

**2. Coordinate system of omnidirectional camera** 

Motion with Point and Line Features by Using Omnidirectional Camera 53

The proposed line-based measurement is divided into two phases. At the first phase, camera rotation and line directions are optimized. Line correspondence makes it possible to estimate camera rotation independently of camera translation (Spacek, 1986). Camera rotation can be estimated by using 3-D line directions. At the second phase, camera translation and 3-D line location are optimized. The optimization is based on Bundle adjustment (Triggs et al., 1999). Some of measurement results have low accuracy. The

Measurement results of feature points and straight-lines are integrated. Triangular meshes are generated from the integrated measurement data. By texture-mapping to these meshes, a

The coordinate system of the omnidirectional camera is shown in Fig. 3. A ray heading to image coordinates *u v*, from the camera lens is reflected on a hyperboloid mirror. In this paper, the reflected vector is called a ray vector. The extension lines of all ray vectors intersect at the focal point of the hyperboloid mirror. The ray vector **r** is calculated by the

> 2

 

 , and 

1 <sup>2</sup> 2

 *β z*

Hyperboloid mirror

<sup>22</sup> *βαγ*

*ucp vcp f*

*x x y y*

**r** (1)

are hyperboloid parameters. These parameters are

(2)

 

2 22 2 22 2 2 2 *f uv f f uv*

 

where *<sup>x</sup> c* and *<sup>y</sup> c* are the center coordinates of the omnidirectional image, *<sup>x</sup> p* and *<sup>y</sup> p* are pixel

*Z*

**r**

*<sup>U</sup> <sup>f</sup>*

Fig. 3. The coordinate system of the omnidirectional camera. Ray vector **r** is defined as a

Lens

unit vector which starts from the focal point of a hyperboloid mirror.

*<sup>γ</sup> <sup>X</sup>*

Image plane

*Y*

(*u*, *v*)

*V*

Then, line features should be utilized for environment measurement, because non-texture objects often have straight-lines. As examples of previous works using lines, a method for precise camera movement estimation by using stereo camera (Chandraker et al., 2009), a method for buildings reconstruction by using orthogonal lines (Schindler, 2006) and so on (Bartoli & Sturm, 2005; Smith et al., 2006; Mariottini & Prattichizzo, 2007) have been proposed.

However, there is a prerequisite on previous line detections of them. A method must obtain a vanishing point (Schindler, 2006) or a pair of end points of the straight-line (Smith et al., 2006). Some of previous line detection is only for a normal camera (Chandraker et al., 2009). Alternatively, some previous methods obtain line correspondences by hand (Bartoli & Sturm, 2005; Mariottini & Prattichizzo, 2007).

We propose a method for straight-line extraction and tracking on distorted omnidirectional images. The method does not require a vanishing point and end points of straight-lines. These straight-lines are regarded as infinite lines in the measurement process (Spacek, 1986). Therefore, the proposed method can measure straight-lines even if a part of the line is covered by obstacles during its tracking.

Our proposed method measures feature points together with straight-lines. If only straightlines are used for camera movement estimation, a non-linear problem must be solved. However, camera movement can be estimated easily by a linear solution with point correspondences. Moreover, although few numbers of straight-lines may be extracted from textured objects, many feature points will be extracted from them. Therefore, we can measure the environment densely by using both feature points and straight-lines.

The process of our proposed method is mentioned below (Fig. 2). First, feature points and straight-lines are extracted and tracked along an acquired omnidirectional image sequence. Camera movement is estimated by point-based Structure from Motion. The estimated camera movement is used for an initial value for line-based measurement.

Fig. 2. Procedure of our proposed method.

52 Recent Advances in Mobile Robotics

Then, line features should be utilized for environment measurement, because non-texture objects often have straight-lines. As examples of previous works using lines, a method for precise camera movement estimation by using stereo camera (Chandraker et al., 2009), a method for buildings reconstruction by using orthogonal lines (Schindler, 2006) and so on (Bartoli & Sturm, 2005; Smith et al., 2006; Mariottini & Prattichizzo, 2007) have been proposed. However, there is a prerequisite on previous line detections of them. A method must obtain a vanishing point (Schindler, 2006) or a pair of end points of the straight-line (Smith et al., 2006). Some of previous line detection is only for a normal camera (Chandraker et al., 2009). Alternatively, some previous methods obtain line correspondences by hand (Bartoli &

We propose a method for straight-line extraction and tracking on distorted omnidirectional images. The method does not require a vanishing point and end points of straight-lines. These straight-lines are regarded as infinite lines in the measurement process (Spacek, 1986). Therefore, the proposed method can measure straight-lines even if a part of the line is

Our proposed method measures feature points together with straight-lines. If only straightlines are used for camera movement estimation, a non-linear problem must be solved. However, camera movement can be estimated easily by a linear solution with point correspondences. Moreover, although few numbers of straight-lines may be extracted from textured objects, many feature points will be extracted from them. Therefore, we can

The process of our proposed method is mentioned below (Fig. 2). First, feature points and straight-lines are extracted and tracked along an acquired omnidirectional image sequence. Camera movement is estimated by point-based Structure from Motion. The estimated

**Image sequence acquisition**

**Feature point tracking**

**Straight-line tracking**

**Point-based measurement**

**Line-based measurement Camera rotation & line direction** 

**Camera translation & line location** 

**Result qualification**

**Modeling**

measure the environment densely by using both feature points and straight-lines.

camera movement is used for an initial value for line-based measurement.

Sturm, 2005; Mariottini & Prattichizzo, 2007).

covered by obstacles during its tracking.

Fig. 2. Procedure of our proposed method.

The proposed line-based measurement is divided into two phases. At the first phase, camera rotation and line directions are optimized. Line correspondence makes it possible to estimate camera rotation independently of camera translation (Spacek, 1986). Camera rotation can be estimated by using 3-D line directions. At the second phase, camera translation and 3-D line location are optimized. The optimization is based on Bundle adjustment (Triggs et al., 1999). Some of measurement results have low accuracy. The proposed method rejects such results.

Measurement results of feature points and straight-lines are integrated. Triangular meshes are generated from the integrated measurement data. By texture-mapping to these meshes, a three-dimensional environment model is constructed.

#### **2. Coordinate system of omnidirectional camera**

The coordinate system of the omnidirectional camera is shown in Fig. 3. A ray heading to image coordinates *u v*, from the camera lens is reflected on a hyperboloid mirror. In this paper, the reflected vector is called a ray vector. The extension lines of all ray vectors intersect at the focal point of the hyperboloid mirror. The ray vector **r** is calculated by the following equations.

$$\mathbf{r} = \begin{bmatrix} \mathcal{A} \begin{pmatrix} \mu - c\_x \end{pmatrix} p\_x \\ \mathcal{A} \begin{pmatrix} \upsilon - c\_y \end{pmatrix} p\_y \\ \mathcal{A}f - 2\gamma \end{pmatrix} \tag{1}$$
 
$$\alpha^2 \left( f\_{f\_{X^2}} + \rho \sqrt{\mu^2 + \upsilon^2 + \epsilon^2} \right)$$

$$\mathcal{A} = \frac{\alpha^2 \left( f\gamma + \beta \sqrt{u^2 + v^2 + f^2} \right)}{\alpha^2 f^2 - \beta^2 \left( u^2 + v^2 \right)} \tag{2}$$

where *<sup>x</sup> c* and *<sup>y</sup> c* are the center coordinates of the omnidirectional image, *<sup>x</sup> p* and *<sup>y</sup> p* are pixel size, *f* is the focal length, , and are hyperboloid parameters. These parameters are calibrated in advance.

Fig. 3. The coordinate system of the omnidirectional camera. Ray vector **r** is defined as a unit vector which starts from the focal point of a hyperboloid mirror.

Three-Dimensional Environment Modeling Based on Structure from

segment looks like a curved line in a distorted omnidirectional image.

Fig. 6. The relationship between a straight-line and a ray vector.

Lens

th

*l*

ray vector and a straight-line is within 1 pixel.

number of corresponding edge points is selected.

RANSAC (Fischler & Bolles, 1981).

Focal point of hyperboloid mirror

Omnidirectional image

separated edge segments.

Motion with Point and Line Features by Using Omnidirectional Camera 55

where *xI* and *yI* are derivatives of image *I* . An edge point which has large value of the ratio of eigenvalues is regarded as a point locating on a line. In the proposed method, if the ratio is smaller than 10, the edge point is rejected as a corner point. This process provides us

A least square plane is calculated from ray vectors of edge points which belong to an edge segment. If the edge segment constitutes a straight-line, these ray vectors are located on a plane (Fig. 6). Therefore, an edge segment which has a small least square error is regarded as a straight-line. The proposed method is able to extract straight-lines, even if an edge

The maximum number of edge points which satisfy equation (4) is calculated by using

Ray vectors

Edge point

where th*l* is a threshold. *i*, *<sup>j</sup>* **r** is a ray vector heading to an edge point *j* included in an edge segment *i* . **n***i* is the normal vector of the least square plane calculated from the edge segment *i* . If over half the edge points of the edge segment *i* satisfy equation (4), the edge segment is

2 2

 

where *r*m is the radius of projected mirror circumference in an omnidirectional image. A threshold th*l* allows angle error within 1 /*r*<sup>m</sup> [rad]. It means that an angle error between a

Straight-lines are tracked along an omnidirectional image sequence. The proposed method extracts points at constant intervals on a straight-line detected in the current frame (Fig. 7 (a) and (b)). These points are tracked to the next frame by KLT tracker (Fig. 7 (d)). Edge segments are detected in the next frame (Fig. 7 (c)). The edge point closest to the tracked point is selected as a corresponding edge point (Fig. 7 (e)). The edge segment which has themaximum number of corresponding edge points is regarded as a corresponding edge segment (Fig. 7 (f)). If an edge segment corresponds to several lines, a line which has larger

cos cos 2

m m 2 1

*r r*

*ij i* , th **r n** *l* (4)

3-D line

Plane

(5)

<sup>2</sup> <sup>T</sup>

determined as a straight-line. The threshold th*l* is calculated by the following equation.

### **3. Feature tracking**

#### **3.1 Point tracking**

Feature points are tracked along an omnidirectional image sequence by KLT tracker (Shi & Tomasi, 1994). These points are used for initial estimation of camera movement and measurement for textured objects. An example of feature point tracking is shown in Fig. 4.

(a) 0 frame. (b) 10 frame. (c) 20 frame.

Fig. 4. An example of feature point tracking. Points which have the same color show corresponding points.

#### **3.2 Straight-line tracking**

Straight-lines are extracted from a distorted omnidirectional image. The proposed method obtains edge points by Canny edge detector (Canny, 1986). An example of edge point detection is shown in Fig. 5 (a) and (b).

Fig. 5. Edge segment extraction. (a) Input image. (b) Detected canny edge points. (c) Edge segments are separated by rejecting corner points.

To separate each straight-line, corner points are rejected as shown in Fig. 5 (c). Corner points are detected by using two eigenvalues of the Hessian of the image. Hessian matrix is calculated by the following equation.

$$\mathbf{H} = \begin{bmatrix} I\_x \\ \end{bmatrix}\_x^2 \begin{bmatrix} I\_x I\_y \\ \end{bmatrix} \tag{3}$$
 
$$\begin{bmatrix} I\_x I\_y & I\_y \end{bmatrix} \tag{4}$$

54 Recent Advances in Mobile Robotics

Feature points are tracked along an omnidirectional image sequence by KLT tracker (Shi & Tomasi, 1994). These points are used for initial estimation of camera movement and measurement for textured objects. An example of feature point tracking is shown in Fig. 4.

 (a) 0 frame. (b) 10 frame. (c) 20 frame. Fig. 4. An example of feature point tracking. Points which have the same color show

(a) (b) (c)

Fig. 5. Edge segment extraction. (a) Input image. (b) Detected canny edge points. (c) Edge

To separate each straight-line, corner points are rejected as shown in Fig. 5 (c). Corner points are detected by using two eigenvalues of the Hessian of the image. Hessian matrix is

2

*I II II I* 

2 *x xy xy y*

**H** (3)

Straight-lines are extracted from a distorted omnidirectional image. The proposed method obtains edge points by Canny edge detector (Canny, 1986). An example of edge point

**3. Feature tracking 3.1 Point tracking** 

corresponding points.

**3.2 Straight-line tracking** 

detection is shown in Fig. 5 (a) and (b).

segments are separated by rejecting corner points.

calculated by the following equation.

where *xI* and *yI* are derivatives of image *I* . An edge point which has large value of the ratio of eigenvalues is regarded as a point locating on a line. In the proposed method, if the ratio is smaller than 10, the edge point is rejected as a corner point. This process provides us separated edge segments.

A least square plane is calculated from ray vectors of edge points which belong to an edge segment. If the edge segment constitutes a straight-line, these ray vectors are located on a plane (Fig. 6). Therefore, an edge segment which has a small least square error is regarded as a straight-line. The proposed method is able to extract straight-lines, even if an edge segment looks like a curved line in a distorted omnidirectional image.

Fig. 6. The relationship between a straight-line and a ray vector.

The maximum number of edge points which satisfy equation (4) is calculated by using RANSAC (Fischler & Bolles, 1981).

$$\left(\mathbf{r}\_{i,j}^{\top}\mathbf{n}\_i\right)^2 < l\_{\text{th}}\tag{4}$$

where th*l* is a threshold. *i*, *<sup>j</sup>* **r** is a ray vector heading to an edge point *j* included in an edge segment *i* . **n***i* is the normal vector of the least square plane calculated from the edge segment *i* . If over half the edge points of the edge segment *i* satisfy equation (4), the edge segment is determined as a straight-line. The threshold th*l* is calculated by the following equation.

$$I\_{\rm th} = \cos^2\left(\frac{2\pi}{2r\_{\rm m}\pi}\right) = \cos^2\left(\frac{1}{r\_{\rm m}}\right) \tag{5}$$

where *r*m is the radius of projected mirror circumference in an omnidirectional image. A threshold th*l* allows angle error within 1 /*r*<sup>m</sup> [rad]. It means that an angle error between a ray vector and a straight-line is within 1 pixel.

Straight-lines are tracked along an omnidirectional image sequence. The proposed method extracts points at constant intervals on a straight-line detected in the current frame (Fig. 7 (a) and (b)). These points are tracked to the next frame by KLT tracker (Fig. 7 (d)). Edge segments are detected in the next frame (Fig. 7 (c)). The edge point closest to the tracked point is selected as a corresponding edge point (Fig. 7 (e)). The edge segment which has themaximum number of corresponding edge points is regarded as a corresponding edge segment (Fig. 7 (f)). If an edge segment corresponds to several lines, a line which has larger number of corresponding edge points is selected.

Three-Dimensional Environment Modeling Based on Structure from

where

where <sup>T</sup>

**4.2 Line-based measurement** 

essential matrix **E** and ray vectors satisfy the following equation.

outliers by using RANSAC algorithm (Fischler & Bolles, 1981).

are calculated from line correspondences and initial camera movements.

**4.2.1 Camera rotation and 3-D line direction optimization** 

Motion with Point and Line Features by Using Omnidirectional Camera 57

An essential matrix **E** is calculated from ray vectors of corresponding feature points. An

where ray vectors <sup>T</sup> , , *i i ii* **<sup>r</sup>** *<sup>x</sup> <sup>y</sup> <sup>z</sup>* and <sup>T</sup> ' ', ', ' *i iii* **<sup>r</sup>** *xyz* are those of the corresponding point in two images. Camera rotation matrix **R** and translation vector **t** are calculated from essential matrix **E** by singular value decomposition. Equation (6) is transformed as follows.

<sup>T</sup> ', ', ', ', ', ', ', ', ' *ii ii ii i i i i i i ii ii ii* **<sup>u</sup>** *xx yx zx xy yy zy xz yz zz* ,

 <sup>T</sup> 11 12 13 21 22 23 31 32 33 **e** *eeeeeeeee* ,,,,,,,, .

*ab e* is the row *a* and column *b* element of Essential matrix **E** . The matrix **E** is obtained by solving simultaneous equations for more than eight pairs of corresponding ray vectors.

eigenvector of the smallest eigenvalues of <sup>T</sup> **U U** . Estimated camera movement in this process is used as an initial value for line-based measurement. However, not all feature points tracked in the image sequence correspond satisfactorily due to image noise, etc. Mistracked feature points should be rejected. The proposed method rejects these points as

Estimated camera movement is optimized by using straight-lines. A straight-line is represented as infinite lines by using its direction vector *<sup>w</sup>* **d** and location vector *<sup>w</sup>*

( *w w* **l d** *k* , *k* is a factor). The superscript *w* means that the vector is in world coordinate system. As a prerequisite for line-based measurement, at least, more than 3 images and 3 pairs of corresponding lines (at least one line is not parallel to others) are needed. In the first step, camera rotation and line directions are estimated. The step is independent of camera translation and line locations estimation. In the next step, camera translation and line locations are optimized by a method based on Bundle adjustment (Triggs et al., 1999). In these phases, initial value of 3-D line direction and location are required. These initial values

Our proposed method calculates a normal vector *<sup>c</sup>* **n***i* of a least square plane calculated from an edge segment *i* in Section 3.2. The superscript *c* means that the vector is in a camera coordinate system at camera position *c* . Camera rotation depends on 3-D line direction vector *<sup>w</sup>* **d***i* and normal vector *<sup>c</sup>* **n***<sup>i</sup>* . By using initial values of camera rotation and normal

1 2 , ,, **U uu u** *<sup>n</sup>* . *n* is the number of feature points. **e** is calculated as the

' 0 **r Er** *i i* (6)

<sup>T</sup> **u e** 0 (7)

<sup>2</sup> *J* **Ue** min (8)

**l**

Fig. 7. Searching for a corresponding edge segment in the next frame. (a) Straight-line extracted in the current frame. (b) Points extracted at constant intervals on the line. (b) Edge segments in the next frame. (c) Points (b) are tracked between the current frame and the next frame. (d) Corresponding edge points. (e) Corresponding edge segment.

(a) 0 frame. (b) 10 frame. (c) 20 frame.

Fig. 8. An example of straight-line tracking. Lines which have a same color show corresponding lines. Although an end point of a line is shown as a white square, it is not used for straight-line detection.

Matching point search on a line has the aperture problem (Nakayama, 1988). However, it is not difficult for the proposed method to obtain corresponding lines, because it does not require point-to-point matching. By continuing the above processes, straight-lines are tracked along the omnidirectional image sequence. An example of line tracking is shown in Fig. 8.

#### **4. Environment measurement**

#### **4.1 Point-based measurement**

Camera movement is estimated by a point-based method (Kawanishi et al., 2009). The method is based on eight-point algorithm (Hartley, 1997).

An essential matrix **E** is calculated from ray vectors of corresponding feature points. An essential matrix **E** and ray vectors satisfy the following equation.

$$\mathbf{r}\_i \mathbf{E} \mathbf{r}\_i^\prime = \mathbf{0} \tag{6}$$

where ray vectors <sup>T</sup> , , *i i ii* **<sup>r</sup>** *<sup>x</sup> <sup>y</sup> <sup>z</sup>* and <sup>T</sup> ' ', ', ' *i iii* **<sup>r</sup>** *xyz* are those of the corresponding point in two images. Camera rotation matrix **R** and translation vector **t** are calculated from essential matrix **E** by singular value decomposition. Equation (6) is transformed as follows.

$$\mathbf{u}^{\mathrm{T}}\mathbf{e} = \mathbf{0} \tag{7}$$

where

56 Recent Advances in Mobile Robotics

(a) (b) Corresponding edge segment

Point extracted at constant

Edge point in the next frame

Corresponding edge point

intervals

Tracked point

Fig. 7. Searching for a corresponding edge segment in the next frame. (a) Straight-line extracted in the current frame. (b) Points extracted at constant intervals on the line. (b) Edge segments in the next frame. (c) Points (b) are tracked between the current frame and the next

(c) (d) (e) (f)

frame. (d) Corresponding edge points. (e) Corresponding edge segment.

 (a) 0 frame. (b) 10 frame. (c) 20 frame. Fig. 8. An example of straight-line tracking. Lines which have a same color show

used for straight-line detection.

**4. Environment measurement 4.1 Point-based measurement** 

method is based on eight-point algorithm (Hartley, 1997).

corresponding lines. Although an end point of a line is shown as a white square, it is not

Matching point search on a line has the aperture problem (Nakayama, 1988). However, it is not difficult for the proposed method to obtain corresponding lines, because it does not require point-to-point matching. By continuing the above processes, straight-lines are tracked along the omnidirectional image sequence. An example of line tracking is shown in Fig. 8.

Camera movement is estimated by a point-based method (Kawanishi et al., 2009). The

$$\begin{aligned} \mathbf{u} &= \begin{bmatrix} \mathbf{x}\_i \mathbf{x}\_{i'} \mathbf{y}\_i \mathbf{x}\_{i'} \mathbf{z}\_i \mathbf{x}\_{i'} \mathbf{x}\_{i'} \mathbf{x}\_i \mathbf{y}\_{i'} \mathbf{y}\_i \mathbf{y}\_i \mathbf{y}\_{i'} \mathbf{z}\_i \mathbf{y}\_{i'} \mathbf{x}\_i \mathbf{z}\_{i'} \mathbf{y}\_i \mathbf{z}\_{i'} \mathbf{z}\_i \mathbf{z}\_i \end{bmatrix}^\mathrm{T} \\\\ \mathbf{e} &= \begin{bmatrix} e\_{11'} e\_{12'} e\_{13'} e\_{21'} e\_{22'} e\_{23'} e\_{31'} e\_{32'} e\_{32'} \end{bmatrix}^\mathrm{T} .\end{aligned}$$

*ab e* is the row *a* and column *b* element of Essential matrix **E** . The matrix **E** is obtained by solving simultaneous equations for more than eight pairs of corresponding ray vectors.

$$J = \left\| \mathbf{U} \mathbf{e} \right\|^2 \to \min \tag{8}$$

where <sup>T</sup> 1 2 , ,, **U uu u** *<sup>n</sup>* . *n* is the number of feature points. **e** is calculated as the eigenvector of the smallest eigenvalues of <sup>T</sup> **U U** . Estimated camera movement in this process is used as an initial value for line-based measurement. However, not all feature points tracked in the image sequence correspond satisfactorily due to image noise, etc. Mistracked feature points should be rejected. The proposed method rejects these points as outliers by using RANSAC algorithm (Fischler & Bolles, 1981).

#### **4.2 Line-based measurement**

Estimated camera movement is optimized by using straight-lines. A straight-line is represented as infinite lines by using its direction vector *<sup>w</sup>* **d** and location vector *<sup>w</sup>* **l** ( *w w* **l d** *k* , *k* is a factor). The superscript *w* means that the vector is in world coordinate system. As a prerequisite for line-based measurement, at least, more than 3 images and 3 pairs of corresponding lines (at least one line is not parallel to others) are needed. In the first step, camera rotation and line directions are estimated. The step is independent of camera translation and line locations estimation. In the next step, camera translation and line locations are optimized by a method based on Bundle adjustment (Triggs et al., 1999). In these phases, initial value of 3-D line direction and location are required. These initial values are calculated from line correspondences and initial camera movements.

#### **4.2.1 Camera rotation and 3-D line direction optimization**

Our proposed method calculates a normal vector *<sup>c</sup>* **n***i* of a least square plane calculated from an edge segment *i* in Section 3.2. The superscript *c* means that the vector is in a camera coordinate system at camera position *c* . Camera rotation depends on 3-D line direction vector *<sup>w</sup>* **d***i* and normal vector *<sup>c</sup>* **n***<sup>i</sup>* . By using initial values of camera rotation and normal

Three-Dimensional Environment Modeling Based on Structure from

The relationship between these vectors is shown in Fig. 10.

large error are rejected as outliers by RANSAC algorithm.

,

*a*

*i m*

, *w*

data.

data.

**4.3 Result qualification** 

result which has low accuracy.

coordinates of these points are calculated by the following equation.

Motion with Point and Line Features by Using Omnidirectional Camera 59

, , min *wc w w w A B ic c i c ic i i* **Rg t d l** (12)

*in i i* **e dl** *hn* (13)

**pppp** (14)

*w i n* **e** . 3-D

<sup>T</sup>

Fig. 10. Relationship between camera translation vector and 3-D line location vector.

In the proposed method, 3-D lines are represented as uniformly-spaced points ,

,

The sum of reprojection errors of straight-lines *E***t** is minimized by a convergent calculation based on Levenburg-Marquardt method. In these two optimization steps, lines which have

*w ww*

where *h* is a uniform distance and *n* is an integer number, respectively. 3-D coordinates of

*i n* **e** is reprojected to the image sequence. When the 2-D coordinates of the reprojection point are close to the corresponding edge segment enough, the point is added into measurement

By using estimated camera movement, 3-D coordinates of feature points which have the minimal reprojection error are calculated and integrated with straight-line measurement

Measurement data which have low accuracy should be rejected before 3-D model construction.

,,,,

1, 1, 2, 2, *im im im im*

*ci ci ci ci*

*uvuv* 

where **p***i m*, is 3-D coordinates calculated from corresponding feature points between camera position 1*c* and 2 *c* . *u v ci ci* 1, 1, , and *u v ci ci* 2, 2, , are image coordinates of feature points. The method calculates *i m*, *a* of all camera position combination. If the smallest value min,*<sup>i</sup> a* is larger than a given threshold th *a* , the feature point is rejected as a measurement

Measurement accuracy of the feature point is evaluated by following equations.

vectors *<sup>c</sup>* **<sup>n</sup>***<sup>i</sup>* , a sum of errors *E***R** between camera rotation matrix *<sup>w</sup>* **<sup>R</sup>***c* and 3-D line direction *<sup>w</sup>* **<sup>d</sup>***i* are calculated as shown in the following equation.

$$E\_{\mathbf{R}} = \sum\_{c} \sum\_{i} \left( \left( \mathbf{R}\_{c}^{wT} \mathbf{n}\_{i}^{c} \right)^{T} \mathbf{d}\_{i}^{w} \right)^{2} \tag{9}$$

where, *<sup>w</sup>* **R***c* is a rotation matrix from the world coordinate system to camera coordinate system *c* . Here, *<sup>w</sup>* **d***i* and *<sup>c</sup>* **n***i* are unit vectors. The relationship between a direction vector and a normal vector is shown in Fig. 9. Camera rotations and line directions are optimized by minimizing *E***<sup>R</sup>** . Levenburg-Marquardt method is used for the minimization.

Fig. 9. Relationship between a direction vector of straight-line and a normal vector of a least square plane.

#### **4.2.2 Camera translation and 3-D line location optimization**

Camera translation vector *<sup>w</sup> <sup>c</sup>* **t** and 3-D line location *<sup>w</sup> <sup>i</sup>* **l** are optimized by Bundle adjustment (Triggs et al., 1999). The method estimates camera movements by minimizing reprojection errors. The projection error of the straight-line is calculated as an angle error between two vectors on a plane which is orthogonal to the line direction. The sum of reprojection errors of straight-lines *E***t** is calculated by the following equation.

$$E\_{\mathbf{t}} = \sum\_{c} \sum\_{i} \left( 1 - \hat{\mathbf{1}}\_{i}^{cT} \mathbf{g}\_{i}^{c} \right)^{2} \tag{10}$$

where *<sup>c</sup>* **<sup>g</sup>***i* is a vector located on the plane *<sup>c</sup>* **<sup>n</sup>***<sup>i</sup>* , and it crosses the 3-D line at a right angle. Thus, *<sup>c</sup>* **<sup>g</sup>***i* satisfies *wc w* <sup>T</sup> **Rg d** *ci i* and *c c* **g n** *i i* . ˆ*<sup>c</sup> <sup>i</sup>* **l** is a vector which connects the camera position *c* and the 3-D line location with the shortest distance. ˆ*<sup>c</sup> <sup>i</sup>* **l** is calculated by the following equation.

$$\hat{\mathbf{1}}\_{i}^{c} = \mathbf{R}\_{c}^{w} \left( B\_{i,c} \mathbf{d}\_{i}^{w} + \mathbf{1}\_{i}^{w} - \mathbf{t}\_{c}^{w} \right) \Big/ \left\| B\_{i,c} \mathbf{d}\_{i}^{w} + \mathbf{1}\_{i}^{w} - \mathbf{t}\_{c}^{w} \right\| \tag{11}$$

where *Bi c*, is a factor which shows a location on the 3-D line. *Bi c*, satisfies the following equation.

58 Recent Advances in Mobile Robotics

vectors *<sup>c</sup>* **<sup>n</sup>***<sup>i</sup>* , a sum of errors *E***R** between camera rotation matrix *<sup>w</sup>* **<sup>R</sup>***c* and 3-D line direction *<sup>w</sup>* **<sup>d</sup>***i* are calculated as shown in the following equation.

*<sup>E</sup>*

where, *<sup>w</sup>* **R***c* is a rotation matrix from the world coordinate system to camera coordinate system *c* . Here, *<sup>w</sup>* **d***i* and *<sup>c</sup>* **n***i* are unit vectors. The relationship between a direction vector and a normal vector is shown in Fig. 9. Camera rotations and line directions are optimized

Fig. 9. Relationship between a direction vector of straight-line and a normal vector of a least

(Triggs et al., 1999). The method estimates camera movements by minimizing reprojection errors. The projection error of the straight-line is calculated as an angle error between two vectors on a plane which is orthogonal to the line direction. The sum of reprojection errors

where *<sup>c</sup>* **<sup>g</sup>***i* is a vector located on the plane *<sup>c</sup>* **<sup>n</sup>***<sup>i</sup>* , and it crosses the 3-D line at a right angle.

, , <sup>ˆ</sup>*c w www www*

where *Bi c*, is a factor which shows a location on the 3-D line. *Bi c*, satisfies the following

 <sup>2</sup> <sup>ˆ</sup> <sup>T</sup> <sup>1</sup> *c c i i*

*<sup>c</sup>* **t** and 3-D line location *<sup>w</sup>*

*c i*

**4.2.2 Camera translation and 3-D line location optimization** 

of straight-lines *E***t** is calculated by the following equation.

position *c* and the 3-D line location with the shortest distance. ˆ*<sup>c</sup>*

Thus, *<sup>c</sup>* **<sup>g</sup>***i* satisfies *wc w* <sup>T</sup> **Rg d** *ci i* and *c c* **g n** *i i* . ˆ*<sup>c</sup>*

square plane.

Camera translation vector *<sup>w</sup>*

following equation.

equation.

*c i*

by minimizing *E***<sup>R</sup>** . Levenburg-Marquardt method is used for the minimization.

<sup>2</sup> <sup>T</sup> *wc w* <sup>T</sup> *ci i*

**<sup>R</sup> Rn d** (9)

*<sup>i</sup>* **l** are optimized by Bundle adjustment

*<sup>i</sup>* **l** is a vector which connects the camera

*<sup>i</sup>* **l** is calculated by the

*<sup>E</sup>***<sup>t</sup> l g** (10)

**lR d l t d l t** *i c ic i i c ic i i c B B* (11)

$$\left\| \left( A\_{i,c} \mathbf{R}\_c^{w\top} \mathbf{g}\_i^c + \mathbf{t}\_c^w \right) - \left( B\_{i,c} \mathbf{d}\_i^w + \mathbf{1}\_i^w \right) \right\| \to \min \tag{12}$$

The relationship between these vectors is shown in Fig. 10.

Fig. 10. Relationship between camera translation vector and 3-D line location vector.

The sum of reprojection errors of straight-lines *E***t** is minimized by a convergent calculation based on Levenburg-Marquardt method. In these two optimization steps, lines which have large error are rejected as outliers by RANSAC algorithm.

In the proposed method, 3-D lines are represented as uniformly-spaced points , *w i n* **e** . 3-D coordinates of these points are calculated by the following equation.

$$\mathbf{e}\_{i,n}^w = hm\mathbf{d}\_i^w + \mathbf{l}\_i^w \tag{13}$$

where *h* is a uniform distance and *n* is an integer number, respectively. 3-D coordinates of , *w i n* **e** is reprojected to the image sequence. When the 2-D coordinates of the reprojection point are close to the corresponding edge segment enough, the point is added into measurement data.

By using estimated camera movement, 3-D coordinates of feature points which have the minimal reprojection error are calculated and integrated with straight-line measurement data.

#### **4.3 Result qualification**

Measurement data which have low accuracy should be rejected before 3-D model construction. Measurement accuracy of the feature point is evaluated by following equations.

$$a\_{i,m} = \left\| \frac{\partial \mathbf{p}\_{i,m}}{\partial u\_{c1,i}} \right\| + \left\| \frac{\partial \mathbf{p}\_{i,m}}{\partial v\_{c1,i}} \right\| + \left\| \frac{\partial \mathbf{p}\_{i,m}}{\partial u\_{c2,i}} \right\| + \left\| \frac{\partial \mathbf{p}\_{i,m}}{\partial v\_{c2,i}} \right\|\tag{14}$$

where **p***i m*, is 3-D coordinates calculated from corresponding feature points between camera position 1*c* and 2 *c* . *u v ci ci* 1, 1, , and *u v ci ci* 2, 2, , are image coordinates of feature points. The method calculates *i m*, *a* of all camera position combination. If the smallest value min,*<sup>i</sup> a* is larger than a given threshold th *a* , the feature point is rejected as a measurement result which has low accuracy.

Three-Dimensional Environment Modeling Based on Structure from

Motion with Point and Line Features by Using Omnidirectional Camera 61

High

Low

(a) Bird's-eye. (b) Top.

(c) Front. (d) Side. Fig. 13. Measurement result for accuracy evaluation. Vertical lines are measured precisely. Measurement results of level lines are removed as low measurement accuracy data.

Standard deviation 1.2 - 2.3 Maximum 1.9 1.5 7.5

A measurement result is shown in Fig. 13. A measurement result which has larger Zcoordinate value is displayed in red, and smaller one is displayed in blue. Angles and depth errors were calculated for evaluation of measurement accuracy in Table 1. An angle error of calculated line directions is 1.2 degree standard deviation. Its maximum error is 1.9 degree. An angle error between two flat walls estimated from measurement data is within 1.5 degrees. A depth error between an estimated flat wall and reconstructed lines has 2.3 mm standard deviation. Its maximum error is 7.5 mm. This experiment shows that our proposed method has sufficient accuracy to accomplish static obstacle avoidance, self-localization. Next, we experimented in an environment including non-textured objects as shown in Fig. 14. We used 84 omnidirectional images. Measurement results of feature points and straightlines are shown in Fig. 15. The blue marks in the figure show the camera trajectory. Although feature point measurement results are sparse, straight-lines can be measured densely. This experimental result shows that our proposed method is effective for a non-

Angle (plane) [deg]

Depth [mm]

Angle (direction) [deg]

Table 1. Evaluation result of vertical line measurements.

textured environment.

Fig. 11. Triangular mesh generation and its optimization. (a) Triangular meshes generated by Delaunay triangulation. (b) Optimized triangular meshes.

Measurement accuracy of straight-line is evaluated by equation (14), too. In this evaluation, **p***i m*, is the middle point of the line connecting two vectors *<sup>c</sup>*<sup>1</sup> **g***i* and *<sup>c</sup>*<sup>2</sup> **g***i* at the shortest distance. Image coordinates 1, 1, , *u v ci ci* and 2, 2, , *u v ci ci* are reprojection points of these vectors to images acquired at camera position 1*c* and 2 *c* .

#### **5. Model construction**

Triangular meshes are generated from integrated measurement data by using the 3-D Delaunay triangulation. However, Delaunay triangulation generates a triangular mesh which contradicts a physical shape because the triangular mesh does not consider the shape of the measurement object. Therefore, we apply the triangular optimization method (Nakatsuji et al., 2005) to the triangular mesh (Fig. 11). The method adapts the triangular mesh to the physical shape by detecting a texture distortion. By texture mapping to these meshes, a 3D environment model is constructed.

#### **6. Experiments**

First, accuracy of line-based measurement is evaluated. Measurement objects are lengthwise-lines on a flat wall shown in Fig. 12. The reason for including crosswise-lines is that the proposed method needs lines having different direction. The moving distance of the camera was about 2m. The number of input images is 72. An input image size is 2496 1664 pixels.

Fig. 12. Measurement objects place on flat walls. Vertical lines are measured for accuracy evaluation. Level lines are set for camera movement estimation.

60 Recent Advances in Mobile Robotics

(a) (b) Fig. 11. Triangular mesh generation and its optimization. (a) Triangular meshes generated

Measurement accuracy of straight-line is evaluated by equation (14), too. In this evaluation, **p***i m*, is the middle point of the line connecting two vectors *<sup>c</sup>*<sup>1</sup> **g***i* and *<sup>c</sup>*<sup>2</sup> **g***i* at the shortest distance. Image coordinates 1, 1, , *u v ci ci* and 2, 2, , *u v ci ci* are reprojection points of these

Triangular meshes are generated from integrated measurement data by using the 3-D Delaunay triangulation. However, Delaunay triangulation generates a triangular mesh which contradicts a physical shape because the triangular mesh does not consider the shape of the measurement object. Therefore, we apply the triangular optimization method (Nakatsuji et al., 2005) to the triangular mesh (Fig. 11). The method adapts the triangular mesh to the physical shape by detecting a texture distortion. By texture mapping to these

First, accuracy of line-based measurement is evaluated. Measurement objects are lengthwise-lines on a flat wall shown in Fig. 12. The reason for including crosswise-lines is that the proposed method needs lines having different direction. The moving distance of the camera was about 2m. The number of input images is 72. An input image size is 2496 1664

(a) Object. (b) Environment. Fig. 12. Measurement objects place on flat walls. Vertical lines are measured for accuracy

evaluation. Level lines are set for camera movement estimation.

600 mm 2 m

by Delaunay triangulation. (b) Optimized triangular meshes.

vectors to images acquired at camera position 1*c* and 2 *c* .

meshes, a 3D environment model is constructed.

**5. Model construction** 

**6. Experiments** 

pixels.

Fig. 13. Measurement result for accuracy evaluation. Vertical lines are measured precisely. Measurement results of level lines are removed as low measurement accuracy data.


Table 1. Evaluation result of vertical line measurements.

A measurement result is shown in Fig. 13. A measurement result which has larger Zcoordinate value is displayed in red, and smaller one is displayed in blue. Angles and depth errors were calculated for evaluation of measurement accuracy in Table 1. An angle error of calculated line directions is 1.2 degree standard deviation. Its maximum error is 1.9 degree. An angle error between two flat walls estimated from measurement data is within 1.5 degrees. A depth error between an estimated flat wall and reconstructed lines has 2.3 mm standard deviation. Its maximum error is 7.5 mm. This experiment shows that our proposed method has sufficient accuracy to accomplish static obstacle avoidance, self-localization.

Next, we experimented in an environment including non-textured objects as shown in Fig. 14. We used 84 omnidirectional images. Measurement results of feature points and straightlines are shown in Fig. 15. The blue marks in the figure show the camera trajectory. Although feature point measurement results are sparse, straight-lines can be measured densely. This experimental result shows that our proposed method is effective for a nontextured environment.

Three-Dimensional Environment Modeling Based on Structure from

Motion with Point and Line Features by Using Omnidirectional Camera 63

Camera trajectory

(a) Feature points.

(b) Straight-lines.

Fig. 15. Measurement results of non-textured environment. (a) Although camera movement estimation is possible, we get sparse measurement results. (b) Straight-lines make it possible

to measure non-textured environments densely.

(a) Non-textured environment.

(b) Input image.

Fig. 14. Non-textured environment. We cannot get enough feature points in the environment because there are few features.

Modeling result is shown in Fig. 16. Images having a view-point which is different from camera observation points can be acquired. A model constructed from feature point measurement data is only a small part of this environment (Fig. 16(a) and (c)). Meanwhile, edge measurement data makes it possible to construct a non-textured environment model (Fig. 16}(b) and (d)).

We also experimented in an outdoor environment including textured objects (trees and so on) and non-textured objects (buildings and so on) as shown in Fig. 17. We used 240 omnidirectional images. An integrated measurement result is shown in Fig. 18. As one of textured objects, the shape of ground surface is measured by point-based measurement. As non-textured objects, the shape of the building is measured by line-based measurement.

62 Recent Advances in Mobile Robotics

(a) Non-textured environment.

(b) Input image.

Fig. 14. Non-textured environment. We cannot get enough feature points in the environment

Modeling result is shown in Fig. 16. Images having a view-point which is different from camera observation points can be acquired. A model constructed from feature point measurement data is only a small part of this environment (Fig. 16(a) and (c)). Meanwhile, edge measurement data makes it possible to construct a non-textured environment model

We also experimented in an outdoor environment including textured objects (trees and so on) and non-textured objects (buildings and so on) as shown in Fig. 17. We used 240 omnidirectional images. An integrated measurement result is shown in Fig. 18. As one of textured objects, the shape of ground surface is measured by point-based measurement. As non-textured objects, the shape of the building is measured by line-based

because there are few features.

(Fig. 16}(b) and (d)).

measurement.

(b) Straight-lines.

Fig. 15. Measurement results of non-textured environment. (a) Although camera movement estimation is possible, we get sparse measurement results. (b) Straight-lines make it possible to measure non-textured environments densely.

Three-Dimensional Environment Modeling Based on Structure from

textured objects (walls etc.)

Motion with Point and Line Features by Using Omnidirectional Camera 65

(a) Environment. (b) Input image. Fig. 17. Outdoor environment. There are textured objects (trees, tiles and so on) and non-

(a) Bird's-eye

(b) Top

Fig. 18. Measurement result of outdoor environment.

(a) Front (feature point only). (b) Front (with straight-line).

(c) Bird's-eye (feature point only).

(d) Bird's-eye (with straight-line)

Fig. 16. Modeling results of non-textured environment. We can construction a model including many non-textured objects by the method with straight-lines.

Modeling result is shown in Fig. 19. By the combination of point-based measurement and line-based measurement, our proposed method can construct a model of 3-D environment including textured and non-textured objects. Experimental results showed the effectiveness of our proposed method.

64 Recent Advances in Mobile Robotics

(a) Front (feature point only). (b) Front (with straight-line).

(c) Bird's-eye (feature point only).

(d) Bird's-eye (with straight-line)

Modeling result is shown in Fig. 19. By the combination of point-based measurement and line-based measurement, our proposed method can construct a model of 3-D environment including textured and non-textured objects. Experimental results showed the effectiveness

Fig. 16. Modeling results of non-textured environment. We can construction a model

including many non-textured objects by the method with straight-lines.

of our proposed method.

(a) Environment. (b) Input image.

Fig. 17. Outdoor environment. There are textured objects (trees, tiles and so on) and nontextured objects (walls etc.)

(a) Bird's-eye

(b) Top

Fig. 18. Measurement result of outdoor environment.

Three-Dimensional Environment Modeling Based on Structure from

**7. Conclusions** 

non-textured objects.

**8. References** 

used for increasing measurement stability.

2, pp. 1403-1410, 2003.

357, 2003.

pp. 405-416, 2003.

*Computer and Robot Vision*, pp. 1-8, 2006.

*Vision*, pp. 1741-1748, 2009.

4, Issue 1, pp. 43-56, 1986.

Vol. 100, Issue 3, pp. 416-441, 2005.

*Mechatronics*, Vol. 21, No. 5, pp. 574-582, 2009.

Motion with Point and Line Features by Using Omnidirectional Camera 67

We proposed an environment modeling method based on structure from motion using both feature points and straight-lines by using an omnidirectional camera. Experimental results showed that our proposed method is effective in environment including both textured and

As future works, the precision improvement of edge tracking is necessary. Moreover, we should evaluate difference of camera movement estimation accuracy between point-based measurement and edge-based measurement. Further, edge position correlation should be

Davison, A. J. (2003). Real-Time Simultaneous Localisation and Mapping with a Single

Ishiguro, H. & Yamamoto, M. & Tsuji, S. (1992). Omni-Directional Stereo, *IEEE Transactions on Pattern Analysis and Machine Intelligence*, Vol. 14, No. 2, pp. 257-262, 1992. Gluckman, J. & Nayar, S. K. (1998). Ego-motion and Omnidirectional Cameras, *Proceedings of the 6th International Conference on Computer Vision*, pp. 999-1005, 1998. Bunschoten, R. & Krose, B. (2003). Robust Scene Reconstruction from an Omnidirectional

Geyer, C. & Daniilidis, K. (2003). Omnidirectional Video, *The Visual Computer*, Vol. 19, No. 6,

Rachmielowski, A. & Cobzas, D. & Jagersand, M. (2006). Robust SSD Tracking with

Kawanishi, R.; Yamashita, A. & Kaneko, T. (2009). Three-Dimensional Environment Model

Chandraker, M. & Lim, J. & Kriegman, D. (2009). Moving in Stereo: Efficient Structure and

Schindler, G.; Krishnamurthy, P. & Dellaert, F. (2006). Line-Based Structure from Motion for

Bartoli, A. & Sturm, P. (2005). Structure-from-motion using lines: representation,

Smith, P. & Reid, I. & Davison, A. (2006). Real Time Monocular SLAM with Straight lines, *Proceedings of the 17th British Machine Vision Conference*, pp. 17-26, 2006. Mariottini, G. L. & Prattichizzo, D. (2007). Uncalibrated video compass for mobile robots

Spacek, L. A. (1986). Edge Detection and Motion Detection, *Image and Vision Computing*, Vol.

*Processing, Visualization, and Transmission*, pp. 846-853, 2006.

*Conference on Intelligent Robots and Systems*, pp. 226-231, 2007.

Camera, *Proceedings of the 9th IEEE International Conference on Computer Vision*, Vol.

Vision System, *IEEE Transactions on Robotics and Automation*, Vol. 19, No. 2, pp. 351-

Incremental 3D Structure Estimation, *Proceedings of the 3rd Canadian Conference on* 

Construction from an Omnidirectional Image Sequence, *Journal of Robotics and* 

Motion Using Lines, *Proceedings of the 12th IEEE International Conference on Computer* 

Urban Environments, *Proceedings of the 3rd International Symposium on 3D Data* 

triangulation, and bundle adjustment, *Computer Vision and Image Understanding*,

from paracatadioptric line images, *Proceedings of the 2007 IEEE/RSJ International* 

(a) Bird's-eye.

(b) Right.

(c) Front.

Fig. 19. Modeling results of outdoor environment.

### **7. Conclusions**

66 Recent Advances in Mobile Robotics

(a) Bird's-eye.

(b) Right.

(c) Front.

Fig. 19. Modeling results of outdoor environment.

We proposed an environment modeling method based on structure from motion using both feature points and straight-lines by using an omnidirectional camera. Experimental results showed that our proposed method is effective in environment including both textured and non-textured objects.

As future works, the precision improvement of edge tracking is necessary. Moreover, we should evaluate difference of camera movement estimation accuracy between point-based measurement and edge-based measurement. Further, edge position correlation should be used for increasing measurement stability.

### **8. References**


**4** 

*USA* 

**Mobile Robot Position Determination** 

One of the most important reasons for the popularity of mobile robots in industrial manufacturing is their capability to move and operate freely. In order for the robots to perform to the expectations in manufacturing, their position and orientation must be determined accurately. In addition, there is a strong tendency to grant more autonomy to robots when they operate in hazardous or unknown environments which also requires accurate position determination. Mobile robots are usually divided into two categories of

Techniques used for position determination of wheeled mobile robots (or simply, mobile robots) are classified into two main groups: relative positioning and absolute positioning (Borenstein, 1996, 1997). In relative positioning, robot's position and orientation will be determined using relative sensors such as encoders attached to the wheels or navigation systems integrated with the robots. Absolute positioning techniques are referred to the methods utilizing a reference for position determination. The Global Positioning Systems,

Calculating position from wheel rotations using the encoders attached to the robot's wheels is called Odometry. Although odometry is the first and most fundamental approach for position determination, due to inherent errors, it is not an accurate method. As a solution to this problem, usually odometry errors are modeled using two different methods of benchmarks and multiple sensors. In this chapter, we will discuss odometry and two different methods to model and estimate odometry errors. At the end, an example for

There is a consensus among researcher that odometry is the vital technique for mobile robot position determinations. The governing equations of odometry are based on converting rotational motion of robot wheels to a translational motion (Borenstein, 1996, 1997). Although odometry is an inexpensive method for position determination, it has several inherent issues. One issue is that errors accumulate over time and consequently make odometry unreliable over time. The odometry errors are mainly classified as systematic and

nonsystematic (Borenstein, 1996). The source of systematic errors usually caused by:

• Average of both wheel diameters differs from nominal diameter

legged and wheeled robots. In this chapter, we focus on wheeled mobile robots.

magnetic compass, active beacons are examples of absolute positioning systems.

mobile robot position determination using multiple sensors will be presented.

**1. Introduction** 

**2. Odometry** 

• Misalignment of wheels

Farouk Azizi and Nasser Houshangi

*Purdue University Calumet* 


## **Mobile Robot Position Determination**

Farouk Azizi and Nasser Houshangi *Purdue University Calumet USA* 

#### **1. Introduction**

68 Recent Advances in Mobile Robotics

Triggs, B. & McLauchlan, P. & Hartley, R. & Fitzgibbon, A. (1999). Bundle Adjustment -A

Shi, J. & Tomasi, C. (1994). Good Features to Track, *Proceedings of the 1994 IEEE Computer Society Conference on Computer Vision and Pattern Recognition*, pp. 593-600, 1994. Canny, J. F. (1986). A Computational Approach to Edge Detection, *IEEE Transactions on Pattern Analysis and Machine Intelligence*, Vol. PAMI-8, No. 6, pp. 679-698, 1986. Fischler, M. A. & Bolles, R. C. (1981). Random Sample Consensus: A Paradigm for Model

Nakayama, K. & Silverman, G. (1988). The Aperture Problem -- II. Spatial Integration of

Hartley, R. (1997). In defence of the eight-point algorithm, *IEEE Transactions on Pattern* 

Nakatsuji, A. & Sugaya, Y. & Kanatani, K. (2005), Optimizing a Triangular Mesh for Shape

*Theory and Practice*, Springer-Verlag LNCS 1883, pp. 298-372, 1999.

*Communications of the ACM*, Vol. 24, No. 6, pp. 381-395, 1981.

*Analysis and Machine Intelligence*, Vol. 19, No. 6, pp. 580-593, 1997.

E88-D, No. 10, pp. 2269-2276, 2005.

1988.

Modern Synthesis, *Proceedings of the International Workshop on Vision Algorithms:* 

Fitting with Applications to Image Analysis and Automated Cartography,

Velocity Information Along Contours, *Vision Research*, Vol. 28, No. 6, pp. 747-753,

Reconstruction from Images, *IEICE Transactions on Information and Systems*, Vol.

One of the most important reasons for the popularity of mobile robots in industrial manufacturing is their capability to move and operate freely. In order for the robots to perform to the expectations in manufacturing, their position and orientation must be determined accurately. In addition, there is a strong tendency to grant more autonomy to robots when they operate in hazardous or unknown environments which also requires accurate position determination. Mobile robots are usually divided into two categories of legged and wheeled robots. In this chapter, we focus on wheeled mobile robots.

Techniques used for position determination of wheeled mobile robots (or simply, mobile robots) are classified into two main groups: relative positioning and absolute positioning (Borenstein, 1996, 1997). In relative positioning, robot's position and orientation will be determined using relative sensors such as encoders attached to the wheels or navigation systems integrated with the robots. Absolute positioning techniques are referred to the methods utilizing a reference for position determination. The Global Positioning Systems, magnetic compass, active beacons are examples of absolute positioning systems.

Calculating position from wheel rotations using the encoders attached to the robot's wheels is called Odometry. Although odometry is the first and most fundamental approach for position determination, due to inherent errors, it is not an accurate method. As a solution to this problem, usually odometry errors are modeled using two different methods of benchmarks and multiple sensors. In this chapter, we will discuss odometry and two different methods to model and estimate odometry errors. At the end, an example for mobile robot position determination using multiple sensors will be presented.

#### **2. Odometry**

There is a consensus among researcher that odometry is the vital technique for mobile robot position determinations. The governing equations of odometry are based on converting rotational motion of robot wheels to a translational motion (Borenstein, 1996, 1997). Although odometry is an inexpensive method for position determination, it has several inherent issues. One issue is that errors accumulate over time and consequently make odometry unreliable over time. The odometry errors are mainly classified as systematic and nonsystematic (Borenstein, 1996). The source of systematic errors usually caused by:


Mobile Robot Position Determination 71

method does not use any signal processing filter to estimate the errors. The main drawback of the method is the use of unloaded wheels along with drive wheels which limits the robot

Antonelli and coworkers proposed a method to calibrate odometry in mobile robots with differential drive (Antonelli, 2005). This method is based on estimating some of odometry parameters using linear identification problems for the parameters. These parameters come from kinematic equations of the odometry which are the sources of odometry's systematic errors. The estimation model is derived from the least square technique which allows a numerical analysis of the recorded data. Unlike UMBmark, this method is based on a more flexible platform and does not require a predefined path for the robot. Since the precision of the robot position and orientation at any given time is very important in estimating the robot position at the next moment, a vision-based measurement system is used to record robot's position and orientation. The experimental results shows this technique provide less error in estimating the robot position and orientation when compared to UMBMark

Larsen and co-workers reported a method to estimate odometry systematic errors using Augmented Kalman Filter (Larsen, 1998). This technique estimates the wheel diameter and distance traveled with a sub-percent precision by augmenting the Kalman filter with a correction factor. The correction factor changes the noise covariance matrix to rely on the measurements more than the model by placing more confidence on the new readings. The augmented Kalman filter uses encoder readings as inputs and vision measurements as the observations. The only condition to achieve such precision is to use more sensitive

Martinelli and Siegwart proposed a method to estimate both systematic and nonsystematic errors odometry errors during the navigation (Martinelli, 2003). The systematic errors are estimated using augmented Kalman filter technique proposed in (Larsen, 1998) and the nonsystematic errors are estimated using another Kalman Filter. The second Kalman filter observations is called Observable Filter (OF) come from the estimates of the robot configuration parameters of the segmented Kalman filter. The main idea of Observable filters is to estimate the mean and variance of the observable variables of the robot

The other method to estimate the odometry errors is to integrate odometry with information from another sensor. The information from another sensor eventually is used to reset odometry errors especially during long runs. In many studies Global Positioning System (GPS), Inertial Navigation System (INS), compass, vision and sonar have been used in conjunction with odometry for position determination. In most cases Kalman filter or a derivation of Kalman filter, such as Indirect Kalman filter (IKF), Extended Kalman filter (EKF) and Unscented Kalman filter (UKF) has been used to integrate the information. In the

Park and coworkers employed a dead reckoning navigation system using differential encoders installed on the robot wheels and a gyroscope which is attached to robot (Park, 1997). The approach is based on estimation and compensation of the errors from the differential encoders and the gyroscope angular rate output. An Indirect Kalman filter (IKF) is used to integrate heading information from gyroscope and odometry. The output of IKF is used to compensate the errors associated with heading information in odometry as well as

measurement system to measure the robot position and orientation.

parameters which are characterizing the odometry error.

following, we discuss few works have been done in this direction.

**2.2 Correcting odometry using multiple sensors** 

motion significantly.

technique.


Nonsystematic errors are caused by the following conditions:


Since these errors drastically affect the accuracy of odometry over short and long distances, it is empirical to accurately estimate the errors. The techniques used to overcome problems with odometry can be categorized to benchmark techniques and utilizing multiple sensors.

#### **2.1 Correcting odometry using benchmark techniques**

Benchmark techniques are based on testing robots over some predefined paths. In each experiment, the actual position and orientation of the robot is compared with the theoretical final position and orientation of the robot. Using the robot's kinematic equation a series of correction factors are derived to be incorporated in future position determination.

Borenstein and Feng proposed a test bench to model and estimate systematic errors (Borenstein, 1995). The method is called UMBmark and is based on running a robot with differential wheel drives on a square clockwise and counter clockwise paths for several times and compare the recording with the actual final position and orientation of the robot to generate the odometry errors. The errors are then classified as type A and type B. Type A error is the error that increases (or decreases) amount of rotation of the robot during the square path experiment in both clockwise and counter clockwise direction while type B error is the kind of error that increases (decreases) amount of rotation in one direction (clockwise or counter clockwise) and decreases (increases) amount of rotation in other direction (counter clockwise or clockwise). The robot's geometric relations are used to calculate the wheelbase and distance errors in terms of type A and B errors. Finally, the results are used to find two correction factors for right and left wheels of the robot to be applied to odometry calculation to improve the positioning system. The advantage of this method lies in the fact that mechanical inaccuracies such as wheel diameters can be modeled and compensated for. However, the method is only suitable to overcome systematic errors and does not estimate nonsystematic errors.

Chong and Kleeman modified UMBmark to model odometry positioning system more effectively by offering a mechanism to include odometry's nonsystematic errors (Chong, 1997). This approach uses robot's geometric and dynamical equation in a more fundamental way to generate an error covariance matrix for estimating the odometry errors. The technique also utilizes an unloaded wheel installed independently on linear bearings of each wheel to minimize wheel slippage and motion distortion. As a result of these modifications, odometry errors are minimized to a level that could be considered a zero-mean white noise signal. The other important characteristic of this method is that the errors calculated at each given time are uncorrelated to the next or from previous errors. The systematic errors are calibrated using UMBmark test. To model non-systematic errors, it is assumed that the robot motion can be divided to small segments of translational and rotational motions. For each segment the associated covariance matrix is calculated based on the physical model of the motion. The covariance matrix is updated using the previous calculation of the matrix. This 70 Recent Advances in Mobile Robotics

• Uncertainty about the effective wheelbase (due to non-point wheel contact with the

Since these errors drastically affect the accuracy of odometry over short and long distances, it is empirical to accurately estimate the errors. The techniques used to overcome problems with odometry can be categorized to benchmark techniques and utilizing multiple sensors.

Benchmark techniques are based on testing robots over some predefined paths. In each experiment, the actual position and orientation of the robot is compared with the theoretical final position and orientation of the robot. Using the robot's kinematic equation a series of

Borenstein and Feng proposed a test bench to model and estimate systematic errors (Borenstein, 1995). The method is called UMBmark and is based on running a robot with differential wheel drives on a square clockwise and counter clockwise paths for several times and compare the recording with the actual final position and orientation of the robot to generate the odometry errors. The errors are then classified as type A and type B. Type A error is the error that increases (or decreases) amount of rotation of the robot during the square path experiment in both clockwise and counter clockwise direction while type B error is the kind of error that increases (decreases) amount of rotation in one direction (clockwise or counter clockwise) and decreases (increases) amount of rotation in other direction (counter clockwise or clockwise). The robot's geometric relations are used to calculate the wheelbase and distance errors in terms of type A and B errors. Finally, the results are used to find two correction factors for right and left wheels of the robot to be applied to odometry calculation to improve the positioning system. The advantage of this method lies in the fact that mechanical inaccuracies such as wheel diameters can be modeled and compensated for. However, the method is only suitable to overcome systematic errors

Chong and Kleeman modified UMBmark to model odometry positioning system more effectively by offering a mechanism to include odometry's nonsystematic errors (Chong, 1997). This approach uses robot's geometric and dynamical equation in a more fundamental way to generate an error covariance matrix for estimating the odometry errors. The technique also utilizes an unloaded wheel installed independently on linear bearings of each wheel to minimize wheel slippage and motion distortion. As a result of these modifications, odometry errors are minimized to a level that could be considered a zero-mean white noise signal. The other important characteristic of this method is that the errors calculated at each given time are uncorrelated to the next or from previous errors. The systematic errors are calibrated using UMBmark test. To model non-systematic errors, it is assumed that the robot motion can be divided to small segments of translational and rotational motions. For each segment the associated covariance matrix is calculated based on the physical model of the motion. The covariance matrix is updated using the previous calculation of the matrix. This

correction factors are derived to be incorporated in future position determination.

floor)

• Limited encoder resolution • Limited encoder sampling rate

• Travel over uneven floors

• Wheel-slippage

Nonsystematic errors are caused by the following conditions:

**2.1 Correcting odometry using benchmark techniques** 

• Travel over unexpected objects on the floor

and does not estimate nonsystematic errors.

method does not use any signal processing filter to estimate the errors. The main drawback of the method is the use of unloaded wheels along with drive wheels which limits the robot motion significantly.

Antonelli and coworkers proposed a method to calibrate odometry in mobile robots with differential drive (Antonelli, 2005). This method is based on estimating some of odometry parameters using linear identification problems for the parameters. These parameters come from kinematic equations of the odometry which are the sources of odometry's systematic errors. The estimation model is derived from the least square technique which allows a numerical analysis of the recorded data. Unlike UMBmark, this method is based on a more flexible platform and does not require a predefined path for the robot. Since the precision of the robot position and orientation at any given time is very important in estimating the robot position at the next moment, a vision-based measurement system is used to record robot's position and orientation. The experimental results shows this technique provide less error in estimating the robot position and orientation when compared to UMBMark technique.

Larsen and co-workers reported a method to estimate odometry systematic errors using Augmented Kalman Filter (Larsen, 1998). This technique estimates the wheel diameter and distance traveled with a sub-percent precision by augmenting the Kalman filter with a correction factor. The correction factor changes the noise covariance matrix to rely on the measurements more than the model by placing more confidence on the new readings. The augmented Kalman filter uses encoder readings as inputs and vision measurements as the observations. The only condition to achieve such precision is to use more sensitive measurement system to measure the robot position and orientation.

Martinelli and Siegwart proposed a method to estimate both systematic and nonsystematic errors odometry errors during the navigation (Martinelli, 2003). The systematic errors are estimated using augmented Kalman filter technique proposed in (Larsen, 1998) and the nonsystematic errors are estimated using another Kalman Filter. The second Kalman filter observations is called Observable Filter (OF) come from the estimates of the robot configuration parameters of the segmented Kalman filter. The main idea of Observable filters is to estimate the mean and variance of the observable variables of the robot parameters which are characterizing the odometry error.

#### **2.2 Correcting odometry using multiple sensors**

The other method to estimate the odometry errors is to integrate odometry with information from another sensor. The information from another sensor eventually is used to reset odometry errors especially during long runs. In many studies Global Positioning System (GPS), Inertial Navigation System (INS), compass, vision and sonar have been used in conjunction with odometry for position determination. In most cases Kalman filter or a derivation of Kalman filter, such as Indirect Kalman filter (IKF), Extended Kalman filter (EKF) and Unscented Kalman filter (UKF) has been used to integrate the information. In the following, we discuss few works have been done in this direction.

Park and coworkers employed a dead reckoning navigation system using differential encoders installed on the robot wheels and a gyroscope which is attached to robot (Park, 1997). The approach is based on estimation and compensation of the errors from the differential encoders and the gyroscope angular rate output. An Indirect Kalman filter (IKF) is used to integrate heading information from gyroscope and odometry. The output of IKF is used to compensate the errors associated with heading information in odometry as well as

Mobile Robot Position Determination 73

starting point. The gyro output drifts over time and this was compensated using the method proposed by Barshan and White (Barshan, 1993, 1994, 1995). Using this method both types of odometry errors are compensated and as result more reliable position and orientation are calculated. This approach is simple and doesn't need any sensor integration. While the method is reliable for the conditions it was tested for, it does not include the situation where odometry fails due to other nonsystematic errors such as

Using ultrasonic beacons is another option to improve odometry which is proposed by Kleeman (Kleeman, 1992). In this approach active ultrasonic beacons are used to locate the robot at any given time. The information from beacons was used as a reliable position to reset the odometry. It is well known that ultrasonic measurements have random errors which don't accumulate over time but in contrast are not smooth and require to be compensated. In this method, an ultrasonic localizer is used which has six ultrasonic beacons and a transmitter controller which sequences the firing of the beacons in a cyclic manner. Beacons have 150 milliseconds intervals to have the previous pulse reverberation settled. Beacon 1 is distinguished by transmitting two bursts 3 milliseconds apart. The robot has an onboard receiver which is composed of eight ultrasonic receiver arranged at 45 degrees intervals. In modeling the positioning system, in addition to position of robot in both x- and y-directions, velocity of the robot, beacon cycle time, speed of sound and beacon firing time are introduced as the states of the system. An Iterative Extended Kalman Filter (IEKF) is utilized to integrate two sets of data from the beacons and from odometry. This method provides reliable information about the robot position in a constructed environment. However, it has several important drawbacks which make it unsuitable in many situations. For example, there is a problem with having delayed time arrival of an ultrasonic beacon due to an indirect path incorporating reflection off obstacles, walls, ceiling or floor. These echoed arrival time should be identified and must not be taken into account in estimation otherwise IEKF result in erroneous state estimation. In addition, one could easily point out that this method is appropriate only for indoor applications when ultrasonic

Barshan and Whyte have developed an Inertial Navigation system to navigate outdoor mobile robots (Barshan, 1993, 1994, 1995). In this system, three solid states gyroscope, a triaxial accelerometer and two Electrolevel tilt sensor are used. One of the important results of this work was the development of a method to model drift error of inertial sensors as an exponential function with time. Modeling the drift error for each sensor was done by leaving the sensor on a surface motionless. The sensor's readings were recorded until it was stabled. The sensor's readings are then modeled as an exponential function. The proposed Navigation system has been tested on radar-equipped land vehicles. The orientation from inertial sensors is reliable for 10 minutes. However, in the presence of electromagnetic fields information regarding the heading is valid only for 10 seconds. Although, using the inertial sensors reliably (after the drift errors were modeled) for 10 minutes is an improvement for vehicle position determination, it is not good enough when compared with systems which integrate two or more sensors. It is assumed that in all cases the errors associated with inertial sensor have been modeled and were taken in to account before the position and

Gyro's scale factor is provided by manufacturer to convert gyro output (digital or analog signal) to degrees per second. The scale factor is not a fixed and varies slightly with gyro's temperature, direction of rotation and rate of rotation. Therefore, modeling the variation of

moving on an uneven surface or in presence of wheel slippage.

beacons can be installed.

orientation of the vehicle was calculated.

the error in gyroscope readings. The improved heading has been used in some formalism to give more accurate position and heading of the mobile robot. The work is followed by introducing a system of linear differential equations for each one of position errors on x- and y- directions, heading rate error, left and right wheel encoder errors, gyro scale factor error, and gyro bias drift error and wheel base error. The differential matrix derived from that linear differential equation is the equation that is used as the input for Indirect Kalman filter. The advantage of this method is that by including both the encoder and the gyroscope errors as the input of Kalman filter, both odometry and gyroscope errors can be estimated. Cui and Ge in their work utilized Differential Global Positioning System (DGPS) as the basic positioning system for autonomous vehicle (Cui, 2001). GPS is currently used in many applications of land vehicle navigation and it is based on using information about the location and direction of motion of a moving vehicle which received from different satellites orbiting the earth. The expected accuracy of this technique is less than 0.5 meters. DGPS is similar to GPS but it uses two or more different satellite signals to localize a moving vehicle with more accuracy. One of the problems with both GPS and DGPS is when these methods are used to track a moving vehicle in an urban canyon. In such an environment, tall buildings prevent GPS to receive signals from the satellites. Therefore, it's critical to decrease the number of required satellite signals when the vehicle is moving in urban canyons. To achieve this goal, a constrained method was used that approximately modeled the path of the moving vehicle in an urban canyons environment as pieces of lines which is the cross section of two different surfaces. Each surface can be covered by two satellites at each moment. This will decrease the number of different satellite signals needed for vehicle localization to two. However, the method can switch back to use more than two satellite signals once the vehicle is not in moving in an urban canyon. In this method a pseudorange measurement is proposed to calculate distance from each satellite. Since the GPS receiver is modeled so that the measurement equation is nonlinear, the Extended Kalman Filter is used to the augmented matrix generated from the measurements to estimate the vehicle's position. While this work has not used odometry as the basic method for positioning of the vehicle, it decreases the number of required satellite signals to estimate the vehicle position when it travels in urban canyons. Recently Chae and coworkers have reported a method that uses EKF to efficiently integrate data from DGPS and INS sensors (Chae, 2010). The proposed method is designed to cover the task of mobile robot position determination during the times the robot navigates in urban areas with very tall buildings in which GPS signals are inaccessible.

Borenstein and Feng in another work have introduced a method called gyrodometry (Bronstein, 1996). In this method, the odometry is corrected for systematic errors using UMBmark. The approach intends also to get around non-systematic errors such as the errors generated when the robot traverses on a bump. The nonsystematic errors impact the robot for short period and is not detected by odometry.Once the robot faced a bump or another source of nonsystematic errors, odometry is replaced by gyro-based positioning system which doesn't fail in that situation and can provide a reliable information as the robot passes the bump. In practice both sets of information are available all the time and it is assumed that two sets of data, as long as there is no bump or object ahead of the robot, return almost the same information. In fact the procedure starts when odometry data differs substantially from gyro data. At the end of the time period robot traversing on a bump, the odometry is set by gyro positioning system. Afterwards, the odometry is the main positioning system and uses the correct new 72 Recent Advances in Mobile Robotics

the error in gyroscope readings. The improved heading has been used in some formalism to give more accurate position and heading of the mobile robot. The work is followed by introducing a system of linear differential equations for each one of position errors on x- and y- directions, heading rate error, left and right wheel encoder errors, gyro scale factor error, and gyro bias drift error and wheel base error. The differential matrix derived from that linear differential equation is the equation that is used as the input for Indirect Kalman filter. The advantage of this method is that by including both the encoder and the gyroscope errors as the input of Kalman filter, both odometry and gyroscope errors can be estimated. Cui and Ge in their work utilized Differential Global Positioning System (DGPS) as the basic positioning system for autonomous vehicle (Cui, 2001). GPS is currently used in many applications of land vehicle navigation and it is based on using information about the location and direction of motion of a moving vehicle which received from different satellites orbiting the earth. The expected accuracy of this technique is less than 0.5 meters. DGPS is similar to GPS but it uses two or more different satellite signals to localize a moving vehicle with more accuracy. One of the problems with both GPS and DGPS is when these methods are used to track a moving vehicle in an urban canyon. In such an environment, tall buildings prevent GPS to receive signals from the satellites. Therefore, it's critical to decrease the number of required satellite signals when the vehicle is moving in urban canyons. To achieve this goal, a constrained method was used that approximately modeled the path of the moving vehicle in an urban canyons environment as pieces of lines which is the cross section of two different surfaces. Each surface can be covered by two satellites at each moment. This will decrease the number of different satellite signals needed for vehicle localization to two. However, the method can switch back to use more than two satellite signals once the vehicle is not in moving in an urban canyon. In this method a pseudorange measurement is proposed to calculate distance from each satellite. Since the GPS receiver is modeled so that the measurement equation is nonlinear, the Extended Kalman Filter is used to the augmented matrix generated from the measurements to estimate the vehicle's position. While this work has not used odometry as the basic method for positioning of the vehicle, it decreases the number of required satellite signals to estimate the vehicle position when it travels in urban canyons. Recently Chae and coworkers have reported a method that uses EKF to efficiently integrate data from DGPS and INS sensors (Chae, 2010). The proposed method is designed to cover the task of mobile robot position determination during the times the robot navigates in urban areas with very tall buildings in which GPS

Borenstein and Feng in another work have introduced a method called gyrodometry (Bronstein, 1996). In this method, the odometry is corrected for systematic errors using UMBmark. The approach intends also to get around non-systematic errors such as the errors generated when the robot traverses on a bump. The nonsystematic errors impact the robot for short period and is not detected by odometry.Once the robot faced a bump or another source of nonsystematic errors, odometry is replaced by gyro-based positioning system which doesn't fail in that situation and can provide a reliable information as the robot passes the bump. In practice both sets of information are available all the time and it is assumed that two sets of data, as long as there is no bump or object ahead of the robot, return almost the same information. In fact the procedure starts when odometry data differs substantially from gyro data. At the end of the time period robot traversing on a bump, the odometry is set by gyro positioning system. Afterwards, the odometry is the main positioning system and uses the correct new

signals are inaccessible.

starting point. The gyro output drifts over time and this was compensated using the method proposed by Barshan and White (Barshan, 1993, 1994, 1995). Using this method both types of odometry errors are compensated and as result more reliable position and orientation are calculated. This approach is simple and doesn't need any sensor integration. While the method is reliable for the conditions it was tested for, it does not include the situation where odometry fails due to other nonsystematic errors such as moving on an uneven surface or in presence of wheel slippage.

Using ultrasonic beacons is another option to improve odometry which is proposed by Kleeman (Kleeman, 1992). In this approach active ultrasonic beacons are used to locate the robot at any given time. The information from beacons was used as a reliable position to reset the odometry. It is well known that ultrasonic measurements have random errors which don't accumulate over time but in contrast are not smooth and require to be compensated. In this method, an ultrasonic localizer is used which has six ultrasonic beacons and a transmitter controller which sequences the firing of the beacons in a cyclic manner. Beacons have 150 milliseconds intervals to have the previous pulse reverberation settled. Beacon 1 is distinguished by transmitting two bursts 3 milliseconds apart. The robot has an onboard receiver which is composed of eight ultrasonic receiver arranged at 45 degrees intervals. In modeling the positioning system, in addition to position of robot in both x- and y-directions, velocity of the robot, beacon cycle time, speed of sound and beacon firing time are introduced as the states of the system. An Iterative Extended Kalman Filter (IEKF) is utilized to integrate two sets of data from the beacons and from odometry. This method provides reliable information about the robot position in a constructed environment. However, it has several important drawbacks which make it unsuitable in many situations. For example, there is a problem with having delayed time arrival of an ultrasonic beacon due to an indirect path incorporating reflection off obstacles, walls, ceiling or floor. These echoed arrival time should be identified and must not be taken into account in estimation otherwise IEKF result in erroneous state estimation. In addition, one could easily point out that this method is appropriate only for indoor applications when ultrasonic beacons can be installed.

Barshan and Whyte have developed an Inertial Navigation system to navigate outdoor mobile robots (Barshan, 1993, 1994, 1995). In this system, three solid states gyroscope, a triaxial accelerometer and two Electrolevel tilt sensor are used. One of the important results of this work was the development of a method to model drift error of inertial sensors as an exponential function with time. Modeling the drift error for each sensor was done by leaving the sensor on a surface motionless. The sensor's readings were recorded until it was stabled. The sensor's readings are then modeled as an exponential function. The proposed Navigation system has been tested on radar-equipped land vehicles. The orientation from inertial sensors is reliable for 10 minutes. However, in the presence of electromagnetic fields information regarding the heading is valid only for 10 seconds. Although, using the inertial sensors reliably (after the drift errors were modeled) for 10 minutes is an improvement for vehicle position determination, it is not good enough when compared with systems which integrate two or more sensors. It is assumed that in all cases the errors associated with inertial sensor have been modeled and were taken in to account before the position and orientation of the vehicle was calculated.

Gyro's scale factor is provided by manufacturer to convert gyro output (digital or analog signal) to degrees per second. The scale factor is not a fixed and varies slightly with gyro's temperature, direction of rotation and rate of rotation. Therefore, modeling the variation of

Mobile Robot Position Determination 75

*<sup>o</sup> R k* <sup>ψ</sup> denotes the rotational transformation matrix from the body coordinate frame to the

cos ( ) sin ( ) ( ) sin ( ) cos ( ) *<sup>o</sup> o o o o*

*k k* <sup>ψ</sup> <sup>⎡</sup> ψ −ψ <sup>⎤</sup> <sup>=</sup> <sup>⎢</sup> <sup>⎥</sup> <sup>⎣</sup> ψ ψ <sup>⎦</sup>

where ( ) *<sup>o</sup>* ψ *k* is heading of mobile robot based on odometry. The velocity component ( ) *<sup>b</sup>*

<sup>1</sup> ( ) ( ( ) ( )) <sup>2</sup>

where ( ) *er v k* is the measured translational velocity of robot's right wheel and ( ) *el v k* is the left wheel's translational velocity. Figure 1 shows the geometric relations between robot's

() () () *<sup>o</sup>*

*<sup>x</sup>* − *)k(pw*

*)1k(pb <sup>x</sup>* −

*)k(pb y*

<sup>1</sup> ( ) tan ( ( ) / ( )) *w w*

*w b v k R kv k* = <sup>ψ</sup> (4)

*<sup>x</sup> )1k(pw*

*o yx k vk vk* <sup>−</sup> ψ = (5)

is assumed to be zero because of forward motion of the mobile robot and ( ) *<sup>b</sup>*

*b*

position and orientation in three consequent samples.

Fig. 1. Position determination based on dead reckoning.

*)0(pb x*

*)1k(pb <sup>y</sup>* −

*)1k(pw*

The heading angle ( ) *<sup>o</sup>* ψ *k* is calculated by

*)0(pb y*

*)1k(pw <sup>y</sup>* −

*)k(pw y*

*)1k(pw <sup>y</sup>* +

*y* 

*R k*

*x y vk vk vk* = , is the robot's velocity in the body coordinate frame, and

*k k*

*<sup>x</sup> er el vk v k vk* = + (3)

*)k(pb x*

> ψ*)k(*

ψ− *)1k(*

(2)

*<sup>y</sup> v k*

*<sup>x</sup> v k* is calculated

*x* 

*<sup>x</sup>* +

where ( ) [ ( ) ( )] *<sup>b</sup> b bT*

world coordinate frame defined by

( )

by

scale factor is critical to accurate calculation of gyro's angular information. Borenstein has used odometry and a gyroscope which was corrected for its scale factor variation along with its drift error (Borenstein, 1998). In addition, an Indirect Kalman Filter is used to model all errors corresponding with this method.

Roumeliotis and Bekey have utilized multiple sensors to navigate a mobile robot with two drive wheel and one cast (Roumeliotis, 2000). In this work one sensor (a potentiometer) was installed on rear wheel, to measure robot's steering angle. Additional sensors were: 1) two encoders on each wheel, 2) one single axis gyroscope, and 3) one sun sensor. A sun sensor is capable of measuring of the robot's absolute orientation based on sun position. Such sensor could be an effective alternative in applications such as Mars explorations where there is no access to GPS signals or strong magnetic field. It is indicated that sun sensor data should be used as often as one fifth of other sensors to achieve better estimation. Also this research shows that, by excluding robot's angular velocity and acceleration and translational acceleration into the estimation system state (in simulations), the system is very sensitive to changes in orientation caused by external sources such as slippage. Therefore to reduce this effect, robot angular velocity and angular acceleration as well as the robot translational acceleration are included into the estimation system's state.

Amarasinghe and co-workers have reported a method to integrate information from a laser range finder and a laser camera to navigate a mobile robot (Amarasinghe, 2010). In this technique, a camera is used to detect landmarks and the position of the landmark is measured by the laser range finder using laser camera. The information from two sensors is integrated in a Simultaneous Localization and Mapping (SLAM) platform supported by an Extended Kalman Filter (EKF). While the main advantage of this work is using appropriate sensors for detecting the landmarks and calculating the robot's position, it provides unreliable information in an indoor setting with no landmarks or in an unfavorable lighting condition such as a smoke-filled environment.

#### **2.2.1 An example of using multiple sensors for position determination**

In this section an example of position determination using sensor integration is discussed. The mobile considered is a wheeled robot with two driving wheels in the front and one dummy wheel in the back. The mobile robot position and orientation is represented by vector ( ) [ ( ), ( )]*<sup>T</sup> P kT p kT kT s ss* = ψ at time step *<sup>s</sup> kT* , where *k* is a positive integer and *Ts* is the sampling period. For simplicity, *Ts* will be dropped from notation after this point and the mobile robot is assumed to be traveling on a flat surface. The vector ( ) [ ( ), ( )]*<sup>T</sup> P k* = ψ *p k k* specifies the Cartesian coordinates of the robot, and the ψ( ) *k* defines the orientation (heading) of the robot. The position and orientation vector *P k*( )is updated in each sampling period during the robot's motion. Two coordinate frames are assigned as shown in figure 1. Body coordinate frame which is attached to the robot and moves with the robot. World coordinate frame, which sometimes is called navigation coordinate frame, is fixed at a reference point and the robot position is measured with respect to this frame. The superscript 'O' is used to denote the information obtained from odometry. The robot position ( ) *<sup>o</sup> p k* is determined incrementally by

$$\underline{\boldsymbol{p}}^{o}(\boldsymbol{k}+\mathbf{1}) = \underline{\boldsymbol{\sigma}}^{o}(\boldsymbol{k}) + R\_{\boldsymbol{\varphi}o}(\boldsymbol{k})\underline{\boldsymbol{\Sigma}}^{b}(\boldsymbol{k})T\_{s} \tag{1}$$

74 Recent Advances in Mobile Robotics

scale factor is critical to accurate calculation of gyro's angular information. Borenstein has used odometry and a gyroscope which was corrected for its scale factor variation along with its drift error (Borenstein, 1998). In addition, an Indirect Kalman Filter is used to model all

Roumeliotis and Bekey have utilized multiple sensors to navigate a mobile robot with two drive wheel and one cast (Roumeliotis, 2000). In this work one sensor (a potentiometer) was installed on rear wheel, to measure robot's steering angle. Additional sensors were: 1) two encoders on each wheel, 2) one single axis gyroscope, and 3) one sun sensor. A sun sensor is capable of measuring of the robot's absolute orientation based on sun position. Such sensor could be an effective alternative in applications such as Mars explorations where there is no access to GPS signals or strong magnetic field. It is indicated that sun sensor data should be used as often as one fifth of other sensors to achieve better estimation. Also this research shows that, by excluding robot's angular velocity and acceleration and translational acceleration into the estimation system state (in simulations), the system is very sensitive to changes in orientation caused by external sources such as slippage. Therefore to reduce this effect, robot angular velocity and angular acceleration as well as the robot translational acceleration are included into the

Amarasinghe and co-workers have reported a method to integrate information from a laser range finder and a laser camera to navigate a mobile robot (Amarasinghe, 2010). In this technique, a camera is used to detect landmarks and the position of the landmark is measured by the laser range finder using laser camera. The information from two sensors is integrated in a Simultaneous Localization and Mapping (SLAM) platform supported by an Extended Kalman Filter (EKF). While the main advantage of this work is using appropriate sensors for detecting the landmarks and calculating the robot's position, it provides unreliable information in an indoor setting with no landmarks or in an unfavorable lighting

In this section an example of position determination using sensor integration is discussed. The mobile considered is a wheeled robot with two driving wheels in the front and one dummy wheel in the back. The mobile robot position and orientation is represented by vector ( ) [ ( ), ( )]*<sup>T</sup> P kT p kT kT s ss* = ψ at time step *<sup>s</sup> kT* , where *k* is a positive integer and *Ts* is the sampling period. For simplicity, *Ts* will be dropped from notation after this point and the mobile robot is assumed to be traveling on a flat surface. The vector ( ) [ ( ), ( )]*<sup>T</sup> P k* = ψ *p k k* specifies the Cartesian coordinates of the robot, and the ψ( ) *k* defines the orientation (heading) of the robot. The position and orientation vector *P k*( )is updated in each sampling period during the robot's motion. Two coordinate frames are assigned as shown in figure 1. Body coordinate frame which is attached to the robot and moves with the robot. World coordinate frame, which sometimes is called navigation coordinate frame, is fixed at a reference point and the robot position is measured with respect to this frame. The superscript 'O' is used to denote the information obtained from odometry. The robot

( 1) ( ) ( ) ( ) *oo b*

*o s p k* += + *p k R kv kT* <sup>ψ</sup> (1)

**2.2.1 An example of using multiple sensors for position determination** 

errors corresponding with this method.

estimation system's state.

position ( ) *<sup>o</sup>*

condition such as a smoke-filled environment.

*p k* is determined incrementally by

where ( ) [ ( ) ( )] *<sup>b</sup> b bT x y vk vk vk* = , is the robot's velocity in the body coordinate frame, and ( ) *<sup>o</sup> R k* <sup>ψ</sup> denotes the rotational transformation matrix from the body coordinate frame to the world coordinate frame defined by

$$R\_{\psi\_o}(k) = \begin{bmatrix} \cos \psi\_o(k) & -\sin \psi\_o(k) \\ \sin \psi\_o(k) & \cos \psi\_o(k) \end{bmatrix} \tag{2}$$

where ( ) *<sup>o</sup>* ψ *k* is heading of mobile robot based on odometry. The velocity component ( ) *<sup>b</sup> <sup>y</sup> v k* is assumed to be zero because of forward motion of the mobile robot and ( ) *<sup>b</sup> <sup>x</sup> v k* is calculated by

$$
\upsilon\_x^b(k) = \frac{1}{2} (\upsilon\_{er}(k) + \upsilon\_{el}(k)) \tag{3}
$$

where ( ) *er v k* is the measured translational velocity of robot's right wheel and ( ) *el v k* is the left wheel's translational velocity. Figure 1 shows the geometric relations between robot's position and orientation in three consequent samples.

Fig. 1. Position determination based on dead reckoning.

The heading angle ( ) *<sup>o</sup>* ψ *k* is calculated by

$$
\underline{\underline{v}}^{w}(k) = R\_{\underline{\psi}\_o}(k)\underline{\underline{v}}^b(k) \tag{4}
$$

$$\Psi\_o(k) = \tan^{-1} \left( v\_y^w(k) / v\_x^w(k) \right) \tag{5}$$

Mobile Robot Position Determination 77

A nonlinear parametric model for bias error was fitted to the data from gyroscope using

1 2 ( ) (1 ) *t T et C e C <sup>m</sup>*

1 2 <sup>1</sup> *m m* ( ) ( ) *C C e t e t T T*

1 2 *m m* ( 1) ( ) ( ) *s s*

The best fitting parameter value to experimental data obtained from gyroscope with zero

If we assume that the original position is known, the next position of the robot in the world frame can be determined using equation (1) by replacing the robot's heading with the

> ( 1) ( ) cos ( ) sin ( ) ( ) ( 1) ( 1) ( ) sin ( ) cos ( ) ( ) *G w G w a a b*

*p k T*

*G w G w s a a b y y G G y*

*pk pk k k v k*

*pk pk k k v k* ⎡ ⎤⎡ ⎤ + ⎡ ⎤ ψ −ψ ⎡ ⎤ += = + ⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥ <sup>+</sup> ψ ψ ⎢ ⎥ ⎣ ⎦⎣ ⎦ ⎣ ⎦⎣ ⎦

*G w x x G G x*

*T T ek ek C C TT TT* += + + + +

with initial conditions *em*(0) 0 = and 1 *e CT <sup>m</sup>*(0) / = . After discretizing (10) becomes:

input for 14 hours sampled every second are *C1=0.06441, T=30.65 s,* and *C2=0.06332*.

/

<sup>−</sup> =− + (9)

(11)

(12)

<sup>+</sup> = + (10)

*p k* in world coordinate frame based on gyro's readings is

least square fit method:

information calculated from the gyro. The mobile robot position ( ) *<sup>G</sup>*

Fig. 3. Unscented Transformation (Wan, 2000).

estimated by

where ( ) [ ( ) ( )] *w w wT x y vk vk vk* = is the translation velocity of mobile robot in world coordinate frame. The orientation based on odometry is updated by

$$
\Psi \psi\_o(k+1) = \psi\_o(k) + \dot{\Psi}\_o(k) T\_s \tag{6}
$$

where the rate of change of orientation ( ) *<sup>o</sup>* ψ *k* is calculated by

$$
\psi\_o(k) = \frac{\upsilon\_{er}(k) - \upsilon\_{el}(k)}{b} \tag{7}
$$

where *b* is the robot's wheelbase.

To alleviate the errors from odometry, the information from second sensor is integrated with odometry. Since based on the equations (1) and (4), the robot's heading plays an important role in position determination, a single axis gyroscope is used to determine the heading of the robot. The gyro's drift error is modeled using the technique introduced by Barshan (Barshan, 1993-1995). Figure 2 shows the gyro's output when the gyro examined on a flat and stationary surface for 14 hours. The result suggests that the gyro output increases exponentially when it was stationary during the test. Gyro's output consists of gyro accurate angle rate, the modeled error and a white noise:

$$
\dot{\boldsymbol{\psi}}\_{G}(k) = \dot{\boldsymbol{\psi}}\_{G}^{a}(k) + \boldsymbol{e}\_{m}(k) + \eta\_{G}(k) \tag{8}
$$

where ( ) *<sup>a</sup> <sup>G</sup>* ψ *k* is actual heading change rate of mobile robot based on gyroscope reading, ( ) *<sup>G</sup>* ψ *k* is heading change rate of mobile robot based on gyroscope reading, *e k <sup>m</sup>*( ) is the gyroscope bias error, and ( ) *<sup>G</sup>*η *k* is the associate white noise.

Fig. 2. The gyroscope's output for 14 hours.

76 Recent Advances in Mobile Robotics

() () ( ) *er el <sup>o</sup> vk vk <sup>k</sup> b*

To alleviate the errors from odometry, the information from second sensor is integrated with odometry. Since based on the equations (1) and (4), the robot's heading plays an important role in position determination, a single axis gyroscope is used to determine the heading of the robot. The gyro's drift error is modeled using the technique introduced by Barshan (Barshan, 1993-1995). Figure 2 shows the gyro's output when the gyro examined on a flat and stationary surface for 14 hours. The result suggests that the gyro output increases exponentially when it was stationary during the test. Gyro's output consists of gyro accurate

() () () () *<sup>a</sup>*

*<sup>G</sup>* ψ *k* is actual heading change rate of mobile robot based on gyroscope reading, ( ) *<sup>G</sup>* ψ *k* is heading change rate of mobile robot based on gyroscope reading, *e k <sup>m</sup>*( ) is the

coordinate frame. The orientation based on odometry is updated by

where the rate of change of orientation ( ) *<sup>o</sup>* ψ *k* is calculated by

*x y vk vk vk* = is the translation velocity of mobile robot in world

( 1) ( ) ( ) *o o os* ψ + =ψ +ψ *k k kT* (6)

<sup>−</sup> ψ = (7)

*G Gm G* ψ =ψ + +η *k k ek k* (8)

where ( ) [ ( ) ( )] *w w wT*

where *b* is the robot's wheelbase.

where ( ) *<sup>a</sup>*

angle rate, the modeled error and a white noise:

Fig. 2. The gyroscope's output for 14 hours.

gyroscope bias error, and ( ) *<sup>G</sup>*η *k* is the associate white noise.

A nonlinear parametric model for bias error was fitted to the data from gyroscope using least square fit method:

$$e\_m(t) = \mathbb{C}\_1(1 - e^{-t/T}) + \mathbb{C}\_2\tag{9}$$

$$
\dot{e}\_m(t) = \frac{C\_1 + C\_2}{T} + \frac{1}{T} e\_m(t) \tag{10}
$$

with initial conditions *em*(0) 0 = and 1 *e CT <sup>m</sup>*(0) / = . After discretizing (10) becomes:

$$e\_m(k+1) = \frac{T}{T+T\_s} e\_m(k) + \frac{T}{T+T\_s} (\mathbf{C}\_1 + \mathbf{C}\_2) \tag{11}$$

The best fitting parameter value to experimental data obtained from gyroscope with zero input for 14 hours sampled every second are *C1=0.06441, T=30.65 s,* and *C2=0.06332*.

If we assume that the original position is known, the next position of the robot in the world frame can be determined using equation (1) by replacing the robot's heading with the information calculated from the gyro.

The mobile robot position ( ) *<sup>G</sup> p k* in world coordinate frame based on gyro's readings is estimated by

$$\underline{\boldsymbol{p}}^{G}\underline{\boldsymbol{p}}^{w}(k+1) = \begin{bmatrix} \,^{G}p\_{x}^{w}(k+1) \\ \,^{G}p\_{y}^{w}(k+1) \end{bmatrix} = \begin{bmatrix} \,^{G}p\_{x}^{w}(k) \\ \,^{G}p\_{y}^{w}(k) \end{bmatrix} + \begin{bmatrix} \cos\psi\_{G}^{a}(k) & -\sin\psi\_{G}^{a}(k) \\ \sin\psi\_{G}^{w}(k) & \cos\psi\_{G}^{a}(k) \end{bmatrix} \begin{bmatrix} \boldsymbol{v}\_{x}^{b}(k) \\ \boldsymbol{v}\_{y}^{b}(k) \end{bmatrix} \tag{12}$$

Fig. 3. Unscented Transformation (Wan, 2000).

Mobile Robot Position Determination 79

Therefore, it is possible to estimate the statistics of a nonlinear function without taking derivatives from the function as it is needed in the case of EKF. This makes implementation much easier than EKF which needs linearizing of the nonlinear function around the operating points and calculation of the Jacobian matrices. The parameter provides another degree of freedom to eliminate the estimation error for higher order statistics. It can be positive or negative but choosing a negative number returns a non-positive semi-definite

κ

Julier and Uhlmann (Julier, 1995, 1996) were first to introduce the UT as a technique for the estimation of nonlinear systems. Their work was based on the intuition that, with a fixed number of parameters it should be easier to approximate a Gaussian distribution than it is to approximate an arbitrary nonlinear function or transformation. This could be easily drawn from the way the sigma points of the UT are being calculated. There are few versions for the UKF which they differ from each other based on whether to include the random noise into

Houshangi and Azizi have used the UKF to successfully integrate information from odometry and a single axis gyroscope (Houshangi, 2005, 2006; Azizi, 2004). The mobile robot used in their work is a Pioneer 2-Dxe from ActivMedia Robotics Corporation. Figure 4 illustrates the robot and the single axis gyroscope used as the second sensor for position determination. The robot is a two-wheel drive and incremental encoders are installed on each wheel. These wheels have pneumatic rubber tires which give better mobility but potentially are additional source of error for positioning based on odometry such as inequality of wheel diameters which inescapable. One rear caster is used to stabilize the

The difference between robot position and orientation comes from odometry and robot

() () () *Gw Ow*

() () () *Gw Ow*

() () () *<sup>a</sup>*

*xx x ek P k P k* = − (21-1)

*yy y ek P k P k* = − (21-2)

*G o ek k k* <sup>ψ</sup> =ψ −ψ (21-3)

estimation for *Pyy* (Wan, 2000).

Fig. 4. The mobile robot with gyroscope.

the calculations.

robot's motion and standing.

position and orientation are calculated by

The next step is to effectively integrate the information from odometry and gyroscope so that the odometry errors can be accurately estimated. As shown, the Kalman filter gives optimal state estimation to a linear system (Kalman, 1982). The EKF is an extension of the Kalman filter for nonlinear functions. The EKF is based on linearizing the nonlinear functions and applying Kalman filter for the estimation. Due to nonlinear nature of errors associated with odometry and other sensors, many studies have used Extended Kalman filter to integrate two or more sets of data for more accurate position determination. However, since the level of nonlinearity for odometry's nonsystematic error is high, the EKF may not be the best choice. Unscented Kalman filter (UKF) is another extension for Kalman filter which can provide accurate estimation for systems with high nonlinearity such as our system.

An important aspect of the UKF is that it can be applied to nonlinear functions without a need for linearizing (Julier, 1995, 1996). It is much simpler than EKF to implement because it doesn't require the calculation of Jacobian matrix which may provide some difficulties in the implementation.

The UKF is based on Unscented Transform (UT) that transforms the domain of the nonlinear function to another space of sigma points that has the same statistics as the original points (Julier, 2000). The UT is capable of estimating the statistics of a random variable defined by a nonlinear function (Julier, 2000). Figure 3 shows the general idea of the UT (Wan, 2000). In this transformation, a set of points called sigma point, are chosen such that it has mean of *x* and covariance of *Px* .The first two moments of *x* can be calculated by choosing *2n+1* sample points (also called sigma points) as follow

$$
\chi\_0(k|k) = \overline{\chi} \tag{13}
$$

$$\chi\_i(k \vert k) = \overline{\mathfrak{x}} + (\sqrt{(n+\kappa)P\_{\ge}})\_i \qquad i = 1, \ldots, n \tag{14}$$

$$\chi\_i(k \, | k) = \overline{\mathfrak{x}} - (\sqrt{(n+\kappa)P\_x})\_i \qquad i = n+1, \ldots, 2n \tag{15}$$

$$\mathcal{W}\_0 = \mathbf{x}/(n+\kappa)\tag{16}$$

$$\mathcal{W}\_{i} = \mathbf{1} \{ \{ \mathbf{2}(n + \kappa) \} \qquad i = 1, \ldots, 2m \tag{17}$$

where κ is scaling factor, (( ) ) *n P* + κ *x i* is the i-th row or column of matrix square root of ( ) *n P* + κ *<sup>x</sup>* and *Wi* s are the weight associated with *i*–th sigma point. It is known that

$$\sum\_{i=0}^{2n} \mathcal{W}\_i = 1 \tag{18}$$

The sigma point are propagated through the nonlinear function of *f* ( ) *x* as

$$Y\_i = f(\chi\_i(k \, |k\,)) \qquad i = 0, \dots, 2n \tag{19}$$

Mean of output of function *f* ( ) *x* can be determined as

$$
\overline{y} = f(Y\_i) \tag{20}
$$

78 Recent Advances in Mobile Robotics

The next step is to effectively integrate the information from odometry and gyroscope so that the odometry errors can be accurately estimated. As shown, the Kalman filter gives optimal state estimation to a linear system (Kalman, 1982). The EKF is an extension of the Kalman filter for nonlinear functions. The EKF is based on linearizing the nonlinear functions and applying Kalman filter for the estimation. Due to nonlinear nature of errors associated with odometry and other sensors, many studies have used Extended Kalman filter to integrate two or more sets of data for more accurate position determination. However, since the level of nonlinearity for odometry's nonsystematic error is high, the EKF may not be the best choice. Unscented Kalman filter (UKF) is another extension for Kalman filter which can provide accurate estimation for systems with high nonlinearity such as our

An important aspect of the UKF is that it can be applied to nonlinear functions without a need for linearizing (Julier, 1995, 1996). It is much simpler than EKF to implement because it doesn't require the calculation of Jacobian matrix which may provide some difficulties in the

The UKF is based on Unscented Transform (UT) that transforms the domain of the nonlinear function to another space of sigma points that has the same statistics as the original points (Julier, 2000). The UT is capable of estimating the statistics of a random variable defined by a nonlinear function (Julier, 2000). Figure 3 shows the general idea of the UT (Wan, 2000). In this transformation, a set of points called sigma point, are chosen such that it has mean of *x* and covariance of *Px* .The first two moments of *x* can be calculated by choosing *2n+1* sample

where κ is scaling factor, (( ) ) *n P* + κ *x i* is the i-th row or column of matrix square root of

1

( ) *n P* + κ *<sup>x</sup>* and *Wi* s are the weight associated with *i*–th sigma point. It is known that 2

0

*i W* =

The sigma point are propagated through the nonlinear function of *f* ( ) *x* as

Mean of output of function *f* ( ) *x* can be determined as

*n i*

<sup>0</sup> <sup>χ</sup> ( ) *kk x* <sup>=</sup> (13)

<sup>0</sup> *W n* = κ +κ ( ) (16)

∑ <sup>=</sup> (18)

( )*<sup>i</sup> y f* = *Y* (20)

( ) (( ) ) *<sup>i</sup> x i* χ = + +κ *kk x n P i n* <sup>=</sup> 1,.., (14)

( ) (( ) ) *<sup>i</sup> x i* χ = − +κ *kk x n P in n* <sup>=</sup> <sup>+</sup> 1,..,2 (15)

*W n <sup>i</sup>* = 1 2( ) { + κ } *i n* = 1,..,2 (17)

( ( )) *Yi i* = χ *<sup>f</sup> k k i n* <sup>=</sup> 0,..,2 (19)

system.

implementation.

points (also called sigma points) as follow

Therefore, it is possible to estimate the statistics of a nonlinear function without taking derivatives from the function as it is needed in the case of EKF. This makes implementation much easier than EKF which needs linearizing of the nonlinear function around the operating points and calculation of the Jacobian matrices. The parameter provides another degree of freedom to eliminate the estimation error for higher order statistics. It can be positive or negative but choosing a negative number returns a non-positive semi-definite estimation for *Pyy* (Wan, 2000). κ

Fig. 4. The mobile robot with gyroscope.

Julier and Uhlmann (Julier, 1995, 1996) were first to introduce the UT as a technique for the estimation of nonlinear systems. Their work was based on the intuition that, with a fixed number of parameters it should be easier to approximate a Gaussian distribution than it is to approximate an arbitrary nonlinear function or transformation. This could be easily drawn from the way the sigma points of the UT are being calculated. There are few versions for the UKF which they differ from each other based on whether to include the random noise into the calculations.

Houshangi and Azizi have used the UKF to successfully integrate information from odometry and a single axis gyroscope (Houshangi, 2005, 2006; Azizi, 2004). The mobile robot used in their work is a Pioneer 2-Dxe from ActivMedia Robotics Corporation. Figure 4 illustrates the robot and the single axis gyroscope used as the second sensor for position determination. The robot is a two-wheel drive and incremental encoders are installed on each wheel. These wheels have pneumatic rubber tires which give better mobility but potentially are additional source of error for positioning based on odometry such as inequality of wheel diameters which inescapable. One rear caster is used to stabilize the robot's motion and standing.

The difference between robot position and orientation comes from odometry and robot position and orientation are calculated by

$$e\_x(k) = \,^G P\_x^w(k) - \,^G P\_x^w(k) \tag{21-1}$$

$$e\_y(k) = \,^G P\_y^w(k) - \,^G P\_y^w(k) \tag{21-2}$$

$$e\_{\Psi}(k) = \psi\_{G}^{a}(k) - \psi\_{o}(k)\tag{21-3}$$

Mobile Robot Position Determination 81

but with many issues described in this chapter. The odometry's errors can be estimated using benchmark techniques and utilizing multiple sensors. Kalman Filtering is the most popular technique to integrate information from multiple sensors. However, most of physical and engineering phenomena are nonlinear and it is necessary to modify Kalman filter to estimate nonlinear systems. The EKF is able to deal with nonlinear systems and is used for mobile robot position determination. As was discussed, the odometry errors have high nonlinearity in nature. The newer extension of Kalman Filter using a transform called Unscented Transform provides another alternative for sensor integration. An example was provided using UKF to integrate gyro data and odometry. The implementation results indicated utilizing multiple sensors using UKF provided more accurate position even as

Borenstein, J. & Feng. L. (1996) Measurement and Correction of Systematic Odometry Errors

Borenstein, J., Everett, H.R., Feng, L., & Wehe, D. (1997) Mobile Robot Positioning: Sensors

Borenstein, J. & Feng. L. (1995) UMBmark: A Benchmark Test for Measuring Odometry

Chong, K.S. & Kleeman, L. (1997) Accurate Odometry and Error Modeling for a Mobile

Antonelli, G, Chiaverini, S. & Fusco, G. (2005) A calibration method for odometry of mobile

Larsen, T. D., Bak, M., Andersen, N. A. & Ravn, O. (1998) Location estimation for an

Martinelli, R. & Siegwart, R. (2003) Estimating the Odometry Error of a Mobile Robot During Navigation *European Conference on Mobile Robots*, Poland (2003). Park, K. C., Chung, H., Choi, J., and Lee, J.G. (1997) Dead Reckoning Navigation for an

Chae, H., Christiand, Choi, S., Yu W & Cho, J. (2010) Autonomous Navigation of Mobile

Bronstein, J. & Feng, L. (1996) Gyrodometry: A New Method for Combining Data from

Mobile Robots. pp. 231-249, Vol. 14, No. 4, (April 1997).

Albuquerque, New Mexico, pp. 2783-2788, (April 1997)

*IEEE Transactions on Robotics*, vol. 21, pp. 994–1004, (Oct. 2005).

in Mobile Robots. *IEEE Journal of Robotics and Automation*, pp. 869-880, Vol 12, No 6,

and Techniques. *Invited paper for the* Journal of Robotic Systems, Special Issue on

Errors in Mobile Robots. *Proceedings of SPIE Conference on Mobile Robots*,

Robot. *Proceeding of the, IEEE Intl. Conference on Robotics and Automation*,

robots based on the least-squares technique: theory and experimental validation.

autonomously guided vehicle using an augmented Kalman filter to autocalibrate the odometry. *First International Conference on Multisource-Multisensor Information* 

Autonomous Mobile Robot Using a Differential Encoder and Gyroscope", Proc. of the 8th Intl. Conference on Advanced Robotics, Monterey, CA, July 1997, 441-446 Cui, Y.J. & Ge, S.S. (2001) Autonomous vehicle positioning with GPS in urban canyon

environments. *Proc. of IEEE International Conference on Robotics and Automation*, pp.

Robot based on DGPS/INS Sensor Fusion by EKF in Semi-outdoor Structured Environment. *Proceedings of the IEEE/RSJ International Conference on Intelligent Robots* 

Gyros and Odometry in Mobile robots. *Proceeding of the 1996 IEEE International* 

compared to EKF.

**4. References** 

(December 1996).

Philadelphia, (October, 1995).

*Fusion (FUSION'98)*, (1998).

1105-1110, Seoul, Korea (Feb. 2001)

*and Systems (IROS)*, pp. 1222-1227 (Oct. 2010)

Each one of these errors has a modeled part and an associate noise as shown in equations (20):

$$e\_{\mathbf{x}}(k) = e\_{mx}(k) + \eta\_{\mathbf{x}}(k) \tag{22-1}$$

$$e\_y(k) = e\_{my}(k) + \eta\_y(k)\tag{22.2}$$

$$e\_{\psi}(k) = e\_{m\psi}(k) + \eta\_{\psi}(k)\tag{22-3}$$

Fig. 5. Robot's position along the x-axis.

Three independent UKF described in equations (11-18) are applied to each one of the errors defined in equation (22). The estimated errors are included when an accurate position and orientation for the robot were calculated in the next sampling interval. The results were compared to the results of applying EKF to equation (22).

To evaluate the approach an experiment was designed so that the robot was run over a 3 meter square for eleven runs. The final position of the robot was calculated with respect to the actual final position. As shown in Figure 5, the UKF returns more accurate results compared to the EKF and better than using odometry or gyroscope alone. This is partly due the nature of odometry errors which are nonlinear and can be estimated more effectively using UKF rather than EKF.

#### **3. Conclusions**

Mobile robot position determination is vital for the effectiveness of robot's operation regardless of the task. Odometry is the fundamental technique for position determination 80 Recent Advances in Mobile Robotics

Each one of these errors has a modeled part and an associate noise as shown in equations

Three independent UKF described in equations (11-18) are applied to each one of the errors defined in equation (22). The estimated errors are included when an accurate position and orientation for the robot were calculated in the next sampling interval. The results were

To evaluate the approach an experiment was designed so that the robot was run over a 3 meter square for eleven runs. The final position of the robot was calculated with respect to the actual final position. As shown in Figure 5, the UKF returns more accurate results compared to the EKF and better than using odometry or gyroscope alone. This is partly due the nature of odometry errors which are nonlinear and can be estimated more effectively

Mobile robot position determination is vital for the effectiveness of robot's operation regardless of the task. Odometry is the fundamental technique for position determination

() () () *x mx x ek e k k* = + η (22-1)

() () () *y my y ek e k k* = + η (22-2)

*ek e k k* ψ ψψ () () () = *<sup>m</sup>* + η (22-3)

(20):

Fig. 5. Robot's position along the x-axis.

using UKF rather than EKF.

**3. Conclusions** 

compared to the results of applying EKF to equation (22).

but with many issues described in this chapter. The odometry's errors can be estimated using benchmark techniques and utilizing multiple sensors. Kalman Filtering is the most popular technique to integrate information from multiple sensors. However, most of physical and engineering phenomena are nonlinear and it is necessary to modify Kalman filter to estimate nonlinear systems. The EKF is able to deal with nonlinear systems and is used for mobile robot position determination. As was discussed, the odometry errors have high nonlinearity in nature. The newer extension of Kalman Filter using a transform called Unscented Transform provides another alternative for sensor integration. An example was provided using UKF to integrate gyro data and odometry. The implementation results indicated utilizing multiple sensors using UKF provided more accurate position even as compared to EKF.

#### **4. References**


**5** 

**Vision Based Obstacle Avoidance Techniques** 

Vision is one of the most powerful and popular sensing method used for autonomous navigation. Compared with other on-board sensing techniques, vision based approaches to navigation continue to demand a lot of attention from the mobile robot research community. This is largely due to its ability to provide detailed information about the environment, which may not be available using combinations of other types of sensors. One of the key research problems in mobile robot navigation is the focus on obstacle avoidance methods. In order to cope this problem, most autonomous navigation systems rely on range data for obstacle detection. Ultrasonic sensors, laser rangefinders and stereo vision techniques are widely used for estimating the range data. However all of these have drawbacks. Ultrasonic sensors suffer from poor angular resolution. Laser range finders and stereo vision systems are quite expensive, and computational complexity of the stereo vision systems is another key challenge (Saitoh et al., 2009). In addition to their individual shortcomings, Range sensors are also unable to distinguish between different types of ground surfaces, such as they are not capable of differentiating between the sidewalk pavement and adjacent flat grassy areas. The computational complexity of the avoidance algorithms and the cost of the sensors are the most critical aspects for real time applications. Monocular vision based systems avoid these problems and are able to provide appropriate solution to the obstacle avoidance problem. There are two fundamental groups of vision based obstacle avoidance techniques; those that compute the apparent motion, and those that rely on the appearance of individual pixels for monocular vision based obstacle avoidance systems. First group is called as Optical flow based techniques, and the main idea behind this technique is to control the robot using optical flow, from which heading of the observer and time-to-contact values are obtained (Guzel & Bicker, 2010). One way of the control using these values is by acting to achieve a certain type of flow. For instance, to maintain ambient orientation, the type of Optic flow required is no flow at all. If some flow is detected, then the robot should change the forces produced by its effectors so as to minimize this flow, based on Law of

A second group is called Appearance Based methods rely on basic image processing techniques, and consist of detecting pixels different in appearance than that of the ground and classifying them as obstacles. The algorithm performs in real-time, provides a highresolution obstacle image, and operates in a variety of environments (DeSouza & Kak, 2002). The main advantages of these two conventional methods are their ease of implementation and high availability for real time applications. However optical flow *based* methods suffer from two major problems, which are the illumination problem that varies with time and the

**1. Introduction** 

Control (Contreras, 2007).

Mehmet Serdar Guzel and Robert Bicker

*Newcastle University, United Kingdom* 

*Conference on Robotics and Automation*, pp. 423–428, Minneapolis, Minnesota, (April 1996)


### **Vision Based Obstacle Avoidance Techniques**

Mehmet Serdar Guzel and Robert Bicker

*Newcastle University, United Kingdom* 

#### **1. Introduction**

82 Recent Advances in Mobile Robotics

Kleeman, L. (1992) Optimal estimation of position and heading for mobile robots using

Barshan, B., and Durrant–Whyte, H. F. (1993) An Inertial Navigation System for a for Mobile

Barshan, B., and Durrant–Whyte, H. F. (1994) Evaluation of a Solid – State Gyroscope for

Barshan, B. & Durrant-Whyte, H. F. (1995)Inertial Navigation Systems for Mobile Robots.

Borenstein, J. (1998) Experimental Evaluation of the Fiber Optics Gyroscope for Improving

Kalman, R. E. (1982) A New Approach to Linear Filtering and Prediction Problems. *Transaction of the ASME – Journal of Basic Engineering*, *(Series D)*, pp. 35 – 45. Julier, S. J. & Uhlmann, J. K. &Durrant (1995) Whyte A New Approach for Filtering

Julier, S. J. & Uhlmann, J. K. (1996) A New Method for Approximating Nonlinear

Julier, S. J. & Uhlmann, J. K. (1997) A New Extension of the Kalman Filter to Nonlinear

Wan, E.A. & van der Merwe, R. (2000) The Unscented Kalman Filter for Nonlinear

Houshangi, N. & Azizi, F. (2005) Accurate mobile robot position determination using

Houshangi, N. & Azizi, F. (2006) Mobile Robot Position Determination Using Data

*Communication and Control*, Lake Louise, Alberta, Canada, (Oct. 2000) Azizi, F. & Houshangi, N. (2004) Mobile robot position determination using data from gyro

*Department of Engineering Science*, University of Oxford, 1996

*Automation*, Nice, France, pp 2582-2587, (May 1992)

*Systems*, pp. 2243 – 2247, Yokohama, Japan, (July 1993)

pp. 61 – 67, Vol. 44, No. 1, (February 1994)

1996)

1995)

volume 28.

(May 2004).

(May 2005)

Seattle, Washington, 1995.

*Conference*, Vol. 6, pp. 4555–4559, (2002)

*Conference on Robotics and Automation*, pp. 423–428, Minneapolis, Minnesota, (April

ultrasonic beacons and dead-reckoning. *IEEE International Conference on Robotics and* 

Robot. *Proceeding of 1993 IEEE/RSJ International Conference on Intelligent Robots and* 

Mobile Robots Application *IEEE Transactions on Instrumentation and Measurement*,

*IEEE Transactions on Robotics and Automation*, pp. 328 – 342, Vol. 11, No. 3, (June

Dead-reckoning Accuracy in Mobile Robots *Proceeding Of the IEEE International Conference on Robotics and Automation*, 3456 – 3461, Leuven, Belgium, (May 1998) Roumeliotis, S.I. & Bekey, G.A. (2000) Bayesian Estimation and Kalman Filtering: A unified

Framework for Mobile Robot Localization. *Proceedings of IEEE International Conference on Robotics and Automation*, pp. 2985-2992, San Fransisco, (April 2000). Amarasinghe, D., Mann, G.K.I. & Gosine, R.G. (2010) Landmark detection and localization

for mobile robot applications: a multisensor approach. *Robotica,* pp. 663–673 (2010)

Nonlinear Systems. *Proceeding of the American Control Conference*, pp. 1628 – 1632,

Transformation of Probability Distributions. *Tech. Report, Robotic Research Group,* 

Systems. Proceeding of Aerospace: *The 11th International Symposium on Aerospace/ Defense Sensing, Simulation and Controls*, pp. 182 – 193, Orlando, Florida, (2000) Julier, S. J. (2002) The scaled unscented transformation. *Proceedings of the American Control* 

Estimation. *Proceedings of Symposium on Adaptive Systems for Signal Processing,* 

and odometry. *Canadian Conference on Electrical Engineering*, Vol. 2, pp.719 – 722,

unscented Kalman filter. *Canadian Conference on Electrical Engineering*, pp. 846 – 851,

Integration of Odometry and Gyroscope. *World Automation Congress*. (2006), pp. 1–8

Vision is one of the most powerful and popular sensing method used for autonomous navigation. Compared with other on-board sensing techniques, vision based approaches to navigation continue to demand a lot of attention from the mobile robot research community. This is largely due to its ability to provide detailed information about the environment, which may not be available using combinations of other types of sensors. One of the key research problems in mobile robot navigation is the focus on obstacle avoidance methods. In order to cope this problem, most autonomous navigation systems rely on range data for obstacle detection. Ultrasonic sensors, laser rangefinders and stereo vision techniques are widely used for estimating the range data. However all of these have drawbacks. Ultrasonic sensors suffer from poor angular resolution. Laser range finders and stereo vision systems are quite expensive, and computational complexity of the stereo vision systems is another key challenge (Saitoh et al., 2009). In addition to their individual shortcomings, Range sensors are also unable to distinguish between different types of ground surfaces, such as they are not capable of differentiating between the sidewalk pavement and adjacent flat grassy areas. The computational complexity of the avoidance algorithms and the cost of the sensors are the most critical aspects for real time applications. Monocular vision based systems avoid these problems and are able to provide appropriate solution to the obstacle avoidance problem. There are two fundamental groups of vision based obstacle avoidance techniques; those that compute the apparent motion, and those that rely on the appearance of individual pixels for monocular vision based obstacle avoidance systems. First group is called as Optical flow based techniques, and the main idea behind this technique is to control the robot using optical flow, from which heading of the observer and time-to-contact values are obtained (Guzel & Bicker, 2010). One way of the control using these values is by acting to achieve a certain type of flow. For instance, to maintain ambient orientation, the type of Optic flow required is no flow at all. If some flow is detected, then the robot should change the forces produced by its effectors so as to minimize this flow, based on Law of Control (Contreras, 2007).

A second group is called Appearance Based methods rely on basic image processing techniques, and consist of detecting pixels different in appearance than that of the ground and classifying them as obstacles. The algorithm performs in real-time, provides a highresolution obstacle image, and operates in a variety of environments (DeSouza & Kak, 2002). The main advantages of these two conventional methods are their ease of implementation and high availability for real time applications. However optical flow *based* methods suffer from two major problems, which are the illumination problem that varies with time and the

Vision Based Obstacle Avoidance Techniques 85

where, *Ix*, *Iy* and *It* are the partial derivatives of image brightness with respect to *x*, *y* and *t*, respectively. Having one equation in two unknowns ��, �� for each pixel is an aperture problem of the optical flow algorithms. To find the optical flow another set of equations is needed, using some additional constraint. All optical flow methods introduce additional conditions for estimating the actual flow. There are several methods employed to determine optical flow, namely: Block-based methods, differential methods, Phase Correlation and General variational methods (Atcheson et al., 2009). Differential methods are widely used for navigation tasks, and are mainly based on partial derivatives of the image signal and/or the sought flow field and higher-order partial derivatives. One of those methods is used to

Horn and Schunk proposed one of the most important optical flow methods using an gradient based approach (Horn & Schunck, 1981). According to their methodology, a regularizing term associated with smoothness is added to the general flow equation, as illustrated in Equation 3, in which neighbouring pixels have the same velocity as moving objects, so the brightness pattern of an image changes regularly. This constraint is demonstrated by minimizing the squares of gradient magnitudes. Smoothness of an optical flow area can also be calculated by determining the Laplacian of optical flow vectors speed both horizontal and vertical directions denoted by *u* and *w* respectively, illustrated in

�� � =

�� � =

��� ��� <sup>+</sup> ��� ���

��� ��� <sup>+</sup> ��� 

given by the following expressions that includes � as the regularization parameter, controlling the association between the detail and the smoothness. High values of � makes

� + ����

Horn and Schunk, can be used as the main reference to understand and solve the given error function, from which a pair of equations for each point can be obtained. Direct solution of these equations such as Gaus-Jordan Elimination (Bogacki, 2005) would be very costly. Instead, an iterative Gauss Seidel, approach is used to reduce the cost and obtain the flow

� �; vn+1 = v¯n - �

where Ix Iy and It are the partial derivatives with respect to x,y and t respectively, and the superscript *n+1* denotes the next iteration, which is to be calculated and *n* is the last

��v¯ � ���v¯ � ���

> ����� ����

estimate flow vectors to steer the robots.

following expressions:

�� �� + ��

�� �� + ��

vectors, as follows (Horn & Schunck, 1981):

calculated result (Horn & Schunck, 1981).

un+1 = u¯n - �

the smoothness constraint dominate and leads to a smoother flow

��u¯ � ���u¯ � ���

> ����� ����

∬���

Where *Ei*= ��

**2.1.1 The optical flow method proposed by Horn and Schunk** 

���+���+�� = 0 (3)

��� (4)

�� ���� (5)

� � (6)

�� and *Es* <sup>=</sup>∇� � + ∇� �. The aim is to minimize the total error

problem of motion discontinuities induced by objects moving with respect to other objects or the background (Contreras, 2007). Various integrated methods for solving these problems have been proposed; nevertheless it is still a key challenge to employ optical flow for mobile robot navigation. Furthermore, appearance based methods also suffer from illumination problems and, are highly sensitive to floor stains, as well as to the physical structure of the terrain.

Consequently, while having significant performance advantages*,* there are certain drawbacks which restrict the applicability of these methods. In order to solve those challenges*,* a novel obstacle avoidance method is introduced in this chapter. The method is principally designed to fuse a Scale invariant features transform (SIFT) algorithm (Lowe, 1999), and template matching with a convolution mask technique, using a *Fuzzy Logic approach*. As opposed to the *Appearance based methods*, previously mentioned, an occupancy map of the environment is generated with respect to the local features and a template. The experimental results reveal that the proposed obstacle avoidance technique allows the robot to move efficiently within its environment and to successfully attain its local goals.

This chapter is organized as follows. In Section 2, the background knowledge to the conventional methods is briefly introduced. In Section 3, new technique is introduced. Section 4 provides the implementation of the behaviour-based robot and the experiment results from both the real and simulation experiments. Section 5 provides a summary of the work.

#### **2. Background**

In this section, optical flow based navigation techniques will first be outlined, followed by a brief introduction of the appearance based methods.

#### **2.1 Optical flow**

Optical flow, illustrated in Fig. 1, is an approximation to the motion field, summarizing the temporal change in an image sequence**.** The main idea behind the technique assumes that for a given scene point, the corresponding image point intensity *I* remain constant over time, which is referred as *conservation of image intensity* (Atcheson et al., 2009). Therefore, if two consecutive images have been obtained at the following time intervals, the basic idea is to detect the motion using image differencing. If any scene point projects onto image point (�� �) at time *t* and onto image point (� � ��� � � ��) at time (� � ��), the following equation is inferred based on the conservative of image intensity assumption.

$$I(\mathbf{x}, \mathbf{y}, \mathbf{t}) = I(\mathbf{x} + \delta \mathbf{x}, \mathbf{y} + \delta \mathbf{y}, \mathbf{t} + \delta \mathbf{t}) \tag{1}$$

Expanding the right-hand side of the Eq. 1 using a Taylor series about (�� �� �), and ignoring the higher order terms then by rearrangement gives the following expression.

$$
\delta \mathfrak{x} \frac{\partial \mathfrak{l}}{\partial \mathfrak{x}} + \delta \mathfrak{y} \frac{\partial \mathfrak{l}}{\partial \mathfrak{y}} + \delta \mathfrak{t} \frac{\partial \mathfrak{l}}{\partial \mathfrak{t}} = \mathbf{0} \tag{2}
$$

A simpler expression, is obtained by dividing by �� throughout and movement along the horizontal (�� �� ), and vertical (�� �� ) directions are *u* and *v* respectively. Having these rearrangements and denoting partial derivatives of *I* by *Ix*, *Iy* and *It* gives the differential flow equation shown in following expressions:

84 Recent Advances in Mobile Robotics

problem of motion discontinuities induced by objects moving with respect to other objects or the background (Contreras, 2007). Various integrated methods for solving these problems have been proposed; nevertheless it is still a key challenge to employ optical flow for mobile robot navigation. Furthermore, appearance based methods also suffer from illumination problems and, are highly sensitive to floor stains, as well as to the physical structure of the

Consequently, while having significant performance advantages*,* there are certain drawbacks which restrict the applicability of these methods. In order to solve those challenges*,* a novel obstacle avoidance method is introduced in this chapter. The method is principally designed to fuse a Scale invariant features transform (SIFT) algorithm (Lowe, 1999), and template matching with a convolution mask technique, using a *Fuzzy Logic approach*. As opposed to the *Appearance based methods*, previously mentioned, an occupancy map of the environment is generated with respect to the local features and a template. The experimental results reveal that the proposed obstacle avoidance technique allows the robot

This chapter is organized as follows. In Section 2, the background knowledge to the conventional methods is briefly introduced. In Section 3, new technique is introduced. Section 4 provides the implementation of the behaviour-based robot and the experiment results from both the real and simulation experiments. Section 5 provides a summary of the

In this section, optical flow based navigation techniques will first be outlined, followed by a

Optical flow, illustrated in Fig. 1, is an approximation to the motion field, summarizing the temporal change in an image sequence**.** The main idea behind the technique assumes that for a given scene point, the corresponding image point intensity *I* remain constant over time, which is referred as *conservation of image intensity* (Atcheson et al., 2009). Therefore, if two consecutive images have been obtained at the following time intervals, the basic idea is to detect the motion using image differencing. If any scene point projects onto image point (�� �) at time *t* and onto image point (� � ��� � � ��) at time (� � ��), the following equation

Expanding the right-hand side of the Eq. 1 using a Taylor series about (�� �� �), and ignoring

A simpler expression, is obtained by dividing by �� throughout and movement along the

rearrangements and denoting partial derivatives of *I* by *Ix*, *Iy* and *It* gives the differential flow

�� � �� �� 

�(�� �� �) = �(� � ��� � � ��� � � ��) (1)

�� ) directions are *u* and *v* respectively. Having these

�� = 0 (2)

to move efficiently within its environment and to successfully attain its local goals.

terrain.

work.

**2. Background** 

**2.1 Optical flow** 

horizontal (��

brief introduction of the appearance based methods.

is inferred based on the conservative of image intensity assumption.

the higher order terms then by rearrangement gives the following expression.

�� � �� ��

�� ��

�� ), and vertical (��

equation shown in following expressions:

$$I\_{\mathbf{x}}\mathbf{u} + I\_{\mathbf{y}}\mathbf{v} + I\_{\mathbf{t}} = \mathbf{0} \tag{3}$$

where, *Ix*, *Iy* and *It* are the partial derivatives of image brightness with respect to *x*, *y* and *t*, respectively. Having one equation in two unknowns ��, �� for each pixel is an aperture problem of the optical flow algorithms. To find the optical flow another set of equations is needed, using some additional constraint. All optical flow methods introduce additional conditions for estimating the actual flow. There are several methods employed to determine optical flow, namely: Block-based methods, differential methods, Phase Correlation and General variational methods (Atcheson et al., 2009). Differential methods are widely used for navigation tasks, and are mainly based on partial derivatives of the image signal and/or the sought flow field and higher-order partial derivatives. One of those methods is used to estimate flow vectors to steer the robots.

#### **2.1.1 The optical flow method proposed by Horn and Schunk**

Horn and Schunk proposed one of the most important optical flow methods using an gradient based approach (Horn & Schunck, 1981). According to their methodology, a regularizing term associated with smoothness is added to the general flow equation, as illustrated in Equation 3, in which neighbouring pixels have the same velocity as moving objects, so the brightness pattern of an image changes regularly. This constraint is demonstrated by minimizing the squares of gradient magnitudes. Smoothness of an optical flow area can also be calculated by determining the Laplacian of optical flow vectors speed both horizontal and vertical directions denoted by *u* and *w* respectively, illustrated in following expressions:

$$
\nabla^2 u = \frac{\partial^2 u}{\partial x^2} + \frac{\partial^2 u}{\partial y^2}
$$

$$
\nabla^2 \nu = \frac{\partial^2 u}{\partial x^2} + \frac{\partial^2 u}{\partial y^2} \tag{4}
$$

Where *Ei*= �� �� �� + �� �� �� + �� �� and *Es* <sup>=</sup>∇� � + ∇� �. The aim is to minimize the total error given by the following expressions that includes � as the regularization parameter, controlling the association between the detail and the smoothness. High values of � makes the smoothness constraint dominate and leads to a smoother flow

$$\iint \left(E\_l^{\ 2} + \sigma^2 E\_s^{\ 2}\right) d\mathbf{x}d\mathbf{y} \tag{5}$$

Horn and Schunk, can be used as the main reference to understand and solve the given error function, from which a pair of equations for each point can be obtained. Direct solution of these equations such as Gaus-Jordan Elimination (Bogacki, 2005) would be very costly. Instead, an iterative Gauss Seidel, approach is used to reduce the cost and obtain the flow vectors, as follows (Horn & Schunck, 1981):

$$\mathbf{U}^{n+1} - \overline{\mathbf{U}}^{n} = \left(\frac{\mathbf{l}\_{\mathbf{x}}\overline{\mathbf{U}}^{\mathbf{n}} + \mathbf{l}\_{\mathbf{y}}\overline{\mathbf{U}}^{\mathbf{n}} + \mathbf{l}\_{\mathbf{f}}}{\sigma^{2} + \mathbf{l}\_{\mathbf{x}}^{2} + \mathbf{l}\_{\mathbf{y}}^{2}}\right); \mathbf{V}^{n+1} - \overline{\mathbf{V}}^{n} = \left(\frac{\mathbf{l}\_{\mathbf{x}}\overline{\mathbf{V}}^{\mathbf{n}} + \mathbf{l}\_{\mathbf{y}}\overline{\mathbf{V}}^{\mathbf{n}} + \mathbf{l}\_{\mathbf{f}}}{\sigma^{2} + \mathbf{l}\_{\mathbf{x}}^{2} + \mathbf{l}\_{\mathbf{y}}^{2}}\right) \tag{6}$$

where Ix Iy and It are the partial derivatives with respect to x,y and t respectively, and the superscript *n+1* denotes the next iteration, which is to be calculated and *n* is the last calculated result (Horn & Schunck, 1981).

Vision Based Obstacle Avoidance Techniques 87

where *k*, is a constant, and used to convert the obtained result to an appropriate control

Appearance based methods that identify locations on the basis of sensory similarities are a promising possible solution to mobile robot navigation. The main idea behind the strategy is to head the robot towards the obstacle-free position using similarities between the template and the active images (F. Vassallo et al., 2000). The similarity between the image patterns can be obtained by using feature detectors, involving corner based detectors, region based detectors and distribution based descriptors (Alper et al., 2006). However, most of these techniques consume a lot of process on time which is not appropriate for real time systems. In order to handle this problem in mobile robot applications, algorithms are designed based on the appearance of individual pixels. The classification of the obstacles is carried out by using the pixel difference between the template and active image patterns.In principally; any pixel that differs in appearance from the ground is classified as an obstacle. However, the method requires three assumptions that are reasonable for a variety of indoor and

The first assumption is to distinguish obstacles from the ground, while the second and third assumptions are required to estimate the distances between detected obstacles and the robot. There are several models for representing colour. The main model is the RGB(Red, Green, Blue) model which is used in monitor screens and most image file formats however, colour information for RGB model is very noisy at low Intensity. The RGB format is mostly converted to a HSV (Hue, Saturation, and Value). In HSV, Hue is what humans perceive as colour, S is saturation and Value is related to brightness, (or HIS(Hue, Intensity, Saturation) model) and in HIS, H and S represents the same as parameters in HSV colour models but I is an intensity value with a range between [0,1] where 0 is black and white is 1. These colour spaces are assumed to be less sensitive to noise and lighting conditions. The flow chart of the appearance based obstacle detection systems is illustrated in Figure 2. The input image is first convolved with a smoothing filter to reduce the noise effects, and then smoothed image is converted to *HIS, HSV* or any related *colour space* with respect to the developed algorithm (Fazl-Ersi & Tsotsos, 2009). A reference area is obtained from this image which might be any shape of geometry such as trapezoidal, triangle or square, and histogram values of this reference area are generated (Saitoh et al., 2009). Finally, a comparison between the reference image and the current image is made using some predefined threshold values. For instance, assume that the bin value, Hist(*H*(*x*, *y*)), of the generated histogram and the threshold value,*TH,* are compared, where *H*(*x*, *y*) is the H value at pixel (*x*, *y*). If Hist(*H*(*x*, *y*)) > *TH* then the pixel (*x*, *y*) is classified into the safe region, or else it is classified into the obstacle region. In order to simply the problem, the results are represented in a binary image in which the safe path is represented with white but the obstacles are represented with black, as illustrated in Figure 3.However, identifying places purely on the basis of sensory similarity is too simplistic; different places may look very similar, even with a rich sensing methodology due to lighting conditions, shadows on illumination Furthermore, for dynamic environments there might be unexpected stains on the ground which may be the detected as an obstacle and leads the robot to an unsafe

parameter to steer the robot.

b. The ground must be flat.

**2.3 Appearance-based methods** 

outdoor environments which are (Saitoh et al., 2009):

c. There must be no overhanging obstacles.

a. Obstacles must be different in appearance from the ground.

path. An example with respect to this case is illustrated in Figure 4.

Fig. 1. Optical Flow vectors (Guzel & Bicker, 2010).

#### **2.2 Optical flow for mobile robot navigation**

Flow vectors are utilized to navigate autonomous systems based on the Balance Strategy (Souhila & Karim, 2007), shown in the following equation, and the depth information which is extracted from the image sequence using Focus of Expansion(FOE) and Time To Contact values(TTC) (Souhila & Karim, 2007). The fundamental idea behind the Balance strategy is that of motion parallax, when the agent is translating, closer objects give rise to faster motion across the retina than farther objects. It also takes advantage of perspective in that closer objects also take up more of the field of view, biasing the average towards their associated flow (Contreras, 2007). The agent turns away from the side of greater flow. This control law is formulated by:

$$
\Delta(F\_l - F\_r) = \left(\frac{\Sigma|\mathbf{w}\_L| - \Sigma|\mathbf{w}\_R|}{\Sigma|\mathbf{w}\_L| + \Sigma|\mathbf{w}\_R|}\right) \tag{7}
$$

where ∑|��| and ∑|��| are the sum of the magnitudes of optical flow in the visual hemi fields on both sides of the robot's body. The following expression gives the new heading angle

$$
\theta\_{new} = \langle \Delta (F\_l - F\_r) \times k \rangle \tag{8}
$$

where *k*, is a constant, and used to convert the obtained result to an appropriate control parameter to steer the robot.

#### **2.3 Appearance-based methods**

86 Recent Advances in Mobile Robotics

Flow vectors are utilized to navigate autonomous systems based on the Balance Strategy (Souhila & Karim, 2007), shown in the following equation, and the depth information which is extracted from the image sequence using Focus of Expansion(FOE) and Time To Contact values(TTC) (Souhila & Karim, 2007). The fundamental idea behind the Balance strategy is that of motion parallax, when the agent is translating, closer objects give rise to faster motion across the retina than farther objects. It also takes advantage of perspective in that closer objects also take up more of the field of view, biasing the average towards their associated flow (Contreras, 2007). The agent turns away from the side of greater flow. This

�(�� � ��) = �∑|��|� ∑|��|

where ∑|��| and ∑|��| are the sum of the magnitudes of optical flow in the visual hemi fields on both sides of the robot's body. The following expression gives the new heading angle

∑|��|� ∑|��|

���� = (�(�� � ��) � �) (8)

� (7)

Fig. 1. Optical Flow vectors (Guzel & Bicker, 2010).

**2.2 Optical flow for mobile robot navigation** 

control law is formulated by:

Appearance based methods that identify locations on the basis of sensory similarities are a promising possible solution to mobile robot navigation. The main idea behind the strategy is to head the robot towards the obstacle-free position using similarities between the template and the active images (F. Vassallo et al., 2000). The similarity between the image patterns can be obtained by using feature detectors, involving corner based detectors, region based detectors and distribution based descriptors (Alper et al., 2006). However, most of these techniques consume a lot of process on time which is not appropriate for real time systems. In order to handle this problem in mobile robot applications, algorithms are designed based on the appearance of individual pixels. The classification of the obstacles is carried out by using the pixel difference between the template and active image patterns.In principally; any pixel that differs in appearance from the ground is classified as an obstacle. However, the method requires three assumptions that are reasonable for a variety of indoor and outdoor environments which are (Saitoh et al., 2009):


The first assumption is to distinguish obstacles from the ground, while the second and third assumptions are required to estimate the distances between detected obstacles and the robot. There are several models for representing colour. The main model is the RGB(Red, Green, Blue) model which is used in monitor screens and most image file formats however, colour information for RGB model is very noisy at low Intensity. The RGB format is mostly converted to a HSV (Hue, Saturation, and Value). In HSV, Hue is what humans perceive as colour, S is saturation and Value is related to brightness, (or HIS(Hue, Intensity, Saturation) model) and in HIS, H and S represents the same as parameters in HSV colour models but I is an intensity value with a range between [0,1] where 0 is black and white is 1. These colour spaces are assumed to be less sensitive to noise and lighting conditions. The flow chart of the appearance based obstacle detection systems is illustrated in Figure 2. The input image is first convolved with a smoothing filter to reduce the noise effects, and then smoothed image is converted to *HIS, HSV* or any related *colour space* with respect to the developed algorithm (Fazl-Ersi & Tsotsos, 2009). A reference area is obtained from this image which might be any shape of geometry such as trapezoidal, triangle or square, and histogram values of this reference area are generated (Saitoh et al., 2009). Finally, a comparison between the reference image and the current image is made using some predefined threshold values. For instance, assume that the bin value, Hist(*H*(*x*, *y*)), of the generated histogram and the threshold value,*TH,* are compared, where *H*(*x*, *y*) is the H value at pixel (*x*, *y*). If Hist(*H*(*x*, *y*)) > *TH* then the pixel (*x*, *y*) is classified into the safe region, or else it is classified into the obstacle region. In order to simply the problem, the results are represented in a binary image in which the safe path is represented with white but the obstacles are represented with black, as illustrated in Figure 3.However, identifying places purely on the basis of sensory similarity is too simplistic; different places may look very similar, even with a rich sensing methodology due to lighting conditions, shadows on illumination Furthermore, for dynamic environments there might be unexpected stains on the ground which may be the detected as an obstacle and leads the robot to an unsafe path. An example with respect to this case is illustrated in Figure 4.

Vision Based Obstacle Avoidance Techniques 89

Fig. 4. Effects of lighting conditions and unexpected stains on the floor.

knowledge regarding the SIFT and Template matching will be presented.

Fig. 5. Control architecture of the obstacle avoidance system.

**3.1 Scale invariant feature transform (SIFT)** 

**3. SIFT and template matching based obstacle avoidance strategy** 

In order to cope with the drawbacks of the conventional appearance based methods, a novel feature matching based technique, comprising a Scale Invariant Feature Transform and Template matching with a convolution mask, will be discussed in this section. The detail of the control algorithms with respect to these techniques is illustrated in Figure 5. Before introducing the proposed control algorithm and fusion technique, essential background

The Scale Invariant Feature Transform formerly abbreviated as SIFT is an algorithm in computer vision to detect and describe local features in images. The algorithm was published by Lowe (Lowe, 1999a, 2004b), and since then has been accepted as one of the most powerful local feature detection technique. The most notable improvements provided by SIFT are invariance to scale and rotation, and accuracy in feature point localization and matching. The evaluations carried out proposes that SIFT-based descriptors, which are region-based, are the most robust and distinctive, and are therefore best suited for feature matching. A summary of the SIFT methodology is illustrated in Fig. 6. (Lowe, 1999a, 2004b). The initial state of this algorithm is Scale space extreme detection where the interest points, which are called key-points in the SIFT framework, are detected. For this, the image is convolved using Gaussian filters, proved the only possible scale-space kernel, at different

Fig. 2. Flow chart of the Appearance Based obstacle detection algorithm.

Fig. 3. Appearance based obstacle detection method.

88 Recent Advances in Mobile Robotics

Fig. 2. Flow chart of the Appearance Based obstacle detection algorithm.

Fig. 3. Appearance based obstacle detection method.

Fig. 4. Effects of lighting conditions and unexpected stains on the floor.

### **3. SIFT and template matching based obstacle avoidance strategy**

In order to cope with the drawbacks of the conventional appearance based methods, a novel feature matching based technique, comprising a Scale Invariant Feature Transform and Template matching with a convolution mask, will be discussed in this section. The detail of the control algorithms with respect to these techniques is illustrated in Figure 5. Before introducing the proposed control algorithm and fusion technique, essential background knowledge regarding the SIFT and Template matching will be presented.

Fig. 5. Control architecture of the obstacle avoidance system.

#### **3.1 Scale invariant feature transform (SIFT)**

The Scale Invariant Feature Transform formerly abbreviated as SIFT is an algorithm in computer vision to detect and describe local features in images. The algorithm was published by Lowe (Lowe, 1999a, 2004b), and since then has been accepted as one of the most powerful local feature detection technique. The most notable improvements provided by SIFT are invariance to scale and rotation, and accuracy in feature point localization and matching. The evaluations carried out proposes that SIFT-based descriptors, which are region-based, are the most robust and distinctive, and are therefore best suited for feature matching. A summary of the SIFT methodology is illustrated in Fig. 6. (Lowe, 1999a, 2004b). The initial state of this algorithm is Scale space extreme detection where the interest points, which are called key-points in the SIFT framework, are detected. For this, the image is convolved using Gaussian filters, proved the only possible scale-space kernel, at different

Vision Based Obstacle Avoidance Techniques 91

When DoG images have been obtained, key- points are identified as local minima/maxima of the DoG images across scales. This is done by comparing each pixel in the DoG images to its eight neighbors at the same scale and nine corresponding neighboring pixels in each of the neighboring scales. If the pixel value is the maximum or minimum among all compared

Following steps are Key-point localization and Orientation assignment (Lowe, 1999a, 2004b). After key-point orientation has been completed, each key specifies stable 2D coordinates, comprising x, y, scale and orientation. Finally, a signature, local descriptor, is computed as a set of orientation histograms on 4x4 pixel neighbourhoods. Histograms have 8 bins each, and each descriptor contains an array of 4 histograms around the key-point. This leads to a SIFT feature vector with 8x4x4 = 128 elements, illustrated in Figure 9. This

Feature vectors which extracted from the SIFT algorithm to solve common computer vision problems, comprising object detection, 3D scene modeling, recognition and tracking, robot

pixels, it is selected as a candidate keypoint', as shown in Figure 8.

vector is normalized to enhance invariance to changes in illumination.

Fig. 8. A key-point is defined as any value in the DoG.

Fig. 9. SIFT Feature Descriptor.

**3.1.1 SIFT matching** 


Fig. 6. SIFT Methodology (Lowe, 1999a, 2004b).

scales, and then the difference of successive Gaussian-blurred images are obtained, illustrated in Figure 7. The convolved images are grouped by octave which corresponds to doubling the value of standard deviation of the Gaussian distribution (�).The Convolution of the image at scale �� with a Gaussian filter is expressed as follows:

$$L(\mathfrak{x}, \mathfrak{y}, k\sigma) = \, \, G(\mathfrak{x}, \mathfrak{y}, k\sigma) \, \ast \, I(\mathfrak{x}, \mathfrak{y}) \tag{9}$$

where,

$$G(\mathbf{x}, \mathbf{y}, \sigma) = \frac{1}{(2\pi\sigma^2)} \exp^{-(\mathbf{x}^2 + \mathbf{y}^2)/2\sigma^2} \tag{10}$$

Fig. 7. Gaussian images are subtracted to produce the DoG.

90 Recent Advances in Mobile Robotics

scales, and then the difference of successive Gaussian-blurred images are obtained, illustrated in Figure 7. The convolved images are grouped by octave which corresponds to doubling the value of standard deviation of the Gaussian distribution (�).The Convolution

(����) ����(�����)����

�(�� �� ��) = ��(�� �� ��) � �(�� �) (9)

(10)

of the image at scale �� with a Gaussian filter is expressed as follows:

�(�� �� �) <sup>=</sup> �

Fig. 7. Gaussian images are subtracted to produce the DoG.

Fig. 6. SIFT Methodology (Lowe, 1999a, 2004b).

where,

When DoG images have been obtained, key- points are identified as local minima/maxima of the DoG images across scales. This is done by comparing each pixel in the DoG images to its eight neighbors at the same scale and nine corresponding neighboring pixels in each of the neighboring scales. If the pixel value is the maximum or minimum among all compared pixels, it is selected as a candidate keypoint', as shown in Figure 8.

Following steps are Key-point localization and Orientation assignment (Lowe, 1999a, 2004b). After key-point orientation has been completed, each key specifies stable 2D coordinates, comprising x, y, scale and orientation. Finally, a signature, local descriptor, is computed as a set of orientation histograms on 4x4 pixel neighbourhoods. Histograms have 8 bins each, and each descriptor contains an array of 4 histograms around the key-point. This leads to a SIFT feature vector with 8x4x4 = 128 elements, illustrated in Figure 9. This vector is normalized to enhance invariance to changes in illumination.

Fig. 8. A key-point is defined as any value in the DoG.

Fig. 9. SIFT Feature Descriptor.

#### **3.1.1 SIFT matching**

Feature vectors which extracted from the SIFT algorithm to solve common computer vision problems, comprising object detection, 3D scene modeling, recognition and tracking, robot

Vision Based Obstacle Avoidance Techniques 93

Template matching is a simple and popular technique in computer vision and image processing to find small parts of an image which match a template image. It can be used in mobile robot navigation or as a way to detect edges or objects in images; an example with respect to this technique is illustrated in Figure 11. A basic method of template matching uses a convolution mask which can be easily performed on grey images. The convolution output will be the highest at places where the image structure matches the mask structure, i.e. where large image values get multiplied by large mask values. This method is normally

For instance, the input and output images are called I(x, y) and O(x, y) respectively, where (x, y) represent the coordinates of each pixel in the images and the template is called T(xt, yt), where (xt, yt) represent the coordinates of each pixel in the template. The technique simply moves the centre of the template T(xt, yt) over each (x, y) point in the search image and calculates the sum of products between the coefficients in I(x, y) and T(xt, yt) over the whole area spanned by the template. As all possible positions of the template with respect to the Input image are considered, the position with the highest score is the best position, and which is represented in the output image. There are several techniques to handle translation problem; these include using SSD (Sum of squared differences), CC (Cross Correlation) and SAD (Sum of absolute differences) (Wen-Chia & Chin-Hsing, 2009). One of the most powerful and accurate of those is CC, which basically measures the similarity of two

��� � <sup>∑</sup> ������)× ���

��� �����)� �∑ �����) ��� � � ��� ×∑ �����) ��� � � ���

Where N is the template image size; �� and �� represents average gray level in the template and source image respectively. The goal is to find the corresponding (correlated) pixel within a certain disparity range that minimizes the associated error and maximizes the similarity. This matching process involves computation of the similarity measure for each

(11)

implemented by first picking out a part of the search image to use as a template.

**3.2 Template matching** 

Fig. 11. Template Matching.

variables and defined as follows (9):

localization and mapping. This procedure requires an appropriate and fast matching algorithm. An example with respect to the SIFT matching is illustrated in Figure 10. The main matching algorithm is able to find each key-point by identifying its nearest neighbor in the database of key-points from training images. The nearest neighbor is defined as the keypoint with minimum Euclidean distance for the invariant descriptor vector, as previously discussed.However, many features may not be matched correctly due to background clutter in natural images or may not have correct match in the training database, and hence, mismatches should be discarded in order to obtain accurate results. Global thresholding on distance to the closest feature does not perform well, as some descriptors are much more discriminative than others. Alternatively, a more effective measure is obtained by comparing the distance of the closest neighbor to that of the second-closest neighbor, which performs well. An appropriate threshold value regarding this comparison, called distance ratio, is employed to reject false matches while increasing the correct matches. This value varies from 0.1 to 0.9, depending on the application type. In this case, 0.7 is employed with respect to the experimental results, which eliminates %90 of the false matches while discarding almost %10 correct matches. In addition, to reject rest of all false matches, an essential statistical method is applied to the matching space, fundamentally using the rate between scale and orientation parameters of the feature vectors. According to this method, even though the matching is validated between any two feature vectors, if the scale or the orientation parameter rate between them is more than a threshold value, matching is discarded; this procedure performs robustly to decrease false matches over all data sets (Lowe, 1999a,2004b).

Fig. 10. SIFT matching.

#### **3.2 Template matching**

92 Recent Advances in Mobile Robotics

localization and mapping. This procedure requires an appropriate and fast matching algorithm. An example with respect to the SIFT matching is illustrated in Figure 10. The main matching algorithm is able to find each key-point by identifying its nearest neighbor in the database of key-points from training images. The nearest neighbor is defined as the keypoint with minimum Euclidean distance for the invariant descriptor vector, as previously discussed.However, many features may not be matched correctly due to background clutter in natural images or may not have correct match in the training database, and hence, mismatches should be discarded in order to obtain accurate results. Global thresholding on distance to the closest feature does not perform well, as some descriptors are much more discriminative than others. Alternatively, a more effective measure is obtained by comparing the distance of the closest neighbor to that of the second-closest neighbor, which performs well. An appropriate threshold value regarding this comparison, called distance ratio, is employed to reject false matches while increasing the correct matches. This value varies from 0.1 to 0.9, depending on the application type. In this case, 0.7 is employed with respect to the experimental results, which eliminates %90 of the false matches while discarding almost %10 correct matches. In addition, to reject rest of all false matches, an essential statistical method is applied to the matching space, fundamentally using the rate between scale and orientation parameters of the feature vectors. According to this method, even though the matching is validated between any two feature vectors, if the scale or the orientation parameter rate between them is more than a threshold value, matching is discarded; this procedure performs robustly to decrease false matches over all data sets

(Lowe, 1999a,2004b).

Fig. 10. SIFT matching.

Template matching is a simple and popular technique in computer vision and image processing to find small parts of an image which match a template image. It can be used in mobile robot navigation or as a way to detect edges or objects in images; an example with respect to this technique is illustrated in Figure 11. A basic method of template matching uses a convolution mask which can be easily performed on grey images. The convolution output will be the highest at places where the image structure matches the mask structure, i.e. where large image values get multiplied by large mask values. This method is normally implemented by first picking out a part of the search image to use as a template.

Fig. 11. Template Matching.

For instance, the input and output images are called I(x, y) and O(x, y) respectively, where (x, y) represent the coordinates of each pixel in the images and the template is called T(xt, yt), where (xt, yt) represent the coordinates of each pixel in the template. The technique simply moves the centre of the template T(xt, yt) over each (x, y) point in the search image and calculates the sum of products between the coefficients in I(x, y) and T(xt, yt) over the whole area spanned by the template. As all possible positions of the template with respect to the Input image are considered, the position with the highest score is the best position, and which is represented in the output image. There are several techniques to handle translation problem; these include using SSD (Sum of squared differences), CC (Cross Correlation) and SAD (Sum of absolute differences) (Wen-Chia & Chin-Hsing, 2009). One of the most powerful and accurate of those is CC, which basically measures the similarity of two variables and defined as follows (9):

$$Corr = \frac{\frac{\sum\_{l=0}^{N-1} (\chi\_l - \mathfrak{x}) \times (\chi\_l - \bar{\chi})}{\sqrt{\sum\_{l=0}^{N-1} (\chi\_l - \bar{\chi})^2 \times \sum\_{l=0}^{N-1} (\chi\_l - \bar{\chi})^2}} \tag{11}$$

Where N is the template image size; �� and �� represents average gray level in the template and source image respectively. The goal is to find the corresponding (correlated) pixel within a certain disparity range that minimizes the associated error and maximizes the similarity. This matching process involves computation of the similarity measure for each

Vision Based Obstacle Avoidance Techniques 95

Fig. 13. An example from the testing environment.

Fig. 14. An example from the testing environment.

disparity value, followed by an aggregation and optimization step (Zitová & Flusser, 2003). An example related to correlation based technique is illustrated in Figure 12.

Fig. 12. Correlation technique based Template Matching.

#### **3.3 Obstacle avoidance using SIFT and appearance based**

Appearance based methods have significant processing and performance advantages which make them a good alternative for vision based obstacle avoidance problems. However as mentioned previously, there are certain drawbacks, which *restricts the applicability of these methods*. In order to handle these drawbacks, the results of the conventional method is improved by using the results of *SIFT* based feature matching approach. The flowchart diagram of the proposed algorithm is illustrated in Figure 13. First the acquired image is smoothed using a Gaussian filter to eliminate the noise in the image. Then a copy of the original image is converted to PGM image format which is required to carry out the *SIFT* matching process. Both images are divided into 16 sub-images composed of 44×36 pixels. For each sub images, template matching using cross correlation and *SIFT* matching are performed against reference images, illustrating the safe route, simultaneously. The results for each segment are fused using fuzzy logic to build up a sufficiently accurate occupation map of the environment.

The robot, which employs a Subsumption Architecture (Brooks, 1986), is successfully directed along a collision-free path using this map. To evaluate the performance of the proposed algorithm, it is applied to a test case, as illustrated in Figure 14. The matching results, shown in Figure 15, indicate that both techniques generate similar results under ideal conditions, involving low illumination changes and flat ground. However, template matching may fail to provide accurate matching results against illumination problem, and non-flat surfaces. In order to handle these cases, *SIFT* matching provides a reliable matching strategy, which is able to match the extracted features, invariant to scale, orientation, affine distortion, and partially invariant to illumination changes, with high accuracy. An example illustrating this condition can be seen in Figure 16, comprising 9th and 10th sub-image sequences. Despite the illumination problem and non-flat ground, the SIFT matching performs well for these cases illustrated in Figure 17. Consequently; the results indicate that instead of using each method separately, fusion of them generates more reliable results. A fuzzy logic based approach with respect to this fusion procedure will be discussed in the following section.

94 Recent Advances in Mobile Robotics

disparity value, followed by an aggregation and optimization step (Zitová & Flusser, 2003).

Appearance based methods have significant processing and performance advantages which make them a good alternative for vision based obstacle avoidance problems. However as mentioned previously, there are certain drawbacks, which *restricts the applicability of these methods*. In order to handle these drawbacks, the results of the conventional method is improved by using the results of *SIFT* based feature matching approach. The flowchart diagram of the proposed algorithm is illustrated in Figure 13. First the acquired image is smoothed using a Gaussian filter to eliminate the noise in the image. Then a copy of the original image is converted to PGM image format which is required to carry out the *SIFT* matching process. Both images are divided into 16 sub-images composed of 44×36 pixels. For each sub images, template matching using cross correlation and *SIFT* matching are performed against reference images, illustrating the safe route, simultaneously. The results for each segment are fused using fuzzy logic to build up a sufficiently accurate occupation

The robot, which employs a Subsumption Architecture (Brooks, 1986), is successfully directed along a collision-free path using this map. To evaluate the performance of the proposed algorithm, it is applied to a test case, as illustrated in Figure 14. The matching results, shown in Figure 15, indicate that both techniques generate similar results under ideal conditions, involving low illumination changes and flat ground. However, template matching may fail to provide accurate matching results against illumination problem, and non-flat surfaces. In order to handle these cases, *SIFT* matching provides a reliable matching strategy, which is able to match the extracted features, invariant to scale, orientation, affine distortion, and partially invariant to illumination changes, with high accuracy. An example illustrating this condition can be seen in Figure 16, comprising 9th and 10th sub-image sequences. Despite the illumination problem and non-flat ground, the SIFT matching performs well for these cases illustrated in Figure 17. Consequently; the results indicate that instead of using each method separately, fusion of them generates more reliable results. A fuzzy logic based approach with respect to this fusion procedure will be discussed in the

An example related to correlation based technique is illustrated in Figure 12.

Fig. 12. Correlation technique based Template Matching.

map of the environment.

following section.

**3.3 Obstacle avoidance using SIFT and appearance based** 

Fig. 13. An example from the testing environment.

Fig. 14. An example from the testing environment.

Vision Based Obstacle Avoidance Techniques 97

Fuzzy logic (FL) is a form of many-valued logic derived from fuzzy set theory to deal with reasoning that is robust and approximate rather than brittle and exact. In contrast with twovalued Boolean logic, FL deals with degrees of membership and degrees of truth. FL uses the continuum of logical values between 0 (completely false) and 1 (completely true). FL has been utilized as a problem-solving control system by several researchers for different problems. Since, FL lends itself to implementation in systems ranging from simple, small, embedded micro-controllers to large, networked, multi-channel PC or workstation-based

FL is fundamentally easy to implement and provide faster and more consistent results than conventional control methods. In this study, a FL based control system based on the Mamdani method is designed to fuse given algorithms. The basic configuration of a fuzzylogic system is composed of three parts: *Fuzzification*, *Inference Mechanism* and *Deffuzification (Driankov, 1987)*. These will be presented and associated with the fusion problem in the

Fuzzfication comprises a scale of transformation of input data of a current process into a normalised domain. This process requires the identification of two parts: the first part defines the fuzzy variables that correspond to the system input variables. The second part is to define the fuzzy sets of the input variables and their representative membership functions including the ranges of the data. Membership function, may cross the boundary of another fuzzy membership function. Each membership function may be triangular, a trapezoidal or bell shaped, as illustrated in Figure 18. The choice of the fuzzy sets is based on expert opinion using natural language terms that describe the fuzzy values. In this study triangle and trapezoid models are utilized to design membership functions of input and output

Fuzzy logic uses intersection, union, and complement operations to represent the standard common operators of AND, OR, and NOT, respectively. The most common method used to calculate intersection and union operations are the Minimum and Maximum functions. For the fuzzy sets *M* and *N* which are subsets of the universe *X*, the following definitions are proposed to represent the AND, OR, and NOT operators, respectively (Ross & Hoboken,

(a) (b) (c)

Fig. 18. Membership function shapes, (a) Triangular, (b) Trapezoidal, (c) Gaussian.

**3.4 Fuzzy logic** 

following parts.

values.

**3.4.1 Fuzzification** 

2004) (see Figure 19).

data acquisition and control systems.

Fig. 15. Comparison of two algorithms.

(a) 9th sub image (b) 10th sub image

Fig. 16. Matching results of two algorithms.

Fig. 17. False matching generated by SIFT matching.

#### **3.4 Fuzzy logic**

96 Recent Advances in Mobile Robotics

 (a) 9th sub image (b) 10th sub image

Fig. 15. Comparison of two algorithms.

Fig. 16. Matching results of two algorithms.

Fig. 17. False matching generated by SIFT matching.

Fuzzy logic (FL) is a form of many-valued logic derived from fuzzy set theory to deal with reasoning that is robust and approximate rather than brittle and exact. In contrast with twovalued Boolean logic, FL deals with degrees of membership and degrees of truth. FL uses the continuum of logical values between 0 (completely false) and 1 (completely true). FL has been utilized as a problem-solving control system by several researchers for different problems. Since, FL lends itself to implementation in systems ranging from simple, small, embedded micro-controllers to large, networked, multi-channel PC or workstation-based data acquisition and control systems.

FL is fundamentally easy to implement and provide faster and more consistent results than conventional control methods. In this study, a FL based control system based on the Mamdani method is designed to fuse given algorithms. The basic configuration of a fuzzylogic system is composed of three parts: *Fuzzification*, *Inference Mechanism* and *Deffuzification (Driankov, 1987)*. These will be presented and associated with the fusion problem in the following parts.

#### **3.4.1 Fuzzification**

Fuzzfication comprises a scale of transformation of input data of a current process into a normalised domain. This process requires the identification of two parts: the first part defines the fuzzy variables that correspond to the system input variables. The second part is to define the fuzzy sets of the input variables and their representative membership functions including the ranges of the data. Membership function, may cross the boundary of another fuzzy membership function. Each membership function may be triangular, a trapezoidal or bell shaped, as illustrated in Figure 18. The choice of the fuzzy sets is based on expert opinion using natural language terms that describe the fuzzy values. In this study triangle and trapezoid models are utilized to design membership functions of input and output values.

Fuzzy logic uses intersection, union, and complement operations to represent the standard common operators of AND, OR, and NOT, respectively. The most common method used to calculate intersection and union operations are the Minimum and Maximum functions. For the fuzzy sets *M* and *N* which are subsets of the universe *X*, the following definitions are proposed to represent the AND, OR, and NOT operators, respectively (Ross & Hoboken, 2004) (see Figure 19).

Fig. 18. Membership function shapes, (a) Triangular, (b) Trapezoidal, (c) Gaussian.

$$\mu(\mathbf{x}) = \begin{cases} \begin{array}{ll} 0, & \mathbf{x} < a \\ \frac{\mathbf{x} - a}{b - a}, & a \le \mathbf{x} < b \end{array} \\\ \frac{c - \chi}{c - b}, & b \le \mathbf{x} \le c \\\ 0, & \chi > c \end{cases} \tag{12}$$

$$\mu(\mathbf{x}) = \begin{cases} 0, & \mathbf{x} < a \\ \frac{\mathbf{x} - a}{b - a}, & a \le \mathbf{x} < b \\ 1, & b \le \mathbf{x} \le c \\ \frac{d - \mathbf{x}}{d - c}, & c < \mathbf{x} \le d \\ 0, & \mathbf{x} > d \end{cases} \tag{13}$$

$$\mu^\* = \frac{\int \mathbf{y} \Sigma\_{n=1}^k \mu\_n(\mathbf{y}) d\_\mathbf{y}}{\int \Sigma\_{n=1}^k \mu\_n(\mathbf{y}) d\_\mathbf{y}} \tag{14}$$

Vision Based Obstacle Avoidance Techniques 101

In order to evaluate the performance of the algorithm, it is integrated using a behavioral based architecture. There are several approaches to designing a behavioural-based architecture depending on the required task. In this study, the architecture has been designed based on the subsumption architecture in which each layer or behaviour implements a particular goal of the robot and higher layers are increasingly abstract. Each layer's goal subsumes that of the underlying layer, and their interaction with each other will be illustrated by using finite state machines (FSM) which defines several states (behaviours) that represents a current situation for the robot. Certain events from the outside of the world can change the state. For instance, the robot could have a *Goto* state whereby it is moving about the environment trying to get closer to its goal. When any obstacle is detected nearby, the state may change from *Goto* to *Obstacle Avoidance*, and the avoidance algorithm will move the robot away from the obstacle. When the obstacle has been avoided, the robot state will change back to the *Goto*. The architecture, designed for this study, comprises three behaviors, namely: *Goto, Avoid\_ Obstacle* and *Finish*. *Goto* behavior steers the robot to a specific goal position*, Avoid\_Obstacle behaviour* utilizes the proposed vision based intelligent algorithm to avoid obstacles, and *Finish* behavior is merely enabled after the goal is found,

Fig. 22. Membership function, 'output' Turning rate.

**4. Evaluation and implementation of the proposed system** 

and the robot is stopped*.* FSM diagram of the system is illustrated in Figure 23.

performed and four of them will be discussed in this section.

The system was developed using a Pioneer 3-DX mobile robot, with an on-board IntelPentium 1.8 GHz (Mobile) processor, and includes 256 Mbytes of RAM memory, as shown in Figure 24 (a). The mobile robot used in this study has been developed as a part of the Intelligent Robot Swarm for Attendance, Recognition, Cleaning and Delivery (IWARD) project. An Axis-213 camera, 25 frame rate, was integrated into this system. The software architecture of the proposed system is supported by CIMG Library and Player Architecture, which are open-source software projects. All experiments were conducted in an area of the Robotics and Automation Research Laboratory of Newcastle University, which has physical dimensions of 15.60m x 17.55m, as illustrated in Figure 24 (b). The camera tilted down 30 degrees to detect the floor precisely, and a reference image was taken from this environment. To evaluate the performance of the system, several different scenarios were

current image, is illustrated in Figure 21(a). Whereas the correlation function represents the similarity between the reference and current images, as illustrated in Figure 21(b). Turning Rate is the output function which represents the angular velocity value to steer the robot whilst avoiding obstacles (see Figure 22). The next step is to design appropriate fuzzy rules depending on the detail of each Fuzzy Inference System (FIS). A set of experiments were carried out until the outputs are judged to satisfy each different situations. Table 2 displays the fuzzy rules for the given problem.


Table 1. Linguistics variables and their linguistics terms.


Table 2. Fuzzy rule-base for Turning rate (w).

Test results lead to tune the system by changing rules, adjusting the membership functions shapes of both input and outputs. Once the procedure has been run several times, a consistent system is attained. The following section will integrate the proposed algorithm to a behavioral based architecture.

Fig. 21. Membership function, (a) 'input' SIFT matching, (b) 'input' Correlation.

100 Recent Advances in Mobile Robotics

current image, is illustrated in Figure 21(a). Whereas the correlation function represents the similarity between the reference and current images, as illustrated in Figure 21(b). Turning Rate is the output function which represents the angular velocity value to steer the robot whilst avoiding obstacles (see Figure 22). The next step is to design appropriate fuzzy rules depending on the detail of each Fuzzy Inference System (FIS). A set of experiments were carried out until the outputs are judged to satisfy each different situations. Table 2 displays

Linguistic Variables Linguistic Terms

Table 1. Linguistics variables and their linguistics terms.

 **Inputs Correlation (cs)** 

Table 2. Fuzzy rule-base for Turning rate (w).

a behavioral based architecture.

SIFT Weak, Medium, Strong Correlation Weak, Medium, Strong

Turning Rate Straight, Less, Medium, Sharp

 **SIFT (sm)** Weak Medium Strong Weak Sharp Medium Less

Medium Sharp MediumStraight Strong Medium Less Straight

Test results lead to tune the system by changing rules, adjusting the membership functions shapes of both input and outputs. Once the procedure has been run several times, a consistent system is attained. The following section will integrate the proposed algorithm to

(a) (b)

Fig. 21. Membership function, (a) 'input' SIFT matching, (b) 'input' Correlation.

the fuzzy rules for the given problem.

Fig. 22. Membership function, 'output' Turning rate.

#### **4. Evaluation and implementation of the proposed system**

In order to evaluate the performance of the algorithm, it is integrated using a behavioral based architecture. There are several approaches to designing a behavioural-based architecture depending on the required task. In this study, the architecture has been designed based on the subsumption architecture in which each layer or behaviour implements a particular goal of the robot and higher layers are increasingly abstract. Each layer's goal subsumes that of the underlying layer, and their interaction with each other will be illustrated by using finite state machines (FSM) which defines several states (behaviours) that represents a current situation for the robot. Certain events from the outside of the world can change the state. For instance, the robot could have a *Goto* state whereby it is moving about the environment trying to get closer to its goal. When any obstacle is detected nearby, the state may change from *Goto* to *Obstacle Avoidance*, and the avoidance algorithm will move the robot away from the obstacle. When the obstacle has been avoided, the robot state will change back to the *Goto*. The architecture, designed for this study, comprises three behaviors, namely: *Goto, Avoid\_ Obstacle* and *Finish*. *Goto* behavior steers the robot to a specific goal position*, Avoid\_Obstacle behaviour* utilizes the proposed vision based intelligent algorithm to avoid obstacles, and *Finish* behavior is merely enabled after the goal is found, and the robot is stopped*.* FSM diagram of the system is illustrated in Figure 23.

The system was developed using a Pioneer 3-DX mobile robot, with an on-board IntelPentium 1.8 GHz (Mobile) processor, and includes 256 Mbytes of RAM memory, as shown in Figure 24 (a). The mobile robot used in this study has been developed as a part of the Intelligent Robot Swarm for Attendance, Recognition, Cleaning and Delivery (IWARD) project. An Axis-213 camera, 25 frame rate, was integrated into this system. The software architecture of the proposed system is supported by CIMG Library and Player Architecture, which are open-source software projects. All experiments were conducted in an area of the Robotics and Automation Research Laboratory of Newcastle University, which has physical dimensions of 15.60m x 17.55m, as illustrated in Figure 24 (b). The camera tilted down 30 degrees to detect the floor precisely, and a reference image was taken from this environment. To evaluate the performance of the system, several different scenarios were performed and four of them will be discussed in this section.

Vision Based Obstacle Avoidance Techniques 103

**Scenario 3:** The mobile robot navigates from 'Start Position' (-7, 0) along a forward direction, two obstacles located on both sides of its path as shown in Figure 27 (a). Performance of the image processing algorithms are illustrated in Figure 27 (b). The robot's GoTo behaviour

**Scenario 4:** The mobile robot is required to navigate from 'Start Position' (-6, 0) to the 'Goal Position' (3, 0.5) in a partially cluttered enviorement as shown in Figure 28(a). Performance of the image processing algorithms during the task is given Figure 28 (b). The robot's GoTo behaviour steers the robot to a specific position which evaluates the performance of the obstacle avoidance algortihm with a Wavefront path planning algorithm (Barraquand &

Figure 25 (a) presents the navigation results for scenario 1, in which the robot steers forward until it percives the obstacle. The robot avoids the obstacle succesfully and then keeps going forward until it detects the wall, after which it avoids the wall and continues heading forward until it encounters the door. The robot succesfully avoids the door and continues

(a)

(b)

Fig. 25. Scenario 1, (a) Estimated Trajectory, (b) Performance of the image processing

steers the robot forward direction.

Latombe, 1991).

moving.

algorithms.

Fig. 23. FSM diagram of the behavioral architecture.

Fig. 24. Test environment, (a) Pioneer 3-DX robot with Axis-213, (b) Robotics and Automation Laboratory

**Scenario 1:** The mobile robot is required to navigate from 'Start Position' (-6, 0) along a forward direction in a partially cluttered enviorement and avoids one obstacle located along its path as shown in Figure 25 (a). Performance of the image processing algorithms during the task is given Figure 25 (b). The robot's GoTo behaviour steers the robot forward direction.

**Scenario 2:** The mobile robot navigates from 'Start Position' (-6, 0) along a forward direction in a partially cluttered enviorement and has to avoid two obstacles located along its path as shown in Figure 26 (a) and evaluation of the image processing algorithms illustrated in Figure 26 (b). The robot's GoTo behaviour steers the robot forward direction.

102 Recent Advances in Mobile Robotics

(a) (b)

**Scenario 1:** The mobile robot is required to navigate from 'Start Position' (-6, 0) along a forward direction in a partially cluttered enviorement and avoids one obstacle located along its path as shown in Figure 25 (a). Performance of the image processing algorithms during the task is given Figure 25 (b). The robot's GoTo behaviour steers the robot forward

**Scenario 2:** The mobile robot navigates from 'Start Position' (-6, 0) along a forward direction in a partially cluttered enviorement and has to avoid two obstacles located along its path as shown in Figure 26 (a) and evaluation of the image processing algorithms illustrated in

Fig. 24. Test environment, (a) Pioneer 3-DX robot with Axis-213, (b) Robotics and

Figure 26 (b). The robot's GoTo behaviour steers the robot forward direction.

Fig. 23. FSM diagram of the behavioral architecture.

Automation Laboratory

direction.

**Scenario 3:** The mobile robot navigates from 'Start Position' (-7, 0) along a forward direction, two obstacles located on both sides of its path as shown in Figure 27 (a). Performance of the image processing algorithms are illustrated in Figure 27 (b). The robot's GoTo behaviour steers the robot forward direction.

**Scenario 4:** The mobile robot is required to navigate from 'Start Position' (-6, 0) to the 'Goal Position' (3, 0.5) in a partially cluttered enviorement as shown in Figure 28(a). Performance of the image processing algorithms during the task is given Figure 28 (b). The robot's GoTo behaviour steers the robot to a specific position which evaluates the performance of the obstacle avoidance algortihm with a Wavefront path planning algorithm (Barraquand & Latombe, 1991).

Figure 25 (a) presents the navigation results for scenario 1, in which the robot steers forward until it percives the obstacle. The robot avoids the obstacle succesfully and then keeps going forward until it detects the wall, after which it avoids the wall and continues heading forward until it encounters the door. The robot succesfully avoids the door and continues moving.

Fig. 25. Scenario 1, (a) Estimated Trajectory, (b) Performance of the image processing algorithms.

Vision Based Obstacle Avoidance Techniques 105

(a)

(b)

Figure 28(a) displays the test results for scenario 4. The aim of this scenario is to test the avoidance strategy with a Wavefront path planning algortihm (Barraquand & Latombe, 1991) provided by the Player Architecutre. The robot navigates towards the 'first waypoint' until it perceves the obstacle. The robot avoids the obstacle and resumes its desired path. As there are no obstacles located along the rest of its path to the goal, the robot, therefore

Fig. 27. Scenario 3, (a) Estimated trajectory, (b) Performance of the image processing

algorithms.

navigates directly to its goal.

Fig. 26. Scenario 2, (a) Estimated trajectory, (b) Performance of the image processing algorithms.

Figure 26(a) displays the test results for scenario 2. Two obstacles are located along the robot's path in the forward direction. The robot navigates until it detects the first obstacle, then it avoids the obstacle and steers forward. Having detected the wall, the robot ceases forward motion and avoids the wall, after which, it moves forward until it detects the second obstacle, which it avoids succesfully. Subsequently, the robot avoids the door and the wall respectively.It finally moves forward until it is ceased. The third simulation is given Figure 27(a) where the robot attempts to move forward while passing the gap between two obstacles. It keeps moving forward until the obstacles are detected, initially turns left, followed by a right maneuver. After the robot succesfully avoids the objects it then resumes its path and continues moving forward.

104 Recent Advances in Mobile Robotics

(a)

(b)

Figure 26(a) displays the test results for scenario 2. Two obstacles are located along the robot's path in the forward direction. The robot navigates until it detects the first obstacle, then it avoids the obstacle and steers forward. Having detected the wall, the robot ceases forward motion and avoids the wall, after which, it moves forward until it detects the second obstacle, which it avoids succesfully. Subsequently, the robot avoids the door and the wall respectively.It finally moves forward until it is ceased. The third simulation is given Figure 27(a) where the robot attempts to move forward while passing the gap between two obstacles. It keeps moving forward until the obstacles are detected, initially turns left, followed by a right maneuver. After the robot succesfully avoids the objects it then resumes

Fig. 26. Scenario 2, (a) Estimated trajectory, (b) Performance of the image processing

algorithms.

its path and continues moving forward.

Fig. 27. Scenario 3, (a) Estimated trajectory, (b) Performance of the image processing algorithms.

Figure 28(a) displays the test results for scenario 4. The aim of this scenario is to test the avoidance strategy with a Wavefront path planning algortihm (Barraquand & Latombe, 1991) provided by the Player Architecutre. The robot navigates towards the 'first waypoint' until it perceves the obstacle. The robot avoids the obstacle and resumes its desired path. As there are no obstacles located along the rest of its path to the goal, the robot, therefore navigates directly to its goal.

Vision Based Obstacle Avoidance Techniques 107

*based* methods utilize simple template matching techniques which are fast but highly sensitive to lighting conditions and structure of the terrains. In addition, *Optical flow-based* methodologies,, the pattern of apparent motion of objects in a visual scene caused by the relative motion relying on apparent motion, are also highly sensitive to lighting conditions

To overcome those problems, a new obstacle avoidance method has been proposed which is inspired from feature matching principal. *SIFT-based* descriptors outperform other local descriptors on both textured and structured scenes, with the difference in performance larger on the textured scene. Accordingly, conventional *SIFT* algorithm, which is able to perform with high accuracy and reasonable processing time, is employed to match images, and it is finally adapted to an obstacle avoidance technique. According to the techniques, reference image, presenting the free path, is matched with the current image during the navigation to estimate the steering direction. However, preliminary experimental results reveals that despite the *SIFT* algorithm performs better than conventional methods with stained environment and compensates lighting failures, they may fail or produce several mismatched features due to low resolution or vibration caused by navigation over rough terrains. Therefore instead of using each method individually, conventional template matching method and *SIFT-based* descriptor are fused by using an appropriate *Fuzzy Fusion*  algorithm which increases the accuracy and compensates the errors caused by lighting

In order to verify the performance of the proposed obstacle avoidance algorithm, Real experiments to guide a *Pioneer 3-DX mobile robot* in a partially cluttered environment are presented, Results validate that proposed method provides an alternative and robust solution for mobile robots using a single low-cost camera as the only sensor to avoid

Alper, Y., Omar, J. & Mubarak, S. (2006) Object tracking: A survey. *ACM Comput. Surv.,* 38,

Atcheson, B., Heidrich, W. & Ihrke, I. (2009) An evaluation of optical flow algorithms for background oriented schlieren imaging. *Experiments in Fluids,* 46, 467-476. BArraquand, J. & Latombe, J.-C. (1991) Robot motion planning. A distributed representation

Bogacki, P. (2005) HINGES - An illustration of Gauss-Jordan reduction. *Journal of Online* 

Brooks, R. A. (1986) ROBUST LAYERED CONTROL SYSTEM FOR A MOBILE ROBOT.

Contreras, E. B. (2007) A biologically inspired solution for an evolved simulated agent.

Desouza, G. N. & Kak, A. C. (2002) Vision for mobile robot navigation: A survey. *IEEE* 

Driankov, D. (1987) Inference with a single fuzzy conditional proposition. *Fuzzy Sets and* 

*Transactions on Pattern Analysis and Machine Intelligence,* 24, 237-267.

*Proceedings of GECCO 2007: Genetic and Evolutionary Computation Conference.*

approach. *International Journal of Robotics Research,* 10, 628-649.

*IEEE journal of robotics and automation,* RA-2, 14-23.

*Mathematics and its Applications*.

and suffer from nose.

conditions and stains.

obstacles.

**6. References** 

13.

London.

*Systems,* 24, 51-63.

Fig. 28. Scenario 4, (a) Estimated trajectory, (b) Performance of the image processing algorithms.

#### **5. Conclusion**

The aim of this research was that it should be possible to develop a robust intelligent vision based obstacle avoidance system that can be adapted to realistic navigation scenarios. Most of the previously proposed techniques suffer from many problems. One of the most popular of those is *Apperance-based* methods which basically consist of detecting pixels different in appearance than the ground and classifying them as obstacles. Conventional *Appearance-* 106 Recent Advances in Mobile Robotics

(a)

(b)

The aim of this research was that it should be possible to develop a robust intelligent vision based obstacle avoidance system that can be adapted to realistic navigation scenarios. Most of the previously proposed techniques suffer from many problems. One of the most popular of those is *Apperance-based* methods which basically consist of detecting pixels different in appearance than the ground and classifying them as obstacles. Conventional *Appearance-*

Fig. 28. Scenario 4, (a) Estimated trajectory, (b) Performance of the image processing

algorithms.

**5. Conclusion** 

*based* methods utilize simple template matching techniques which are fast but highly sensitive to lighting conditions and structure of the terrains. In addition, *Optical flow-based* methodologies,, the pattern of apparent motion of objects in a visual scene caused by the relative motion relying on apparent motion, are also highly sensitive to lighting conditions and suffer from nose.

To overcome those problems, a new obstacle avoidance method has been proposed which is inspired from feature matching principal. *SIFT-based* descriptors outperform other local descriptors on both textured and structured scenes, with the difference in performance larger on the textured scene. Accordingly, conventional *SIFT* algorithm, which is able to perform with high accuracy and reasonable processing time, is employed to match images, and it is finally adapted to an obstacle avoidance technique. According to the techniques, reference image, presenting the free path, is matched with the current image during the navigation to estimate the steering direction. However, preliminary experimental results reveals that despite the *SIFT* algorithm performs better than conventional methods with stained environment and compensates lighting failures, they may fail or produce several mismatched features due to low resolution or vibration caused by navigation over rough terrains. Therefore instead of using each method individually, conventional template matching method and *SIFT-based* descriptor are fused by using an appropriate *Fuzzy Fusion*  algorithm which increases the accuracy and compensates the errors caused by lighting conditions and stains.

In order to verify the performance of the proposed obstacle avoidance algorithm, Real experiments to guide a *Pioneer 3-DX mobile robot* in a partially cluttered environment are presented, Results validate that proposed method provides an alternative and robust solution for mobile robots using a single low-cost camera as the only sensor to avoid obstacles.

#### **6. References**


**1. Introduction**

**2. Classification of obstacles**

motion can be changed by themselves.

**2.1 Rigid obstacles**

these views on obstacles and details are discussed below.

In mobile robotics, obstacle avoidance problem has been studied as a classical issue. In practical applications, a mobile robot should move to a goal in the environment where obstacles coexist. It is necessary for a mobile robot to safely arrive at its goal without being collided with the obstacles. A variety of obstacle avoidance algorithms have been developed for a long time. These algorithms have tried to resolve the collisions with different kinds of obstacles. This chapter classifies the types of obstacles and presents characteristics of each type. We focus on the new obstacle type that has been less studied before, and apply representative avoidance strategies to solve this new type of obstacle avoidance problem. The

**Non-Rigid Obstacle Avoidance** 

**for Mobile Robots** 

*Korea Military Academy* 

*Korea* 

**6**

Junghee Park and Jeong S. Choi

This section summarizes previous points of view on obstacles, and presents a new angle on obstacles for advanced robot navigation. A robotic system often suffers from severe damage as result of a physical collision with obstacles. Thus, there is no doubt that robots should have a perfect competency to avoid all kinds of obstacles. In robotics, an obstacle is generally defined as a physical object that is in the way or that makes it hard for robot to move freely in a space, and thereby classified into *static and moving obstacles*: the former are sometimes subdivided into stationary (or fixed) and movable ones of which position can be changed by force from a robot. Also, moving robots are often separated from moving obstacles, since their

On the basis of this definition, various strategies have been presented to resolve anticipated physical collisions systematically. Since the majority of strategies focus on generating immediate reaction to environments, obstacles are considered static for an instant although they are moving in most previous studies. Recently, some studies formalized way to explicitly consider current velocity of moving obstacles for coping with real collisions caused by the static assumption. This chapter views obstacles from a different standpoint and introduces a concept of *non-rigid obstacles*, which differs from the conformable obstacle in that the concept covers various physical entities as well as the shape (or boundary) of obstacle. Fig. 1 shows

Early studies on robot motion assumed that a robot is the only moving object in the workspace and all obstacles are fixed and distributed in a workspace (Latombe, 1991). Under the

results of different strategies would be compared and evaluated in this chapter.


### **Non-Rigid Obstacle Avoidance for Mobile Robots**

Junghee Park and Jeong S. Choi *Korea Military Academy Korea* 

#### **1. Introduction**

108 Recent Advances in Mobile Robotics

F. Vassallo, R., Schneebeli, H. J. & Santos-Victor, J. (2000) Visual servoing and appearance

Fazl-Ersi, E. & Tsotsos, J. K. (2009) Region classification for robust floor detection in indoor

Guzel, M. S. & Bicker, R. (2010) Optical flow based system design for mobile robots. *2010 IEEE Conference on Robotics, Automation and Mechatronics, RAM 2010.* Singapore. Horn, B. K. P. & Schunck, B. G. (1981) Determining optical flow. *Artificial Intelligence,* 17, 185-

Lowe, D. G. (1999) Object recognition from local scale-invariant features. *Proceedings of the IEEE International Conference on Computer Vision.* Kerkyra, Greece, IEEE. Ross, T. J. & Hoboken, N. J. (2004) *Fuzzy Logic with engineering applications* Hoboken,

Saitoh, T., Tada, N. & Konishi, R. (2009) Indoor mobile robot navigation by central following

Souhila, K. & Karim, A. (2007) Optical flow based robot obstacle avoidance. *International* 

Wen-Chia, L. & Chin-Hsing, C. (2009) A Fast Template Matching Method for Rotation

*Signal Processing, 2009. IIH-MSP '09. Fifth International Conference on.*  Zitovã¡, B. & Flusser, J. (2003) Image registration methods: A survey. *Image and Vision* 

based on monocular vision. *IEEJ Transactions on Electronics, Information and Systems,*

Invariance Using Two-Stage Process. *Intelligent Information Hiding and Multimedia* 

*Artificial Intelligence and Lecture Notes in Bioinformatics).* Halifax, NS.

environments. *Lecture Notes in Computer Science (including subseries Lecture Notes in* 

for navigation. *Robotics and Autonomous Systems,* 31, 87-97.

203.

NJ:Wiley.

129, 1576-1584+18.

*Computing,* 21, 977-1000.

*Journal of Advanced Robotic Systems,* 4, 13-16.

In mobile robotics, obstacle avoidance problem has been studied as a classical issue. In practical applications, a mobile robot should move to a goal in the environment where obstacles coexist. It is necessary for a mobile robot to safely arrive at its goal without being collided with the obstacles. A variety of obstacle avoidance algorithms have been developed for a long time. These algorithms have tried to resolve the collisions with different kinds of obstacles. This chapter classifies the types of obstacles and presents characteristics of each type. We focus on the new obstacle type that has been less studied before, and apply representative avoidance strategies to solve this new type of obstacle avoidance problem. The results of different strategies would be compared and evaluated in this chapter.

#### **2. Classification of obstacles**

This section summarizes previous points of view on obstacles, and presents a new angle on obstacles for advanced robot navigation. A robotic system often suffers from severe damage as result of a physical collision with obstacles. Thus, there is no doubt that robots should have a perfect competency to avoid all kinds of obstacles. In robotics, an obstacle is generally defined as a physical object that is in the way or that makes it hard for robot to move freely in a space, and thereby classified into *static and moving obstacles*: the former are sometimes subdivided into stationary (or fixed) and movable ones of which position can be changed by force from a robot. Also, moving robots are often separated from moving obstacles, since their motion can be changed by themselves.

On the basis of this definition, various strategies have been presented to resolve anticipated physical collisions systematically. Since the majority of strategies focus on generating immediate reaction to environments, obstacles are considered static for an instant although they are moving in most previous studies. Recently, some studies formalized way to explicitly consider current velocity of moving obstacles for coping with real collisions caused by the static assumption. This chapter views obstacles from a different standpoint and introduces a concept of *non-rigid obstacles*, which differs from the conformable obstacle in that the concept covers various physical entities as well as the shape (or boundary) of obstacle. Fig. 1 shows these views on obstacles and details are discussed below.

#### **2.1 Rigid obstacles**

Early studies on robot motion assumed that a robot is the only moving object in the workspace and all obstacles are fixed and distributed in a workspace (Latombe, 1991). Under the

Fig. 2. A configuration-time space of static rigid obstacles

Fig. 3. A configuration-time space of moving rigid obstacles

2006).

because the position of the obstacle is varied with time along its path. These representations indicate that the problem of global moving obstacle avoidance is reduced to finding a 3-D geometric path obeying two conditions: safety and time efficiency. Sampling, combinatorial, and velocity-tuning methods have been introduced to solve the geometric problem (Lavalle,

Non-Rigid Obstacle Avoidance for Mobile Robots 111

Local moving obstacle avoidance is split into two groups, according to whether or not obstacles can be seen static for a short time, as mentioned above. Several recent papers argued that the problem can be made more realistic by explicitly considering the robot's velocity and acceleration, and included the constraints to problem formulation. The collision representation tools based on the consideration include the velocity obstacle (VO) (Fiorini & Shiller, 1993), inevitable collision states (ICS) (Martinez-Gomez & Fraichard, 2009) and the triangular collision object (TCO) (Choi et al., 2010). The major difference is that object shapes representing potential collisions are determined by obstacle's velocity, unlike the tools for the static obstacle avoidance problem. In the benchmarking (Martinez-Gomez & Fraichard, 2009), it was shown that the method using the VO is superior to the methods for static

Fig. 1. Classification of obstacles

assumption, obstacle avoidance is automatically solved if we solve the global path planning problem in which the robot is assumed to have complete knowledge of the environment and generates a full path from its start to goal. The famous examples include the following approaches: probabilistic road map (Kavraki et al., 1996), cell decomposition (Boissonnat & Yvinec, 1998), grid cells (Hee & Jr., 2009), and rapidly-exploring randomizing tree (LaValle, 1998). To realize the approaches, several systematic tools for path generation were developed: visibility graph, Voronoi diagram, random sampling method, gradient method, node-edge graph, and mathematical programming (Ge & McCarthy, 1990). The final objective of the approaches is to find the global optimal (or shortest) one in the path candidates given by an optimization algorithm, such as Dijkstra and A\* method.

Some studies relaxed the assumption of complete knowledge about environments to take into consideration the case that there is no device to provide the complete knowledge. Accordingly, the studies focused on following a detour (or local path) to avoid a detected static obstacle, instead of finding the shortest path to its goal. As a result, the obstacle avoidance is separated from the global path planning, which implies that it can be performed by on-line planning. The simplest strategy for the static obstacle avoidance with a limited sensing range may be the bug algorithm in which the robot is controlled to simply follow the contour of an obstacle in the robot's way (Lumelsky & Skewis, 1990). As with the bug algorithm, other avoidance strategies involving such obstacles create an immediate reaction to nearby environments for robot navigation. The popular methods include vector field histogram (Borenstein & Koren, 1991), nearness diagram (Minguez & Montano, 2004), fuzzy rules (Lee & Wang, 1994), force (or potential) fields (Wang et al., 2006), and dynamic window (Fox et al., 1997). In fact, these tools have been widely used for even the following moving obstacle avoidance problem by virtue of their simplicity in formulation and low operational speed of robots which permits the assumption that moving obstacles can be seen as static ones for a short time.

Moving obstacle avoidance differs from the static case in that an irreversible dimension - time - should be added into problem formulation. Like the static obstacle avoidance problem, the solution for the moving case can be computed globally or locally according to the assumption of the complete knowledge of environments. The configuration-time (CT) space presented in fig. 2 and 3 is a well-known tool for global problem solving. The CT space is a three-dimensional space in which a time axis is added into a xy-plane where a robot is moving. Static obstacles are represented as a straight pillar as shown in fig. 2 when they are represented with respect to CT space. On the other hands, if a moving obstacle is modeled as a circle, its representation with respect to CT space is an oblique cylinder as shown in fig. 3, 2 Mobile Robots

assumption, obstacle avoidance is automatically solved if we solve the global path planning problem in which the robot is assumed to have complete knowledge of the environment and generates a full path from its start to goal. The famous examples include the following approaches: probabilistic road map (Kavraki et al., 1996), cell decomposition (Boissonnat & Yvinec, 1998), grid cells (Hee & Jr., 2009), and rapidly-exploring randomizing tree (LaValle, 1998). To realize the approaches, several systematic tools for path generation were developed: visibility graph, Voronoi diagram, random sampling method, gradient method, node-edge graph, and mathematical programming (Ge & McCarthy, 1990). The final objective of the approaches is to find the global optimal (or shortest) one in the path candidates given by an

Some studies relaxed the assumption of complete knowledge about environments to take into consideration the case that there is no device to provide the complete knowledge. Accordingly, the studies focused on following a detour (or local path) to avoid a detected static obstacle, instead of finding the shortest path to its goal. As a result, the obstacle avoidance is separated from the global path planning, which implies that it can be performed by on-line planning. The simplest strategy for the static obstacle avoidance with a limited sensing range may be the bug algorithm in which the robot is controlled to simply follow the contour of an obstacle in the robot's way (Lumelsky & Skewis, 1990). As with the bug algorithm, other avoidance strategies involving such obstacles create an immediate reaction to nearby environments for robot navigation. The popular methods include vector field histogram (Borenstein & Koren, 1991), nearness diagram (Minguez & Montano, 2004), fuzzy rules (Lee & Wang, 1994), force (or potential) fields (Wang et al., 2006), and dynamic window (Fox et al., 1997). In fact, these tools have been widely used for even the following moving obstacle avoidance problem by virtue of their simplicity in formulation and low operational speed of robots which permits

the assumption that moving obstacles can be seen as static ones for a short time.

Moving obstacle avoidance differs from the static case in that an irreversible dimension - time - should be added into problem formulation. Like the static obstacle avoidance problem, the solution for the moving case can be computed globally or locally according to the assumption of the complete knowledge of environments. The configuration-time (CT) space presented in fig. 2 and 3 is a well-known tool for global problem solving. The CT space is a three-dimensional space in which a time axis is added into a xy-plane where a robot is moving. Static obstacles are represented as a straight pillar as shown in fig. 2 when they are represented with respect to CT space. On the other hands, if a moving obstacle is modeled as a circle, its representation with respect to CT space is an oblique cylinder as shown in fig. 3,

Fig. 1. Classification of obstacles

optimization algorithm, such as Dijkstra and A\* method.

Fig. 2. A configuration-time space of static rigid obstacles

Fig. 3. A configuration-time space of moving rigid obstacles

because the position of the obstacle is varied with time along its path. These representations indicate that the problem of global moving obstacle avoidance is reduced to finding a 3-D geometric path obeying two conditions: safety and time efficiency. Sampling, combinatorial, and velocity-tuning methods have been introduced to solve the geometric problem (Lavalle, 2006).

Local moving obstacle avoidance is split into two groups, according to whether or not obstacles can be seen static for a short time, as mentioned above. Several recent papers argued that the problem can be made more realistic by explicitly considering the robot's velocity and acceleration, and included the constraints to problem formulation. The collision representation tools based on the consideration include the velocity obstacle (VO) (Fiorini & Shiller, 1993), inevitable collision states (ICS) (Martinez-Gomez & Fraichard, 2009) and the triangular collision object (TCO) (Choi et al., 2010). The major difference is that object shapes representing potential collisions are determined by obstacle's velocity, unlike the tools for the static obstacle avoidance problem. In the benchmarking (Martinez-Gomez & Fraichard, 2009), it was shown that the method using the VO is superior to the methods for static

Fig. 4. A configuration-time space of non-rigid obstacles

**3.1 Reactive method**

environment is known in advance.

algorithms and how they can be applied to non-rigid obstacle avoidance.

algorithms to the problem of non-rigid obstacle avoidance. On a big scale, these algorithms can be classified into four categories: reactive method, grid-based method, randomization method, and path-velocity decomposed method. We will explain this classification of

Non-Rigid Obstacle Avoidance for Mobile Robots 113

Reactive methods utilize nearby environment information for robot navigation, i.e., the robot has a sensing capability with a limited coverage. In the case where the robot is sent to a unknown region for exploration or reconnaissance, it is reasonable to assume that information of the environment is not known beforehand. In addition, if the robot has no communication connection with other sensors on the environment, then it has to rely on the nearby information acquired from its own sensor. In this case, the robot should find a suitable motion by making full use of given information in order to attain two purposes: avoiding obstacles and moving to its goal. In other words, it can be widely used when the information of the environment is limited and not deterministic, whereas other methods, which will be explained in following sections, are based on the assumption that the information of

This reactive method can also be called short-term planning because it plans a few next steps of robot motion. The planner focuses on creation of immediate respondence of the robot in order to avoid obstacles and move to the goal. On a rough view, the reactive method yields instantaneous moving direction by synthesizing attractive attribute to the goal and repulsive attribute from obstacles. In non-rigid obstacle avoidance, the robot is planned to be repulsive from non-rigid obstacles. As mentioned in section 2.1, there have been many researches to find reactive respondence of the robot, such as vector field histogram(VFH) (Borenstein & Koren, 1991), force field (Wang et al., 2006), dynamic window (Fox et al., 1997), and behavior-based robotics (Arkin, 1985). In this chapter, we implemented one of representative reactive methods, vector field histogram, for comparison. It constructs a polar histogram,

obstacle avoidance because it takes into account objects' future behavior, and suggested that the future motion of moving obstacles should explicitly been deal with. So far, we have briefly reviewed conventional rigid obstacle types and avoidance strategies which are popularly used in robotics. Now turn our attention to a new type, non-rigid obstacle in the following section.

#### **2.2 Non-rigid obstacles**

In this section, we explain a new obstacle type, non-rigid obstacle. This concept emerges as a consequence of the need for non-physical collisions, such as observation. More specifically, robots only need to have considered a single physical element, obstacle's boundary, for safe navigation to date, but they now have to consider other elements, such as agent's visibility, for smart navigation. Thus, to accommodate such a need, we here define a new term, *non-rigid obstacle*, which is an abstract object of which boundary is changeable and determined by an element involving an obstacle.

We explain this concept, non-rigid obstacle, through a moving observer with omnidirectional view. In a infiltration mission, the robot should covertly trespass on the environment without being detected by sentries. As a reverse example, in a security mission, the most important thing is that robots are not observed (or detected) by the intruder until it reaches the intruder (Park et al., 2010). In this way, the robot can covertly capture or besiege the intruder who would try to run away from the robot when detecting the approaching robot. It causes that robots can closely approach the intruder without being detected, and the intruder cannot recognize himself being besieged until the robots construct a perfect siege. In these cases, the robot should avoid the intruder's field of view during the navigation, rather than its physical boundary, which is called *stealth navigation*. This signifies that the proposed concept of non-rigid obstacle could be exceptionally helpful in planning an advanced robot motion.

The visible area determined by the field is a changeable element, especially in a cluttered indoor environment. So, we can regard the area as an abstract non-rigid obstacle that robots should avoid. This kind of obstacle can be generated by any element the obstacle has, which is a remarkable extension of the conventional concept on obstacles. Fig. 4 illustrates an example of the changes of visible areas with time traveled (or z-axis) in a CT space. As shown in the figure, the shape (or boundary) of visible area is greatly varied with the observer's motion which is a set of position parameterized by time. For this reason, it is needed to develop a new strategy to avoid the highly changeable obstacle, which will be discussed in detail in Section 3. If the robot succeeds in the avoidance with a plausible strategy, it gives a completely new value to a robotic system.

#### **3. Avoidance strategies for non-rigid obstacle avoidance**

As we mentioned in section 2, researches on obstacle avoidance problem have been focused on dealing with rigid (static or moving) obstacles. On the other hand, researches on avoidance strategies of non-rigid obstacles scarcely exist except grid-based methods (Marzouqi & Jarvis, 2006; Teng et al., 1993) a few decades ago. These methods were based on the grid map which approximates the environment to spatially sampled space. In this grid map, all possible movements of the robot were taken into consideration to find the optimal(fastest) motion of the robot to the goal. This brought about high burden of computation time. In addition, the arrival time of the planned motion and the computation time highly depended on the grid size.

Since then, any other strategies have not been developed for non-rigid obstacle avoidance. Even though other methods have not been suggested, we can apply rigid obstacle avoidance 4 Mobile Robots

obstacle avoidance because it takes into account objects' future behavior, and suggested that the future motion of moving obstacles should explicitly been deal with. So far, we have briefly reviewed conventional rigid obstacle types and avoidance strategies which are popularly used in robotics. Now turn our attention to a new type, non-rigid obstacle in the following section.

In this section, we explain a new obstacle type, non-rigid obstacle. This concept emerges as a consequence of the need for non-physical collisions, such as observation. More specifically, robots only need to have considered a single physical element, obstacle's boundary, for safe navigation to date, but they now have to consider other elements, such as agent's visibility, for smart navigation. Thus, to accommodate such a need, we here define a new term, *non-rigid obstacle*, which is an abstract object of which boundary is changeable and determined by an

We explain this concept, non-rigid obstacle, through a moving observer with omnidirectional view. In a infiltration mission, the robot should covertly trespass on the environment without being detected by sentries. As a reverse example, in a security mission, the most important thing is that robots are not observed (or detected) by the intruder until it reaches the intruder (Park et al., 2010). In this way, the robot can covertly capture or besiege the intruder who would try to run away from the robot when detecting the approaching robot. It causes that robots can closely approach the intruder without being detected, and the intruder cannot recognize himself being besieged until the robots construct a perfect siege. In these cases, the robot should avoid the intruder's field of view during the navigation, rather than its physical boundary, which is called *stealth navigation*. This signifies that the proposed concept of non-rigid obstacle could be exceptionally helpful in planning an advanced robot motion. The visible area determined by the field is a changeable element, especially in a cluttered indoor environment. So, we can regard the area as an abstract non-rigid obstacle that robots should avoid. This kind of obstacle can be generated by any element the obstacle has, which is a remarkable extension of the conventional concept on obstacles. Fig. 4 illustrates an example of the changes of visible areas with time traveled (or z-axis) in a CT space. As shown in the figure, the shape (or boundary) of visible area is greatly varied with the observer's motion which is a set of position parameterized by time. For this reason, it is needed to develop a new strategy to avoid the highly changeable obstacle, which will be discussed in detail in Section 3. If the robot succeeds in the avoidance with a plausible strategy, it gives a completely

As we mentioned in section 2, researches on obstacle avoidance problem have been focused on dealing with rigid (static or moving) obstacles. On the other hand, researches on avoidance strategies of non-rigid obstacles scarcely exist except grid-based methods (Marzouqi & Jarvis, 2006; Teng et al., 1993) a few decades ago. These methods were based on the grid map which approximates the environment to spatially sampled space. In this grid map, all possible movements of the robot were taken into consideration to find the optimal(fastest) motion of the robot to the goal. This brought about high burden of computation time. In addition, the arrival time of the planned motion and the computation time highly depended on the grid

Since then, any other strategies have not been developed for non-rigid obstacle avoidance. Even though other methods have not been suggested, we can apply rigid obstacle avoidance

**2.2 Non-rigid obstacles**

element involving an obstacle.

new value to a robotic system.

size.

**3. Avoidance strategies for non-rigid obstacle avoidance**

Fig. 4. A configuration-time space of non-rigid obstacles

algorithms to the problem of non-rigid obstacle avoidance. On a big scale, these algorithms can be classified into four categories: reactive method, grid-based method, randomization method, and path-velocity decomposed method. We will explain this classification of algorithms and how they can be applied to non-rigid obstacle avoidance.

#### **3.1 Reactive method**

Reactive methods utilize nearby environment information for robot navigation, i.e., the robot has a sensing capability with a limited coverage. In the case where the robot is sent to a unknown region for exploration or reconnaissance, it is reasonable to assume that information of the environment is not known beforehand. In addition, if the robot has no communication connection with other sensors on the environment, then it has to rely on the nearby information acquired from its own sensor. In this case, the robot should find a suitable motion by making full use of given information in order to attain two purposes: avoiding obstacles and moving to its goal. In other words, it can be widely used when the information of the environment is limited and not deterministic, whereas other methods, which will be explained in following sections, are based on the assumption that the information of environment is known in advance.

This reactive method can also be called short-term planning because it plans a few next steps of robot motion. The planner focuses on creation of immediate respondence of the robot in order to avoid obstacles and move to the goal. On a rough view, the reactive method yields instantaneous moving direction by synthesizing attractive attribute to the goal and repulsive attribute from obstacles. In non-rigid obstacle avoidance, the robot is planned to be repulsive from non-rigid obstacles. As mentioned in section 2.1, there have been many researches to find reactive respondence of the robot, such as vector field histogram(VFH) (Borenstein & Koren, 1991), force field (Wang et al., 2006), dynamic window (Fox et al., 1997), and behavior-based robotics (Arkin, 1985). In this chapter, we implemented one of representative reactive methods, vector field histogram, for comparison. It constructs a polar histogram,

in order to cope with non-rigid obstacles which change its shape while time passes. The modified RRT algorithm for solving non-rigid obstacle avoidance problem is described in

Non-Rigid Obstacle Avoidance for Mobile Robots 115

4: **s***near* = (*tnear*, **q***near*) ← NEAREST\_VERTEX(**q***rand*,G);

0: **s***init* ← (*tinit*, **q***init*); 1: G.init(**s***init*); 2: while

3: **q***rand* ← RAND\_FREE\_CONF();

6: if CHECK\_SAFETY(**s***near*,**q***rand*,*vrand*) then 7: *tnew* ← NEW\_TIME(**s***near*,**q***rand*,*vrand*);

12: if CHECK\_SAFETY(**s***new*,**q***goal*,*vg*\_*rand*) then 13: *tarrival* ← NEW\_TIME(**s***new*,**q***goal*,*vg*\_*rand*);

Table 1. Construction procedure of rapidly-exploring randomizing tree G for non-rigid

In the algorithm, the tree G is constructed with randomly selected vertexes. Each vertex **s** consists of time *t* and configuration **q**, i.e., **s** = (*t*, **q**). In non-rigid obstacle avoidance problem, the time *t* when the robot passes by the configuration **q** should be coupled with **q** as a vertex. Thus, the time *t* also should be selected randomly. In this algorithm, *t* is calculated as the time at which the robot is able to arrive at **q** with the randomly selected velocity. Each randomly selected vertex becomes component of the tree G, and the tree G is continuously extended until new selected vertex is able to be linked with the goal vertex. After construction of tree G is completed, the connection from the start vertex to goal vertex is determined as the robot's

In table 1, RAND\_FREE\_CONF() returns randomly selected free configuration. NEAREST\_VERTEX(**q**,G) returns the vertex **s***<sup>r</sup>* = (*tr*, **q***r*) in G, which has the minimum distance from **q** to **q***r*. RAND\_VEL() is the function that yield randomly selected velocity *v* which should satisfy the robot's physical constraints, i.e., 0 ≤ *v* ≤ *vmax*. In this procedure, the probability distribution function *fV*(*v*) for selecting *v* was designed as follows because it is

, 0 ≤ *v* ≤ *vmax*

*fV*(*v*) = <sup>2</sup>*<sup>v</sup> v*2 *max*

5: *vrand* ← RAND\_VEL();

8: **s***new* ← (*tnew*, **q***rand*); 9: G.add\_vertex(**s***new*); 10: G.add\_edge(**s***near*,**s***new*); 11: *vg*\_*rand* ← RAND\_VEL();

14: **s***goal* ← (*tarrival*, **q***goal*); 15: G.add\_vertex(**s***goal*); 16: G.add\_edge(**s***new*,**s***goal*);

17: break; 18: end if; 19: end if; 20: end while;

beneficial for the robot to move as fast as possible.

obstacle avoidance

movement.

table 1.

which contains nearby environment information, and finds suitable detour direction towards its goal.

#### **3.2 Grid-based method: (Teng et al., 1993)**

Grid-based methods use spatially sampled environment map as mentioned earlier. This algorithm has a merit that all possible robot motion can be considered in the grid map at the expense of increasing computation time. Therefore it can guarantee optimal solution in the grid map. That is, this method can have the completeness even though it is limited to the given grid map which depends on the shape and the size of grids.

Fig. 5. Construction procedure of safely reachable region(purple) in grid-based method

Differently from other methods, this grid-based algorithm was designated for solving this non-rigid obstacle avoidance and introduced in (Teng et al., 1993). In this algorithm, the concept of the *safely reachable region* was introduced and used. It represents the region that the robot is able to arrive without being detected by the observer at each time step. The planner sequentially constructs safely reachable region of each time step until the last safely reachable region contains the final goal. Fig. 5 represents the procedure for obtaining safely reachable region from a previous one. Let the purple region of fig. 5(a) be the safely reachable region at time *tk*. This region is dilated to the reachable region from it one time step later, which is shown in fig. 5(c). This dilated region is trimmed by the visible area of the observer at time *tk*+1. Then it becomes the region that the robot can arrive without being detected by the observer at time *tk*+1, which is shown in fig. 5(e). In this way, all possible safely reachable grid points were calculated at each time, and the shortest time for arrival is yielded.

#### **3.3 Randomization method: modified RRT**

In order to avoid a high computational burden of brute-force technique, randomization methods have been developed in robot motion planning. In these methods, free configurations are randomly selected and discriminated whether they can be via configurations that the robot is able to safely pass by. These kinds of algorithms have been developed for a long time, such as probabilistic road map(RPM) (Kavraki et al., 1996) and rapidly-exploring randomizing tree(RRT) (LaValle, 1998). These randomization methods could be usefully adopted for solving non-rigid obstacle avoidance problem in order to prevent burgeoning computational burden. In this chapter, we have modified RRT algorithm 6 Mobile Robots

which contains nearby environment information, and finds suitable detour direction towards

Grid-based methods use spatially sampled environment map as mentioned earlier. This algorithm has a merit that all possible robot motion can be considered in the grid map at the expense of increasing computation time. Therefore it can guarantee optimal solution in the grid map. That is, this method can have the completeness even though it is limited to the

> (c) Reachable region at time *tk*+1, which is dilated from the safely reachable region at time *tk*

Fig. 5. Construction procedure of safely reachable region(purple) in grid-based method

grid points were calculated at each time, and the shortest time for arrival is yielded.

In order to avoid a high computational burden of brute-force technique, randomization methods have been developed in robot motion planning. In these methods, free configurations are randomly selected and discriminated whether they can be via configurations that the robot is able to safely pass by. These kinds of algorithms have been developed for a long time, such as probabilistic road map(RPM) (Kavraki et al., 1996) and rapidly-exploring randomizing tree(RRT) (LaValle, 1998). These randomization methods could be usefully adopted for solving non-rigid obstacle avoidance problem in order to prevent burgeoning computational burden. In this chapter, we have modified RRT algorithm

Differently from other methods, this grid-based algorithm was designated for solving this non-rigid obstacle avoidance and introduced in (Teng et al., 1993). In this algorithm, the concept of the *safely reachable region* was introduced and used. It represents the region that the robot is able to arrive without being detected by the observer at each time step. The planner sequentially constructs safely reachable region of each time step until the last safely reachable region contains the final goal. Fig. 5 represents the procedure for obtaining safely reachable region from a previous one. Let the purple region of fig. 5(a) be the safely reachable region at time *tk*. This region is dilated to the reachable region from it one time step later, which is shown in fig. 5(c). This dilated region is trimmed by the visible area of the observer at time *tk*+1. Then it becomes the region that the robot can arrive without being detected by the observer at time *tk*+1, which is shown in fig. 5(e). In this way, all possible safely reachable

(d) The dilated region should be trimmed in order to avoid visibility from the observer (e) Safely reachable region at time *tk*<sup>+</sup><sup>1</sup>

its goal.

(a) Safely reachable region at time *tk*

**3.2 Grid-based method: (Teng et al., 1993)**

given grid map which depends on the shape and the size of grids.

(b) Each boundary point is dilated in amount of the movable distance of the robot in one time

step

**3.3 Randomization method: modified RRT**

in order to cope with non-rigid obstacles which change its shape while time passes. The modified RRT algorithm for solving non-rigid obstacle avoidance problem is described in table 1.


Table 1. Construction procedure of rapidly-exploring randomizing tree G for non-rigid obstacle avoidance

In the algorithm, the tree G is constructed with randomly selected vertexes. Each vertex **s** consists of time *t* and configuration **q**, i.e., **s** = (*t*, **q**). In non-rigid obstacle avoidance problem, the time *t* when the robot passes by the configuration **q** should be coupled with **q** as a vertex. Thus, the time *t* also should be selected randomly. In this algorithm, *t* is calculated as the time at which the robot is able to arrive at **q** with the randomly selected velocity. Each randomly selected vertex becomes component of the tree G, and the tree G is continuously extended until new selected vertex is able to be linked with the goal vertex. After construction of tree G is completed, the connection from the start vertex to goal vertex is determined as the robot's movement.

In table 1, RAND\_FREE\_CONF() returns randomly selected free configuration. NEAREST\_VERTEX(**q**,G) returns the vertex **s***<sup>r</sup>* = (*tr*, **q***r*) in G, which has the minimum distance from **q** to **q***r*. RAND\_VEL() is the function that yield randomly selected velocity *v* which should satisfy the robot's physical constraints, i.e., 0 ≤ *v* ≤ *vmax*. In this procedure, the probability distribution function *fV*(*v*) for selecting *v* was designed as follows because it is beneficial for the robot to move as fast as possible.

$$f\_V(v) = \frac{2v}{v\_{\max}^2}, \ 0 \le v \le v\_{\max}$$

on the path that are detected by the observer, which is called detection region. We can draw the robot's trajectory on the detection map without intersecting these detection regions in order to make the robot safely move to its goal. After the detection map is constructed, the safely velocity profile can be found in a shorter time because moving direction is restricted to

Non-Rigid Obstacle Avoidance for Mobile Robots 117

The planning method of the velocity profile can be selected in a various way. (Park et al., 2010) applied the grid-based method to this velocity profile planning. The concept in (Teng et al., 1993), which constructs safely reachable region by dilation and trimming in order to find the optimal solution on the grid map, is adopted to solve the similar problem with the one-dimensional moving direction. The points on path is sampled with uniform interval and examined whether it is safely reachable by the robot for each discretized time step. This grid-based method also finds the velocity profile that has the shortest arrival time. We note that this optimality is valid only when the predetermined path could not be modified by the

The randomization method could be proposed in velocity profile planning. Without considering all possible safe movement of the robot, we can find the robot's safe trajectory by randomly selecting via points on the detection map. In this chapter, we applied the modified RRT method explained in section 3.3 t velocity profile planning. In this application, the one-dimensional value is randomly selected as via point that the robot would pass. The RRT would be constructed on the detection map without intersecting detection regions. The

The method of selecting path on the road map also could be suitably adopted. (Park et al., 2010) has selected the path that has the shortest moving distance. If the safe velocity profile did not exist on the selected path, the next shortest path was examined in consecutive order. However, it often led to a situation that not a few path had to be examined because this method completely ignored the safety condition on the path planning step. We proposed the method to select the path highly possible to have safe trajectory of the robot. We designed the cost of each link on the road map as follows. Let *Ei*,*<sup>j</sup>* be the link between node *i* and node *j* on the

*cost*(*Ei*,*j*) = *length*(*Ei*,*j*) ∗ (*detection*\_*ratio*(*Ei*,*j*)) where *detection*\_*ratio*(*Ei*,*j*) = *area o f detection region on the detection map o f Ei*,*<sup>j</sup>*

Compared to the method which considered only path distance, this cost function makes the path planner to consider not only distance but also safely condition. If we select the path that has the least cost on the road map, this path would be relatively less detected by the observer. If a safe velocity profile is not found on the selected path, then the other path which has next

We have conducted a computer simulation on the environment in fig. 7. In this simulation, the visible area of the observer is considered as a example of the non-rigid obstacle. The robot is planned to move stealthy from the observer. The observer is patrolling on the environment following a specific path with the speed of 2.0m/s. It is assumed that the robot has a maximum speed of 4.0m/s and it can change its speed and its moving direction instantaneously. We assumed that the information of the environment and the non-rigid obstacle is known to the

*area o f whole detection map o f Ei*,*<sup>j</sup>*

detection map makes it easy to discriminate whether tree link is safe or not.

one dimension.

planner.

road map.

least cost could be found and examined again.

**4. Simulation results**

CHECK\_SAFETY(**s**1,**q**2,*v*) returns whether it is possible that the robot is able to safely move from the configuration **q**<sup>1</sup> of vertex **s**<sup>1</sup> to **q**<sup>2</sup> with the velocity *v* or not. NEW\_TIME(**s**1,**q**2,*v*) returns the time *tr* taken for the robot to move from **q**<sup>1</sup> of **s**<sup>1</sup> to **q**<sup>2</sup> with the speed *v*. In this chapter, the configuration **q** of the robot is two dimensional point **p** because the robot is assumed to be holonomic and has circular shape. In this case, returned time *tr* is calculated as follows.

$$t\_r = t\_1 + \frac{||\mathbf{p}\_2 - \mathbf{p}\_1||}{v}$$

#### **3.4 Path-velocity decomposed method**

Lastly, we can consider the path-velocity decomposed(PVD) method. This concept was firstly introduced in (Lee & Lee, 1987) and has been developed for resolving collisions of multiple robots in (Akella & Hurchinson, 2002; Lavalle & Hurchinson, 1998). This method decomposes the robot motion into the path, which is a geometric specification of a curve in the configuration space, and the velocity profile, which is a series of velocities according to the time following the path. In other words, the PVD method firstly plans the path, and adjusts the velocity of the robot following this path. It reduces computation burden by not guaranteeing the safety condition when planning the path at the cost of optimality. This safety condition is considered only in planning of velocity profile. If we predefine the geometric road map (topological graph) on the environment, we can easily plan the robot's path on the road map and find the velocity profile that guarantees safely movement of the robot following the path. This method used for solving moving rigid obstacle avoidance also can be applied to non-rigid obstacle avoidance problem without difficulty.

Fig. 6. Construction of the detection map

(Park et al., 2010) has proposed the concept of detection map. The detection map is a two-dimensional(time and point on the path) map that represents whether each time and point on the path is detected by the observer or not. It depends on the path chosen on the road map. Fig. 6 shows the construction of the detection map on the predetermined path. When the detected intervals on the path are accumulated according to the time, the detection map is constructed. In the detection map, the shaped regions represents the time and points 8 Mobile Robots

CHECK\_SAFETY(**s**1,**q**2,*v*) returns whether it is possible that the robot is able to safely move from the configuration **q**<sup>1</sup> of vertex **s**<sup>1</sup> to **q**<sup>2</sup> with the velocity *v* or not. NEW\_TIME(**s**1,**q**2,*v*) returns the time *tr* taken for the robot to move from **q**<sup>1</sup> of **s**<sup>1</sup> to **q**<sup>2</sup> with the speed *v*. In this chapter, the configuration **q** of the robot is two dimensional point **p** because the robot is assumed to be holonomic and has circular shape. In this case, returned time *tr* is calculated

*tr* <sup>=</sup> *<sup>t</sup>*<sup>1</sup> <sup>+</sup> � **<sup>p</sup>**<sup>2</sup> <sup>−</sup> **<sup>p</sup>**<sup>1</sup> �

Lastly, we can consider the path-velocity decomposed(PVD) method. This concept was firstly introduced in (Lee & Lee, 1987) and has been developed for resolving collisions of multiple robots in (Akella & Hurchinson, 2002; Lavalle & Hurchinson, 1998). This method decomposes the robot motion into the path, which is a geometric specification of a curve in the configuration space, and the velocity profile, which is a series of velocities according to the time following the path. In other words, the PVD method firstly plans the path, and adjusts the velocity of the robot following this path. It reduces computation burden by not guaranteeing the safety condition when planning the path at the cost of optimality. This safety condition is considered only in planning of velocity profile. If we predefine the geometric road map (topological graph) on the environment, we can easily plan the robot's path on the road map and find the velocity profile that guarantees safely movement of the robot following the path. This method used for solving moving rigid obstacle avoidance also can be applied to

(Park et al., 2010) has proposed the concept of detection map. The detection map is a two-dimensional(time and point on the path) map that represents whether each time and point on the path is detected by the observer or not. It depends on the path chosen on the road map. Fig. 6 shows the construction of the detection map on the predetermined path. When the detected intervals on the path are accumulated according to the time, the detection map is constructed. In the detection map, the shaped regions represents the time and points

*v*

as follows.

**3.4 Path-velocity decomposed method**

non-rigid obstacle avoidance problem without difficulty.

Fig. 6. Construction of the detection map

on the path that are detected by the observer, which is called detection region. We can draw the robot's trajectory on the detection map without intersecting these detection regions in order to make the robot safely move to its goal. After the detection map is constructed, the safely velocity profile can be found in a shorter time because moving direction is restricted to one dimension.

The planning method of the velocity profile can be selected in a various way. (Park et al., 2010) applied the grid-based method to this velocity profile planning. The concept in (Teng et al., 1993), which constructs safely reachable region by dilation and trimming in order to find the optimal solution on the grid map, is adopted to solve the similar problem with the one-dimensional moving direction. The points on path is sampled with uniform interval and examined whether it is safely reachable by the robot for each discretized time step. This grid-based method also finds the velocity profile that has the shortest arrival time. We note that this optimality is valid only when the predetermined path could not be modified by the planner.

The randomization method could be proposed in velocity profile planning. Without considering all possible safe movement of the robot, we can find the robot's safe trajectory by randomly selecting via points on the detection map. In this chapter, we applied the modified RRT method explained in section 3.3 t velocity profile planning. In this application, the one-dimensional value is randomly selected as via point that the robot would pass. The RRT would be constructed on the detection map without intersecting detection regions. The detection map makes it easy to discriminate whether tree link is safe or not.

The method of selecting path on the road map also could be suitably adopted. (Park et al., 2010) has selected the path that has the shortest moving distance. If the safe velocity profile did not exist on the selected path, the next shortest path was examined in consecutive order. However, it often led to a situation that not a few path had to be examined because this method completely ignored the safety condition on the path planning step. We proposed the method to select the path highly possible to have safe trajectory of the robot. We designed the cost of each link on the road map as follows. Let *Ei*,*<sup>j</sup>* be the link between node *i* and node *j* on the road map.

$$\text{cost}(\mathbf{E}\_{i,j}) = \text{length}(\mathbf{E}\_{i,j}) \* (\text{detection\\_ratio}(\mathbf{E}\_{i,j}))$$

$$\text{where } \text{detaction\\_ratio}(\mathbf{E}\_{i,j}) = \frac{\text{area of } \text{detaction\\_region} \text{ on the detection map of } \mathbf{E}\_{i,j}}{\text{area of whole detection map of } \mathbf{E}\_{i,j}}$$

Compared to the method which considered only path distance, this cost function makes the path planner to consider not only distance but also safely condition. If we select the path that has the least cost on the road map, this path would be relatively less detected by the observer. If a safe velocity profile is not found on the selected path, then the other path which has next least cost could be found and examined again.

#### **4. Simulation results**

We have conducted a computer simulation on the environment in fig. 7. In this simulation, the visible area of the observer is considered as a example of the non-rigid obstacle. The robot is planned to move stealthy from the observer. The observer is patrolling on the environment following a specific path with the speed of 2.0m/s. It is assumed that the robot has a maximum speed of 4.0m/s and it can change its speed and its moving direction instantaneously. We assumed that the information of the environment and the non-rigid obstacle is known to the

at time 5.7(s), nevertheless, it could not avoid the obstacle due to the lack of speed, and it collided with the obstacle at time 6.0(s). We note that this chapter assumes that non-rigid obstacle is also detected by the sensor of the robot in order to apply the reactive method for rigid obstacle avoidance to non-rigid obstacle avoidance problem. In real application, some non-rigid obstacle such as observer's field of view could not be detected by the sensor. As this result shows, the robot has a difficulty in stealth navigation when it has a limited sensing

Non-Rigid Obstacle Avoidance for Mobile Robots 119

(a) path (b) speed (c) direction

(a) (b) (c) (d)

From the results in table 2, we can find that the arrival time was the least when the grid-based method was applied. This is because the grid-based method considers all possible motions of the robot and finds the fastest motion among them, i.e., it guarantees the optimality and completeness. However, these are valid only on given grid map. That is, if the grid map is set in a different way, the robot's fastest motion and the arrival time would become different. Since it examines all grid cells whether they are possibly reached safely by the robot at each time, it needs high computational burden. In table 2, the computation time of the grid-based method is higher than that of any other methods. More detailed results are shown in fig. 10. The speed and direction of the robot were not changed extremely. Between time 6.8(s) and 9.1(s), the robot remained stationary in the shaded region until the obstacle was away from itself. The path in fig. 10(a) shows that the robot was able to almost directly traverse the environment to the goal without being detected by the observer. The performance in path

Fig. 9. The movement of observer(red) and robot(blue) in the reactive method result

capability of the environment.

Fig. 8. The reactive method (VFH) results

Fig. 7. Simulation environment (27.5m x 36.5m) and the patrol path of the observer

robot. This computer simulation is conducted on a commercial computer with 3.34GHz CPU and 3.00GB RAM.

Table 2 shows the simulation results according to algorithms from reactive method to path-velocity decomposed method. In table 2, the *exposure rate* means a ratio of the time interval that the robot is detected by the observer to the whole arrival time. In addition, we define *safety* as the distance between the robot and the point which is contained in the visible area of the observer and is the closest to the robot. When the robot is contained in the visible area of the observer, i.e., the robot is detected by the observer, the safety value is set to zero. In table 2, the average value of safety during the operation time is revealed. Also, the computation times of each method are shown in table in order to analyze practical implementation feasibility of each method.


Table 2. Simulation results

In table 2, the exposure rate had non-zero value only when the reactive method was applied. This is because the reactive method does not utilize whole information of visible areas of the observer. It assumes that the robot is able to sense only nearby information of environment and visible area. In this simulation, the robot's sensing coverage is assumed to be omnidirectional with 3.0m radius. Therefore, it is possible that the robot is placed in the situation that it is too late for avoiding the visible area that emerges suddenly near by. At the cost of this dangerous situation, the reactive method does require a negligible amount of the computation time. More detailed results are shown in fig. 8 and fig. 9. As we can see in fig. 8(b), and 8(c), the robot's speed and direction were changed extremely for a whole operation time. In fig. 9, the robot detects the non-rigid obstacle in the rear of itself 10 Mobile Robots

Fig. 7. Simulation environment (27.5m x 36.5m) and the patrol path of the observer

and 3.00GB RAM.

implementation feasibility of each method.

Table 2. Simulation results

robot. This computer simulation is conducted on a commercial computer with 3.34GHz CPU

Table 2 shows the simulation results according to algorithms from reactive method to path-velocity decomposed method. In table 2, the *exposure rate* means a ratio of the time interval that the robot is detected by the observer to the whole arrival time. In addition, we define *safety* as the distance between the robot and the point which is contained in the visible area of the observer and is the closest to the robot. When the robot is contained in the visible area of the observer, i.e., the robot is detected by the observer, the safety value is set to zero. In table 2, the average value of safety during the operation time is revealed. Also, the computation times of each method are shown in table in order to analyze practical

Arrival time 10.8s 10.4s 20.1s 16.2s 16.6s Exposure rate 3.0% 0.0% 0.0% 0.0% 0.0% Average safety 2.97m 2.88m 3.14m 3.32m 3.40m Computation 0.01s 33.67s 17.15s path planning: 3.70s time 0.01s 0.01s

In table 2, the exposure rate had non-zero value only when the reactive method was applied. This is because the reactive method does not utilize whole information of visible areas of the observer. It assumes that the robot is able to sense only nearby information of environment and visible area. In this simulation, the robot's sensing coverage is assumed to be omnidirectional with 3.0m radius. Therefore, it is possible that the robot is placed in the situation that it is too late for avoiding the visible area that emerges suddenly near by. At the cost of this dangerous situation, the reactive method does require a negligible amount of the computation time. More detailed results are shown in fig. 8 and fig. 9. As we can see in fig. 8(b), and 8(c), the robot's speed and direction were changed extremely for a whole operation time. In fig. 9, the robot detects the non-rigid obstacle in the rear of itself

Reactive Grid-based Randomization Path-velocity decomposed

(VFH) (Teng et al., 1993) (Modified RRT) (Park et al., 2010) (Modified RRT)

Grid-based Randomization

at time 5.7(s), nevertheless, it could not avoid the obstacle due to the lack of speed, and it collided with the obstacle at time 6.0(s). We note that this chapter assumes that non-rigid obstacle is also detected by the sensor of the robot in order to apply the reactive method for rigid obstacle avoidance to non-rigid obstacle avoidance problem. In real application, some non-rigid obstacle such as observer's field of view could not be detected by the sensor. As this result shows, the robot has a difficulty in stealth navigation when it has a limited sensing capability of the environment.

Fig. 8. The reactive method (VFH) results

Fig. 9. The movement of observer(red) and robot(blue) in the reactive method result

From the results in table 2, we can find that the arrival time was the least when the grid-based method was applied. This is because the grid-based method considers all possible motions of the robot and finds the fastest motion among them, i.e., it guarantees the optimality and completeness. However, these are valid only on given grid map. That is, if the grid map is set in a different way, the robot's fastest motion and the arrival time would become different. Since it examines all grid cells whether they are possibly reached safely by the robot at each time, it needs high computational burden. In table 2, the computation time of the grid-based method is higher than that of any other methods. More detailed results are shown in fig. 10. The speed and direction of the robot were not changed extremely. Between time 6.8(s) and 9.1(s), the robot remained stationary in the shaded region until the obstacle was away from itself. The path in fig. 10(a) shows that the robot was able to almost directly traverse the environment to the goal without being detected by the observer. The performance in path

(a) path (b) speed (c) direction

Non-Rigid Obstacle Avoidance for Mobile Robots 121

edges that are in the vicinity of walls, because they have relatively small detection ratios. The detection map corresponding to the selected path was constructed as shown in fig. 13(b). In the detection map, the colored regions, which are detection regions, are the set of time and point that is detected by the observer on the determined path. The trajectory of the robot following this path could be planned on the detection map without traversing detection

As shown in table 2, the arrival time of the grid-based result in PVD method was larger than that of the grid-based method. This is because the PVD method restricts the path to be selected on the predefined roadmap, not among whole possible movement. Therefore, the PVD method has the lower performance than the grid-based method even though it also finds the optimal solution on the determined path. However, its computation time is much smaller than the grid-based method because it reduces the dimension of the grid map. This planned result is shown in fig. 14(a). The movement of the observer and the robot in this result is shown in fig. 15. In fig. 15(b) and 15(c), we can see that the robot remains stationary because

(b) detection map on the selected path

Fig. 12. The randomized method (modified RRT) results

(a) road map on the environment and selected

Fig. 13. The road map and the detection map

path

regions.

length and arrival time is the best in the grid-based method. Fig. 11 shows safely reachable regions changing in accordance with the passed time.

Fig. 10. The grid-based method results(grid size is 64*cm*2/cell)

Fig. 11. Safely reachable regions of the grid-based method in fig. 10

The result of randomization method in table 2 has the lowest performance in the arrival time. This result is not deterministic and can be changed in each trial. There is possibility that relatively better performance can be yielded, but it could not be guaranteed. This method does not purpose on greatly reducing the arrival time, but it concerns a practical computation time. In table 2, modified RRT algorithm needed not a low computation time. This is because it needs relatively much time to check that the tree link candidate is safe for the movement of the robot. In order to reduce the computation time in the randomized method, it is necessary to develop a specialized randomization method to solve the non-rigid obstacle avoidance problem. Fig. 12 shows the result of modified RRT method. In fig. 12(a) and 12(c), the path of the robot was planned as zigzagged shape because via points are selected randomly. As shown in fig. 12(b), the segments between via points have respectively uniform speeds which are also selected randomly.

Lastly, we implemented the PVD method in the same environment. The road map of the environment was predefined, and the path with the least cost on the road map was selected as shown in fig. 13(a). We can see that the path with the least cost is selected as series of 12 Mobile Robots

length and arrival time is the best in the grid-based method. Fig. 11 shows safely reachable

(a) path (b) speed (c) direction

(b) safely reachable region at time t=6.0(s)

The result of randomization method in table 2 has the lowest performance in the arrival time. This result is not deterministic and can be changed in each trial. There is possibility that relatively better performance can be yielded, but it could not be guaranteed. This method does not purpose on greatly reducing the arrival time, but it concerns a practical computation time. In table 2, modified RRT algorithm needed not a low computation time. This is because it needs relatively much time to check that the tree link candidate is safe for the movement of the robot. In order to reduce the computation time in the randomized method, it is necessary to develop a specialized randomization method to solve the non-rigid obstacle avoidance problem. Fig. 12 shows the result of modified RRT method. In fig. 12(a) and 12(c), the path of the robot was planned as zigzagged shape because via points are selected randomly. As shown in fig. 12(b), the segments between via points have respectively uniform speeds

Lastly, we implemented the PVD method in the same environment. The road map of the environment was predefined, and the path with the least cost on the road map was selected as shown in fig. 13(a). We can see that the path with the least cost is selected as series of

(c) safely reachable region at time t=10.4(s)

regions changing in accordance with the passed time.

Fig. 10. The grid-based method results(grid size is 64*cm*2/cell)

Fig. 11. Safely reachable regions of the grid-based method in fig. 10

(a) safely reachable region at time t=3.0(s)

which are also selected randomly.

Fig. 12. The randomized method (modified RRT) results

Fig. 13. The road map and the detection map

edges that are in the vicinity of walls, because they have relatively small detection ratios. The detection map corresponding to the selected path was constructed as shown in fig. 13(b). In the detection map, the colored regions, which are detection regions, are the set of time and point that is detected by the observer on the determined path. The trajectory of the robot following this path could be planned on the detection map without traversing detection regions.

As shown in table 2, the arrival time of the grid-based result in PVD method was larger than that of the grid-based method. This is because the PVD method restricts the path to be selected on the predefined roadmap, not among whole possible movement. Therefore, the PVD method has the lower performance than the grid-based method even though it also finds the optimal solution on the determined path. However, its computation time is much smaller than the grid-based method because it reduces the dimension of the grid map. This planned result is shown in fig. 14(a). The movement of the observer and the robot in this result is shown in fig. 15. In fig. 15(b) and 15(c), we can see that the robot remains stationary because

obstacle avoidance problem. We examined how the inherent characteristics of each algorithms are revealed in non-rigid obstacle avoidance and evaluated the performance. The result in the reactive method informed us that limited information of the environment and obstacle greatly hinders the robot from avoiding non-rigid obstacles. The grid-based method could find the optimal safe motion of the robot at the cost of high computation burden. The randomization method gave priority to reducing computational burden over improving the performance. Finally, the path-velocity decomposed(PVD) method greatly reduced computation burden by abandoning the diversity of path selection. This method does not cause great loss in arrival time. This method makes the robot avoid the obstacle by adjusting its speed. In velocity profile planning, several detail methods, such as the grid-based method and the randomization method, can be applied with practical computation time. When complete information of the

Non-Rigid Obstacle Avoidance for Mobile Robots 123

environment and the obstacle is given, this PVD method could be efficiently used.

New Orleans, LA, pp. 3337–3344. Arkin, R. C. (1985). *Behavior-based Robotics*, MIT Press.

*Automation*, Vol. Vol. 1, pp. 560–566.

pp. 1542–1547.

6): 912–925.

*Conference on Robotics and Systems*, pp. 1656–1661.

*Encyclopedia of Artificial Intelligence* pp. 1072–1079.

Latombe, J. C. (1991). *Robot Motion Planning*, Kluwer academic publishers.

Lavalle, S. M. (2006). *Planning Algorithms*, Cambridge University Press.

*report 98-11, Computer Science Department, Iowa State University* .

*Transactions on Systems Man and Cybernetics* Vol. 17(No. 1): 21–31.

*and Automation* Vol. 12(No. 4): 566–580.

Akella, S. & Hurchinson, S. (2002). Coordinating the motions of multiple robots with specified

Boissonnat, J. D. & Yvinec, M. (1998). *Algorithm Geometry*, Cambridge University Press. Borenstein, J. & Koren, Y. (1991). The vector field histogram - fast obstacle avoidance for mobile robot, *Journal of Robotics and Automation* Vol. 7(No. 3): 278–288. Choi, J. S., Eoh, G., Kim, J., Yoon, Y., Park, J. & Lee, B. H. (2010). Analytic collision anticipation

trajectories, *Proceedings of IEEE International Conference of Robotics and Automation*,

technology considering agents' future behavior, *Proceedings of IEEE International*

relative velocity paradigm, *Proceedings of IEEE International Conference on Robotics and*

for special robots, *Proceedings of International Conference on Robotics and Automation*,

path planning in high-dimensional configuration space, *IEEE Transactions on Robotics*

having independent goals, *IEEE Transactions on Robotics and Automation* Vol. 14(No.

Fiorini, P. & Shiller, Z. (1993). Motion planning in dynamic environments using the

Fox, D., Burgard, W. & Thrun, S. (1997). The dynamic window approach to collision avoidance, *IEEE Robotics and Automation Magazine* Vol. 4(No. 1): 23–33. Ge, Q. J. & McCarthy, J. M. (1990). An algebraic formulation of configuration-space obstacles

Hee, L. & Jr., M. H. A. (2009). Mobile robots navigation, mapping, and localization part i,

Kavraki, L. E., Svestka, P., Latombe, J. C. & Overmars, M. H. (1996). Probabilistic roadmaps for

LaValle, S. M. (1998). Rapidly-exploring random trees: A new tool for path planning, *Technical*

Lavalle, S. M. & Hurchinson, S. A. (1998). Optimal motion planning for multiple robots

Lee, B. H. & Lee, C. S. G. (1987). Collision-free motion planning of two robots, *IEEE*

**6. References**

(a) planned trajectory of the grid-based method (b) planned trajectory of the randomization method

Fig. 14. The trajectory results of two detail velocity profile planning algorithm in the path-velocity decomposed(PVD) method

Fig. 15. The movement of observer(red) and robot(blue) in the grid-based result of path-velocity decomposed method

the observer's field of view obstructs the front and the rear of the robot. In fig. 15(d), the robot started to move after the observer's field of view is away from itself.

In the case of the randomization result in PVD method in table 2, it not only needs much lower computation time but also yields the better performance than the modified RRT methods. The randomization methods cannot guarantee the optimality, and its performance is on a case by case basis. Therefore, it is possible that the case restricting path in PVD method has the shorter arrival time than the case having whole opportunity of path selection. This planned result is shown in fig. 14(b).

#### **5. Conclusion**

In this chapter, we classified the type of obstacles into three categories: static rigid obstacles, moving rigid obstacles, and non-rigid obstacles. However, most researches of obstacle avoidance have tried to solve the rigid obstacles. In reality, the non-rigid obstacle avoidance problem is often emerged and needed to be solved, such as stealth navigation. We adopted the representative algorithms of classic rigid obstacle avoidance, and applied to non-rigid obstacle avoidance problem. We examined how the inherent characteristics of each algorithms are revealed in non-rigid obstacle avoidance and evaluated the performance. The result in the reactive method informed us that limited information of the environment and obstacle greatly hinders the robot from avoiding non-rigid obstacles. The grid-based method could find the optimal safe motion of the robot at the cost of high computation burden. The randomization method gave priority to reducing computational burden over improving the performance. Finally, the path-velocity decomposed(PVD) method greatly reduced computation burden by abandoning the diversity of path selection. This method does not cause great loss in arrival time. This method makes the robot avoid the obstacle by adjusting its speed. In velocity profile planning, several detail methods, such as the grid-based method and the randomization method, can be applied with practical computation time. When complete information of the environment and the obstacle is given, this PVD method could be efficiently used.

#### **6. References**

14 Mobile Robots

(a) planned trajectory of the grid-based method (b) planned trajectory of the randomization method

(a) (b) (c) (d)

the observer's field of view obstructs the front and the rear of the robot. In fig. 15(d), the robot

In the case of the randomization result in PVD method in table 2, it not only needs much lower computation time but also yields the better performance than the modified RRT methods. The randomization methods cannot guarantee the optimality, and its performance is on a case by case basis. Therefore, it is possible that the case restricting path in PVD method has the shorter arrival time than the case having whole opportunity of path selection. This planned result is

In this chapter, we classified the type of obstacles into three categories: static rigid obstacles, moving rigid obstacles, and non-rigid obstacles. However, most researches of obstacle avoidance have tried to solve the rigid obstacles. In reality, the non-rigid obstacle avoidance problem is often emerged and needed to be solved, such as stealth navigation. We adopted the representative algorithms of classic rigid obstacle avoidance, and applied to non-rigid

Fig. 15. The movement of observer(red) and robot(blue) in the grid-based result of

started to move after the observer's field of view is away from itself.

Fig. 14. The trajectory results of two detail velocity profile planning algorithm in the

path-velocity decomposed(PVD) method

path-velocity decomposed method

shown in fig. 14(b).

**5. Conclusion**


**Part 2** 

**Path Planning and Motion Planning** 


**Part 2** 

**Path Planning and Motion Planning** 

16 Mobile Robots

124 Recent Advances in Mobile Robotics

Lee, P. S. & Wang, L. L. (1994). Collision avoidance by fuzzy logic for agv navigation, *Journal*

Lumelsky, V. & Skewis, T. (1990). Incorporating range sensing in the robot navigation function,

Martinez-Gomez, L. & Fraichard, T. (2009). Collision avoidance in dynamic environments: an

Marzouqi, M. S. & Jarvis, R. A. (2006). New visibility-based path-planning approach for covert

Minguez, J. & Montano, L. (2004). Nearness diagram navigation: collision avoidance

Park, J., Choi, J. S., Kim, J., Ji, S.-H. & Lee, B. H. (2010). Long-term stealth navigation in a

Teng, Y. A., Dementhon, D. & Davis, L. S. (1993). Stealth terrain navigation, *IEEE Transactions*

Wang, D., Liu, D. & Dissanayake, G. (2006). A variable speed force field method for

ics-based solution and its comparative evaluation, *Proceedings of IEEE International*

robotics navigation, *IEEE Transactions on Systems Man and Cybernetics* Vol. 24(No.

in troublesome scenarios, *IEEE Transactions on Robots and Automation* Vol. 20(No.

security zone where the movement of the invader is monitored, *International Journal*

multi-robot collaboration, *Proceedings of IEEE International Conference on Intelligent*

*IEEE Transactions on Systems Man and Cybernetics* Vol. 20: 1058–1068.

*Conference on Robotics and Automation*, Kobe, pp. 100–105.

*of Control, Automation and Systems* Vol. 8(No. 3): 604–614.

*on Systems Man and Cybernetics* Vol. 23(No. 1): 96–113.

*Robots and Systems*, pp. 2697–2702.

*of Robotic Systems* Vol. 11(No. 8): 743–760.

6): 759–773.

1): 45–59.

**7** 

*China* 

**Path Planning of Mobile Robot in** 

*1State Key Laboratory of Robotics, Shenyang Institute of Automation,* 

Path planning of mobile robot in dynamic environment is one of the most challenging issues. To be more specific, path planning in multi-obstacle avoidance environment is defined as: given a vehicle *A* and a target *G* that are moving, planning a trajectory that will allow the vehicle to catch the target satisfy some specified constrains while avoiding obstacle *O*, and each of the obstacles can be either mobile or immobile in the environment. The corresponding problem is named target-pursuit and obstacles-avoidance (TPOA) and

The traditional method, such as probability road map, can achieve a successful path in 2D static environments. The planning process using this method generally consists of two phases: a construction and a query phase. In construction stage, the workspace of the robot is sampled randomly for generating candidate waypoints. In the query stage, the waypoints between the start and goal position are connected to be a graph, and the path is obtained by some searching algorithm, such as Dijkstra, A\* algorithm and so on. Hraba researched the 3D application of probability road map where A\* algorithm is used to find the near-optimal path (Hrabar, 2006). Although probability road map method is provably probabilistically complete (Ladd & Kavraki, 2004), it does not deal with the environment where the information is time-varying. The underlying reason is that this method only focuses on the certain environment. Once some uncertainty appears in the robot workspace, probability road map can not update with the changing environment and plan a valid trajectory for the

Artificial potential field is another traditional method which is generally used in both 2D and 3D environment. The mechanism that the robot is driven by attractive and repulsive force in a cooperative way is simple and often works efficiently even in dynamic environment. Kitamura et al. construct the path planning model based on the artificial potential field in threedimensional space which is described by octree (Kitamura et al, 1995). Traditionally, artificial potential field applies in two dimensions extensively. Also some other field concepts are invented. For example, there are harmonic potential functions (Kim & Khosla, 1992; Fahimi et al, 2009; Cocaud et al, 2008; Zhang & Valavanis, 1997), hydrodynamics (Liu et al, 2007),

**1. Introduction** 

will be researched extensively in this chapter.

mobile robot, never an optimal path.

**Relative Velocity Coordinates** 

Yang Chen1,2,3, Jianda Han1 and Liying Yang1,3

*2Department of Information Science and Engineering, Wuhan University of Science and Technology, Wuhan 3Graduate School, Chinese Academy of Sciences, Beijing* 

*Chinese Academy of Sciences, Shenyang* 

## **Path Planning of Mobile Robot in Relative Velocity Coordinates**

Yang Chen1,2,3, Jianda Han1 and Liying Yang1,3

*1State Key Laboratory of Robotics, Shenyang Institute of Automation, Chinese Academy of Sciences, Shenyang 2Department of Information Science and Engineering, Wuhan University of Science and Technology, Wuhan 3Graduate School, Chinese Academy of Sciences, Beijing China* 

#### **1. Introduction**

Path planning of mobile robot in dynamic environment is one of the most challenging issues. To be more specific, path planning in multi-obstacle avoidance environment is defined as: given a vehicle *A* and a target *G* that are moving, planning a trajectory that will allow the vehicle to catch the target satisfy some specified constrains while avoiding obstacle *O*, and each of the obstacles can be either mobile or immobile in the environment. The corresponding problem is named target-pursuit and obstacles-avoidance (TPOA) and will be researched extensively in this chapter.

The traditional method, such as probability road map, can achieve a successful path in 2D static environments. The planning process using this method generally consists of two phases: a construction and a query phase. In construction stage, the workspace of the robot is sampled randomly for generating candidate waypoints. In the query stage, the waypoints between the start and goal position are connected to be a graph, and the path is obtained by some searching algorithm, such as Dijkstra, A\* algorithm and so on. Hraba researched the 3D application of probability road map where A\* algorithm is used to find the near-optimal path (Hrabar, 2006). Although probability road map method is provably probabilistically complete (Ladd & Kavraki, 2004), it does not deal with the environment where the information is time-varying. The underlying reason is that this method only focuses on the certain environment. Once some uncertainty appears in the robot workspace, probability road map can not update with the changing environment and plan a valid trajectory for the mobile robot, never an optimal path.

Artificial potential field is another traditional method which is generally used in both 2D and 3D environment. The mechanism that the robot is driven by attractive and repulsive force in a cooperative way is simple and often works efficiently even in dynamic environment. Kitamura et al. construct the path planning model based on the artificial potential field in threedimensional space which is described by octree (Kitamura et al, 1995). Traditionally, artificial potential field applies in two dimensions extensively. Also some other field concepts are invented. For example, there are harmonic potential functions (Kim & Khosla, 1992; Fahimi et al, 2009; Cocaud et al, 2008; Zhang & Valavanis, 1997), hydrodynamics (Liu et al, 2007),

Path Planning of Mobile Robot in Relative Velocity Coordinates 129

*A, G, O* Vehicle, target, obstacle

V*A A* = **V** Magnitude of the vehicle velocity

V*O O* = **V** Magnitude of the obstacle velocity

*VG G* = **V** Magnitude of the target velocity

*AO A O* **V VV** = − Vehicle's relative velocity to the obstacle

*AO* Δ**V** Vehicle acceleration relative to the obstacle

*AG A G* **V VV** = − Vehicle velocity relative to the target G

*AG* Δ**V** Vehicle acceleration relative to the target *AO* **L** Vehicle position relative to the obstacle

*AG* **L** Vehicle position relative to target

Table 1. Nomenclatures used in this chapter (see Fig.1 and Fig. 2).

**2. Relative velocity coordinates and the TPOA principles** 

*AO AO <sup>P</sup>* <sup>=</sup> **V L** Temporal variable

*AG AG AG <sup>P</sup>* <sup>=</sup> **V L** Temporal variable

*V AO* = **V** Magnitude of the vehicle velocity relative to the obstacle

*VAG AG* = **V** Magnitude of the vehicle's velocity relative to the target

*AO L* = **L** Magnitude of the vehicle position relative to the obstacle

*AG AG L* = **L** Magnitude of the vehicle position relative to the target

shown in Section 4.1 and 4.2, respectively. In Section 5, an application about the multiple task assignment (MTA) is used to verify the method proposed in this chapter. Finally, a brief

RVCs are constructed on the mass point of the vehicle as shown in Fig.1 and Fig.2. Fig.1 shows the relative coordinates when the obstacle-avoiding is considered while Fig.2 shows the scenario where the target-pursuing is considered. In these figures, the target *G* and obstacle *O*, which are 2D or 3D movers, are assumed as circles or spheres having

*τ* Planning period

*N* Sum of obstacles **V***<sup>A</sup>* Vehicle velocity

*<sup>A</sup>* Δ**V** Vehicle acceleration **V***<sup>O</sup>* Obstacle velocity

**V***<sup>G</sup>* Target velocity

T

T

conclusion is drawn in Section 6.

**2.1 Relative velocity coordinates** 

*k* Time step

gradient field (Konolige, 2000), and virtual force field (Oh et al., 2007). Unfortunately, path planning approach based on the function family of potential field cannot obtain the optimal objective function which, in turn, cannot guarantee the desired optimal trajectories. Additionally, some of them, for example, potential panel method and harmonic potential function, can plan a path for an autonomous vehicle, but the computing burdens is huge and real time performance hardly satisfies the practical requirement.

Inspired by biological intelligence, many approaches, such as ant colony optimization (ACO) (Chen et al., 2008), particle swarm optimization (PSO) (Jung et al., 2006), genetic algorithm (GA) (Allaire et al., 2009), evolution algorithm (EA) (Zhao & Murthy, 2007), and their combination, are introduced to solve the path planning problem. They mostly rely on the stochastic searching, known as non-deterministic algorithm. These methods will eventually find an optimal solution, but no estimate on the time of convergence can be given. Thus, it may take long even infinite time to find the best solution. Furthermore, all of them have a great number of parameters to tune and that is never an easy job particularly when users are short of prior knowledge. Some comparisons (Allaire et al., 2009; Krenzke, 2006) show that the expensive calculations limit their real application.

Another kind of method is mathematic programming. Based on the relative velocity coordinates (RVCs), linear programming (LP) and mixed integer linear programming (MILP) can be employed for path planning problem. For example, Wang et al. converted the 2D path planning of an unmanned underwater vehicle to constrained optimization or semiinfinite constrained optimization problem (Wang et al., 2000). Zu et al. discussed the path planning in 2D and an LP method was proposed for the problem of dynamic target pursuit and obstacle avoidance (Zu et al., 2006). This method tried to plan the variables of the linear acceleration and the angular acceleration of a ground vehicle. Schouwenaars et al. proposed a MILP formulation with receding horizon strategy where a minimum velocity and a limited turn rate of aircraft are constrained (Schouwenaars et al., 2004). However, these results still focused on the two dimensions.

In this chapter, we consider the same problem of TPOA but in three dimensions. To our problem, the uncertain environment has one target or some of them, denoted by *G*, and many obstacles *O* that are all velocity-changeable with their moving actions. The aim of the vehicle *A* is to find an optimal path for pursuing the target while, at the same time, avoiding collision threaten from obstacles. The position and velocity of movers, including the target and obstacles, are assumed known or estimated at current time. To be more specific, it is assumed that the noise contained in the data can be eliminated with certain filters. However, this paper has no intend to discuss the filtering algorithm for achieving the feasible data of on-board sensors. The trajectory from a start to a destination location typically needs to be computed gradually over time in the fashion of receding horizon (Schouwenaars et al., 2004) in which a new waypoint of the total path is computed at each time step by solving a linear programming problem. The target and obstacles, probably static or dynamic, are modeled by spheres having a certain radius for their impact area, and the vehicle is modeled by a mass point. In order to optimize the vehicle's acceleration online, we construct a linear programming model in Cartesian orthogonal coordinates based on relative velocity space (Fiorini & Shiller, 1998).

This chapter is organized as follows. First, the relative velocity coordinates are introduced in both two dimensions and three dimensions and then the TPOA principles are introduced, including obstacle-avoidance and target-pursuit principles. The formulation of the standard linear programming is mathematically introduced in Section 3.1, and the path planning model is described in detail in Section 3.2. Simulations of path planning in 2D and 3D are 128 Recent Advances in Mobile Robotics

gradient field (Konolige, 2000), and virtual force field (Oh et al., 2007). Unfortunately, path planning approach based on the function family of potential field cannot obtain the optimal objective function which, in turn, cannot guarantee the desired optimal trajectories. Additionally, some of them, for example, potential panel method and harmonic potential function, can plan a path for an autonomous vehicle, but the computing burdens is huge and

Inspired by biological intelligence, many approaches, such as ant colony optimization (ACO) (Chen et al., 2008), particle swarm optimization (PSO) (Jung et al., 2006), genetic algorithm (GA) (Allaire et al., 2009), evolution algorithm (EA) (Zhao & Murthy, 2007), and their combination, are introduced to solve the path planning problem. They mostly rely on the stochastic searching, known as non-deterministic algorithm. These methods will eventually find an optimal solution, but no estimate on the time of convergence can be given. Thus, it may take long even infinite time to find the best solution. Furthermore, all of them have a great number of parameters to tune and that is never an easy job particularly when users are short of prior knowledge. Some comparisons (Allaire et al., 2009; Krenzke,

Another kind of method is mathematic programming. Based on the relative velocity coordinates (RVCs), linear programming (LP) and mixed integer linear programming (MILP) can be employed for path planning problem. For example, Wang et al. converted the 2D path planning of an unmanned underwater vehicle to constrained optimization or semiinfinite constrained optimization problem (Wang et al., 2000). Zu et al. discussed the path planning in 2D and an LP method was proposed for the problem of dynamic target pursuit and obstacle avoidance (Zu et al., 2006). This method tried to plan the variables of the linear acceleration and the angular acceleration of a ground vehicle. Schouwenaars et al. proposed a MILP formulation with receding horizon strategy where a minimum velocity and a limited turn rate of aircraft are constrained (Schouwenaars et al., 2004). However, these

In this chapter, we consider the same problem of TPOA but in three dimensions. To our problem, the uncertain environment has one target or some of them, denoted by *G*, and many obstacles *O* that are all velocity-changeable with their moving actions. The aim of the vehicle *A* is to find an optimal path for pursuing the target while, at the same time, avoiding collision threaten from obstacles. The position and velocity of movers, including the target and obstacles, are assumed known or estimated at current time. To be more specific, it is assumed that the noise contained in the data can be eliminated with certain filters. However, this paper has no intend to discuss the filtering algorithm for achieving the feasible data of on-board sensors. The trajectory from a start to a destination location typically needs to be computed gradually over time in the fashion of receding horizon (Schouwenaars et al., 2004) in which a new waypoint of the total path is computed at each time step by solving a linear programming problem. The target and obstacles, probably static or dynamic, are modeled by spheres having a certain radius for their impact area, and the vehicle is modeled by a mass point. In order to optimize the vehicle's acceleration online, we construct a linear programming model in Cartesian orthogonal coordinates based on relative velocity space (Fiorini & Shiller, 1998). This chapter is organized as follows. First, the relative velocity coordinates are introduced in both two dimensions and three dimensions and then the TPOA principles are introduced, including obstacle-avoidance and target-pursuit principles. The formulation of the standard linear programming is mathematically introduced in Section 3.1, and the path planning model is described in detail in Section 3.2. Simulations of path planning in 2D and 3D are

real time performance hardly satisfies the practical requirement.

2006) show that the expensive calculations limit their real application.

results still focused on the two dimensions.


Table 1. Nomenclatures used in this chapter (see Fig.1 and Fig. 2).

shown in Section 4.1 and 4.2, respectively. In Section 5, an application about the multiple task assignment (MTA) is used to verify the method proposed in this chapter. Finally, a brief conclusion is drawn in Section 6.

#### **2. Relative velocity coordinates and the TPOA principles**

#### **2.1 Relative velocity coordinates**

RVCs are constructed on the mass point of the vehicle as shown in Fig.1 and Fig.2. Fig.1 shows the relative coordinates when the obstacle-avoiding is considered while Fig.2 shows the scenario where the target-pursuing is considered. In these figures, the target *G* and obstacle *O*, which are 2D or 3D movers, are assumed as circles or spheres having

Path Planning of Mobile Robot in Relative Velocity Coordinates 131

Fig. 2. Geometrical representation of the relative parameters in the relative coordinates when the target-pursuing problem is considered. (a) and (b) show the definition of *relative* 

which a new waypoint of the total path is computed gradually over time. Here, we obtain

(a) Target pursuit in two dimensions (b) Target pursuit in three dimensions

*A*

*<sup>x</sup> <sup>y</sup>*

*A*

*z*

vehicle moves from time step *k* to *k*+1. This fact suggests that the obstacle-avoiding principle

*AOC k AO k* ( ) ( 1)

*AOCi k AOi k* ( ) ( 1)

γ

where the subscript *i* denotes the label of the obstacles. *i*=1, 2… *N*. *N* stands for the number

The **V***AG* can be resolved into a pair of orthogonal components, as shown in Fig. 3. The vehicle is expected to tune its velocity to the optimum. Only when the velocity direction of the vehicle is identical to **L***AG*, it will not lose its target. In the meanwhile, the vehicle should better minimize the magnitude of **V**T. Consequently, the policy for the optimal velocity is obvious in that **V**T should be minimized while **V**C maximized. We formulate these principles

γ

where γ*AOC(k)* is the *collision region angle* in time step *k,* which is shown in Fig.1. If there are

 π

> π

Δ**VVV** *AO AG A* =Δ =Δ (2)

*AG* γ

τ

<sup>+</sup> ≤ (3)

<sup>+</sup> ≤ (4)

, it will be avoided

( , , )

*AG Gx Gy Gz* **L** = *lll*

*G*

( , , ) *T*

*AG Gx Gy Gz* **V** = *vvv*

*RG*

*T*

when the

τ

*target angle* in 2D scenario and 3D scenario, respectively.

( , ) *T*

*F*

*AG Gx Gy* **L** = *l l*

*G*

( , ) *T*

*AG Gx Gy* **V** = *v v*

*RG*

*E*

For each obstacle *O*, under the assumption that its velocity is constant in

γ≤

γ≤

if the γ*AO* is large enough to make the **V***AO* out of the *CCAO* over the interval

the following equivalence.

*AG* γ

*x*

*y*

*A*

*A*

**2.2 Obstacle-avoidance principle** 

hold the following inequality. That is

multi obstacles, Eq. (3) changes to

**2.3 Target-pursuit principle** 

as two cost functions. They are

of obstacles.

certain radiuses which are denoted by *RO*s. As mentioned above, the relative parameters are measurable or estimable for the current planning time, by using certain on-board sensors. For the convenience of description, the relative velocity and relative position between the vehicle *A* and the target *G* are denoted by *VAG* and *LAG*. Similarly, parameters about the obstacle *O* are denoted by *VAO* and *LAO*. Some other nomenclatures are listed in Table 1.

(a) Obstacle avoidance in two dimensions (b) Obstacle avoidance in three dimensions

Fig. 1. Geometrical representation of the relative parameters in the relative coordinates when the obstacle-avoiding problem is considered. (a) and (b) show the definition of *relative obstacle angle* in 2D scenario and 3D scenario, respectively.

We define the *relative obstacle angle*, γ*AO*, as the angle between **V***AO* and **L***AO*. γ*AO*∈[0, π]. *Collision cone* is defined as an area in which the vehicle will collide with an obstacle by current velocity **V***AO*, and it is denoted by *CCAO*. Generally, *CCAO* is in the cone *AEF,* as shown in Fig.1. The half cone angle of *CCAO*, γ*AOC*, is defined as *collision region angle*, i.e.

$$\mathcal{H}\_{\rm{ACO}} = \arcsin\left(\frac{R\_O}{L\_{AO}}\right) \tag{1}$$

Similar to the definition in the obstacle-avoiding scenario in Fig.1, the angle between the relative velocity **V***AG* and relative distance **L***AG* is defined as *relative target angle*, denoted by γ*AG*∈[0,π], as shown in Fig.2. The *pursuit cone*, as well as the *pursuit region angle*, is defined in the same as obstacle avoidance scenario.

Some assumptions should be declared as the prerequisites of the planning principles. First, in order to guarantee the robot to catch the target successfully, the maximum velocity of the robot is assumed to be larger than that of the target and the obstacles. Due to this precondition, the vehicle *A* has the ability to catch the target, as well as avoid the collision with obstacles. Second, the target and the obstacles are supposed to keep their speed as constants during the planning period, τ. In fact, this hypothesis is usually accepted because τ is short enough for a real vehicle. Owe to the mechanism of numerical approximation, the path planning problem can be solved and optimized with a receding horizon fashion in 130 Recent Advances in Mobile Robotics

certain radiuses which are denoted by *RO*s. As mentioned above, the relative parameters are measurable or estimable for the current planning time, by using certain on-board sensors. For the convenience of description, the relative velocity and relative position between the vehicle *A* and the target *G* are denoted by *VAG* and *LAG*. Similarly, parameters about the obstacle *O* are denoted by *VAO* and *LAO*. Some other nomenclatures are listed in

Fig. 1. Geometrical representation of the relative parameters in the relative coordinates when the obstacle-avoiding problem is considered. (a) and (b) show the definition of *relative* 

arcsin *AOC*

<sup>=</sup> ⎛ ⎞

Similar to the definition in the obstacle-avoiding scenario in Fig.1, the angle between the relative velocity **V***AG* and relative distance **L***AG* is defined as *relative target angle*, denoted by γ*AG*∈[0,π], as shown in Fig.2. The *pursuit cone*, as well as the *pursuit region angle*, is defined in

Some assumptions should be declared as the prerequisites of the planning principles. First, in order to guarantee the robot to catch the target successfully, the maximum velocity of the robot is assumed to be larger than that of the target and the obstacles. Due to this precondition, the vehicle *A* has the ability to catch the target, as well as avoid the collision with obstacles. Second, the target and the obstacles are supposed to keep their speed as

is short enough for a real vehicle. Owe to the mechanism of numerical approximation, the path planning problem can be solved and optimized with a receding horizon fashion in

γ

τ

We define the *relative obstacle angle*, γ*AO*, as the angle between **V***AO* and **L***AO*. γ*AO*∈[0, π]. *Collision cone* is defined as an area in which the vehicle will collide with an obstacle by current velocity **V***AO*, and it is denoted by *CCAO*. Generally, *CCAO* is in the cone *AEF,* as shown in Fig.1. The half cone angle of *CCAO*, γ*AOC*, is defined as *collision region angle*, i.e.

(a) Obstacle avoidance in two dimensions (b) Obstacle avoidance in three dimensions

*A*

*O AO R L*

(1)

( ) , , *<sup>T</sup> AO x y z* **L** = *lll*

*F*

*O*

( , , ) *T*

( ) *<sup>A</sup> Ax Ay Az* **V** = *v* ,*v* ,*v*

*AO x y z* **V** = *vvv*

*RO*

*E*

*AO* γ

*AOC* γ

*CCAO*

*<sup>x</sup> <sup>y</sup>*

*A*

*z*

τ

. In fact, this hypothesis is usually accepted because

⎜ ⎟ ⎝ ⎠

*obstacle angle* in 2D scenario and 3D scenario, respectively.

( , ) *T*

*F*

*O*

*AO x y* **L** = *l l*

( , ) *T*

*RO*

*E*

*AO x y* **V** = *v v*

*AO* γ

*AOC* γ

*x*

*CCAO*

the same as obstacle avoidance scenario.

constants during the planning period,

Table 1.

*y*

*A*

*A*

(a) Target pursuit in two dimensions (b) Target pursuit in three dimensions

Fig. 2. Geometrical representation of the relative parameters in the relative coordinates when the target-pursuing problem is considered. (a) and (b) show the definition of *relative target angle* in 2D scenario and 3D scenario, respectively.

which a new waypoint of the total path is computed gradually over time. Here, we obtain the following equivalence.

$$
\Delta \mathbf{V}\_{AO} = \Delta \mathbf{V}\_{AG} = \Delta \mathbf{V}\_A \tag{2}
$$

#### **2.2 Obstacle-avoidance principle**

For each obstacle *O*, under the assumption that its velocity is constant in τ, it will be avoided if the γ*AO* is large enough to make the **V***AO* out of the *CCAO* over the interval τ when the vehicle moves from time step *k* to *k*+1. This fact suggests that the obstacle-avoiding principle hold the following inequality. That is

$$
\gamma\_{A\rm OC} \le \gamma\_{A\rm O(k+1)} \le \pi \tag{3}
$$

where γ*AOC(k)* is the *collision region angle* in time step *k,* which is shown in Fig.1. If there are multi obstacles, Eq. (3) changes to

$$
\gamma\_{A \text{OCi}(k)} \le \gamma\_{A \text{Oi}(k+1)} \le \pi \tag{4}
$$

where the subscript *i* denotes the label of the obstacles. *i*=1, 2… *N*. *N* stands for the number of obstacles.

#### **2.3 Target-pursuit principle**

The **V***AG* can be resolved into a pair of orthogonal components, as shown in Fig. 3. The vehicle is expected to tune its velocity to the optimum. Only when the velocity direction of the vehicle is identical to **L***AG*, it will not lose its target. In the meanwhile, the vehicle should better minimize the magnitude of **V**T. Consequently, the policy for the optimal velocity is obvious in that **V**T should be minimized while **V**C maximized. We formulate these principles as two cost functions. They are

Path Planning of Mobile Robot in Relative Velocity Coordinates 133

the target-pursuit problem is transferred to some cost functions. The final linear programming model consists of a weighted sum of the cost functions and a set of linear constraints. It is

min : *jj v v* 11 22

1 2 1 *jv v*

planning is operated in two-dimensional environment, *j* belongs to a two-element-set, i.e., *j*∈{x, y}. And if in three-dimensional environment, *j* belongs to a three-element-set, i.e.,

arccos arccos *AO AO*

⎛ ⎞ <sup>⋅</sup> ⎛ ⎞ <sup>=</sup> ⎜ ⎟ <sup>=</sup> ⎜ ⎟ <sup>⋅</sup> ⎝ ⎠ ⎝ ⎠ **V L**

( )

*d dt*

*AO AO AO k AO AO k d d*

> *k k P*

*i k i k P*

> *i R L*

*AO AO*

The definitions of V, L, and P refer to Table 1. Here, Eq. (10) is linearized by Taylor's

 ω  ω

*<sup>v</sup>*2≥0. The letter *j* denotes the subscript of the components of **V***AG*. If the path

*q q* (8)

+ + = (9)

*P VL*

**V L** (10)

( )

<sup>−</sup> ⎝ ⎠ **L VV** (12)

<sup>−</sup> ⎝ ⎠ **LVV** (13)

(14)

(11)

*o*

**<sup>V</sup> <sup>V</sup>**

*J d* = ++ ∑ωω

*j*

*j* ∑ωω

( 1) ( ) ( )

 γ

 τ

τ

τ

*AOCi*

γ

γ

**V**

( ) <sup>2</sup> 22 2 ( ) ( ) *AO k AO AO AO*

Let γ*AO(k+*1*)* represent the *relative obstacle angle* in time step *k*+1 after vehicle's movement. The variables, Δ**V***AO*, in (11), are that we are trying to plan in step *k* for step *k*+1. If there are

> ( ) <sup>2</sup> 22 2 ,( ) ,( ) *AOi k AO AO AO*

> > arcsin *Oi*

⎛ ⎞ <sup>=</sup> ⎜ ⎟ ⎝ ⎠

In this subsection, the relative distance between the robot and the target-objective is minimized. By tuning the velocity of the robot, the relative distance between the robot

*VL P V*

⎛ ⎞ Δ =− ⎜ ⎟ − Δ

*VL P V*

⎛ ⎞ Δ =− ⎜ ⎟ − Δ

⎛ ⎞ = + ⎜ ⎟ + Δ ⎝ ⎠

*AO k AO k AO k*

<sup>+</sup> = + Δ

γ

γ

( )

*j*∈{x, y, z}. The variables *dj*, *q*1, *q*2 will be explained later.

*AO*

γ

theorem and we obtain the following equation.

γ

γ

multiple obstacles, Eq. (12) changes to Eq. (13).

γ

**3.2.2 Minimize the relative distance** 

**3.2.1 Linearization of the avoidance constrain**  From Fig.1, we get the *relative obstacle angle* function γ*AO*.

satisfying

where ω*j*, ω*v*1, ω

where

$$\min: \mathbf{V}\_{\mathbb{T}} = \|\mathbf{V}\_{AG}\| \sin \mathbf{y}\_{AG} \tag{5}$$

and

$$\max \colon \mathbf{V}\_{\mathbb{C}} = \|\mathbf{V}\_{AG}\| \cos \mathbf{y}\_{AG} \tag{6}$$

$$\mathbf{v}\_{\mathbb{C}} \quad \bigwedge\_{\mathbf{u}\_{\mathcal{G}}}^{G} \mathbf{L}\_{\mathcal{A}} $$

$$\mathcal{T}\_{\mathcal{A}} \quad \bigvee\_{\mathbf{u}\_{\mathcal{G}}} \mathbf{v}\_{\mathcal{G}}$$

Fig. 3. The resolution of **V***AG*. The velocity of the vehicle relative to the target is resolved into two orthogonal components, **V**C and **V**T. One component, **V**C, is in the direction of **L***AG* and pointed to the target. The other, **V**T, is orthogonal to **L***AG*.

VT

*A*

#### **3. Linear programming model**

In this section, we first introduce some knowledge about the mathematical description of the standard linear programming model. Then, the mentioned principles for path planning are decomposed and linearized according to the linear prototype of the standard model.

#### **3.1 Standard linear programming model**

The standard Linear Programming is composed of a cost function and some constrains, all of which need to be affine and linear (Boyd & Vandenberghe, 2004). The following is the standard and inequality form.

$$\begin{array}{l}\min: \mathbf{c}^{\mathsf{T}}\mathbf{x} \\ \text{subject to:} \\ \text{subject to:} \mathbf{Dx} \le \mathbf{h} \end{array} \tag{7}$$

where **D**∈Rm×n. The vector **x**∈Rn×1 denotes the variables. In the standard linear programming formulation, the vector **c**∈Rn×1, representing the coefficients, the vector **h**∈Rm×1, and the matrix **D** are all known when the model is going to be solved by some method.

During the decades, several methods have been invented to solve the optimization as shown in (7). From those methods, simplex algorithm and interior-point method are two most famous algorithms. More information about these algorithms can be found in (Boyd & Vandenberghe, 2004). In this chapter, an open library named Qsopt is used where the primal and dual simplex algorithms are imbedded. For the corresponding procedures, the user can specify the particular variant of the algorithm which is applied (David et al., 2011).

#### **3.2 Linear programming model for path planning problem**

In this subsection, the problem of obstacle-avoidance is formulated and we obtain some inequality constraints after we introduce two slack variables. Additionally, the formulation of the target-pursuit problem is transferred to some cost functions. The final linear programming model consists of a weighted sum of the cost functions and a set of linear constraints. It is

$$\min : J = \sum\_{j} o\_{j} d\_{j} + o\_{v1} q\_{1} + o\_{v2} q\_{2} \tag{8}$$

satisfying

132 Recent Advances in Mobile Robotics

Fig. 3. The resolution of **V***AG*. The velocity of the vehicle relative to the target is resolved into two orthogonal components, **V**C and **V**T. One component, **V**C, is in the direction of **L***AG* and

*AG* γ

VC

VT

**V***AG*

**L***AG*

*G*

In this section, we first introduce some knowledge about the mathematical description of the standard linear programming model. Then, the mentioned principles for path planning are

The standard Linear Programming is composed of a cost function and some constrains, all of which need to be affine and linear (Boyd & Vandenberghe, 2004). The following is the

> subject to: ≤ **<sup>x</sup> c x**

where **D**∈Rm×n. The vector **x**∈Rn×1 denotes the variables. In the standard linear programming formulation, the vector **c**∈Rn×1, representing the coefficients, the vector **h**∈Rm×1, and the

During the decades, several methods have been invented to solve the optimization as shown in (7). From those methods, simplex algorithm and interior-point method are two most famous algorithms. More information about these algorithms can be found in (Boyd & Vandenberghe, 2004). In this chapter, an open library named Qsopt is used where the primal and dual simplex algorithms are imbedded. For the corresponding procedures, the user can

In this subsection, the problem of obstacle-avoidance is formulated and we obtain some inequality constraints after we introduce two slack variables. Additionally, the formulation of

**Dx h**

(7)

decomposed and linearized according to the linear prototype of the standard model.

<sup>T</sup> min :

matrix **D** are all known when the model is going to be solved by some method.

specify the particular variant of the algorithm which is applied (David et al., 2011).

**3.2 Linear programming model for path planning problem** 

pointed to the target. The other, **V**T, is orthogonal to **L***AG*.

*A*

**3. Linear programming model** 

standard and inequality form.

**3.1 Standard linear programming model** 

and

min : V sin <sup>T</sup> γ *AG AG* = **V** (5)

max : V cos <sup>C</sup> γ *AG AG* = **V** (6)

$$\sum\_{j} o o\_{j} + o\_{v1} + o\_{v2} = 1\tag{9}$$

where ω*j*, ω*v*1, ω*<sup>v</sup>*2≥0. The letter *j* denotes the subscript of the components of **V***AG*. If the path planning is operated in two-dimensional environment, *j* belongs to a two-element-set, i.e., *j*∈{x, y}. And if in three-dimensional environment, *j* belongs to a three-element-set, i.e., *j*∈{x, y, z}. The variables *dj*, *q*1, *q*2 will be explained later.

#### **3.2.1 Linearization of the avoidance constrain**

From Fig.1, we get the *relative obstacle angle* function γ*AO*.

$$\gamma\_{AO} = \arccos\left(\frac{\mathbf{V}\_{AO} \cdot \mathbf{L}\_{AO}}{\|\mathbf{V}\_{AO}\| \cdot \|\mathbf{L}\_{AO}\|}\right) = \arccos\left(\frac{P}{VL}\right) \tag{10}$$

The definitions of V, L, and P refer to Table 1. Here, Eq. (10) is linearized by Taylor's theorem and we obtain the following equation.

$$\begin{split} \mathcal{Y}\_{AO(k+1)} &= \mathcal{Y}\_{AO(k)} + \Delta \mathcal{Y}\_{AO(k)} \\ &= \mathcal{Y}\_{AO(k)} + \pi \left( \frac{d\mathcal{Y}\_{AO}}{d\mathbf{V}\_{AO}} \right)\_{\text{(k)}} \frac{d\mathbf{V}\_{AO}}{dt} + o\left( \|\Delta \mathbf{V}\_{AO}\| \right) \end{split} \tag{11}$$

$$
\Delta\gamma\_{AO(k)} = -\frac{\tau}{\sqrt{V^2 L^2 - P^2}} \left( \mathbf{L}\_{AO} - \frac{P}{V^2} \mathbf{V}\_{AO} \right)\_{\text{(k)}} \Delta\mathbf{V}\_{AO} \tag{12}
$$

Let γ*AO(k+*1*)* represent the *relative obstacle angle* in time step *k*+1 after vehicle's movement. The variables, Δ**V***AO*, in (11), are that we are trying to plan in step *k* for step *k*+1. If there are multiple obstacles, Eq. (12) changes to Eq. (13).

$$
\Delta\boldsymbol{\gamma}\_{AO(k)} = -\frac{\boldsymbol{\tau}}{\sqrt{\boldsymbol{V}^2 \boldsymbol{L}^2 - \boldsymbol{P}^2}} \left( \mathbf{L}\_{AO} - \frac{\boldsymbol{P}}{\boldsymbol{V}^2} \mathbf{V}\_{AO} \right)\_{i,(k)} \Delta\mathbf{V}\_{AO} \tag{13}
$$

where

$$\gamma\_{A\text{OCl}} = \arcsin\left(\frac{R\_{\text{Oi}}}{L\_{\text{i}}}\right) \tag{14}$$

#### **3.2.2 Minimize the relative distance**

In this subsection, the relative distance between the robot and the target-objective is minimized. By tuning the velocity of the robot, the relative distance between the robot

Path Planning of Mobile Robot in Relative Velocity Coordinates 135

Since we can maximize **V**C by minimizing -**V**C, we refer to a minimize problem with affine cost function and constraint functions as a linear programming. Consequently, the problem

Linearized using Taylor's theorem, Eq. (21) changes to a standard linear programming

where Δ**V***AG* is the variable that we are trying to include in our model. *q*2 is also a slack

One of the advantages of the LP method is that various constraints can easily be added in the opening constraint set. Here we provide some demonstrations of constraints that are

The kinematics and dynamics are extremely simplified in the path planning model where only the bound of the velocity and acceleration are added in the feasible set. These bound

> *V vV v*

where *vAj* denotes the components of **V**A, as shown in Fig. 1. Δmax denotes the max

*V v vV A Aj j A* max max − ≤ +Δ ≤

max , min , *Vv v Vv A Aj j A Aj*

All sensors can not detect the area that is beyond their capability. In real application, the obstacle that has been avoided by the vehicle will possibly disappear from the sensor's operating region. Similarly, any new obstacle from long distance is possible entering the detecting area. On this point, we evaluate the threat of each obstacle in the environment and propose a threat factor for each of them to estimate the performance. More specially, if the relative distance between the vehicle and the obstacle *Oi* satisfies L*AOi*≥Lmin*i*, this

⎧− ≤≤ ⎪

−Δ ≤ Δ ≤ Δ ⎪⎩

max max max max *A Aj A j*

> τ.

> > τ

max ( ) max max max ( ) 1 1

<sup>⎧</sup> ⎫⎧ ⎫ ⎨−Δ − + ≤ Δ ≤ Δ ⎬⎨ ⎬ <sup>−</sup> ⎩ ⎭⎩ ⎭ (25)

 τ

T

 **L***AG*/L*AG*.

max 2 max

*C C AG AG AG*

*V V q V qV* − −∇ Δ ≤ − ≤≤

τ

2

2

*q*

transformed from the dynamics, sensor data, and searching boundaries.

⎨

We get the net constraints (25) from the simultaneous equations (23) and (24).

**3.3.1 The constraints from the kinematics and dynamics** 

min : subject to:

variable. Here, the grades is computed with ∇**V**C=

min : V cos <sup>C</sup> γ *AG AG* − =− **V** (21)

**V** (22)

(23)

(24)

(2) Maximize the magnitude of the component **V**C.

described by (6) can be rewritten as

problem. That is

**3.3 Other constraints** 

can be described mathematically as

magnitude of the changing of *vAj* in each period

τ

**3.3.2 The constraints from the limit of the sensor** 

and target will became smaller through each step time. This distance is resolved into two or three elements by the axis number. That is to minimize the elements of the vector **L**AG, i.e.,

$$\min \mathbf{x} : \left\| l\_{\mathbf{G}\mathbf{j}} - \left( \upsilon\_{\mathbf{G}\mathbf{j}} \mathbf{r} + \Delta \upsilon\_{\mathbf{G}\mathbf{j}} \mathbf{r}^{2} \right) \right\| \tag{15}$$

where *lGj* is the element of **L***AG* and *vGj* is the element of **V***AG*. See Fig.2. Δ*vGj* denotes the acceleration of *vGj*. *j*∈{x, y} is for two-dimensional environment and *j*∈{x, y, z} is for threedimensional environment. The inherited objective function is derived from Eq. (15).

$$\begin{aligned} \text{min} &: d\_{\rangle} \\ \text{subject to } & -d\_{\rangle} \le l\_{G\rangle} - \left( \upsilon\_{G\rangle} \tau + \Delta \upsilon\_{G\rangle} \tau^2 \right) \le d\_{\rangle} \end{aligned} \tag{16}$$

where Δ*vGj* is the variable that we hope to compute in the linear programming model, as stated in Eq. (2). In a manner of preserving convexity of the problem, *dj*≥0 is the newly introduced slack variable associated with each element (Boyd & Vandenberghe, 2004).

#### **3.2.3 Optimize the relative velocity**

The relative velocity between the robot and the target-objective needs to optimize on the consideration of target-pursuit principle. We respectively discuss the optimization of the pair component, **V**T and **V**C, as shown in Fig. 3.

(1) Minimize the magnitude of the component **V**T. We compute **V**T with

$$V\_{\rm T} = \sqrt{V\_{\rm AG}^2 - \frac{P\_{\rm AG}^2}{L\_{\rm AG}^2}} \tag{17}$$

So the optimization of (5) is equal to

$$\min \mathbf{m} : \mathbf{g}\left(\bar{V}\_{\rm AG}\right) = V\_{\rm T}^2 = V\_{\rm AG}^2 - \frac{P\_{\rm AG}^2}{L\_{\rm AG}^2} \tag{18}$$

Using Taylor's theorem and introducing slack variable, *q*1, we get the new formulation for optimization.

$$\begin{aligned} \min &: q\_1\\ \text{subject to:} & \quad 0 \le \text{g}\left(\mathbf{V}\_{AG}\right) + \nabla \mathbf{g} \Delta \mathbf{V}\_{AG}^T \le q\_1\\ & \quad 0 \le q\_1 \le \mathbf{V}\_{AG\,\max}^2 \end{aligned} \tag{19}$$

where Δ**V**AG is the variable vector that we hope to include in the linear programming model. *q*1 is the slack variable. ∇g represents the grades of the function g(.). It is computed with

$$\nabla \mathbf{g} = 2\pi \left( \mathbf{V}\_{AG} - \frac{P\_{AG}}{L\_{AG}^2} \mathbf{L}\_{AG} \right) \tag{20}$$

*VAG*max is the maximum of the relative velocity between the robot and the target. We estimate it by *VAG*max = 2*VA*max. *VA*max denotes the upper boundary of *VA*.

(2) Maximize the magnitude of the component **V**C.

Since we can maximize **V**C by minimizing -**V**C, we refer to a minimize problem with affine cost function and constraint functions as a linear programming. Consequently, the problem described by (6) can be rewritten as

$$\text{minim}: -\mathbf{V}\_{\text{C}} = -\left\| \mathbf{V}\_{AG} \right\| \cos \mathbf{y}\_{AG} \tag{21}$$

Linearized using Taylor's theorem, Eq. (21) changes to a standard linear programming problem. That is

$$\begin{aligned} \text{min}: q\_2\\ \text{subject to: } & -V\_{\text{C}} - \nabla V\_{\text{C}} \Delta \mathbf{V}\_{AG}^{\text{T}} \le q\_2\\ & -V\_{AG, \text{max}} \le q\_2 \le V\_{AG, \text{max}} \end{aligned} \tag{22}$$

where Δ**V***AG* is the variable that we are trying to include in our model. *q*2 is also a slack variable. Here, the grades is computed with ∇**V**C= τ **L***AG*/L*AG*.

#### **3.3 Other constraints**

134 Recent Advances in Mobile Robotics

and target will became smaller through each step time. This distance is resolved into two or three elements by the axis number. That is to minimize the elements of the vector **L**AG,

> ( ) <sup>2</sup> min : *Gj Gj Gj lv v* − +Δ τ

where *lGj* is the element of **L***AG* and *vGj* is the element of **V***AG*. See Fig.2. Δ*vGj* denotes the acceleration of *vGj*. *j*∈{x, y} is for two-dimensional environment and *j*∈{x, y, z} is for three-

where Δ*vGj* is the variable that we hope to compute in the linear programming model, as stated in Eq. (2). In a manner of preserving convexity of the problem, *dj*≥0 is the newly introduced slack variable associated with each element (Boyd & Vandenberghe, 2004).

The relative velocity between the robot and the target-objective needs to optimize on the consideration of target-pursuit principle. We respectively discuss the optimization of the

> 2 T 2

*<sup>P</sup> V V*

2 2 min : <sup>T</sup> <sup>2</sup>

*AG AG*

Using Taylor's theorem and introducing slack variable, *q*1, we get the new formulation for

( )

where Δ**V**AG is the variable vector that we hope to include in the linear programming model. *q*1 is the slack variable. ∇g represents the grades of the function g(.). It is computed with

> <sup>2</sup> <sup>2</sup> *AG AG AG AG P*

*VAG*max is the maximum of the relative velocity between the robot and the target. We

*<sup>g</sup> <sup>L</sup>* ∇= − τ

*q V*

≤ ≤

2 1 max

*AG*

⎛ ⎞ ⎜ ⎟ ⎝ ⎠

( )

*AG*

dimensional environment. The inherited objective function is derived from Eq. (15).

min :

**3.2.3 Optimize the relative velocity** 

So the optimization of (5) is equal to

optimization.

pair component, **V**T and **V**C, as shown in Fig. 3.

subject to : *j*

(1) Minimize the magnitude of the component **V**T. We compute **V**T with

1

*q*

subject to: 0 0

estimate it by *VAG*max = 2*VA*max. *VA*max denotes the upper boundary of *VA*.

min :

*d*

 τ

( ) <sup>2</sup>

 τ

*j Gj Gj Gj j*

2

*AG*

*AG*

2

*AG*

*AG*

T

*AG AG*

*g gq*

≤ +∇ Δ ≤

1

**V V** (19)

**V L** (20)

*<sup>L</sup>* = − (17)

*<sup>P</sup> gV V V <sup>L</sup>* = = − <sup>K</sup> (18)

−*dl v v d* ≤ − +Δ ≤ τ

(15)

(16)

i.e.,

One of the advantages of the LP method is that various constraints can easily be added in the opening constraint set. Here we provide some demonstrations of constraints that are transformed from the dynamics, sensor data, and searching boundaries.

#### **3.3.1 The constraints from the kinematics and dynamics**

The kinematics and dynamics are extremely simplified in the path planning model where only the bound of the velocity and acceleration are added in the feasible set. These bound can be described mathematically as

$$\begin{cases} -V\_{A\max} \le \upsilon\_{Aj} \le V\_{A\max} \\ -\Delta\_{\max} \le \Delta \upsilon\_j \le \Delta\_{\max} \end{cases} \tag{23}$$

where *vAj* denotes the components of **V**A, as shown in Fig. 1. Δmax denotes the max magnitude of the changing of *vAj* in each period τ.

$$-V\_{A\max} \le \upsilon\_{A\circ} + \Delta \upsilon\_j \pi \le V\_{A\max} \tag{24}$$

We get the net constraints (25) from the simultaneous equations (23) and (24).

$$\max \left\{ -\Lambda\_{\max}, -\frac{1}{\tau} (V\_{A\max} + \upsilon\_{Aj}) \right\} \le \Lambda \upsilon\_j \le \min \left\{ \Lambda\_{\max}, \frac{1}{\tau} (V\_{A\max} - \upsilon\_{Aj}) \right\} \tag{25}$$

#### **3.3.2 The constraints from the limit of the sensor**

All sensors can not detect the area that is beyond their capability. In real application, the obstacle that has been avoided by the vehicle will possibly disappear from the sensor's operating region. Similarly, any new obstacle from long distance is possible entering the detecting area. On this point, we evaluate the threat of each obstacle in the environment and propose a threat factor for each of them to estimate the performance. More specially, if the relative distance between the vehicle and the obstacle *Oi* satisfies L*AOi*≥Lmin*i*, this

Path Planning of Mobile Robot in Relative Velocity Coordinates 137

{ } ( ) { } ( )

*A Aj j A Aj*

*v v v*

 τ

τ

**Initial Position (cm) Initial Velocity (cm/s) Radius (cm)** 

=20ms. The three parameters

(29)

max max max max

1 1

−Δ − + ≤ Δ ≤ Δ −

The approach proposed in this chapter is simulated with three obstacles and the results are given scenarios of in 2D and 3D, respectively. See Fig.5 and Fig.6. All the simulations run on the platform of WinXP/Pentium IV 2.53 GHz/2G RAM. A linear programming solver, named QSopt, is called from the library (David et al., 2011). We run the examples with three obstacles and give the results in two-dimensional environment and three-dimensional

According to the LP model, as shown with Eq. (29), all initial parameters are listed in Table 2. Assuming that the maximal velocity of the vehicle in 2D is 50cm/s, and the maximal

Fig. 5(a)~(d) show the key scenarios when avoiding the three obstacles. Fig. 5(a) shows the situation when the robot avoiding SO. It is obvious that the robot can rapidly adjust its velocity to the most favorable one and go on pursuing the target. The planner calls LP algorithm about 13 times in this stage. Fig. 5(b) shows the case while the robot has to avoid the moving obstacle MO1. At this time, the robot turns left to avoid MO1 because the over speeding is accessible for the robot. Fig. 5(c) shows the different decision that the robot selected to avoid MO2 from its back. Two conclusions can be drawn from this simulation. First, the robot is indeed qualified the avoiding and pursuing ability in uncertain environment. Second, the robot can adjust its velocity autonomously, including the magnitude and the direction, and adapt the optimal decision to avoid the obstacle and to catch the target. The whole pursuing process of this example lasts about 640ms and each period of calculation of our algorithm takes only about 0.469ms which is a low time

**Robot (A)** (0,0) (10,0) N/A **Target (G)** (1000,1000) (-12,0) 50 **Static Obstacle (SO)** (300,300) N/A 100 **Moving Obstacle (MO1)** (800,600) (-20,0) 50 **Moving Obstacle (MO2)** (300,900) (6,-3) 50

( )

≤ +Δ ≤

 τ 2

 γ

*T*

2

*q*

1

 π

max , V min , V

( )

*AG AG T C C AG*

τ

**4. Path planning simulation for TPOA simulation** 

acceleration of the vehicle is 350cm/s2. The panning period is

will be kept the same in the following simulation.

complexity for real-time application.

Table 2. The initial parameters for simulation in 2D.

*i AOCi i AOi AOi jj j j j*

*dl v v d g gq*

**V V V V**

τ

− ≤ − +Δ ≤ ≤ +∇ ⋅Δ ≤

( )

− −∇ Δ ≤

 λ γ

*dom*

0 V

λγ

⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪⎩

*A*

Δ ∈

**V**

environment, respectively.

**4.1 Path planning in 2D** 

obstacle is assigned λ*i*=0. Lmin*i* is the threshold. On the contrary, *Oi* is assigned λ*i*=1 if L*AOi*<Lmin*i*.

In addition, the *relative obstacle angle* has impact on the vehicle. If the velocity vector of the vehicle satisfies γ*AOi* ≥ γ*AOCi* + Δγ*AOi*max, the vehicle can be looked as moving from the obstacle *Oi*, λ*i* will be assigned zero.

#### **3.3.3 The constraints from the searching region**

The domain of the variables is generally computed in a square in 2D or a cube in 3D for the interceptive magnitude limit of the acceleration on each axis. In fact, the acceleration on each axis is coupled. So if the domain is treated as a circle in 2D (Richards & How, 2002) or a sphere in 3D, the algorithm will lower its conservation. See Fig. 4. We denote the domain as *dom*. That is

$$
\Delta \mathbf{V}\_A \in dom \tag{26}
$$

(a) Searching region in 2D; (b) Searching region in 3D

Fig. 4. Searching region for acceleration. (a) The domain of the acceleration will be approximated by multiple lines in 2D. (b) The domain of the acceleration will be approximated by multiple planes in 3D.

We try to approximate the *dom* with multiple lines in 2D, or multiple planes in 3D. That is

$$
\sin\frac{2m\pi}{M}\Delta v\_x + \cos\frac{2m\pi}{M}\Delta v\_y \le \Delta\_{\text{max}}\tag{27}
$$

where *m*=0,1,2,…, *M*-1. Here, *M* represents the quantity of the line used for approximation. In three-dimensional environment,

$$
\sin\frac{2m\pi}{M}\cos\frac{2n\pi}{M}\Delta v\_x \to \sin\frac{2m\pi}{M}\sin\frac{2n\pi}{M}\Delta v\_y \to \cos\frac{2m\pi}{M}\Delta v\_z \le \Lambda\_{\text{max}}\tag{28}
$$

where *m*, *n*=0,1,2,…,*M*-1. Here, *M* represents the quantity of the plane used for approximation. At this point, the linear programming model is formulized. The objective function is (8). All constrains are recapitulated as the following.

$$\begin{cases} \lambda\_i \mathbf{y}\_{ACG} \le \Delta\_i \left( \mathbf{y}\_{Aoi} + \Delta \mathbf{y}\_{Aoi} \right) \le \pi \\ -d\_j \le l\_j - \left( v\_j \pi + \Delta v\_j \pi^2 \right) \le d\_j \\ 0 \le g\left( \mathbf{V}\_{AC} \right) + \nabla g \cdot \Delta \mathbf{V}\_{AC}^T \le q\_1 \\ -\mathbf{V}\_C - \nabla \mathbf{V}\_C \Delta \mathbf{V}\_{AC}^T \le q\_2 \\ \max\left\{ -\Delta\_{\max}, -\frac{1}{\tau} \left( \mathbf{V}\_{A\max} + v\_{Aj} \right) \right\} \le \Delta v\_j \le \min\left\{ \Delta\_{\max}, \frac{1}{\tau} \left( \mathbf{V}\_{A\max} - v\_{Aj} \right) \right\} \\ \Delta \mathbf{V}\_A \in dom \end{cases} \tag{29}$$

#### **4. Path planning simulation for TPOA simulation**

The approach proposed in this chapter is simulated with three obstacles and the results are given scenarios of in 2D and 3D, respectively. See Fig.5 and Fig.6. All the simulations run on the platform of WinXP/Pentium IV 2.53 GHz/2G RAM. A linear programming solver, named QSopt, is called from the library (David et al., 2011). We run the examples with three obstacles and give the results in two-dimensional environment and three-dimensional environment, respectively.

#### **4.1 Path planning in 2D**

136 Recent Advances in Mobile Robotics

obstacle is assigned λ*i*=0. Lmin*i* is the threshold. On the contrary, *Oi* is assigned λ*i*=1 if

In addition, the *relative obstacle angle* has impact on the vehicle. If the velocity vector of the vehicle satisfies γ*AOi* ≥ γ*AOCi* + Δγ*AOi*max, the vehicle can be looked as moving from the obstacle

The domain of the variables is generally computed in a square in 2D or a cube in 3D for the interceptive magnitude limit of the acceleration on each axis. In fact, the acceleration on each axis is coupled. So if the domain is treated as a circle in 2D (Richards & How, 2002) or a sphere in 3D, the algorithm will lower its conservation. See Fig. 4. We denote the domain as

(a) Searching region in 2D; (b) Searching region in 3D

We try to approximate the *dom* with multiple lines in 2D, or multiple planes in 3D. That is

where *m*=0,1,2,…, *M*-1. Here, *M* represents the quantity of the line used for approximation.

where *m*, *n*=0,1,2,…,*M*-1. Here, *M* represents the quantity of the plane used for approximation. At this point, the linear programming model is formulized. The objective function is (8). All

 π

*<sup>x</sup>* Δ*v*

max

Δ + Δ ≤Δ (27)

 πΔ + Δ + Δ ≤Δ (28)

max

Fig. 4. Searching region for acceleration. (a) The domain of the acceleration will be approximated by multiple lines in 2D. (b) The domain of the acceleration will be

*x*

max 2Δ

2 2 sin cos *x y m m v v M M*

22 22 2 sin cos sin sin cos *<sup>x</sup> y z mn mn m v vv MM MM M*

 ππ

π

*<sup>A</sup>* Δ**V** ∈ *dom* (26)

*<sup>z</sup>* Δ*v*

*C*

*M* 2*n*π

Δ**V***<sup>A</sup> Q*

*M* 2*n*π

> *y* Δ*v*

L*AOi*<Lmin*i*.

*dom*. That is

*y*

*A*

*Oi*, λ*i* will be assigned zero.

**3.3.3 The constraints from the searching region** 

approximated by multiple planes in 3D.

**V***A*

2Δmax

Δ**V***<sup>A</sup>*

In three-dimensional environment,

ππ

constrains are recapitulated as the following.

According to the LP model, as shown with Eq. (29), all initial parameters are listed in Table 2. Assuming that the maximal velocity of the vehicle in 2D is 50cm/s, and the maximal acceleration of the vehicle is 350cm/s2. The panning period is τ=20ms. The three parameters will be kept the same in the following simulation.

Fig. 5(a)~(d) show the key scenarios when avoiding the three obstacles. Fig. 5(a) shows the situation when the robot avoiding SO. It is obvious that the robot can rapidly adjust its velocity to the most favorable one and go on pursuing the target. The planner calls LP algorithm about 13 times in this stage. Fig. 5(b) shows the case while the robot has to avoid the moving obstacle MO1. At this time, the robot turns left to avoid MO1 because the over speeding is accessible for the robot. Fig. 5(c) shows the different decision that the robot selected to avoid MO2 from its back. Two conclusions can be drawn from this simulation. First, the robot is indeed qualified the avoiding and pursuing ability in uncertain environment. Second, the robot can adjust its velocity autonomously, including the magnitude and the direction, and adapt the optimal decision to avoid the obstacle and to catch the target. The whole pursuing process of this example lasts about 640ms and each period of calculation of our algorithm takes only about 0.469ms which is a low time complexity for real-time application.


Table 2. The initial parameters for simulation in 2D.

Path Planning of Mobile Robot in Relative Velocity Coordinates 139

Fig. 6 shows the results. At the beginning, SO is on the line between the robot and the target, and the robot is blocked. In the following time, the robot avoids MO1 and MO2 at step=19 and step=24, respectively. See Fig. 6(b) and (c). It is evident that the velocity-decision of the robot is optimized online. The time complexities of this simulation are 0.556ms in every

Fig. 6. Simulation in dynamic environment of 3D. The dot-line denotes the trajectories, and

the sphere denotes the target and the obstacles.

period averagely.

Fig. 5. Simulation in dynamic environment of 2D. The dot-line denotes the trajectories of the robot, the obstacles, and the target. And the circles with dot-line edge are the shape profile of any mover. SO represents static obstacle. MO1 and MO2 represent moving obstacles. *G* represents the moving target.

#### **4.2 Path planning in 3D**

For the specific 3D environment, assuming that there is one static obstacle SO1, and two moving obstacles, MO1 and MO2. All the initial parameters are listed in Table 3.


Table 3. The initial parameters for simulation in 3D.

138 Recent Advances in Mobile Robotics

0

0

Start

SO

200

400

600

y(cm)

800

1000

1200

Start

MO2

SO

MO2

200

400

600

y(cm)

800

1000

1200

Fig. 5. Simulation in dynamic environment of 2D. The dot-line denotes the trajectories of the robot, the obstacles, and the target. And the circles with dot-line edge are the shape profile of any mover. SO represents static obstacle. MO1 and MO2 represent moving obstacles. *G*

For the specific 3D environment, assuming that there is one static obstacle SO1, and two

**Robot (A)** (0,1000,0) (15,-15,0) N/A **Target (G)** (1000,0,1000) (-5,5,0) 50 **Static Obstacle (SO)** (300,700,300) N/A 100 **Moving Obstacle (MO1)** (450,500,900) (0,0,-10) 80 **Moving Obstacle (MO2)** (450,250,850) (0,10,0) 50

**Initial Position (cm) Initial Velocity (cm/s) Radius (cm)** 

0 200 400 600 800 1000 1200

MO1

Step=20

G

MO1

Step=32

G

x(cm)

(b)

End

0 200 400 600 800 1000 1200

x(cm)

(d)

moving obstacles, MO1 and MO2. All the initial parameters are listed in Table 3.

represents the moving target.

Start

SO

Table 3. The initial parameters for simulation in 3D.

0 200 400 600 800 1000 1200

MO1

Step=13

G

MO1

Step=24

G

x(cm)

(a)

0 200 400 600 800 1000 1200

x(cm)

(c)

**4.2 Path planning in 3D** 

0

200

400

600

y(cm)

800

1000

1200

0

Start

MO2

SO

MO2

200

400

600

y(cm)

800

1000

1200

Fig. 6 shows the results. At the beginning, SO is on the line between the robot and the target, and the robot is blocked. In the following time, the robot avoids MO1 and MO2 at step=19 and step=24, respectively. See Fig. 6(b) and (c). It is evident that the velocity-decision of the robot is optimized online. The time complexities of this simulation are 0.556ms in every period averagely.

Fig. 6. Simulation in dynamic environment of 3D. The dot-line denotes the trajectories, and the sphere denotes the target and the obstacles.

Path Planning of Mobile Robot in Relative Velocity Coordinates 141

*N +N v o*

∑

*dkij* is the "additional distance" due to the obstacles and the vehicles other than V-i itself (with respect to vehicle-i, not only the obstacles, but also the other vehicles are its

needed by V-i to pursue T-j, and the "extended distance" consists of the linear distance, the angle distance, as well as the possible obstacle avoidance between V-i and T-j (Yang et

The simulations demonstrated here include three robots, three moving target and three moving obstacles in two-dimensional environment (see Fig. 7). All the initial parameters are

**Robot (R1)** (0,200) (17,0) N/A

**Robot (R2)** (0,0) (17,0) N/A

**Robot (R3)** (200,0) (17,0) N/A

**Target (G1)** (900,600) (-11,9) 40

**Target (G2)** (800,800) (-10,10) 40

**Target (G3)** (600,900) (-9,11) 40

**Moving Obstacle (MO1)** (850,450) (-11,9) 60

**Moving Obstacle (MO2)** (750,650) (-10,10) 60

**Moving Obstacle (MO3)** (550,750) (-9,11) 60

In order to test the performance of the proposed linear programming model in the presence of different-motion vehicles, the following modifications are made during the pursuit

① at *k*=0, the maximal robot velocities are *v*1-max=30 cm/s, *v*2-max=68 cm/s, *v*3-max=45 cm/s; ② at *k*=16, the maximal robot velocities are *v*1-max=30 cm/s, *v*2-max=68 cm/s, *v*3-max=22.5 cm/s; ③ at *k*=31, the maximal robot velocities are *v*1-max=42.5 cm/s, *v*2-max=68 cm/s, *v*3-

Table 4. The initial parameters for MTP simulation in 2D.

1 2

*ij i*

ξ

ω

<sup>≠</sup> ΔΦ

ω

2 are positive constants. the *cij* in Eq. (33) indicates the time possibly

**Initial Position (cm) Initial Velocity (cm/s) Radius (cm)** 

*ij* are, respectively, the distance, velocity difference and heading

(31)

*<sup>i</sup>* is the maximum turning rate of V-i;

*ij kij ij k= ,k i*

1

ξ

*d+ d c = + v*

Δ

*ij*

Φ

difference between vehicle-i (V-i) and target-j (T-j).

where *dij*, Δ*vij* and Δ

ξ1 and ξ

**5.3 Simulation of MTP in 2D** 

obstacles).

al., 2009).

process.

max=22.5 cm/s.

listed in Table 4.

#### **5. Path planning for multiple tasks planning (MTP): an application**

MTP problem is a variant of multiple TPOA. It can be stated that given *N* targets and *N* vehicles, let each vehicle can only pursue one target and each target is pursued exactly by one vehicle at any interval. The task is finished until all the targets have been caught. Then, the goal of MTP problem is to assign the tasks to vehicles within each interval online as well as complete the whole mission as fast as possible.

In this section, MTP problem is decomposed into two consecutive models, i.e., the taskassignment model and the path planning model. The path planning model has already been introduced above. With respect to the task-assignment model, a minimax assignment criterion is proposed to direct the task assignment. Under this assignment criterion, the optimal assignment will cost the least time to catch all the targets from the current measurable information.

Some simulations in Fig. 7 are given to show the efficiency of the method. Vehicles pursuing target are assigned according to the minimax assignment. According to the criterion, the current task assignment prefers to finish the whole mission fastest (Yang et al., 2009).

#### **5.1 Assignment model under minimax assignment criterion**

In the assignment problem, the payment in each vehicle for a target is assumed to be known. The problem is how to assign the target for each particular vehicle that the total pursuit mission can be completed as fast as possible.

Let xij be the n2 0-1 decision variables, where xij=1 represents vehicle *i* for target *j*; otherwise, xij=0. Because the "payment" is understood as time, it is important to minimize the maximal time expended by vehicle for its pursuing process. This task assignment problem may be described as the minimax assignment model mathematically.

$$\begin{aligned} \text{min} &: z = \max\_{i,j} \{ c\_{ij} x\_{ij} \} \\ \text{subject to: } & \sum\_{i=1}^{n} x\_{ij} = 1 \quad (j = 1, \ldots, n) \\ & \sum\_{j=1}^{n} x\_{ij} = 1 \quad (i = 1, \ldots, n) \\ & x\_{ij} = 0 \text{ or } 1, \quad i, j \in \{1, \ldots, n\} \end{aligned} \tag{30}$$

where cij represents the payment in vehicle *i* for target *j* and will be given in Section 5.3. The elements *c*ij give an n2 binary cost matrix. This is a classic integer programming where the objective function is nonlinear and difficult to solve.

A solution method for the above minimax assignment problem named the operations on matrix is proposed. This solution method finds the solution directly from the cost matrix ( ) *C c n i* = *<sup>j</sup>* and the objective function (Yang et al., 2008).

#### **5.2 Global cost function**

We give the cost function of this MTP problem as the following. Let *Nv*, *NT*, and *No* be the number of vehicles, targets and obstacles, respectively. The payment of the vehicle-i to pursue the target-j in the assignment problem is written as:

$$c\_{ij} = \frac{d\_{ij} + \xi\_1 \sum\_{k=1, k \neq i}^{N\_v + N\_o} d\_{kij}}{\Delta v\_{ij}} + \frac{\xi\_2 \left| \Delta \Phi\_{ij} \right|}{o o\_i} \tag{31}$$

where *dij*, Δ*vij* and ΔΦ*ij* are, respectively, the distance, velocity difference and heading difference between vehicle-i (V-i) and target-j (T-j). ω*<sup>i</sup>* is the maximum turning rate of V-i; *dkij* is the "additional distance" due to the obstacles and the vehicles other than V-i itself (with respect to vehicle-i, not only the obstacles, but also the other vehicles are its obstacles). ξ1 and ξ2 are positive constants. the *cij* in Eq. (33) indicates the time possibly needed by V-i to pursue T-j, and the "extended distance" consists of the linear distance, the angle distance, as well as the possible obstacle avoidance between V-i and T-j (Yang et al., 2009).

#### **5.3 Simulation of MTP in 2D**

140 Recent Advances in Mobile Robotics

MTP problem is a variant of multiple TPOA. It can be stated that given *N* targets and *N* vehicles, let each vehicle can only pursue one target and each target is pursued exactly by one vehicle at any interval. The task is finished until all the targets have been caught. Then, the goal of MTP problem is to assign the tasks to vehicles within each interval online as well

In this section, MTP problem is decomposed into two consecutive models, i.e., the taskassignment model and the path planning model. The path planning model has already been introduced above. With respect to the task-assignment model, a minimax assignment criterion is proposed to direct the task assignment. Under this assignment criterion, the optimal assignment will cost the least time to catch all the targets from the current

Some simulations in Fig. 7 are given to show the efficiency of the method. Vehicles pursuing target are assigned according to the minimax assignment. According to the criterion, the

In the assignment problem, the payment in each vehicle for a target is assumed to be known. The problem is how to assign the target for each particular vehicle that the total pursuit

Let xij be the n2 0-1 decision variables, where xij=1 represents vehicle *i* for target *j*; otherwise, xij=0. Because the "payment" is understood as time, it is important to minimize the maximal time expended by vehicle for its pursuing process. This task assignment problem may be

{ }

(30)

current task assignment prefers to finish the whole mission fastest (Yang et al., 2009).

,

*z cx*

min : max{ }

=

1

=

∑

subject to: 1 ( 1,..., )

*x jn*

= =

*x in*

= =

*x ij n*

∈

*ij ij i j n ij i n ij j ij*

1 ( 1,..., )

=0 or 1, , 1,...,

where cij represents the payment in vehicle *i* for target *j* and will be given in Section 5.3. The elements *c*ij give an n2 binary cost matrix. This is a classic integer programming where the

A solution method for the above minimax assignment problem named the operations on matrix is proposed. This solution method finds the solution directly from the cost matrix

We give the cost function of this MTP problem as the following. Let *Nv*, *NT*, and *No* be the number of vehicles, targets and obstacles, respectively. The payment of the vehicle-i to

1

=

∑

**5.1 Assignment model under minimax assignment criterion** 

described as the minimax assignment model mathematically.

objective function is nonlinear and difficult to solve.

( ) *C c n i* = *<sup>j</sup>* and the objective function (Yang et al., 2008).

pursue the target-j in the assignment problem is written as:

**5.2 Global cost function** 

**5. Path planning for multiple tasks planning (MTP): an application** 

as complete the whole mission as fast as possible.

mission can be completed as fast as possible.

measurable information.

The simulations demonstrated here include three robots, three moving target and three moving obstacles in two-dimensional environment (see Fig. 7). All the initial parameters are listed in Table 4.


Table 4. The initial parameters for MTP simulation in 2D.

In order to test the performance of the proposed linear programming model in the presence of different-motion vehicles, the following modifications are made during the pursuit process.

① at *k*=0, the maximal robot velocities are *v*1-max=30 cm/s, *v*2-max=68 cm/s, *v*3-max=45 cm/s;

② at *k*=16, the maximal robot velocities are *v*1-max=30 cm/s, *v*2-max=68 cm/s, *v*3-max=22.5 cm/s; ③ at *k*=31, the maximal robot velocities are *v*1-max=42.5 cm/s, *v*2-max=68 cm/s, *v*3 max=22.5 cm/s.

Path Planning of Mobile Robot in Relative Velocity Coordinates 143

MO3

(a)

0

200

R1

R2 R3

400

600

y(cm)

0

1

2

Target

3

4

800

1000

1200

0 200 400 600 800 1000 1200

MO1

MO2

G2

G3

G1

R1 R2 R3

x(cm)

(b)

1 3 5 7 9 11 13 15 17 19 21 23 25 27 29 31 33 35 37 39 41 43 45

generation

Fig. 8. Simulation of MTP in 2D. (a) Planning trajectories; (b) robot-target assignment pairs. As Fig. 8 shows, the pair assignment alters between {R1-G3, R2-G2, R3-G1} and {R1-G1, R2- G2, R3-G3} at initial 15 steps according to the minimax assignment computation. At time step 16, assignment is kept for {R1-G3, R2-G2, R3-G1} due to that R3 is the slowest robot after its maximal velocity reducing 50%, and it can only catch G1. R2 is the fastest robot, so it

(a) Initial positions at *k*=0; (b) The target and obstacle trajectories till *k*=30. The targets and the obstacles are moving in a pso-sine curve.

142 Recent Advances in Mobile Robotics

MO3

G3

(a)

MO3

0 200 400 600 800 1000

G3

MO1

G2

G1

MO2

x(cm)

(b)

400 500 600 700 800 900 1000 1100

MO1

G2

G1

MO2

x(cm)

(a) Initial positions at *k*=0; (b) The target and obstacle trajectories till *k*=30. The targets and

Fig. 7. Robots, targets and obstacles.

the obstacles are moving in a pso-sine curve.

400

500

600

700

y(cm)

800

900

1000

1100

0

R1

R2 R3

y(cm)

1000

Fig. 8. Simulation of MTP in 2D. (a) Planning trajectories; (b) robot-target assignment pairs.

As Fig. 8 shows, the pair assignment alters between {R1-G3, R2-G2, R3-G1} and {R1-G1, R2- G2, R3-G3} at initial 15 steps according to the minimax assignment computation. At time step 16, assignment is kept for {R1-G3, R2-G2, R3-G1} due to that R3 is the slowest robot after its maximal velocity reducing 50%, and it can only catch G1. R2 is the fastest robot, so it

Path Planning of Mobile Robot in Relative Velocity Coordinates 145

Cocaud, C.; Jnifene, A. & Kim, B. (2008). Environment mapping using hybrid octree

David, A.; William, C.; Sanjeeb, D. & Monika, M. (2011). *QSopt Linear Programming Solver*

Fahimi, F.; Nataraj, C & Ashrafiuon, H. (2009). Real-time obstacle avoidance for multiple

Fiorini, P. & Shiller, Z. (1998). Motion Planning in Dynamic Environments Using Velocity

Hrabar, S. E. (2006). Vision-based 3D navigation for an autonomous helicopter, PhD thesis,

Jung, L. F.; Knutzon, J. S.; Oliver, J. H, et al. (2006). Three-dimensional path planning of

Kim, J. O. & Khosla, P. K. (1992). Real-time obstacle avoidance using harmonic potential

Kitamura, Y.; Tanaka, T.; Kishino, F.; et al. (1995). 3-D path planning in a dynamic

Konolige, K. (2000). A gradient method for realtime robot control, *IEEE/RSJ International* 

Krenzke, T. (2006). Ant Colony Optimization for agile motion planning. Master thesis.

Ladd, A. M. & Kavraki, L. E. (2004). Measure theoretic analysis of probabilistic path

Liu, C.; Wei, Z. & Liu, C. (2007). A new algorithm for mobile robot obstacle avoidance based

Oh, T. S.; Shin, Y. S.; Yun, S. Y., et al. (2007). A feature information based VPH for local path

pp.2310-2313, ISBN-10: 1424415314, Jinan, China, August 18-21, 2007 Nie, Y. Y.; Su, L. J. & Li, C. (2003). An Isometric Surface Method for Integer Linear

936-1941, ISSN 07431619, Anchorage, AK, USA, May 8-10, 2002

mobile robots. *Robotica*, Vol.27, No.2, pp.189-198, ISSN 02635747

China, June 26-28, 2010

02783649

ISSN 1042296X

August 5-9, 1995

ISSN 1042296X

844, ISSN 00207160

October 31-November 5, 2000

No.4, pp.405-417, ISSN 07038992

[Online], June 1, 2011, available from:

<http://www2.isye.gatech.edu/~wcook/qsopt/index.html>

University of Southern California, United States

1563478234, Portsmouth, VA, USA, September 6-8, 2006

Massachusetts Institute of Technology, United States

*Electrical and Control Engineering*, pp.1531-1536, ISBN-13: 9780769540313, Wuhan,

knowledge for UAV trajectory planning. *Canadian Journal of Remote Sensing*, Vol.34,

Obstacles. *International Journal of Robotics Research*, Vol. 17, No.7, pp.760-772. ISSN

unmanned aerial vehicles using particle swarm optimization, *11th AIAA/ISSMO Multidisciplinary Analysis and Optimization Conference*, pp.992-1001, ISBN-10:

functions. *IEEE Transactions on Robotics and Automation*, Vol.8, No.3, pp.338-349,

environment using an octree and an artificial potential field, *IEEE/RSJ International Conference on Intelligent Robots and Systems*, Vol.2, pp.474-481, Pittsburgh, PA, USA,

*Conference on Intelligent Robots and Systems*, Vol.1, pp.639-646, Piscataway, NJ, USA,

planning, *IEEE Transactions on Robotics and Automation*, Vol.20, No.2, pp.229-242,

on hydrodynamics, *IEEE International Conference on Automation and Logistics*,

Programming, *International Journal of Computer Mathematics*, Vol.80, No.7, pp.835-

planning with obstacle avoidance of the mobile robot, *4th International Conference on Mechatronics and Information Technology*. Vol.6794. Bellingham, WA, USA: SPIE Richards, A. & How, J. P. (2002). Aircraft trajectory planning with collision avoidance using

mixed integer linear programming. *Proceeding of American Control Conference*, pp.

pursues the fastest target G2. When *k*=35 and *k*=42, target G2 and G3 are caught by R2 and R1, respectively. Finally, G1 is caught at *k*=45 by R3, so the mission is completed successfully. The dot-lines are the planned trajectories of the robots and the colorful dots distinguish every step. From the figures we can see that the robots avoid the obstacles and catch all the moving targets successfully.

### **6. Conclusion**

Path planning is a fundamental problem in robot application. In order to solve the path planning in dynamic environment, this chapter proposes a method based on LP/MILP to plan the acceleration of the robot in relative velocity coordinates. This method has the uniform formulation for 2D and 3D environment and it can give the information of the optimal velocity and acceleration in the view of the special cost function. Multiple constrains, such as the bounds of velocity, acceleration, and sensors, are included in the LP model and the MILP model. We also can add other constrains easily.

A particular application of this method is discussed for the problem of the multi-task planning where several robots are set to pursuit several targets. In the classical cooperation problem, the targets are assumed to be dynamic, similar to the TPOA problem. In the solution of MTP problem, a minimax assignment criterion and a global cost function are proposed to direct the task assignment.

Many simulations about the path planning in 2D/3D and in the multi-task planning requirements are taken to verify this novel method. The results show the low computing load of this method so it is potential to apply in real time manner.

### **7. Acknowledgment**

This work was partially supported by Natural Science Foundation of China (NSFC) under grant #61035005 and #61075087, Hubei Provincial Education Department Foundation of China under grant #Q20111105, and Hubei Provincial Science and Technology Department Foundation of China under grant #2010CDA005.

#### **8. References**


144 Recent Advances in Mobile Robotics

pursues the fastest target G2. When *k*=35 and *k*=42, target G2 and G3 are caught by R2 and R1, respectively. Finally, G1 is caught at *k*=45 by R3, so the mission is completed successfully. The dot-lines are the planned trajectories of the robots and the colorful dots distinguish every step. From the figures we can see that the robots avoid the obstacles and

Path planning is a fundamental problem in robot application. In order to solve the path planning in dynamic environment, this chapter proposes a method based on LP/MILP to plan the acceleration of the robot in relative velocity coordinates. This method has the uniform formulation for 2D and 3D environment and it can give the information of the optimal velocity and acceleration in the view of the special cost function. Multiple constrains, such as the bounds of velocity, acceleration, and sensors, are included in the LP

A particular application of this method is discussed for the problem of the multi-task planning where several robots are set to pursuit several targets. In the classical cooperation problem, the targets are assumed to be dynamic, similar to the TPOA problem. In the solution of MTP problem, a minimax assignment criterion and a global cost function are

Many simulations about the path planning in 2D/3D and in the multi-task planning requirements are taken to verify this novel method. The results show the low computing

This work was partially supported by Natural Science Foundation of China (NSFC) under grant #61035005 and #61075087, Hubei Provincial Education Department Foundation of China under grant #Q20111105, and Hubei Provincial Science and Technology Department

Allaire, F. C. J.; Tarbouchi, M.; Labont´e G.; et al. (2009). FPGA implementation of genetic

Boyd S. & Vandenberghe L. (2004). Convex Optimization. Cambridge University Press,

Chen, M.; Wu, Q. X. & Jiang, C. S. (2008). A modified ant optimization algorithm for path

Chen, Y. & Han J. (2010). LP-Based Path Planning for Target Pursuit and Obstacle

algorithm for UAV real-time path planning. *Journal of Intelligent and Robotic Systems*,

planning of UCAV. *Applied Soft Computing Journal*, Vol.8, No.4, pp.1712-1718, ISSN:

Avoidance in 3D Relative Coordinates. *Proceeding of American Control Conference*, pp.5394-5399, ISBN-13: 9781424474264, Baltimore, MD, USA, June 30- July 2, 2010 Chen, Yang; Zhao, Xingang; Zhang, Chan; et al. (2010). Relative coordination 3D trajectory

generation based on the trimmed ACO. *Proceedings of International Conference on* 

model and the MILP model. We also can add other constrains easily.

load of this method so it is potential to apply in real time manner.

catch all the moving targets successfully.

proposed to direct the task assignment.

Foundation of China under grant #2010CDA005.

ISBN: 0521833787, New York

Vol.54, No.1-3, pp.495-510, ISSN 09210296

**7. Acknowledgment** 

**8. References** 

15684946

**6. Conclusion** 

*Electrical and Control Engineering*, pp.1531-1536, ISBN-13: 9780769540313, Wuhan, China, June 26-28, 2010


<http://www2.isye.gatech.edu/~wcook/qsopt/index.html>


**0**

**8**

<sup>1</sup>*Russia* <sup>2</sup>*Germany*

**Reachable Sets for Simple Models of Car Motion**

<sup>1</sup>*Institute of Mathematics and Mechanics, Ural Branch of the Russian Academy of Sciences*

In 1889, Andrey Andreevich Markov published a paper in "Soobscenija Charkovskogo Matematiceskogo Obscestva" where he considered four mathematical problems related to the design of railways. The simplest among these problems (and the first one in course of the presentation) is described as follows. Find a minimum length curve between two points in the plane provided that the curvature radius of the curve should not be less than a given quantity and the tangent to the curve should have a given direction at the initial point. In 1951, Rufus Philip Isaacs submitted his first Rand Corporation Report on differential game theory where he stated and lined out a solution to the "homicidal chauffeur" problem. In that problem, a "car" with a bounded turning radius and a constant magnitude of the linear velocity tries as soon as possible to approach an avoiding the encounter "pedestrian". At the

In 1957, in American Journal of Mathematics, Lester Eli Dubins considered a problem in the plane on finding among smooth curves of bounded curvature a minimum length curve connecting two given points provided that the outgoing direction at the first point and

Obviously, if one takes a particular case of Isaacs' problem with the immovable "pedestrian", then the "car" will minimize the length of the curve with the bounded turning radius. The arising task coincides with the problem considered by A. A. Markov. The difference from the problem by L. E. Dubins is in the absence of a specified direction at the incoming point. The fixation of incoming and outgoing directions presents in the other three problems by A. A. Markov. However, they contain additional conditions inherent to the railway

In such a way the notion of a "car" which moves only forward and has bounded turning

In 1990, in Pacific Journal of Mathematics, James Alexander Reeds and Lawrence Alan Shepp considered an optimization problem where the object with bounded turning radius and constant magnitude of the linear velocity can instantaneously change the direction of motion to the opposite one. In a similar way, carts move around storage rooms. Thus, the model of

**1. Introduction**

construction.

radius appeared.

initial time, the direction of the car velocity is given.

incoming direction at the second point are specified.

the car that can move forward and backward has appeared.

Andrey Fedotov1, Valerii Patsko1 and Varvara Turova2

<sup>2</sup>*Mathematics Centre, Technische Universität München*


### **Reachable Sets for Simple Models of Car Motion**

Andrey Fedotov1, Valerii Patsko1 and Varvara Turova2

<sup>1</sup>*Institute of Mathematics and Mechanics, Ural Branch of the Russian Academy of Sciences* <sup>2</sup>*Mathematics Centre, Technische Universität München* <sup>1</sup>*Russia*

<sup>2</sup>*Germany*

#### **1. Introduction**

146 Recent Advances in Mobile Robotics

Schouwenaars, T.; How, J. & Feron, E. (2004). Receding Horizon Path Planning with Implicit

Schumacher, C. J.; Chandler, P. R.; Pachter, M.; et al. (2004). Constrained optimization for

Shima, T.; Rasmussen, S. J.; Sparks, A. G.; et al. (2006). Multiple task assignments for

Wang, Y.; Lane, D. M. & Falconer, G. J. (2000). Two novel approaches for unmanned

constrained optimization. *Robotica*, Vol.18, No.2, pp.123-142, ISSN 02635747 Yang, L. Y.; Nie, M. H.; Wu, Z. W.; et al. (2008). Modeling and Solution for Assignment

Yang, L. Y.; Wu, C. D.; Han, J. D.; et al. (2009). The task assignment solution for multi-target

Zhao, L. & Murthy, V. R. (2007). Optimal flight path planner for an unmanned helicopter by

*Operations Research*, Vol.33, No.11, pp.3252-3269, ISSN 03050548

07431619, Boston, MA, USA, June 30 - July 2, 2004

*conference*, AIAA 2004-5352, Providence, RI., 2004

Vol. 2, No. 2, pp.205-212, ISSN: 1998-0140

China, June 21-23, 2006

*Robotica*, Vol.15, No.4, pp.421-434, ISSN 02635747

Safety Guarantees. *Proceeding of the American Control Conference*, pp. 5576-5581, ISSN

UAV task assignment, *Proceedings of the AIAA guidance, navigation, and control* 

cooperating uninhabited aerial Vehicles using genetic algorithms, *Computers &* 

underwater vehicle path planning: constrained optimisation and semi-infinite

Problem, *International Journal of Mathematical Models and Methods in Applied Sciences*,

pursuit problem. *International Conference on Computational Intelligence and Software Engineering*, pp.1–6, ISBN-13 9781424445073, Wuhan, China, December 11-13, 2009 Zhang, Y. & Valavanis K P. (1997). A 3-D potential panel method for robot motion planning.

evolutionary algorithms, *AIAA Guidance, Navigation and Control Conference,* Vol.4, pp.3716-3739, ISBN-10: 1563479044, Hilton Head, SC, USA, August 20-23, 2007 Zu, D.; Han, J. & Tan, D. (2006). Acceleration Space LP for the Path Planning of Dynamic

Target Pursuit and Obstacle Avoidance. *Proceedings of the 6th World Congress on Intelligent Control and Automation*, Vol.2, pp.9084-9088, ISBN-10 1424403324, Dalian, In 1889, Andrey Andreevich Markov published a paper in "Soobscenija Charkovskogo Matematiceskogo Obscestva" where he considered four mathematical problems related to the design of railways. The simplest among these problems (and the first one in course of the presentation) is described as follows. Find a minimum length curve between two points in the plane provided that the curvature radius of the curve should not be less than a given quantity and the tangent to the curve should have a given direction at the initial point.

In 1951, Rufus Philip Isaacs submitted his first Rand Corporation Report on differential game theory where he stated and lined out a solution to the "homicidal chauffeur" problem. In that problem, a "car" with a bounded turning radius and a constant magnitude of the linear velocity tries as soon as possible to approach an avoiding the encounter "pedestrian". At the initial time, the direction of the car velocity is given.

In 1957, in American Journal of Mathematics, Lester Eli Dubins considered a problem in the plane on finding among smooth curves of bounded curvature a minimum length curve connecting two given points provided that the outgoing direction at the first point and incoming direction at the second point are specified.

Obviously, if one takes a particular case of Isaacs' problem with the immovable "pedestrian", then the "car" will minimize the length of the curve with the bounded turning radius. The arising task coincides with the problem considered by A. A. Markov. The difference from the problem by L. E. Dubins is in the absence of a specified direction at the incoming point. The fixation of incoming and outgoing directions presents in the other three problems by A. A. Markov. However, they contain additional conditions inherent to the railway construction.

In such a way the notion of a "car" which moves only forward and has bounded turning radius appeared.

In 1990, in Pacific Journal of Mathematics, James Alexander Reeds and Lawrence Alan Shepp considered an optimization problem where the object with bounded turning radius and constant magnitude of the linear velocity can instantaneously change the direction of motion to the opposite one. In a similar way, carts move around storage rooms. Thus, the model of the car that can move forward and backward has appeared.

Finally, one can consider non-symmetric constraint *u* ∈ [*b*, 1] instead of the bound |*u*| ≤ 1. Assume that the values of the parameter *b* satisfy the inclusion *b* ∈ [−1, 0). In this case, controlled system preserves essential properties inherent to the case |*u*| ≤ 1 (since the value

Reachable Sets for Simple Models of Car Motion 149

Let us write down the dynamics of the last system as the most general one among the above

The model (4) is kinematic, since it does not take into account forces acting on the body. The car is represented as a point mass. The control *u* determines the angular rate of the linear

In the paper, reachable sets at given time and those ones by given time for system (4) are studied using numerical constructions. The reachable set at time *t*<sup>∗</sup> is a collection of all states which can be obtained exactly at given time *t*∗. If such states are considered in the plane *x*, *y*, then we have a two-dimensional reachable set; if the consideration takes place in space *x*, *y*, *θ*, then a three-dimensional set is obtained. The peculiarity of the reachable set "by given time" is in accounting for the states reachable not only at time *t*<sup>∗</sup> but on the whole interval [0, *t*∗].

Let *z*<sup>0</sup> = (*x*0, *y*0) be the initial geometric position and *θ*<sup>0</sup> be the initial angle at time *t*<sup>0</sup> = 0. The reachable set *<sup>G</sup>*2(*t*∗; *<sup>z</sup>*0, *<sup>θ</sup>*0) at given time *<sup>t</sup>*<sup>∗</sup> in the plane *<sup>x</sup>*, *<sup>y</sup>* is the set of all geometric positions which can be reached from the starting point *z*0, *θ*<sup>0</sup> at time *t*<sup>∗</sup> by the trajectories of system (4)

Introducing the notation *z*(*t*∗; *z*0, *θ*0, *u*(·), *w*(·)) for the solution of differential equation (4) with

Since the right-hand side of system (4) does not contain variables *x*, *y*, and due to the type of appearance of *θ* in the right-hand side of (4), the geometry of reachable sets does not depend

*<sup>G</sup>*2(*t*∗; *<sup>z</sup>*0, *<sup>θ</sup>*0) = <sup>Π</sup>*θ*<sup>0</sup> (*G*2(*t*∗)) + *<sup>z</sup>*0. Here, Π*<sup>θ</sup>* is the operator of clockwise rotation of the plane *x*, *y* by the angle *θ*0. Therefore, the study of reachable sets for point-wise initial conditions can be restricted to the case *z*<sup>0</sup> = 0,

*t*∈[0,*t*∗]

*z*(*t*∗; *z*0, *θ*0, *u*(·), *w*(·)).

*G*2(*t*; *z*0, *θ*0).

*u*(·),*w*(·)

*θ* = *u*; *u* ∈ [*b*, 1], *w* ∈ [*a*, 1]. (4)

*u* = 0 remains the internal point of the restriction on *u*).

**3. Two-dimensional reachable sets**

the initial point *z*0, *θ*0, one can write

on *z*0, *θ*0. Namely,

*θ*<sup>0</sup> = 0.

**3.1 Reachable sets at given and by given time**

using admissible piecewise continuous controls *u*(·), *w*(·).

*<sup>G</sup>*2(*t*∗; *<sup>z</sup>*0, *<sup>θ</sup>*0) :<sup>=</sup>

The reachable sets by given time are defined in a similar way. Let

Other formulas remain the same, we only replace *<sup>G</sup>*<sup>2</sup> by <sup>G</sup>2.

<sup>G</sup>2(*t*∗; *<sup>z</sup>*0, *<sup>θ</sup>*0) :<sup>=</sup>

For *<sup>z</sup>*<sup>0</sup> <sup>=</sup> 0, *<sup>θ</sup>*<sup>0</sup> <sup>=</sup> 0, the notation *<sup>G</sup>*2(*t*∗) will be used instead of *<sup>G</sup>*2(*t*∗; 0, 0).

*x*˙ = *w* sin *θ*, *y*˙ = *w* cos *θ*, ˙

velocity vector; the control *w* changes the magnitude of the linear velocity.

Here, *a* and *b* are fixed parameters with *a* ∈ [−1, 1], *b* ∈ [−1, 0).

mentioned:

Two models (the forward moving car with bounded turning radius; the forward and backward moving car with bounded turning radius) gave rise to numerous literature where various optimization problems related to the transportation are studied. More sophisticated models in which a moving object is considered in a more realistic way have arisen (controlled wheel, bicycle, car with two chassis, car with a trailer). Optimization problems related to such complicated tasks are extremely difficult. The simplest models serve as a "sample" showing where the situation is easy and where it becomes complex.

One of the key notion in the mathematical control theory (Pontryagin et al., 1962), (Lee & Markus, 1967), (Agrachev & Sachkov, 2004) is reachable set. The reachable set is a collection of states which can be attained within the framework of the motion model under consideration. The reachable set at given time describes a collection of states realizable at a specified time instant. The reachable set by given time is a collection of states that can be obtained on the whole time interval from an initial time instant to a specified one.

This paper is devoted to the investigation of reachable sets at given and by given time for simplest models of car motion.

#### **2. Simple models of car motion**

The simplest car dynamics are described in dimensionless variables by the following system of equations

$$\dot{\mathfrak{x}} = \sin \theta, \ \dot{y} = \cos \theta, \ \dot{\theta} = u; \quad |u| \le 1. \tag{1}$$

The variables *x*, *y* specify the center mass position in the two-dimensional plane; *θ* is the angle between the direction of the velocity vector and that of the vertical axis *y* measured clockwise from the latter. The value *u* acts as a control input. The control *u*(*t*) specifies the instantaneous angular velocity of the vector (*x*˙(*t*), *y*˙(*t*)) of linear velocity and is bounded as |*u*(*t*)| ≤ 1.

The motion trajectories in the plane *x*, *y* are curves of bounded curvature. The paper (Markov, 1889) considers four optimization problems related to curves of bounded curvature. The first problem (Markov, 1889), p. 250, can be interpreted as a time-optimal control problem for car dynamics (1). Also, the main theorem (Dubins, 1957), p. 515, allows an interpretation in the context of time-optimal problem for such a car. In works on theoretical robotics (Laumond, 1998), an object with dynamics (1) is called Dubins' car. Model (1) is often utilized in differential game problem formulations (Isaacs, 1951; 1965).

Next in complexity is the car model by Reeds and Shepp (Reeds & Shepp, 1990):

$$\dot{\mathbf{x}} = w \sin \theta, \ \dot{y} = w \cos \theta, \ \dot{\theta} = u; \quad |u| \le 1, \ |w| \le 1. \tag{2}$$

Control *u* changes the angular velocity, control *w* is responsible for instantaneous changes of the linear velocity magnitude. In particular, the car can instantaneously change its motion direction to the opposite one. The angle *θ* is the angle between the direction of the axis *y* and the direction of the forward motion of the car.

It is natural to consider control dynamics where the control *w* is from the interval [*a*, 1]:

$$\dot{x} = w \sin \theta, \ \dot{y} = w \cos \theta, \ \dot{\theta} = u; \quad |u| \le 1, \ w \in [a, 1]. \tag{3}$$

Here *a* ∈ [−1, 1] is the parameter of the problem. If *a* = 1, Dubins' car is obtained; if *a* = −1, Reeds and Shepp's car appears.

2 Will-be-set-by-IN-TECH

Two models (the forward moving car with bounded turning radius; the forward and backward moving car with bounded turning radius) gave rise to numerous literature where various optimization problems related to the transportation are studied. More sophisticated models in which a moving object is considered in a more realistic way have arisen (controlled wheel, bicycle, car with two chassis, car with a trailer). Optimization problems related to such complicated tasks are extremely difficult. The simplest models serve as a "sample" showing

One of the key notion in the mathematical control theory (Pontryagin et al., 1962), (Lee & Markus, 1967), (Agrachev & Sachkov, 2004) is reachable set. The reachable set is a collection of states which can be attained within the framework of the motion model under consideration. The reachable set at given time describes a collection of states realizable at a specified time instant. The reachable set by given time is a collection of states that can be obtained on the

This paper is devoted to the investigation of reachable sets at given and by given time for

The simplest car dynamics are described in dimensionless variables by the following system

The variables *x*, *y* specify the center mass position in the two-dimensional plane; *θ* is the angle between the direction of the velocity vector and that of the vertical axis *y* measured clockwise from the latter. The value *u* acts as a control input. The control *u*(*t*) specifies the instantaneous angular velocity of the vector (*x*˙(*t*), *y*˙(*t*)) of linear velocity and is bounded as |*u*(*t*)| ≤ 1. The motion trajectories in the plane *x*, *y* are curves of bounded curvature. The paper (Markov, 1889) considers four optimization problems related to curves of bounded curvature. The first problem (Markov, 1889), p. 250, can be interpreted as a time-optimal control problem for car dynamics (1). Also, the main theorem (Dubins, 1957), p. 515, allows an interpretation in the context of time-optimal problem for such a car. In works on theoretical robotics (Laumond, 1998), an object with dynamics (1) is called Dubins' car. Model (1) is often utilized

*θ* = *u*; |*u*| ≤ 1. (1)

*θ* = *u*; |*u*| ≤ 1, |*w*| ≤ 1. (2)

*θ* = *u*; |*u*| ≤ 1, *w* ∈ [*a*, 1]. (3)

*x*˙ = sin *θ*, *y*˙ = cos *θ*, ˙

Next in complexity is the car model by Reeds and Shepp (Reeds & Shepp, 1990):

Control *u* changes the angular velocity, control *w* is responsible for instantaneous changes of the linear velocity magnitude. In particular, the car can instantaneously change its motion direction to the opposite one. The angle *θ* is the angle between the direction of the axis *y* and

Here *a* ∈ [−1, 1] is the parameter of the problem. If *a* = 1, Dubins' car is obtained; if *a* = −1,

It is natural to consider control dynamics where the control *w* is from the interval [*a*, 1]:

where the situation is easy and where it becomes complex.

whole time interval from an initial time instant to a specified one.

in differential game problem formulations (Isaacs, 1951; 1965).

the direction of the forward motion of the car.

Reeds and Shepp's car appears.

*x*˙ = *w* sin *θ*, *y*˙ = *w* cos *θ*, ˙

*x*˙ = *w* sin *θ*, *y*˙ = *w* cos *θ*, ˙

simplest models of car motion.

of equations

**2. Simple models of car motion**

Finally, one can consider non-symmetric constraint *u* ∈ [*b*, 1] instead of the bound |*u*| ≤ 1. Assume that the values of the parameter *b* satisfy the inclusion *b* ∈ [−1, 0). In this case, controlled system preserves essential properties inherent to the case |*u*| ≤ 1 (since the value *u* = 0 remains the internal point of the restriction on *u*).

Let us write down the dynamics of the last system as the most general one among the above mentioned:

$$
\dot{\mathbf{x}} = w \sin \theta, \ \dot{y} = w \cos \theta, \ \dot{\theta} = u; \quad u \in [b, 1], \ w \in [a, 1]. \tag{4}
$$

Here, *a* and *b* are fixed parameters with *a* ∈ [−1, 1], *b* ∈ [−1, 0).

The model (4) is kinematic, since it does not take into account forces acting on the body. The car is represented as a point mass. The control *u* determines the angular rate of the linear velocity vector; the control *w* changes the magnitude of the linear velocity.

In the paper, reachable sets at given time and those ones by given time for system (4) are studied using numerical constructions. The reachable set at time *t*<sup>∗</sup> is a collection of all states which can be obtained exactly at given time *t*∗. If such states are considered in the plane *x*, *y*, then we have a two-dimensional reachable set; if the consideration takes place in space *x*, *y*, *θ*, then a three-dimensional set is obtained. The peculiarity of the reachable set "by given time" is in accounting for the states reachable not only at time *t*<sup>∗</sup> but on the whole interval [0, *t*∗].

#### **3. Two-dimensional reachable sets**

#### **3.1 Reachable sets at given and by given time**

Let *z*<sup>0</sup> = (*x*0, *y*0) be the initial geometric position and *θ*<sup>0</sup> be the initial angle at time *t*<sup>0</sup> = 0. The reachable set *<sup>G</sup>*2(*t*∗; *<sup>z</sup>*0, *<sup>θ</sup>*0) at given time *<sup>t</sup>*<sup>∗</sup> in the plane *<sup>x</sup>*, *<sup>y</sup>* is the set of all geometric positions which can be reached from the starting point *z*0, *θ*<sup>0</sup> at time *t*<sup>∗</sup> by the trajectories of system (4) using admissible piecewise continuous controls *u*(·), *w*(·).

Introducing the notation *z*(*t*∗; *z*0, *θ*0, *u*(·), *w*(·)) for the solution of differential equation (4) with the initial point *z*0, *θ*0, one can write

$$G^2(t\_\*; z\_0, \theta\_0) := \bigcup\_{\iota(\cdot), \varpi(\cdot)} z(t\_\*; z\_0, \theta\_0, \iota(\cdot), w(\cdot)).$$

For *<sup>z</sup>*<sup>0</sup> <sup>=</sup> 0, *<sup>θ</sup>*<sup>0</sup> <sup>=</sup> 0, the notation *<sup>G</sup>*2(*t*∗) will be used instead of *<sup>G</sup>*2(*t*∗; 0, 0).

Since the right-hand side of system (4) does not contain variables *x*, *y*, and due to the type of appearance of *θ* in the right-hand side of (4), the geometry of reachable sets does not depend on *z*0, *θ*0. Namely,

$$G^2(t\_\*; z\_0, \theta\_0) = \Pi\_{\theta\_0}(G^2(t\_\*)) + z\_0.$$

Here, Π*<sup>θ</sup>* is the operator of clockwise rotation of the plane *x*, *y* by the angle *θ*0. Therefore, the study of reachable sets for point-wise initial conditions can be restricted to the case *z*<sup>0</sup> = 0, *θ*<sup>0</sup> = 0.

The reachable sets by given time are defined in a similar way. Let

$$\mathcal{G}^2(t\_\*; z\_0, \theta\_0) := \bigcup\_{t \in [0, t\_\*]} G^2(t; z\_{0\prime}\theta\_0).$$

Other formulas remain the same, we only replace *<sup>G</sup>*<sup>2</sup> by <sup>G</sup>2.

t<sup>∗</sup> = π

system (1)

*y*

u −= 1

u = 0

u += 1

Reachable Sets for Simple Models of Car Motion 151

Fig. 2. Structure of the controls steering to the boundary of the reachable set *G*2(*π*) for


& Bui, 1994). Fig. 3 shows the form of the sets <sup>G</sup>2(*t*) computed by the authors on the interval

Fig. 3. Time-limited reachable sets <sup>G</sup>2(*t*) for system (1), *<sup>τ</sup><sup>f</sup>* <sup>=</sup> 7.3, *<sup>δ</sup>* <sup>=</sup> 0.1

[0, *tf* ], *tf* = 7.3, the output step for the construction results is *δ* = 0.1.

*x*

u = 0

Classical and the best known is the construction of the reachable sets *<sup>G</sup>*2(*t*∗) and <sup>G</sup>2(*t*∗) for system (1). It was established in paper (Cockayne & Hall, 1975) that any point of the boundary *<sup>∂</sup>G*2(*t*∗) can be attained using piecewise control with at most one switch. Namely, the following variants of the control actions are possible: (−1, 0),(1, 0),(−1, 1),(1, −1). The first variant means that control *<sup>u</sup>* <sup>≡</sup> 1 acts on some interval [0, <sup>ˆ</sup>*t*), and control *<sup>u</sup>* <sup>≡</sup> 0 works on the interval [ˆ*t*, *<sup>t</sup>*∗]. The similar is true for the rest of the four variants. Using this proposition and running the switch instant <sup>ˆ</sup>*<sup>t</sup>* from 0 to *<sup>t</sup>*∗, we can construct the boundary of the set *<sup>G</sup>*2(*t*∗). The form of the sets *<sup>G</sup>*2(*t*∗) for four instants of time *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> *<sup>i</sup>* 0.5*π*, *<sup>i</sup>* <sup>=</sup> 1, 2, 3, 4, is shown in Fig. 1. The chosen values *t*<sup>∗</sup> correspond to the time needed to turn the velocity vector by the angle *i* 0.5*π* when moving with the control *u* = 1 (*u* = −1). For every *t*∗, the figure has its own scale. In Fig. 2, several trajectories arriving at the boundary of the reachable set are shown. The trajectories with the extreme controls *u* = 1 and *u* = −1 are the circles of radius 1. For *u* = 0, the motion along a straight line occurs.

Fig. 1. Form of the reachable sets *<sup>G</sup>*2(*t*∗) for system (1)

Since the set <sup>G</sup>2(*t*∗) is the union of the sets *<sup>G</sup>*2(*t*) over all *<sup>t</sup>* <sup>∈</sup> [0, *<sup>t</sup>*∗], then any point of the boundary *<sup>∂</sup>*G2(*t*∗) is reachable with the control of the above mentioned structure (but this control is defined in general on [0, ˜*t*] where ˜*<sup>t</sup>* <sup>∈</sup> [0, *<sup>t</sup>*∗]). The minimum time problem for system (1) with the initial point *z*<sup>0</sup> and given direction *θ*<sup>0</sup> of the velocity vector to the point *z* for which the angle *θ* is not specified is very popular. The structure of the solution to this problem was described by A. A. Markov. It was also known to R. Isaacs, since such problem is a degenerate case of the differential game "homicidal chauffeur". Nevertheless, accurate images of reachable sets <sup>G</sup>2(*t*∗) have been appeared only in the beginning of 1990s (Boissonnat 4 Will-be-set-by-IN-TECH

Classical and the best known is the construction of the reachable sets *<sup>G</sup>*2(*t*∗) and <sup>G</sup>2(*t*∗) for system (1). It was established in paper (Cockayne & Hall, 1975) that any point of the boundary *<sup>∂</sup>G*2(*t*∗) can be attained using piecewise control with at most one switch. Namely, the following variants of the control actions are possible: (−1, 0),(1, 0),(−1, 1),(1, −1). The first variant means that control *<sup>u</sup>* <sup>≡</sup> 1 acts on some interval [0, <sup>ˆ</sup>*t*), and control *<sup>u</sup>* <sup>≡</sup> 0 works on the interval [ˆ*t*, *<sup>t</sup>*∗]. The similar is true for the rest of the four variants. Using this proposition and running the switch instant <sup>ˆ</sup>*<sup>t</sup>* from 0 to *<sup>t</sup>*∗, we can construct the boundary of the set *<sup>G</sup>*2(*t*∗). The form of the sets *<sup>G</sup>*2(*t*∗) for four instants of time *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> *<sup>i</sup>* 0.5*π*, *<sup>i</sup>* <sup>=</sup> 1, 2, 3, 4, is shown in Fig. 1. The chosen values *t*<sup>∗</sup> correspond to the time needed to turn the velocity vector by the angle *i* 0.5*π* when moving with the control *u* = 1 (*u* = −1). For every *t*∗, the figure has its own scale. In Fig. 2, several trajectories arriving at the boundary of the reachable set are shown. The trajectories with the extreme controls *u* = 1 and *u* = −1 are the circles of radius 1. For

π

π

Since the set <sup>G</sup>2(*t*∗) is the union of the sets *<sup>G</sup>*2(*t*) over all *<sup>t</sup>* <sup>∈</sup> [0, *<sup>t</sup>*∗], then any point of the boundary *<sup>∂</sup>*G2(*t*∗) is reachable with the control of the above mentioned structure (but this control is defined in general on [0, ˜*t*] where ˜*<sup>t</sup>* <sup>∈</sup> [0, *<sup>t</sup>*∗]). The minimum time problem for system (1) with the initial point *z*<sup>0</sup> and given direction *θ*<sup>0</sup> of the velocity vector to the point *z* for which the angle *θ* is not specified is very popular. The structure of the solution to this problem was described by A. A. Markov. It was also known to R. Isaacs, since such problem is a degenerate case of the differential game "homicidal chauffeur". Nevertheless, accurate images of reachable sets <sup>G</sup>2(*t*∗) have been appeared only in the beginning of 1990s (Boissonnat

*u* = 0, the motion along a straight line occurs.

*t*<sup>∗</sup> =

*t*<sup>∗</sup> = 2

Fig. 1. Form of the reachable sets *<sup>G</sup>*2(*t*∗) for system (1)

*t*<sup>∗</sup> = 5.0 π

*t*<sup>∗</sup> = 5.1 π

Fig. 2. Structure of the controls steering to the boundary of the reachable set *G*2(*π*) for system (1)

Fig. 3. Time-limited reachable sets <sup>G</sup>2(*t*) for system (1), *<sup>τ</sup><sup>f</sup>* <sup>=</sup> 7.3, *<sup>δ</sup>* <sup>=</sup> 0.1

& Bui, 1994). Fig. 3 shows the form of the sets <sup>G</sup>2(*t*) computed by the authors on the interval [0, *tf* ], *tf* = 7.3, the output step for the construction results is *δ* = 0.1.

*x*

*θ*(*t*)(*x*˜ − *x*(*t*)) − cos *θ*(*t*)*x*˙(*t*) −

*θ*(*t*)(*x*˜ − *x*(*t*)) − sin *θ*(*t*)*x*˙(*t*) −

cos *θ*(*t*)*x*˙(*t*) − sin *θ*(*t*)*y*˙(*t*) = 0, (9)

sin *θ*(*t*)*x*˙(*t*) + cos *θ*(*t*)*y*˙(*t*) = *w*(*t*). (10)

**<sup>y</sup>**˙ <sup>=</sup> **<sup>x</sup>***<sup>u</sup>* <sup>−</sup> *<sup>w</sup>*, *<sup>u</sup>* <sup>∈</sup> [*b*, 1], *<sup>w</sup>* <sup>∈</sup> [*a*, 1]. (13)

*θ*(*t*) − cos *θ*(*t*)*x*˙(*t*) + sin *θ*(*t*)*y*˙(*t*), (7)

*θ*(*t*) − sin *θ*(*t*)*x*˙(*t*) − cos *θ*(*t*)*y*˙(*t*). (8)

*θ*(*t*). (11)

*θ*(*t*) − *w*(*t*). (12)

(*t*)(*z*˜ − *z*(*t*)) = sin *θ*(*t*)(*x*˜ − *x*(*t*)) + cos *θ*(*t*)(*y*˜ − *y*(*t*)). (6)

*y*

0

Here, the prime denotes the operation of transposition.

*<sup>θ</sup>*(*t*)(*y*˜ <sup>−</sup> *<sup>y</sup>*(*t*)) + sin *<sup>θ</sup>*(*t*)*y*˙(*t*) = <sup>−</sup>**y**(*t*) ˙

*<sup>θ</sup>*(*t*)(*y*˜ <sup>−</sup> *<sup>y</sup>*(*t*)) <sup>−</sup> cos *<sup>θ</sup>*(*t*)*y*˙(*t*) = **<sup>x</sup>**(*t*) ˙

Fig. 4. Movable reference system

cos *θ*(*t*) ˙

sin *θ*(*t*) ˙

on the axis **y** is *w*(*t*). Therefore,

Using (7) and (9), we obtain

With (8) and (10), we get

Equalities (11), (12), and ˙

**y**(*t*) = *h*�

Taking into account (5) and (6), one obtains

*z*(*t*)

*θ*(*t*) *h*(*t*)

Reachable Sets for Simple Models of Car Motion 153

**y**

*<sup>k</sup>*(*t*) *<sup>z</sup>*˜

**x**

**<sup>x</sup>**˙(*t*) = <sup>−</sup> sin *<sup>θ</sup>*(*t*) ˙

**y**˙(*t*) = cos *θ*(*t*) ˙

The projection of the velocity vector of system (4) on the axis **x** at time *t* is zero, the projection

**<sup>x</sup>**˙(*t*) = <sup>−</sup>**y**(*t*) ˙

**y**˙(*t*) = **x**(*t*) ˙

*θ* = *u* yield the system

**x**˙ = −**y***u*,

#### **3.2 Reachable sets with regard to an orientedly added set**

Let us now define the reachable set at time *t*<sup>∗</sup> with regard to an orientedly added set *D*:

$$G\_D^2(t\_\*) := \bigcup\_{\mu(\cdot), w(\cdot)} [z(t\_\*; 0, 0, \mu(\cdot), w(\cdot)) + \Pi\_{\theta(t\_\*; 0, 0, \mu(\cdot), w(\cdot))}(D)].$$

Hence, when constructing *G*<sup>2</sup> *<sup>D</sup>*(*t*∗), we add the set *D* which is rigidly rotated with respect to the origin by the angle *θ*(*t*∗; 0, 0, *u*(·), *w*(·)) to each point (being attainable at the time *t*<sup>∗</sup> with *u*(·), *w*(·)) of the usual reachable set. It is assumed that *z*<sup>0</sup> = 0 and *θ*<sup>0</sup> = 0.

The sense of the set *G*<sup>2</sup> *<sup>D</sup>*(*t*∗) can be explained using the following example. Let us fix controls *u*(·), *w*(·). At the time *t*∗, the geometric position *z*(*t*∗; 0, 0, *u*(·), *w*(·)) and the slope *θ*(*t*∗; 0, 0, *u*(·), *w*(·)) of the velocity vector are realized. Suppose we are interested at this time instant in a point located at the distance *d* from the point *z*(*t*∗; 0, 0, *u*(·), *w*(·)) orthogonally to the velocity vector *z*˙(*t*∗)=(*x*˙(*t*∗), *y*˙(*t*∗)) to the left from its direction. Such point is written as *<sup>z</sup>*(*t*∗; 0, 0, *<sup>u</sup>*(·), *<sup>w</sup>*(·)) + <sup>Π</sup>*θ*(*t*∗;0,0,*u*(·),*w*(·))(*D*), if we take the set consisting from the point (−*d*, 0) in the plane *x*, *y* as the set *D*. The total collection of points at the time *t*<sup>∗</sup> with the property we are interested in is obtained by the enumeration of admissible controls *u*(·), *w*(·) and forms the set *G*<sup>2</sup> *<sup>D</sup>*(*t*∗).

The reachable set <sup>G</sup><sup>2</sup> *<sup>D</sup>*(*t*∗) by the time *t*<sup>∗</sup> with regard to an orientedly added set *D* is defined as

$$\mathcal{G}\_D^2(t\_\*) := \bigcup\_{t \in [0, t\_\*]} G\_D^2(t).$$

#### **3.3 Isaacs' transformation**

System (4) is of the third order with respect to the state variables. Since we are interested in the construction of reachable sets in the plane of geometric coordinates, it is convenient to pass to a system of the second order with respect to the state variables. This can be done using Isaacs' transformation.

Isaacs utilized system (4) with *w* ≡ 1, *u* ∈ [−1, 1] (i.e. system (1)) for the formulation and solution of several pursuit-evasion differential games. The pursuit-evasion game "homicidal chauffeur" proposed by R. Isaacs in his report (Isaacs, 1951) was then published in his famous book "Differential games" (Isaacs, 1965) and became classical problem. We will apply the transformation, which R. Isaacs used in this game, for our purposes.

Let *h*(*t*) be a unit vector in the direction of forward motion of system (4) at time *t*. The orthogonal to *h*(*t*) unit vector is denoted by *k*(*t*) (see Fig. 4). We have

$$h(t) = \begin{pmatrix} \sin\theta(t) \\ \cos\theta(t) \end{pmatrix}, \quad k(t) = \begin{pmatrix} \cos\theta(t) \\ -\sin\theta(t) \end{pmatrix}.$$

Axis **y** of the reference system is directed along the vector *h*(*t*), axis **x** is directed along the vector *k*(*t*).

Let *z*˜ = (*x*˜, *y*˜) be a fixed point in the plane *x*, *y*. The coordinates **x**(*t*), **y**(*t*) of this point in the movable reference system whose origin coincides with the point *z*(*t*) are

$$\mathbf{x}(t) = k'(t)(\tilde{\mathbf{z}} - z(t)) = \cos\theta(t)(\tilde{\mathbf{x}} - \mathbf{x}(t)) - \sin\theta(t)(\tilde{\mathbf{y}} - y(t)),\tag{5}$$

6 Will-be-set-by-IN-TECH

the origin by the angle *θ*(*t*∗; 0, 0, *u*(·), *w*(·)) to each point (being attainable at the time *t*<sup>∗</sup> with

controls *u*(·), *w*(·). At the time *t*∗, the geometric position *z*(*t*∗; 0, 0, *u*(·), *w*(·)) and the slope *θ*(*t*∗; 0, 0, *u*(·), *w*(·)) of the velocity vector are realized. Suppose we are interested at this time instant in a point located at the distance *d* from the point *z*(*t*∗; 0, 0, *u*(·), *w*(·)) orthogonally to the velocity vector *z*˙(*t*∗)=(*x*˙(*t*∗), *y*˙(*t*∗)) to the left from its direction. Such point is written as *<sup>z</sup>*(*t*∗; 0, 0, *<sup>u</sup>*(·), *<sup>w</sup>*(·)) + <sup>Π</sup>*θ*(*t*∗;0,0,*u*(·),*w*(·))(*D*), if we take the set consisting from the point (−*d*, 0) in the plane *x*, *y* as the set *D*. The total collection of points at the time *t*<sup>∗</sup> with the property we are interested in is obtained by the enumeration of admissible controls *u*(·), *w*(·) and forms

[*z*(*t*∗; 0, 0, *<sup>u</sup>*(·), *<sup>w</sup>*(·)) + <sup>Π</sup>*θ*(*t*∗;0,0,*u*(·),*w*(·))(*D*)].

*<sup>D</sup>*(*t*∗), we add the set *D* which is rigidly rotated with respect to

*<sup>D</sup>*(*t*∗) can be explained using the following example. Let us fix

*<sup>D</sup>*(*t*∗) by the time *t*<sup>∗</sup> with regard to an orientedly added set *D* is defined as

*G*2 *<sup>D</sup>*(*t*).

Let us now define the reachable set at time *t*<sup>∗</sup> with regard to an orientedly added set *D*:

**3.2 Reachable sets with regard to an orientedly added set**

*u*(·),*w*(·)

*u*(·), *w*(·)) of the usual reachable set. It is assumed that *z*<sup>0</sup> = 0 and *θ*<sup>0</sup> = 0.

G2

transformation, which R. Isaacs used in this game, for our purposes.

orthogonal to *h*(*t*) unit vector is denoted by *k*(*t*) (see Fig. 4). We have

 sin *θ*(*t*) cos *θ*(*t*)

movable reference system whose origin coincides with the point *z*(*t*) are

*h*(*t*) =

**x**(*t*) = *k*�

*<sup>D</sup>*(*t*∗) :<sup>=</sup>

*t*∈[0,*t*∗]

System (4) is of the third order with respect to the state variables. Since we are interested in the construction of reachable sets in the plane of geometric coordinates, it is convenient to pass to a system of the second order with respect to the state variables. This can be done using Isaacs'

Isaacs utilized system (4) with *w* ≡ 1, *u* ∈ [−1, 1] (i.e. system (1)) for the formulation and solution of several pursuit-evasion differential games. The pursuit-evasion game "homicidal chauffeur" proposed by R. Isaacs in his report (Isaacs, 1951) was then published in his famous book "Differential games" (Isaacs, 1965) and became classical problem. We will apply the

Let *h*(*t*) be a unit vector in the direction of forward motion of system (4) at time *t*. The

, *k*(*t*) =

Axis **y** of the reference system is directed along the vector *h*(*t*), axis **x** is directed along the

Let *z*˜ = (*x*˜, *y*˜) be a fixed point in the plane *x*, *y*. The coordinates **x**(*t*), **y**(*t*) of this point in the

cos *θ*(*t*)

(*t*)(*z*˜ − *z*(*t*)) = cos *θ*(*t*)(*x*˜ − *x*(*t*)) − sin *θ*(*t*)(*y*˜ − *y*(*t*)), (5)

− sin *θ*(*t*)

 .

*<sup>D</sup>*(*t*∗) :<sup>=</sup>

*G*2

Hence, when constructing *G*<sup>2</sup>

The sense of the set *G*<sup>2</sup>

*<sup>D</sup>*(*t*∗). The reachable set <sup>G</sup><sup>2</sup>

**3.3 Isaacs' transformation**

transformation.

vector *k*(*t*).

the set *G*<sup>2</sup>

Fig. 4. Movable reference system

$$\mathbf{y}(t) = h'(t)(\tilde{z} - z(t)) = \sin\theta(t)(\tilde{x} - x(t)) + \cos\theta(t)(\tilde{y} - y(t)).\tag{6}$$

Here, the prime denotes the operation of transposition. Taking into account (5) and (6), one obtains

$$\dot{\mathbf{x}}(t) = -\sin\theta(t)\dot{\theta}(t)(\mathbf{\tilde{x}} - \mathbf{x}(t)) - \cos\theta(t)\dot{\mathbf{x}}(t) - $$

$$\cos\theta(t)\dot{\theta}(t)(\mathbf{\tilde{y}} - \mathbf{y}(t)) + \sin\theta(t)\dot{\mathbf{y}}(t) = -\mathbf{y}(t)\dot{\theta}(t) - \cos\theta(t)\dot{\mathbf{x}}(t) + \sin\theta(t)\dot{\mathbf{y}}(t), \tag{7}$$

$$\dot{\mathbf{y}}(t) = \cos\theta(t)\dot{\theta}(t)(\ddot{\mathbf{x}} - \mathbf{x}(t)) - \sin\theta(t)\dot{\mathbf{x}}(t) - $$

$$\sin\theta(t)\dot{\theta}(t)(\ddot{y} - y(t)) - \cos\theta(t)\dot{y}(t) = \mathbf{x}(t)\dot{\theta}(t) - \sin\theta(t)\dot{\mathbf{x}}(t) - \cos\theta(t)\dot{y}(t). \tag{8}$$

The projection of the velocity vector of system (4) on the axis **x** at time *t* is zero, the projection on the axis **y** is *w*(*t*). Therefore,

$$
\cos\theta(t)\dot{x}(t) - \sin\theta(t)\dot{y}(t) = 0,\tag{9}
$$

$$
\sin\theta(t)\dot{x}(t) + \cos\theta(t)\dot{y}(t) = w(t). \tag{10}
$$

Using (7) and (9), we obtain

$$
\dot{\mathbf{x}}(t) = -\mathbf{y}(t)\dot{\theta}(t). \tag{11}
$$

With (8) and (10), we get

$$
\dot{\mathbf{y}}(t) = \mathbf{x}(t)\dot{\theta}(t) - w(t). \tag{12}
$$

Equalities (11), (12), and ˙ *θ* = *u* yield the system

$$\begin{aligned} \dot{\mathbf{x}} &= -\mathbf{y}u, \\ \dot{\mathbf{y}} &= \mathbf{x}u - w, \ u \in [b, 1], \ w \in [a, 1]. \end{aligned} \tag{13}$$

which are considered as approximations of the ideal sets *W*(*τ*, *M*). Specify a time step Δ (it

Reachable Sets for Simple Models of Car Motion 155

For system (4), we have **u** = (*u*, *w*), *P* = {(*u*, *w*) : *u* ∈ [*b*, 1], *w* ∈ [*a*, 1]}. The set **z** − Δ*E*(**z**) describes approximately the collection of points from which the point **z** can be approached at time Δ. Running over the boundary *∂W*(*τi*, *M*) and attaching the set **z** − Δ*E*(**z**) "to every" point of the boundary, we approximately construct the boundary *∂W*(*τi*<sup>+</sup>1, *M*) of the set

Theoretically, the solvability set W(*τ*∗, *M*) for the problem of approaching the set *M* by system (13) by the time *τ*<sup>∗</sup> is defined through the union of the sets *W*(*τ*, *M*), *τ* ∈ [0, *τ*∗]. However, one can reject the explicit realization of the union operation and try to construct the boundary of the set W(*τi*<sup>+</sup>1, *M*) directly on the base of the boundary of the set W(*τi*, *M*). To this end, the

Using the optimal result function *V*, the front corresponding to the time *τ* is formally defined

*F*(*τ*) := {**z** ∈ *∂*W(*τ*, *M*) : *V*(**z**) = *τ*}. If ¯**z** ∈ W/ (*τ*, *M*), then every trajectory of system (13) starting from the point ¯**z** can approach

It is known (see e.g. Bardi & Capuzzo-Dolcetta (1997)) that if *F*(*τ*∗) = *∂*W(*τ*∗, *M*) for some *τ*∗,

Let *A*(*τ*) := *∂*W(*τ*, *M*) \ *F*(*τ*). It follows from results of optimal control theory and theory of

Possible structure of the set A is not well explored for time-optimal problems. It is reasonable to assume that if the set B := A \ *M* is not empty, then it consists of a collection of smooth

By the definition of the front *F*(*τ*), we have *F*(*τ*) ⊂ *W*(*τ*, *M*). From here, with accounting for

The main idea of the algorithm of the backward construction of the sets W(*τ*, *M*) is explained in Fig. 5. The next set W(*τi*<sup>+</sup>1, *M*) for *τi*+<sup>1</sup> = *τ<sup>i</sup>* + Δ is computed on the basis of the previous set W(*τi*, *M*). The central role in this computation belongs to the front *F*(*τi*). As a result, the front *F*(*τi*+1) is obtained, and a new set *A*(*τi*+1) is formed via the extension or reduction of

initial front *F*(0) coincides with those part of the boundary *∂M* that consists of the points from which the trajectories of system (13) (being written in backward time) leave *M* with increasing *τ* for at least one pair of admissible controls *u*(·), *w*(·). According to (Isaacs, 1965), such part

Several typical cases (local fragments) of the front propagation are presented in Figs. 6 and 7. Fig. 6a shows the case in which the left end of the front is moving from the endpoint *c* of the usable part of *∂M* with increasing *τ*. In the algorithm, simultaneously with the computation of the next front *F*(*τi*+1), the extension of the barrier is computed by means of connection of the left ends of the fronts *F*(*τi*) and *F*(*τi*+1). In the case considered, this results in the local increase of the totality *A*(*τi*+1) with respect to *A*(*τi*). The extension of the barrier forms a line

*<sup>A</sup>*(*τi*+1) is the boundary of the next set W(*τi*<sup>+</sup>1, *<sup>M</sup>*). The

differential games that the function **<sup>z</sup>** → *<sup>V</sup>*(**z**) is discontinuous on the set A :=

the relations *F*(*τ*) ⊂ *∂*W(*τ*, *M*), *W*(*τ*, *M*) ⊂ W(*τ*, *M*), we obtain *F*(*τ*) ⊂ *∂W*(*τ*, *M*).

*f*(**z**, **u**) of the right hand side of the controlled system.

*τ*≥0

*A*(*τ*) and

can also be variable) and define *τ*<sup>0</sup> = 0, ..., *τi*+<sup>1</sup> = *τ<sup>i</sup>* + Δ.

**u**∈*P*

Consider the vectogram *E*(**z**) =

notion of a front is introduced.

W(*τ*, *M*) through the front *F*(*τ*) only.

then *F*(*τ*) = *∂*W(*τ*, *M*) for all *τ* ≥ *τ*∗.

the set *A*(*τi*). The union *F*(*τi*+1)

continuous in the remaining part of the plane.

arcs. Such arcs are called barriers (Isaacs, 1965).

of the boundary of *M* is called the usable part of *M*. Therefore, the algorithm resembles the front propagation.

on which the value function is discontinuous.

*W*(*τi*<sup>+</sup>1, *M*).

as

#### **3.4 Solvability sets for problems of approach at given time and by given time**

Let *M* be a closed set in the plane **x**, **y**. Denote by *W*(*τ*, *M*) (respectively, by W(*τ*, *M*)) the set of all points **z** = (**x**, **y**) with the property: there exist piecewise continuous admissible controls *u*(·), *w*(·) which bring system (13) from the initial point **z** to the set *M* at time *τ* (respectively, by time *τ*). The set *W*(*τ*, *M*) (W(*τ*, *M*)) is the solvability set in the problem of reaching the set *M* at time *τ* (by time *τ*).

Take now the same set *M* as a terminal set in the problem of reaching a given set for system (13) and as a set *D* in the problem of finding reachable set in geometric coordinates for system (4) with regard to an orientedly added set. It follows from the sense of Isaacs' transformation that the set *W*(*τ*, *M*) drawn in coordinates **x**, **y** coincides with the set *G*<sup>2</sup> *<sup>M</sup>*(*τ*) depicted in coordinates *x*,*y*. Therefore,

$$\mathcal{W}(\mathfrak{r}, \mathcal{M}) = \mathcal{G}\_{\mathcal{M}}^2(\mathfrak{r}). \tag{14}$$

Similarly,

$$\mathcal{W}(\boldsymbol{\tau}, \boldsymbol{M}) = \mathcal{G}\_{\boldsymbol{M}}^{2}(\boldsymbol{\tau}). \tag{15}$$

In the following, we will utilize relations (14), (15) in order to obtain the reachable sets *G*<sup>2</sup> *<sup>D</sup>*(*t*) and <sup>G</sup><sup>2</sup> *<sup>D</sup>*(*t*) of system (4) using the sets *W*(*τ*, *M*) and W(*τ*, *M*) computed for system (13) with *τ* = *t* and *M* = *D*. In addition, if *M* is a one-point set that coincides with the origin in the plane **x**, **y**, then

$$\mathcal{W}(\tau, M) = \mathcal{G}\_M^2(\tau) = \mathcal{G}^2(\tau), \quad \mathcal{W}(\tau, M) = \mathcal{G}\_M^2(\tau) = \mathcal{G}^2(\tau).$$

By fixing some point *z*¯ = (*x*¯, *y*¯) in the plane and by increasing *t*, let us find the first instant ¯*t* when *<sup>z</sup>*¯ ∈ G<sup>2</sup> *<sup>M</sup>*(¯*t*) (equivalently, *<sup>z</sup>*¯ <sup>∈</sup> *<sup>G</sup>*<sup>2</sup> *<sup>M</sup>*(¯*t*)). Such ¯*<sup>t</sup>* be the optimal time *<sup>V</sup>*(*z*¯) of passing from the point *z*<sup>0</sup> = 0, *θ*<sup>0</sup> = 0 to the point *z*¯ for system (4) with accounting for *M*. Hence, the Lebesgue set {*<sup>z</sup>* : *<sup>V</sup>*(*z*) <sup>≤</sup> *<sup>t</sup>*} of the optimal result function coincides with the set <sup>G</sup><sup>2</sup> *<sup>M</sup>*(*t*) = W(*t*, *M*). For the sets shown in Fig. 3 (where *M* = {0}), the function *z* → *V*(*z*) is discontinuous on the upper semi-circumferences of radius 1 with the centers at the points (−1, 0), (1, 0).

#### **3.5 Backward procedure for construction of solvability sets**

The algorithms developed by the authors for the numerical construction of the sets *W*(*τ*, *M*) and W(*τ*, *M*) are based on the backward procedure (the parameter *τ* increases starting from *τ* = 0) and being variants of the dynamic programming method for the considered class of problems. Backward procedures for the construction of the solvability sets at given time and by given time are intensively developed (Grigor'eva et al., 2005), (Mikhalev & Ushakov, 2007) for control problems and differential games. Elements of the backward constructions are included in one or another form (Sethian, 1999), (Cristiani & Falcone, 2009) into grid methods for solving differential games. For control problems, the backward procedures are simpler, since the second player whose actions should be accounted for in differential games is absent. Especially simple are the backward procedures for problems in the plane. In this case, one can storage the boundary of the sets *W*(*τ*, *M*) and W(*τ*, *M*) in form of polygonal lines.

The general idea for the construction of the solvability sets *W*(*τ*, *M*) in the problem of approaching a set *M* by system (13) at time *τ* is the following. We deal with polygonal sets 8 Will-be-set-by-IN-TECH

Let *M* be a closed set in the plane **x**, **y**. Denote by *W*(*τ*, *M*) (respectively, by W(*τ*, *M*)) the set of all points **z** = (**x**, **y**) with the property: there exist piecewise continuous admissible controls *u*(·), *w*(·) which bring system (13) from the initial point **z** to the set *M* at time *τ* (respectively, by time *τ*). The set *W*(*τ*, *M*) (W(*τ*, *M*)) is the solvability set in the problem of reaching the set

Take now the same set *M* as a terminal set in the problem of reaching a given set for system (13) and as a set *D* in the problem of finding reachable set in geometric coordinates for system (4) with regard to an orientedly added set. It follows from the sense of Isaacs' transformation

*W*(*τ*, *M*) = *G*<sup>2</sup>

<sup>W</sup>(*τ*, *<sup>M</sup>*) = <sup>G</sup><sup>2</sup>

In the following, we will utilize relations (14), (15) in order to obtain the reachable sets *G*<sup>2</sup>

*<sup>M</sup>*(*τ*) = *<sup>G</sup>*2(*τ*), <sup>W</sup>(*τ*, *<sup>M</sup>*) = <sup>G</sup><sup>2</sup>

By fixing some point *z*¯ = (*x*¯, *y*¯) in the plane and by increasing *t*, let us find the first instant ¯*t*

point *z*<sup>0</sup> = 0, *θ*<sup>0</sup> = 0 to the point *z*¯ for system (4) with accounting for *M*. Hence, the Lebesgue

For the sets shown in Fig. 3 (where *M* = {0}), the function *z* → *V*(*z*) is discontinuous on the

The algorithms developed by the authors for the numerical construction of the sets *W*(*τ*, *M*) and W(*τ*, *M*) are based on the backward procedure (the parameter *τ* increases starting from *τ* = 0) and being variants of the dynamic programming method for the considered class of problems. Backward procedures for the construction of the solvability sets at given time and by given time are intensively developed (Grigor'eva et al., 2005), (Mikhalev & Ushakov, 2007) for control problems and differential games. Elements of the backward constructions are included in one or another form (Sethian, 1999), (Cristiani & Falcone, 2009) into grid methods for solving differential games. For control problems, the backward procedures are simpler, since the second player whose actions should be accounted for in differential games is absent. Especially simple are the backward procedures for problems in the plane. In this case, one can

set {*<sup>z</sup>* : *<sup>V</sup>*(*z*) <sup>≤</sup> *<sup>t</sup>*} of the optimal result function coincides with the set <sup>G</sup><sup>2</sup>

upper semi-circumferences of radius 1 with the centers at the points (−1, 0), (1, 0).

storage the boundary of the sets *W*(*τ*, *M*) and W(*τ*, *M*) in form of polygonal lines.

The general idea for the construction of the solvability sets *W*(*τ*, *M*) in the problem of approaching a set *M* by system (13) at time *τ* is the following. We deal with polygonal sets

*<sup>D</sup>*(*t*) of system (4) using the sets *W*(*τ*, *M*) and W(*τ*, *M*) computed for system (13) with *τ* = *t* and *M* = *D*. In addition, if *M* is a one-point set that coincides with the origin in the

*<sup>M</sup>*(*τ*) depicted in

*<sup>M</sup>*(*t*) = W(*t*, *M*).

*<sup>D</sup>*(*t*)

*<sup>M</sup>*(*τ*). (14)

*<sup>M</sup>*(*τ*). (15)

*<sup>M</sup>*(*τ*) = <sup>G</sup>2(*τ*).

*<sup>M</sup>*(¯*t*)). Such ¯*<sup>t</sup>* be the optimal time *<sup>V</sup>*(*z*¯) of passing from the

**3.4 Solvability sets for problems of approach at given time and by given time**

that the set *W*(*τ*, *M*) drawn in coordinates **x**, **y** coincides with the set *G*<sup>2</sup>

*M* at time *τ* (by time *τ*).

coordinates *x*,*y*. Therefore,

Similarly,

and <sup>G</sup><sup>2</sup>

plane **x**, **y**, then

when *<sup>z</sup>*¯ ∈ G<sup>2</sup>

*W*(*τ*, *M*) = *G*<sup>2</sup>

*<sup>M</sup>*(¯*t*) (equivalently, *<sup>z</sup>*¯ <sup>∈</sup> *<sup>G</sup>*<sup>2</sup>

**3.5 Backward procedure for construction of solvability sets**

which are considered as approximations of the ideal sets *W*(*τ*, *M*). Specify a time step Δ (it can also be variable) and define *τ*<sup>0</sup> = 0, ..., *τi*+<sup>1</sup> = *τ<sup>i</sup>* + Δ.

Consider the vectogram *E*(**z**) = **u**∈*P f*(**z**, **u**) of the right hand side of the controlled system. For system (4), we have **u** = (*u*, *w*), *P* = {(*u*, *w*) : *u* ∈ [*b*, 1], *w* ∈ [*a*, 1]}. The set **z** − Δ*E*(**z**) describes approximately the collection of points from which the point **z** can be approached at time Δ. Running over the boundary *∂W*(*τi*, *M*) and attaching the set **z** − Δ*E*(**z**) "to every" point of the boundary, we approximately construct the boundary *∂W*(*τi*<sup>+</sup>1, *M*) of the set *W*(*τi*<sup>+</sup>1, *M*).

Theoretically, the solvability set W(*τ*∗, *M*) for the problem of approaching the set *M* by system (13) by the time *τ*<sup>∗</sup> is defined through the union of the sets *W*(*τ*, *M*), *τ* ∈ [0, *τ*∗]. However, one can reject the explicit realization of the union operation and try to construct the boundary of the set W(*τi*<sup>+</sup>1, *M*) directly on the base of the boundary of the set W(*τi*, *M*). To this end, the notion of a front is introduced.

Using the optimal result function *V*, the front corresponding to the time *τ* is formally defined as

$$F(\boldsymbol{\tau}) := \{ \mathbf{z} \in \partial \mathcal{W}(\boldsymbol{\tau}, \boldsymbol{M}) : \ V(\mathbf{z}) = \boldsymbol{\tau} \}.$$

If ¯**z** ∈ W/ (*τ*, *M*), then every trajectory of system (13) starting from the point ¯**z** can approach W(*τ*, *M*) through the front *F*(*τ*) only.

It is known (see e.g. Bardi & Capuzzo-Dolcetta (1997)) that if *F*(*τ*∗) = *∂*W(*τ*∗, *M*) for some *τ*∗, then *F*(*τ*) = *∂*W(*τ*, *M*) for all *τ* ≥ *τ*∗.

Let *A*(*τ*) := *∂*W(*τ*, *M*) \ *F*(*τ*). It follows from results of optimal control theory and theory of differential games that the function **<sup>z</sup>** → *<sup>V</sup>*(**z**) is discontinuous on the set A := *τ*≥0 *A*(*τ*) and

continuous in the remaining part of the plane.

Possible structure of the set A is not well explored for time-optimal problems. It is reasonable to assume that if the set B := A \ *M* is not empty, then it consists of a collection of smooth arcs. Such arcs are called barriers (Isaacs, 1965).

By the definition of the front *F*(*τ*), we have *F*(*τ*) ⊂ *W*(*τ*, *M*). From here, with accounting for the relations *F*(*τ*) ⊂ *∂*W(*τ*, *M*), *W*(*τ*, *M*) ⊂ W(*τ*, *M*), we obtain *F*(*τ*) ⊂ *∂W*(*τ*, *M*).

The main idea of the algorithm of the backward construction of the sets W(*τ*, *M*) is explained in Fig. 5. The next set W(*τi*<sup>+</sup>1, *M*) for *τi*+<sup>1</sup> = *τ<sup>i</sup>* + Δ is computed on the basis of the previous set W(*τi*, *M*). The central role in this computation belongs to the front *F*(*τi*). As a result, the front *F*(*τi*+1) is obtained, and a new set *A*(*τi*+1) is formed via the extension or reduction of the set *A*(*τi*). The union *F*(*τi*+1) *<sup>A</sup>*(*τi*+1) is the boundary of the next set W(*τi*<sup>+</sup>1, *<sup>M</sup>*). The initial front *F*(0) coincides with those part of the boundary *∂M* that consists of the points from which the trajectories of system (13) (being written in backward time) leave *M* with increasing *τ* for at least one pair of admissible controls *u*(·), *w*(·). According to (Isaacs, 1965), such part of the boundary of *M* is called the usable part of *M*.

Therefore, the algorithm resembles the front propagation.

Several typical cases (local fragments) of the front propagation are presented in Figs. 6 and 7. Fig. 6a shows the case in which the left end of the front is moving from the endpoint *c* of the usable part of *∂M* with increasing *τ*. In the algorithm, simultaneously with the computation of the next front *F*(*τi*+1), the extension of the barrier is computed by means of connection of the left ends of the fronts *F*(*τi*) and *F*(*τi*+1). In the case considered, this results in the local increase of the totality *A*(*τi*+1) with respect to *A*(*τi*). The extension of the barrier forms a line on which the value function is discontinuous.

*<sup>M</sup>*

*c*

*F*(*τi*+1)

*F*(*τi*)

Reachable Sets for Simple Models of Car Motion 157

Let *M* be a one-point set that coincides with the origin (in the numerical computations, a circle with a very small radius is taken). For system (3) with *a* < 0, the set *G*2(*t*) "swells" monotonically with increasing *<sup>t</sup>*, i.e. *<sup>G</sup>*2(*t*2) <sup>⊃</sup> *<sup>G</sup>*2(*t*1) for *<sup>t</sup>*<sup>2</sup> <sup>&</sup>gt; *<sup>t</sup>*1, where the strict inclusion holds. This provides that the sets *<sup>G</sup>*2(*t*) and <sup>G</sup>2(*t*) coincide. For *<sup>a</sup>* <sup>=</sup> <sup>−</sup>1 (i.e. for system (2))

After publishing the paper (Reeds & Shepp, 1990) related to the minimum time problem for system (2), the obtained results were refined and essentially propelled in the works (Sussmann & Tang, 1991), (Soueres et al., 1994), and (Soueres & Laumond, 1996) by using the Pontryagin maximum principle. The second paper describes in particular the construction of reachable sets *G*2(*t*) and give the structure of controls steering to the boundary of these sets. The properties of monotonic swelling of the sets *G*2(*t*) and the symmetry make system (2) very convenient for solving very complex problems of robot transportation (Laumond, 1998), pp. 23 – 43. For *a* = −0.8 and *a* = −1, the reachable sets are shown in Fig. 8. As before, the notation *tf* means the end time of the construction interval. The symbol *δ* denotes the output step of the representation, which is not necessarily equal to the step Δ of the backward

For *a* = 0.8 and *a* = 0.2, the behavior of the reachable sets *G*2(*t*) for system (3) with increasing *<sup>t</sup>* is shown in Fig. 9. The dependency of the sets *<sup>G</sup>*2(*t*) and <sup>G</sup>2(*t*) on the parameter *<sup>a</sup>* is presented for *t* = 1.8 in Fig. 10. Similar sets but for non-symmetric constraint *u* ∈ [−0.6, 1] (i.e. for system (4)) are depicted in Fig. 11. Non-symmetry of the restriction on the control *u* results in the non-symmetry of the obtained sets with respect to the vertical axis. Note that from the theoretical point of view, the minimum time problem and the related problem of the construction of the sets <sup>G</sup>2(*t*) for system (4) with *<sup>a</sup>* <sup>=</sup> 1 and *<sup>b</sup>* <sup>∈</sup> [−1, 0) were studied in

*<sup>M</sup>*(*τ*) = W(*τ*, *M*) becomes for the first time non-simply-connected at *τ*<sup>∗</sup> = 4.452 when the right part of the front collides with *M*. Here, one needs to fill a "hole" adjoining to the back side of initial part of the barrier. The hole is completely filled out by the time *τ*¯ = 4.522. The second hole occurs at time *τ*∗ = 5.062 when left and right parts of the front meet. In this case, the filling out of the hole ends at *τ*¯¯ = 5.21. The function *V* of the optimal result is

*<sup>M</sup>*(*τ*) computed for *a* = 0.2 and *b* = −0.6 are shown. The set

Fig. 7. The left end of the front is bending around the barrier line

the set *<sup>G</sup>*2(*t*) = <sup>G</sup>2(*t*) is symmetric with respect to the axes *<sup>x</sup>*, *<sup>y</sup>*.

constructions. The latter is, as a rule, smaller.

(Bakolas & Tsiotras, 2011). In Figs. 12 and 13, the sets <sup>G</sup><sup>2</sup>

G2

**3.6 Results of numerical construction of solvability sets (reachable sets)**

Fig. 5. Backward construction of the solvability sets W(*τ*, *M*). The boundary of the set W(*τi*<sup>+</sup>1, *M*) is *F*(*τi*+1)∪ *A*(*τi*+1)

Fig. 6. a) The movement of the left end of the front generates the barrier line on which the value function is discontinuous; b) The right end of the front is moving along the boundary of the terminal set

In the case shown in Fig. 6b, the right end of the front starts to move along *∂M* from the very beginning i.e. for small *τ* > 0. Here, no barrier line emanates from the right endpoint *d* of the usable part. The value function near the point *d* outside the set *M* is continuous.

Fig. 7 represents the case where the left end of the front is running along the back side of the already constructed barrier. This results in the local decrease of the totality *A*(*τi*+1) comparing to *A*(*τi*).

A more detailed description of the algorithm is given in (Patsko & Turova, 2009).

10 Will-be-set-by-IN-TECH

*F*(*τi*)

*F*(*τi*+1)

*<sup>F</sup>*(*τi*+1)

*F*(*τi*)

*M*

❊ ❊ ❊ ❊ ❊

*<sup>M</sup> <sup>d</sup>*

◗◗◗◗ ✏✏✏✏✏❊

Fig. 5. Backward construction of the solvability sets W(*τ*, *M*). The boundary of the set

Fig. 6. a) The movement of the left end of the front generates the barrier line on which the value function is discontinuous; b) The right end of the front is moving along the boundary

In the case shown in Fig. 6b, the right end of the front starts to move along *∂M* from the very beginning i.e. for small *τ* > 0. Here, no barrier line emanates from the right endpoint *d* of the

Fig. 7 represents the case where the left end of the front is running along the back side of the already constructed barrier. This results in the local decrease of the totality *A*(*τi*+1) comparing

usable part. The value function near the point *d* outside the set *M* is continuous.

A more detailed description of the algorithm is given in (Patsko & Turova, 2009).

W(*τi*<sup>+</sup>1, *M*)

*<sup>c</sup> <sup>M</sup>*

W(*τi*<sup>+</sup>1, *M*) is *F*(*τi*+1)∪ *A*(*τi*+1)

B

of the terminal set

to *A*(*τi*).

*A*(*τi*+1)

✁ ✁ ✁ ✁

❤◗❤❤

*F*(*τi*+1)

*F*(*τi*)

a b

*F*(0)

Fig. 7. The left end of the front is bending around the barrier line

#### **3.6 Results of numerical construction of solvability sets (reachable sets)**

Let *M* be a one-point set that coincides with the origin (in the numerical computations, a circle with a very small radius is taken). For system (3) with *a* < 0, the set *G*2(*t*) "swells" monotonically with increasing *<sup>t</sup>*, i.e. *<sup>G</sup>*2(*t*2) <sup>⊃</sup> *<sup>G</sup>*2(*t*1) for *<sup>t</sup>*<sup>2</sup> <sup>&</sup>gt; *<sup>t</sup>*1, where the strict inclusion holds. This provides that the sets *<sup>G</sup>*2(*t*) and <sup>G</sup>2(*t*) coincide. For *<sup>a</sup>* <sup>=</sup> <sup>−</sup>1 (i.e. for system (2)) the set *<sup>G</sup>*2(*t*) = <sup>G</sup>2(*t*) is symmetric with respect to the axes *<sup>x</sup>*, *<sup>y</sup>*.

After publishing the paper (Reeds & Shepp, 1990) related to the minimum time problem for system (2), the obtained results were refined and essentially propelled in the works (Sussmann & Tang, 1991), (Soueres et al., 1994), and (Soueres & Laumond, 1996) by using the Pontryagin maximum principle. The second paper describes in particular the construction of reachable sets *G*2(*t*) and give the structure of controls steering to the boundary of these sets. The properties of monotonic swelling of the sets *G*2(*t*) and the symmetry make system (2) very convenient for solving very complex problems of robot transportation (Laumond, 1998), pp. 23 – 43. For *a* = −0.8 and *a* = −1, the reachable sets are shown in Fig. 8. As before, the notation *tf* means the end time of the construction interval. The symbol *δ* denotes the output step of the representation, which is not necessarily equal to the step Δ of the backward constructions. The latter is, as a rule, smaller.

For *a* = 0.8 and *a* = 0.2, the behavior of the reachable sets *G*2(*t*) for system (3) with increasing *<sup>t</sup>* is shown in Fig. 9. The dependency of the sets *<sup>G</sup>*2(*t*) and <sup>G</sup>2(*t*) on the parameter *<sup>a</sup>* is presented for *t* = 1.8 in Fig. 10. Similar sets but for non-symmetric constraint *u* ∈ [−0.6, 1] (i.e. for system (4)) are depicted in Fig. 11. Non-symmetry of the restriction on the control *u* results in the non-symmetry of the obtained sets with respect to the vertical axis. Note that from the theoretical point of view, the minimum time problem and the related problem of the construction of the sets <sup>G</sup>2(*t*) for system (4) with *<sup>a</sup>* <sup>=</sup> 1 and *<sup>b</sup>* <sup>∈</sup> [−1, 0) were studied in (Bakolas & Tsiotras, 2011).

In Figs. 12 and 13, the sets <sup>G</sup><sup>2</sup> *<sup>M</sup>*(*τ*) computed for *a* = 0.2 and *b* = −0.6 are shown. The set G2 *<sup>M</sup>*(*τ*) = W(*τ*, *M*) becomes for the first time non-simply-connected at *τ*<sup>∗</sup> = 4.452 when the right part of the front collides with *M*. Here, one needs to fill a "hole" adjoining to the back side of initial part of the barrier. The hole is completely filled out by the time *τ*¯ = 4.522. The second hole occurs at time *τ*∗ = 5.062 when left and right parts of the front meet. In this case, the filling out of the hole ends at *τ*¯¯ = 5.21. The function *V* of the optimal result is

a

*y*

0

*<sup>G</sup>*2(*t*); b) <sup>G</sup>2(*t*)

a

*y*

2

0

*<sup>G</sup>*2(*t*); b) <sup>G</sup>2(*t*)

0.5

1

1.5



discontinuous outside of *M* on the barrier *ce*.

*a* = 1

*a* = 0.8

*a* = 0.2

*a* = 0.2

*a* = 0.8 *a* = 1

*x*

Fig. 10. Comparison of the reachable sets for system (3) for different values of *a*, *t* = 1.8: a)

*x*

Fig. 11. Comparison of the reachable sets for system (4) for different values of *a*, *t* = 1.8: a)

propagate in a regular way with increasing *τ* and which ones (and from what time) are developed in a more complex manner. With increasing *τ*, the left end of the front moves along the barrier line (as in Fig. 6a). After passing the point *d*, the left end begins to move along the same barrier line but over its back side. The right end runs along the boundary of the terminal set with increasing *τ* (as in Fig. 6b), then changes over to the back side of the barrier. At time *τ*∗ = 4.864, the self-intersection of the front occurs. The front is divided into two parts: the inner and the outer ones. The ends of the inner front slide along the left barrier until they meet each other and a closed front is formed. The construction then is continued for the closed front. At time *τ*∗ = 6.08, the inner front shrinks into a point. The computation of the closed outer front is also continued till this time *τ*∗. The optimal result function *V* is

0

0.5

1

1.5

0

b

*y*



*M*

*M*

*a* = 1

*a* = 0.2

*a* = 1

*a* = 0.8

*a* = 0.8

*x*

*x*

*a* = 0.2

0.4

0.8

1.2

1.6

2

Reachable Sets for Simple Models of Car Motion 159

b

*y*

0.4

0.8

1.2

1.6

2

Fig. 8. Reachable sets <sup>G</sup>2(*t*) = *<sup>G</sup>*2(*t*) for system (3), *tf* <sup>=</sup> 1.8, *<sup>δ</sup>* <sup>=</sup> 0.04: a) *<sup>a</sup>* <sup>=</sup> <sup>−</sup>0.8; b) *<sup>a</sup>* <sup>=</sup> <sup>−</sup><sup>1</sup>

Fig. 9. The reachable sets *G*2(*t*) at given time for system (3), *tf* = 1.8: a) *a* = 0.8, *δ* = 0.2; b) *a* = 0.2, *δ* = 0.3

discontinuous on the two barrier lines being the upper semi-circumferences with the centers at the points (*a*/*b*, 0)=(−1/3, 0) and (*a*, 0)=(0.2, 0) and the radiuses <sup>1</sup> <sup>3</sup> <sup>−</sup> *<sup>r</sup>* and 0.2 <sup>−</sup> *<sup>r</sup>*, where *r* = 0.01 is the radius of the terminal circle *M*.

Let us consider an example where the set *M* is a circle of radius 0.3 centered at the point (0.7, 0). Put *a* = 0.8. In Figs. 14 and 15, results of the construction of the sets W(*τ*, *M*) are presented. We see which parts of the boundary of the reachable set *G*<sup>2</sup> *<sup>M</sup>*(*τ*) = *W*(*τ*, *M*) 12 Will-be-set-by-IN-TECH

a b


*x*

Fig. 8. Reachable sets <sup>G</sup>2(*t*) = *<sup>G</sup>*2(*t*) for system (3), *tf* <sup>=</sup> 1.8, *<sup>δ</sup>* <sup>=</sup> 0.04: a) *<sup>a</sup>* <sup>=</sup> <sup>−</sup>0.8; b) *<sup>a</sup>* <sup>=</sup> <sup>−</sup><sup>1</sup>

0

*x*

Fig. 9. The reachable sets *G*2(*t*) at given time for system (3), *tf* = 1.8: a) *a* = 0.8, *δ* = 0.2;

discontinuous on the two barrier lines being the upper semi-circumferences with the centers

Let us consider an example where the set *M* is a circle of radius 0.3 centered at the point (0.7, 0). Put *a* = 0.8. In Figs. 14 and 15, results of the construction of the sets W(*τ*, *M*)

at the points (*a*/*b*, 0)=(−1/3, 0) and (*a*, 0)=(0.2, 0) and the radiuses <sup>1</sup>

are presented. We see which parts of the boundary of the reachable set *G*<sup>2</sup>

0.4

0.8

1.2

1.6

2

*y*



*M*

*x*

*x*

<sup>3</sup> <sup>−</sup> *<sup>r</sup>* and 0.2 <sup>−</sup> *<sup>r</sup>*,

*<sup>M</sup>*(*τ*) = *W*(*τ*, *M*)



0

0.5

1

1.5

*y*


0

b) *a* = 0.2, *δ* = 0.3

0.4

0.8

1.2

1.6

2

*y*



*M*

where *r* = 0.01 is the radius of the terminal circle *M*.

a b



0

0.5

1

1.5

*y*

Fig. 10. Comparison of the reachable sets for system (3) for different values of *a*, *t* = 1.8: a) *<sup>G</sup>*2(*t*); b) <sup>G</sup>2(*t*)

Fig. 11. Comparison of the reachable sets for system (4) for different values of *a*, *t* = 1.8: a) *<sup>G</sup>*2(*t*); b) <sup>G</sup>2(*t*)

propagate in a regular way with increasing *τ* and which ones (and from what time) are developed in a more complex manner. With increasing *τ*, the left end of the front moves along the barrier line (as in Fig. 6a). After passing the point *d*, the left end begins to move along the same barrier line but over its back side. The right end runs along the boundary of the terminal set with increasing *τ* (as in Fig. 6b), then changes over to the back side of the barrier. At time *τ*∗ = 4.864, the self-intersection of the front occurs. The front is divided into two parts: the inner and the outer ones. The ends of the inner front slide along the left barrier until they meet each other and a closed front is formed. The construction then is continued for the closed front. At time *τ*∗ = 6.08, the inner front shrinks into a point. The computation of the closed outer front is also continued till this time *τ*∗. The optimal result function *V* is discontinuous outside of *M* on the barrier *ce*.


Fig. 14. Reachable sets <sup>G</sup><sup>2</sup>


Fig. 15. Enlarged fragment of Fig. 14



0

0.5

1

*y*

*b* = −1, *τ<sup>f</sup>* = 6.08, *δ* = 0.076



*<sup>M</sup> <sup>e</sup> <sup>c</sup>*

*<sup>M</sup>*(*τ*) = W(*τ*, *M*) for the circle *M* centered at (0.7, 0); *a* = 0.8,

*M*

Reachable Sets for Simple Models of Car Motion 161

*x*

*x*


0

2

4

6

*y*

Fig. 12. Reachable sets <sup>G</sup><sup>2</sup> *<sup>M</sup>*(*τ*) = W(*τ*, *M*) for system (4) with *a* = 0.2, *b* = −0.6. The set *M* is the circle of radius 0.01 with the center at the origin. The output step of the representation is *δ* = 0.12, the terminal time is *τ<sup>f</sup>* = 5.21

Fig. 13. Enlarged fragment of Fig. 12. The output step of the representation is *δ* = 0.046

14 Will-be-set-by-IN-TECH


the circle of radius 0.01 with the center at the origin. The output step of the representation is


Fig. 13. Enlarged fragment of Fig. 12. The output step of the representation is *δ* = 0.046

*<sup>M</sup>*(*τ*) = W(*τ*, *M*) for system (4) with *a* = 0.2, *b* = −0.6. The set *M* is

*x*

*x*


Fig. 12. Reachable sets <sup>G</sup><sup>2</sup>




0

0.2

0.4

*y*

*δ* = 0.12, the terminal time is *τ<sup>f</sup>* = 5.21



0

1

2

3

4

5

*y*

Fig. 14. Reachable sets <sup>G</sup><sup>2</sup> *<sup>M</sup>*(*τ*) = W(*τ*, *M*) for the circle *M* centered at (0.7, 0); *a* = 0.8, *b* = −1, *τ<sup>f</sup>* = 6.08, *δ* = 0.076

Fig. 15. Enlarged fragment of Fig. 14

The visualization of the three-dimensional sets is done with the program "Cortona VRML Client" utilizing the open standard format VRML/X3D for the demonstration of interactive

Reachable Sets for Simple Models of Car Motion 163

Fig. 16 shows the boundary of the set *<sup>G</sup>*3(*t*∗) at time *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> 1.5*<sup>π</sup>* from two perspectives. The initial values of *x*0, *y*0, and *θ*<sup>0</sup> are equal to zero. The different parts of the boundary are marked with different colors. For example, part 2 is reachable for the trajectories with the control *u*(*t*) of the form −1, 0, 1 with two switches. The sections of the reachable set by the plane *θ* = const are depicted with some step along the axis *θ*. The points of junction lines of parts 1,2; 1,3; 2,4; 2,5; 2,6; 3,4; 3,5; 3,6 are obtained with a single-switch control. Any point of the common line of parts 5 and 6 is reachable for two trajectories with two switches each. Parts 5 and 6 have non-smooth junction along this line. The angle of the junction is not visible because it is rather

2) -1, 0, 1

3) 1, 0, -1

Fig. 17 shows reachable sets *<sup>G</sup>*3(*t*∗) at the same perspective but with different scales for four time instants *t*∗. The transformation of the structure of the reachable set boundary is clearly seen. With increasing time, the forward part of the boundary covers the back part composed

Passing from *t*<sup>∗</sup> = 3*π* to *t*<sup>∗</sup> = 4*π*, one arrives at the time *t*<sup>∗</sup> ≈ 3.65*π* when the reachable set *<sup>G</sup>*3(*t*∗) becomes non-simply-connected for some small time interval. Namely, a cavity that does not belong to the reachable set arises. In Fig. 18, an origination of such a situation is shown. Here, a cut of two sets *<sup>G</sup>*3(*t*∗) corresponding to instants *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> <sup>3</sup>*<sup>π</sup>* and *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> 3.65*<sup>π</sup>* is depicted. The cut is done using the plane *θ* = 0. The set *G*3(3*π*) is simply connected and the

Fig. 19 shows the set *<sup>G</sup>*3(*t*∗) for *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> 1.6*π*, *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> <sup>2</sup>*π*, and *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> 2.5*π*. Here the angle *<sup>θ</sup>* is

6) -1, 1, -1

t<sup>∗</sup> =1.5π

6)

3)

1) 1, 0, 1

1)

small. The control *u*(*t*) ≡ 0 steers to the junction point of parts 1−4.

4) -1, 0, -1

5) 1, -1, 1

θ

θ

5)

x

y

2)

Fig. 16. The set *<sup>G</sup>*3(*t*∗) for *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> 1.5*<sup>π</sup>* shown from the two perspectives

of patches 5, 6. Note that the angle *θ* is not restricted as *θ* ∈ [−*π*, *π*).

y

4)

x

set *G*(3.65*π*) is not.

calculated modulo 2*π*.

vector graphics.

#### **4. Three-dimensional reachable sets**

Let us describe the reachable sets

$$G^{\mathfrak{J}}(t\_\*) := \bigcup\_{u(\cdot), w(\cdot)} (z(t\_\*; 0, 0, u(\cdot), w(\cdot)), \theta(t\_\*; 0, 0, u(\cdot), w(\cdot)), \quad \mathcal{G}^{\mathfrak{J}}(t\_\*) := \bigcup\_{t \in [0, t\_\*]} G^{\mathfrak{J}}(t)$$

in the three-dimensional space *x*, *y*, *θ*. We restrict ourselves to the case of system (1) and a close to it system in which the control parameter *u* is restricted as *u* ∈ [*b*, 1], where *b* ∈ [−1, 0) is the fixed parameter.

#### **4.1 Structure of controls steering to the boundary of reachable sets at given time**

In paper (Patsko et al., 2003), it was established based on the application of the Pontryagin maximum principle to system (1) that for any point (*z*, *<sup>θ</sup>*) <sup>∈</sup> *<sup>∂</sup>G*3(*t*∗) the control steering to this point is piecewise continuous and has at most two switches. In addition, there are only 6 variants of changing the control:

1) 1, 0, 1; 2) −1, 0, 1; 3) 1, 0, −1; 4) −1, 0, −1; 5) 1, −1, 1; 6) −1, 1, −1.

The second variant means that the control *u* ≡ −1 acts on some interval [0, *t*1), the control *u* ≡ 0 works on an interval [*t*1, *t*2), and the control *u* ≡ 1 operates on the interval [*t*2, *t*∗]. If *t*<sup>1</sup> = *t*2, then the second interval (where *u* ≡ 0) vanishes, and we obtain a single switch from *u* = −1 to *u* = 1. In the case *t*<sup>1</sup> = 0, the first interval where *u* ≡ −1 vanishes; in the case *t*<sup>2</sup> = *t*<sup>∗</sup> the third interval with *u* ≡ 1 is absent. The control has constant value for all *t* ∈ [0, *t*∗] if one of the following three conditions holds: *t*<sup>1</sup> = *t*∗, *t*<sup>2</sup> = 0, or both *t*<sup>1</sup> = 0 and *t*<sup>2</sup> = *t*∗. Similar is true for the other variants.

The proposition on six variants of the control *u*(*t*) steering to the boundary of the reachable set *<sup>G</sup>*3(*t*∗) is similar in form to the Dubins theorem on the variants of the controls steering to the boundary of the reachable set <sup>G</sup>3(*t*∗). The same variants are valid. However, due to the relation between the sets *<sup>G</sup>*3(*t*∗) and <sup>G</sup>3(*t*∗) (the set <sup>G</sup>3(*t*∗) is the union of the sets *<sup>G</sup>*3(*t*) over *t* ∈ [0, *t*∗]), the above mentioned properties of the controls leading to the boundary of the set *<sup>G</sup>*3(*t*∗) result in the analogous properties of the controls leading to the boundary of the set <sup>G</sup>3(*t*∗), but the converse is false.

#### **4.2 Numerical construction of three-dimensional reachable sets at given time**

Let us apply the above formulated result on the structure of the control *<sup>u</sup>*(*t*) steering to *<sup>∂</sup>G*3(*t*∗) for the numerical construction of the boundary *<sup>∂</sup>G*3(*t*∗).

To construct the boundary *<sup>∂</sup>G*3(*t*∗) of the set *<sup>G</sup>*3(*t*∗), we search through all controls of the form 1– 6 with two switches *t*1, *t*2. For every variant of switches, the parameter *t*<sup>1</sup> is chosen from the interval [0, *t*∗], and the parameter *t*<sup>2</sup> from the interval [*t*1, *t*∗]. In addition, controls with one switch and without switches are also considered. Taken a specific variant of switching and searching through the parameters *t*1, *t*<sup>2</sup> on some sufficiently fine grid, we obtain a collection of points generating a surface in the three-dimensional space *x*, *y*, *θ*.

Therefore, each of the six variants yields its own surface in the three-dimensional space. The boundary of the reachable set *<sup>G</sup>*3(*t*∗) is composed of pieces of these surfaces. The six surfaces are loaded into the visualization program without any additional processing of data. Using this program, the boundary of the reachable sets is extracted. Some surfaces (in part or as a whole) find themselves inside of the reachable set. The visualization program does not plot such pieces.

16 Will-be-set-by-IN-TECH

in the three-dimensional space *x*, *y*, *θ*. We restrict ourselves to the case of system (1) and a close to it system in which the control parameter *u* is restricted as *u* ∈ [*b*, 1], where *b* ∈ [−1, 0)

In paper (Patsko et al., 2003), it was established based on the application of the Pontryagin maximum principle to system (1) that for any point (*z*, *<sup>θ</sup>*) <sup>∈</sup> *<sup>∂</sup>G*3(*t*∗) the control steering to this point is piecewise continuous and has at most two switches. In addition, there are only 6

The second variant means that the control *u* ≡ −1 acts on some interval [0, *t*1), the control *u* ≡ 0 works on an interval [*t*1, *t*2), and the control *u* ≡ 1 operates on the interval [*t*2, *t*∗]. If *t*<sup>1</sup> = *t*2, then the second interval (where *u* ≡ 0) vanishes, and we obtain a single switch from *u* = −1 to *u* = 1. In the case *t*<sup>1</sup> = 0, the first interval where *u* ≡ −1 vanishes; in the case *t*<sup>2</sup> = *t*<sup>∗</sup> the third interval with *u* ≡ 1 is absent. The control has constant value for all *t* ∈ [0, *t*∗] if one of the following three conditions holds: *t*<sup>1</sup> = *t*∗, *t*<sup>2</sup> = 0, or both *t*<sup>1</sup> = 0 and *t*<sup>2</sup> = *t*∗.

The proposition on six variants of the control *u*(*t*) steering to the boundary of the reachable set *<sup>G</sup>*3(*t*∗) is similar in form to the Dubins theorem on the variants of the controls steering to the boundary of the reachable set <sup>G</sup>3(*t*∗). The same variants are valid. However, due to the relation between the sets *<sup>G</sup>*3(*t*∗) and <sup>G</sup>3(*t*∗) (the set <sup>G</sup>3(*t*∗) is the union of the sets *<sup>G</sup>*3(*t*) over *t* ∈ [0, *t*∗]), the above mentioned properties of the controls leading to the boundary of the set *<sup>G</sup>*3(*t*∗) result in the analogous properties of the controls leading to the boundary of the set

Let us apply the above formulated result on the structure of the control *<sup>u</sup>*(*t*) steering to *<sup>∂</sup>G*3(*t*∗)

To construct the boundary *<sup>∂</sup>G*3(*t*∗) of the set *<sup>G</sup>*3(*t*∗), we search through all controls of the form 1– 6 with two switches *t*1, *t*2. For every variant of switches, the parameter *t*<sup>1</sup> is chosen from the interval [0, *t*∗], and the parameter *t*<sup>2</sup> from the interval [*t*1, *t*∗]. In addition, controls with one switch and without switches are also considered. Taken a specific variant of switching and searching through the parameters *t*1, *t*<sup>2</sup> on some sufficiently fine grid, we obtain a collection

Therefore, each of the six variants yields its own surface in the three-dimensional space. The boundary of the reachable set *<sup>G</sup>*3(*t*∗) is composed of pieces of these surfaces. The six surfaces are loaded into the visualization program without any additional processing of data. Using this program, the boundary of the reachable sets is extracted. Some surfaces (in part or as a whole) find themselves inside of the reachable set. The visualization program does not plot

**4.1 Structure of controls steering to the boundary of reachable sets at given time**

1) 1, 0, 1; 2) −1, 0, 1; 3) 1, 0, −1; 4) −1, 0, −1; 5) 1, −1, 1; 6) −1, 1, −1.

**4.2 Numerical construction of three-dimensional reachable sets at given time**

for the numerical construction of the boundary *<sup>∂</sup>G*3(*t*∗).

of points generating a surface in the three-dimensional space *x*, *y*, *θ*.

(*z*(*t*∗; 0, 0, *<sup>u</sup>*(·), *<sup>w</sup>*(·)), *<sup>θ</sup>*(*t*∗; 0, 0, *<sup>u</sup>*(·), *<sup>w</sup>*(·)), <sup>G</sup>3(*t*∗) :<sup>=</sup>

*t*∈[0,*t*∗]

*G*3(*t*)

**4. Three-dimensional reachable sets**

Let us describe the reachable sets

variants of changing the control:

Similar is true for the other variants.

<sup>G</sup>3(*t*∗), but the converse is false.

such pieces.

*u*(·),*w*(·)

*<sup>G</sup>*3(*t*∗) :<sup>=</sup>

is the fixed parameter.

The visualization of the three-dimensional sets is done with the program "Cortona VRML Client" utilizing the open standard format VRML/X3D for the demonstration of interactive vector graphics.

Fig. 16 shows the boundary of the set *<sup>G</sup>*3(*t*∗) at time *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> 1.5*<sup>π</sup>* from two perspectives. The initial values of *x*0, *y*0, and *θ*<sup>0</sup> are equal to zero. The different parts of the boundary are marked with different colors. For example, part 2 is reachable for the trajectories with the control *u*(*t*) of the form −1, 0, 1 with two switches. The sections of the reachable set by the plane *θ* = const are depicted with some step along the axis *θ*. The points of junction lines of parts 1,2; 1,3; 2,4; 2,5; 2,6; 3,4; 3,5; 3,6 are obtained with a single-switch control. Any point of the common line of parts 5 and 6 is reachable for two trajectories with two switches each. Parts 5 and 6 have non-smooth junction along this line. The angle of the junction is not visible because it is rather small. The control *u*(*t*) ≡ 0 steers to the junction point of parts 1−4.

Fig. 16. The set *<sup>G</sup>*3(*t*∗) for *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> 1.5*<sup>π</sup>* shown from the two perspectives

Fig. 17 shows reachable sets *<sup>G</sup>*3(*t*∗) at the same perspective but with different scales for four time instants *t*∗. The transformation of the structure of the reachable set boundary is clearly seen. With increasing time, the forward part of the boundary covers the back part composed of patches 5, 6. Note that the angle *θ* is not restricted as *θ* ∈ [−*π*, *π*).

Passing from *t*<sup>∗</sup> = 3*π* to *t*<sup>∗</sup> = 4*π*, one arrives at the time *t*<sup>∗</sup> ≈ 3.65*π* when the reachable set *<sup>G</sup>*3(*t*∗) becomes non-simply-connected for some small time interval. Namely, a cavity that does not belong to the reachable set arises. In Fig. 18, an origination of such a situation is shown. Here, a cut of two sets *<sup>G</sup>*3(*t*∗) corresponding to instants *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> <sup>3</sup>*<sup>π</sup>* and *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> 3.65*<sup>π</sup>* is depicted. The cut is done using the plane *θ* = 0. The set *G*3(3*π*) is simply connected and the set *G*(3.65*π*) is not.

Fig. 19 shows the set *<sup>G</sup>*3(*t*∗) for *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> 1.6*π*, *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> <sup>2</sup>*π*, and *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> 2.5*π*. Here the angle *<sup>θ</sup>* is calculated modulo 2*π*.

t<sup>∗</sup> = 2.5π

Fig. 19. The set *<sup>G</sup>*3(*t*∗) for three time instants with *<sup>θ</sup>* computed modulo 2*<sup>π</sup>*

**4.3 Numerical construction of three-dimensional reachable sets by given time**

*t*∈[0,*t*∗]

generated by the controls of the kind 5,6 is outside of the set *<sup>G</sup>*3(*t*<sup>∗</sup> <sup>+</sup> <sup>Δ</sup>*t*).

The observation of change of the set *G*3(*t*) gives the following.

Let us describe the reachable sets <sup>G</sup>3(*t*∗) by given time. Theoretically, their construction can

obtained by running *t* on [0, *t*∗] with a small step. However, this is very difficult approach for practical constructions. The analysis of the development of the sets *<sup>G</sup>*2(*t*∗) and <sup>G</sup>2(*t*∗) in

For *<sup>t</sup>*<sup>∗</sup> <sup>∈</sup> (0, *<sup>π</sup>*), any point inside of that part of the boundary *<sup>∂</sup>G*3(*t*∗) which is generated by the controls of the kind 1−4 is strictly inside of the set *<sup>G</sup>*3(*t*<sup>∗</sup> <sup>+</sup> <sup>Δ</sup>*t*) for any sufficiently small <sup>Δ</sup>*<sup>t</sup>* <sup>&</sup>gt; 0. Conversely, any point lying inside of that part of the boundary *<sup>∂</sup>G*3(*t*∗) which is

*G*3(*t*), and the boundary of the sets *G*3(*t*) can be

t<sup>∗</sup> = 2π

θ

base on the definition <sup>G</sup>3(*t*∗) =

the plane *x*, *y* suggests a more thrifty method.

y

x

t<sup>∗</sup> =1.6π

Reachable Sets for Simple Models of Car Motion 165

Fig. 17. Development of the reachable set *<sup>G</sup>*3(*t*∗)

Fig. 18. Loss of simple connectivity

18 Will-be-set-by-IN-TECH

θ

Fig. 17. Development of the reachable set *<sup>G</sup>*3(*t*∗)

t<sup>∗</sup> = 3.65

x

Fig. 18. Loss of simple connectivity

π

y

θ

y

x

t<sup>∗</sup> = 4π

> t<sup>∗</sup> = 3π

t<sup>∗</sup> = 3π

t<sup>∗</sup> = 2π

t<sup>∗</sup> = π

Fig. 19. The set *<sup>G</sup>*3(*t*∗) for three time instants with *<sup>θ</sup>* computed modulo 2*<sup>π</sup>*

#### **4.3 Numerical construction of three-dimensional reachable sets by given time**

Let us describe the reachable sets <sup>G</sup>3(*t*∗) by given time. Theoretically, their construction can base on the definition <sup>G</sup>3(*t*∗) = *t*∈[0,*t*∗] *G*3(*t*), and the boundary of the sets *G*3(*t*) can be

obtained by running *t* on [0, *t*∗] with a small step. However, this is very difficult approach for practical constructions. The analysis of the development of the sets *<sup>G</sup>*2(*t*∗) and <sup>G</sup>2(*t*∗) in the plane *x*, *y* suggests a more thrifty method.

The observation of change of the set *G*3(*t*) gives the following.

For *<sup>t</sup>*<sup>∗</sup> <sup>∈</sup> (0, *<sup>π</sup>*), any point inside of that part of the boundary *<sup>∂</sup>G*3(*t*∗) which is generated by the controls of the kind 1−4 is strictly inside of the set *<sup>G</sup>*3(*t*<sup>∗</sup> <sup>+</sup> <sup>Δ</sup>*t*) for any sufficiently small <sup>Δ</sup>*<sup>t</sup>* <sup>&</sup>gt; 0. Conversely, any point lying inside of that part of the boundary *<sup>∂</sup>G*3(*t*∗) which is generated by the controls of the kind 5,6 is outside of the set *<sup>G</sup>*3(*t*<sup>∗</sup> <sup>+</sup> <sup>Δ</sup>*t*).

*t*<sup>∗</sup> = 1.5π

6)

1) 1, 0, 1

1)

t<sup>∗</sup> = 4π

t<sup>∗</sup> = 3π

t<sup>∗</sup> = 2π

t<sup>∗</sup> = π

3) 1, 0, -1

3)

6) -1, 1, -1

θ

θ

5)

*x*

*y*

2)

θ

y

x

Fig. 21. Development of the reachable set <sup>G</sup>3(*t*∗)

Fig. 20. The set <sup>G</sup>3(*t*∗) for *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> 1.5*<sup>π</sup>* shown from the two perspectives

5) 1, -1, 1

4) -1, 0, -1

2) -1, 0, 1

Reachable Sets for Simple Models of Car Motion 167

**I**) 1, -1;

**II**) -1, 1;

*t* ≤ 1.5*π*

*t* ≤ 1.5*π*

*y*

4)

*x*

Therefore, one can say that for *<sup>t</sup>*<sup>∗</sup> <sup>∈</sup> (0, *<sup>π</sup>*), the front part of the boundary *<sup>∂</sup>G*3(*t*∗) be the piece of the boundary generated by the controls 1−4, and the back part be the piece of the boundary *<sup>∂</sup>G*3(*t*∗) corresponding to the controls 5,6. The junction of the front and back parts of the boundary *<sup>∂</sup>G*3(*t*∗) occurs along two one-dimensional arcs in space *<sup>x</sup>*, *<sup>y</sup>*, *<sup>θ</sup>*, which correspond to the controls of the form (1, -1) and (-1, 1) with one switch on [0, *t*∗]. Computing the collection of such arcs for every *t* ∈ [0, *t*∗], we obtain a surface which forms the "barrier" part of the boundary of the set <sup>G</sup>3(*t*∗). In total, the boundary of the set <sup>G</sup>3(*t*∗) is composed of the front and barrier parts.

Thus, the construction of the boundary *<sup>∂</sup>*G3(*t*∗) for *<sup>t</sup>*<sup>∗</sup> <sup>∈</sup> [0, *<sup>π</sup>*] requires loading of 4 surfaces corresponding to the controls 1−4 with two switches and 2 surfaces corresponding to the controls of the form (1, -1) (surface I) and (-1, 1) (surface II) with one switch on the interval [0, *t*], where *t* ∈ [0, *t*∗], to the visualization program. The program constructs automatically the visible from the outside boundary of the set <sup>G</sup>3(*t*∗).

Let now *<sup>t</sup>*<sup>∗</sup> <sup>∈</sup> (*π*, 4*π*]. In this case, some part of the boundary *<sup>∂</sup>G*3(*t*∗) generated by the controls of the form 5 and 6 becomes the front one. For the construction of the boundary *<sup>∂</sup>*G3(*t*∗), 6 surfaces corresponding to the controls 1−6 with two switches and 2 surfaces I and II corresponding to the controls of the form (1, -1) and (-1, 1) with one switch on [0, *t*], where *t* ∈ [0, *t*∗], are loaded into the visualization program. Note that for *t*<sup>∗</sup> ∈ [2*π*, 4*π*] it is not necessary to increment two latter surfaces. It is sufficient to use their parts constructed up to time 2*π*. It should be emphasized that similarly to the case of the sets *<sup>G</sup>*3(*t*∗), there is a small time interval from [3*π*, 4*π*] on which the set <sup>G</sup>3(*t*∗) becomes non-simply-connected. For *t*<sup>∗</sup> from such an interval, the above described rule of the construction of the boundary using the visualization program gives only the external part of the boundary of the sets <sup>G</sup>3(*t*∗). The detection of the "internal" boundary requires additional analysis and is not described here. Starting from the instant of time *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> <sup>4</sup>*π*, the boundary of the set *<sup>G</sup>*3(*t*∗) becomes entirely a

front. In this case, <sup>G</sup>3(*t*∗) = *<sup>G</sup>*3(*t*∗), *<sup>t</sup>*<sup>∗</sup> <sup>≥</sup> <sup>4</sup>*π*.

The set <sup>G</sup>3(*t*∗) for *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> 1.5*<sup>π</sup>* is shown from two perspectives in Fig. 20; development of <sup>G</sup>3(*t*∗) with increasing *t*<sup>∗</sup> is given in Fig. 21. These pictures can be compared with Figs. 16 and 17. The difference of the reachable sets by given time from the reachable sets at given time is in the presence of the barrier part formed by the smooth surfaces I and II. To understand better its arrangement, Fig. 22 gives the cut-off sets <sup>G</sup>3(2*π*) and <sup>G</sup>3(3*π*) (cutting plane is *<sup>θ</sup>* <sup>=</sup> 0). The barrier part is developed from the initial point (0, 0, 0) (white point in the pictures). Every of the shown level lines on the barrier part corresponds to its own instant of time. Till *t*<sup>∗</sup> = *π*, the level lines are closed curves. With *t*<sup>∗</sup> increasing, new level lines which are not anymore closed occur. In addition, the old level lines are reduced at their ends, among them those ones constructed until *t*<sup>∗</sup> = *π*. Starting from the instant *t*<sup>∗</sup> = 2*π*, the reduction of the constructed lines begins. It finishes at the time *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> <sup>4</sup>*<sup>π</sup>* when the set *<sup>G</sup>*3(4*π*) captures the point (0, 0, 0). The set <sup>G</sup>3(*t*∗) for *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> <sup>2</sup>*<sup>π</sup>* with the angle *<sup>θ</sup>* computed modulo 2*<sup>π</sup>* is shown in Fig. 23.

Time-dependent construction of the reachable sets <sup>G</sup>3(*t*∗) with the indication of which control from variants 1– 6 corresponds to every particular piece on the front part of these sets is close to finding of optimal feedback control synthesis for the minimum time problem of steering to the point (0, 0, 0). The optimal feedback control synthesis for system (1) was obtained in (Pecsvaradi, 1972) (*θ* is taken modulo 2*π*).

Note that some individual images of three-dimensional reachable sets by given time (with *θ* taken modulo 2*π*) obtained by other means are available in the works (Laumond, 1998), p. 7, and (Takei & Tsai, 2010), pp. 22, 23, 26, and 27.

20 Will-be-set-by-IN-TECH

Therefore, one can say that for *<sup>t</sup>*<sup>∗</sup> <sup>∈</sup> (0, *<sup>π</sup>*), the front part of the boundary *<sup>∂</sup>G*3(*t*∗) be the piece of the boundary generated by the controls 1−4, and the back part be the piece of the boundary *<sup>∂</sup>G*3(*t*∗) corresponding to the controls 5,6. The junction of the front and back parts of the boundary *<sup>∂</sup>G*3(*t*∗) occurs along two one-dimensional arcs in space *<sup>x</sup>*, *<sup>y</sup>*, *<sup>θ</sup>*, which correspond to the controls of the form (1, -1) and (-1, 1) with one switch on [0, *t*∗]. Computing the collection of such arcs for every *t* ∈ [0, *t*∗], we obtain a surface which forms the "barrier" part of the boundary of the set <sup>G</sup>3(*t*∗). In total, the boundary of the set <sup>G</sup>3(*t*∗) is composed of the front

Thus, the construction of the boundary *<sup>∂</sup>*G3(*t*∗) for *<sup>t</sup>*<sup>∗</sup> <sup>∈</sup> [0, *<sup>π</sup>*] requires loading of 4 surfaces corresponding to the controls 1−4 with two switches and 2 surfaces corresponding to the controls of the form (1, -1) (surface I) and (-1, 1) (surface II) with one switch on the interval [0, *t*], where *t* ∈ [0, *t*∗], to the visualization program. The program constructs automatically

Let now *<sup>t</sup>*<sup>∗</sup> <sup>∈</sup> (*π*, 4*π*]. In this case, some part of the boundary *<sup>∂</sup>G*3(*t*∗) generated by the controls of the form 5 and 6 becomes the front one. For the construction of the boundary *<sup>∂</sup>*G3(*t*∗), 6 surfaces corresponding to the controls 1−6 with two switches and 2 surfaces I and II corresponding to the controls of the form (1, -1) and (-1, 1) with one switch on [0, *t*], where *t* ∈ [0, *t*∗], are loaded into the visualization program. Note that for *t*<sup>∗</sup> ∈ [2*π*, 4*π*] it is not necessary to increment two latter surfaces. It is sufficient to use their parts constructed up to time 2*π*. It should be emphasized that similarly to the case of the sets *<sup>G</sup>*3(*t*∗), there is a small time interval from [3*π*, 4*π*] on which the set <sup>G</sup>3(*t*∗) becomes non-simply-connected. For *t*<sup>∗</sup> from such an interval, the above described rule of the construction of the boundary using the visualization program gives only the external part of the boundary of the sets <sup>G</sup>3(*t*∗). The detection of the "internal" boundary requires additional analysis and is not described here. Starting from the instant of time *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> <sup>4</sup>*π*, the boundary of the set *<sup>G</sup>*3(*t*∗) becomes entirely a

The set <sup>G</sup>3(*t*∗) for *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> 1.5*<sup>π</sup>* is shown from two perspectives in Fig. 20; development of <sup>G</sup>3(*t*∗) with increasing *t*<sup>∗</sup> is given in Fig. 21. These pictures can be compared with Figs. 16 and 17. The difference of the reachable sets by given time from the reachable sets at given time is in the presence of the barrier part formed by the smooth surfaces I and II. To understand better its arrangement, Fig. 22 gives the cut-off sets <sup>G</sup>3(2*π*) and <sup>G</sup>3(3*π*) (cutting plane is *<sup>θ</sup>* <sup>=</sup> 0). The barrier part is developed from the initial point (0, 0, 0) (white point in the pictures). Every of the shown level lines on the barrier part corresponds to its own instant of time. Till *t*<sup>∗</sup> = *π*, the level lines are closed curves. With *t*<sup>∗</sup> increasing, new level lines which are not anymore closed occur. In addition, the old level lines are reduced at their ends, among them those ones constructed until *t*<sup>∗</sup> = *π*. Starting from the instant *t*<sup>∗</sup> = 2*π*, the reduction of the constructed lines begins. It finishes at the time *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> <sup>4</sup>*<sup>π</sup>* when the set *<sup>G</sup>*3(4*π*) captures the point (0, 0, 0). The set <sup>G</sup>3(*t*∗) for *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> <sup>2</sup>*<sup>π</sup>* with the angle *<sup>θ</sup>* computed modulo 2*<sup>π</sup>* is shown in Fig. 23.

Time-dependent construction of the reachable sets <sup>G</sup>3(*t*∗) with the indication of which control from variants 1– 6 corresponds to every particular piece on the front part of these sets is close to finding of optimal feedback control synthesis for the minimum time problem of steering to the point (0, 0, 0). The optimal feedback control synthesis for system (1) was obtained in

Note that some individual images of three-dimensional reachable sets by given time (with *θ* taken modulo 2*π*) obtained by other means are available in the works (Laumond, 1998), p. 7,

the visible from the outside boundary of the set <sup>G</sup>3(*t*∗).

front. In this case, <sup>G</sup>3(*t*∗) = *<sup>G</sup>*3(*t*∗), *<sup>t</sup>*<sup>∗</sup> <sup>≥</sup> <sup>4</sup>*π*.

(Pecsvaradi, 1972) (*θ* is taken modulo 2*π*).

and (Takei & Tsai, 2010), pp. 22, 23, 26, and 27.

and barrier parts.

Fig. 20. The set <sup>G</sup>3(*t*∗) for *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> 1.5*<sup>π</sup>* shown from the two perspectives

Fig. 21. Development of the reachable set <sup>G</sup>3(*t*∗)

**4.4 Case of non-symmetric constraint on control** *u*

*u* [−0.25, 1]

∈

θ

5) 1, -0.25, 1

constraint on the control *u*

*x*

*y*

4) -0.25, 0, -0.25 2) -0.25, 0, 1

6) -0.25, 1, -0.25

modulo 2*π* is shown in Fig. 26.

The proposition on the structure of controls steering trajectories of system (1) to the boundary of the reachable set *<sup>G</sup>*3(*t*∗) is also preserved for the case of non-symmetric constraint *<sup>u</sup>* <sup>∈</sup> [*b*, 1] with *b* ∈ [−1, 0). One should only replace *u* = −1 by *u* = *b*. Results of the construction of the sets *<sup>G</sup>*3(*t*∗) are shown for *<sup>b</sup>* <sup>=</sup> <sup>−</sup>0.25 in Fig. 24. The sets *<sup>G</sup>*3(4*π*) and *<sup>G</sup>*3(6*π*) are depicted from the same perspective and have the same scale. Approximately the same perspective but a larger scale is used in Fig. 25 presenting the set <sup>G</sup>3(4*π*). This set with the angle *<sup>θ</sup>* taken

Reachable Sets for Simple Models of Car Motion 169

With a fixed point (*x*, *y*, *θ*), we can compute the first instant *V*(*x*, *y*, *θ*) when this point is on the boundary of the set *<sup>G</sup>*3(*t*) or, what is the same, on the boundary of <sup>G</sup>3(*t*). The value *<sup>V</sup>*(*x*, *<sup>y</sup>*, *<sup>θ</sup>*) be the optimal steering time from the point (0, 0, 0) to the point (*x*, *y*, *θ*). Paper (Bakolas & Tsiotras, 2011) gives results on the computation of level sets of the function *V*(*x*, *y*, *θ*) for fixed values *θ* (modulo 2*π*) and different values of the parameter *b*. This is equivalent to

3) 1, 0, -0.25

Fig. 24. The reachable sets *<sup>G</sup>*3(*t*∗) for the instants *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> <sup>4</sup>*<sup>π</sup>* and *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> <sup>6</sup>*<sup>π</sup>* for non-symmetric

*t*<sup>∗</sup> = 4π

*t*<sup>∗</sup> = 6π

1) 1, 0, 1

the computation of *<sup>θ</sup>*-sections of the sets <sup>G</sup>3(*t*) for different values of *<sup>b</sup>*.

Fig. 22. The cut-off sets <sup>G</sup>3(*t*∗) for *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> <sup>2</sup>*<sup>π</sup>* and *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> <sup>3</sup>*π*, cutting plane *<sup>θ</sup>* <sup>=</sup> <sup>0</sup>

Fig. 23. The set <sup>G</sup>3(2*π*) with the angle *<sup>θ</sup>* computed modulo 2*<sup>π</sup>*

22 Will-be-set-by-IN-TECH

Fig. 22. The cut-off sets <sup>G</sup>3(*t*∗) for *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> <sup>2</sup>*<sup>π</sup>* and *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> <sup>3</sup>*π*, cutting plane *<sup>θ</sup>* <sup>=</sup> <sup>0</sup>

y

x

Fig. 23. The set <sup>G</sup>3(2*π*) with the angle *<sup>θ</sup>* computed modulo 2*<sup>π</sup>*

θ

t<sup>∗</sup> = 2π

> t<sup>∗</sup> = 3π

x

θ

y

t<sup>∗</sup> = 2π

#### **4.4 Case of non-symmetric constraint on control** *u*

The proposition on the structure of controls steering trajectories of system (1) to the boundary of the reachable set *<sup>G</sup>*3(*t*∗) is also preserved for the case of non-symmetric constraint *<sup>u</sup>* <sup>∈</sup> [*b*, 1] with *b* ∈ [−1, 0). One should only replace *u* = −1 by *u* = *b*. Results of the construction of the sets *<sup>G</sup>*3(*t*∗) are shown for *<sup>b</sup>* <sup>=</sup> <sup>−</sup>0.25 in Fig. 24. The sets *<sup>G</sup>*3(4*π*) and *<sup>G</sup>*3(6*π*) are depicted from the same perspective and have the same scale. Approximately the same perspective but a larger scale is used in Fig. 25 presenting the set <sup>G</sup>3(4*π*). This set with the angle *<sup>θ</sup>* taken modulo 2*π* is shown in Fig. 26.

With a fixed point (*x*, *y*, *θ*), we can compute the first instant *V*(*x*, *y*, *θ*) when this point is on the boundary of the set *<sup>G</sup>*3(*t*) or, what is the same, on the boundary of <sup>G</sup>3(*t*). The value *<sup>V</sup>*(*x*, *<sup>y</sup>*, *<sup>θ</sup>*) be the optimal steering time from the point (0, 0, 0) to the point (*x*, *y*, *θ*). Paper (Bakolas & Tsiotras, 2011) gives results on the computation of level sets of the function *V*(*x*, *y*, *θ*) for fixed values *θ* (modulo 2*π*) and different values of the parameter *b*. This is equivalent to the computation of *<sup>θ</sup>*-sections of the sets <sup>G</sup>3(*t*) for different values of *<sup>b</sup>*.

Fig. 24. The reachable sets *<sup>G</sup>*3(*t*∗) for the instants *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> <sup>4</sup>*<sup>π</sup>* and *<sup>t</sup>*<sup>∗</sup> <sup>=</sup> <sup>6</sup>*<sup>π</sup>* for non-symmetric constraint on the control *u*

**6. Acknowledgement**

Berlin.

177–206.

(No. 2), 233–250.

Birkhäuser, Boston.

*Mathematics*, Vol. 79, 497–516.

Vol. 196 (No. 3-4), 513–539.

(No. 5,6), 250–276 (in Russian).

Isaacs, R. (1965). *Differential Games*, John Wiley, New York.

Information Sciences, Vol. 229, Springer, New York.

*Sciences (New York)*, Vol. 147 (No. 2), 6569–6606.

*Remote Control*, Vol. 68 (No. 11), 2056–2070.

**7. References**

Presidium of RAS (Ural Branch project 09-Π-1-1015).

This work is partially supported by the Russian Foundation for Basic Research (project nos. 09-01-00436 and 10-01-96006) and by the Program "Mathematical control theory" of the

Reachable Sets for Simple Models of Car Motion 171

Agrachev, A. A. & Sachkov, Yu. (2004). *Control Theory from the Geometric Viewpoint*, Springer,

Bakolas, E. & Tsiotras, P. (2011). Optimal synthesis of the asymmetric sinistral/dextral

Bardi, M. & Capuzzo-Dolcetta, I. (1997). *Optimal Control and Viscosity Solutions of*

Boissonnat, J.-D. & Bui, X.-N. (1994). *Accessibility region for a car that only moves forwards along*

Cockayne, E. J. & Hall, G. W. C. (1975). Plane motion of a particle subject to curvature constraints, *SIAM Journal on Control and Optimization*, Vol. 13 (No. 1), 197–220. Cristiani, E. & Falcone, M. (2009). Fully-discrete schemes for the value function of

Dubins, L. E. (1957). On curves of minimal length with a constraint on average curvature

Grigor'eva, S. V.; Pakhotinskikh, V. Yu.; Uspenskii, A. A.; Ushakov, V. N. (2005). Construction

Isaacs, R. (1951). *Games of pursuit*, Scientific report of the RAND Corporation, Santa Monica.

Laumond, J.-P. (ed.) (1998). *Robot Motion Planning and Control*, Lecture Notes in Control and

Martynenko, Yu. G. (2007). Motion control of mobile wheeled robots, *Journal of Mathematical*

Mikhalev, D. K. & Ushakov, V. N. (2007). On two algorithms for the approximate construction

LaValle, S. M. (2006). *Planning Algorithms*, Chapters 13, 15, Cambridge University Press. Lee, E. B. & Markus, L. (1967). *Foundations of Optimal Control Theory*, Wiley, New York. Lensky, A. V. & Formal'sky, A. M. (2003). Two-wheel robot-bicycle with a gyroscopic stabilizer, *Journal of Computer and Systems Sciences International*, No. 3, 482–489. Markov, A. A. (1889). Some examples of the solution of a special kind of problem on greatest

*optimal paths*, Rapport de recherche N◦ 2181, INRIA.

Markov – Dubins problem, *Journal of Optimization Theory and Applications*, Vol. 150

*Hamilton – Jacobi – Bellman Equations. Systems & Control: Foundations and Applications*.

pursuit-evasion games with state constraints, *Advances in Dynamic Games and Their Applications, Ann. Internat. Soc. Dynam. Games*, Vol. 10, Birkhäuser, Boston, MA,

and with prescribed initial and terminal positions and tangents, *American Journal of*

of solutions in some differential games with phase constraints, *Sbornik: Mathematics*,

and least quantities, *Soobscenija Charkovskogo Matematiceskogo Obscestva*, Vol. 2-1

of a positional absorption set in a game-theoretic approach problem, *Automation &*

Fig. 25. The reachable set <sup>G</sup>3(4*π*) for non-symmetric constraint on the control *<sup>u</sup>*

Fig. 26. The reachable set <sup>G</sup>3(4*π*) with the angle *<sup>θ</sup>* computed modulo 2*<sup>π</sup>*

#### **5. Conclusion**

The paper considers reachable sets and inherent character of their development for simplest models of "car" motion used in the mathematical literature. Our investigation is restricted to the cases where reachable sets are constructed in two- and three-dimensional spaces. The understanding of the features and complexities that appear in low dimensional problems can be useful for the analysis of more complex models and for solving real practical problems (Laumond, 1998),(Lensky & Formal'sky, 2003), (LaValle, 2006), (Martynenko, 2007).

#### **6. Acknowledgement**

This work is partially supported by the Russian Foundation for Basic Research (project nos. 09-01-00436 and 10-01-96006) and by the Program "Mathematical control theory" of the Presidium of RAS (Ural Branch project 09-Π-1-1015).

#### **7. References**

24 Will-be-set-by-IN-TECH

3)

4) 2) *u* [−0.25, 1.0]

Fig. 26. The reachable set <sup>G</sup>3(4*π*) with the angle *<sup>θ</sup>* computed modulo 2*<sup>π</sup>*

The paper considers reachable sets and inherent character of their development for simplest models of "car" motion used in the mathematical literature. Our investigation is restricted to the cases where reachable sets are constructed in two- and three-dimensional spaces. The understanding of the features and complexities that appear in low dimensional problems can be useful for the analysis of more complex models and for solving real practical problems

(Laumond, 1998),(Lensky & Formal'sky, 2003), (LaValle, 2006), (Martynenko, 2007).

**I**) 1, -0.25; *t* [0, 4 ]

*π*

*π*

**II**) -0.25, 1; *t* [0, 4 ]

θ

*x*

**5. Conclusion**

*y*

∈

θ

*y* ∈

*x*

6)

∈

Fig. 25. The reachable set <sup>G</sup>3(4*π*) for non-symmetric constraint on the control *<sup>u</sup>*

*t*<sup>∗</sup> = 4π

1)

*t*<sup>∗</sup> = 4π

*u* [−0.25, 1]

∈


**0**

**9**

**Neural Networks Based Path Planning and**

<sup>1</sup>*Department of Electrical & Electronic Engineering, Okayama University of Science,*

<sup>2</sup>*Department of Electronics and Information, Zhongyuan University of Technology,*

The path planning for mobile robots is a fundamental issue in the field of unmanned vehicles control. The purpose of the path planner is to compute a path from the start position of the vehicle to the goal to be reached. The primary concern of path planning is to compute *collision-free* paths. Another, equally important issue is to compute a *realizable* and, if possible,

Although humans have the superb capability to plan motions of their body and limbs effortlessly, the motion planning turns out to be a very complex problem. The best known algorithm has a complexity that is exponential to the number of degrees of freedom and polynomial in the geometric complexities of the robot and the obstacles in the environment (Chen & Hwang (1998)). Even for motion planning problems in the 2-dimensional space, existing complete algorithms that guarantee a solution often need large amount of memory and in some cases may take long computational time. On the other hand, fast heuristic

In this paper we present a fast algorithm for solving the path planning problem for differential drive (holonomic)<sup>1</sup> robots. The algorithm can be applied to free-flying and snake type robots, too. Generally, we treat the two-dimensional known environment, where the obstacles are stationary polygons or ovals, but the algorithm can easily be extended for the three-dimensional case (Kroumov et al. (2010)). The proposed algorithm is, in general, based on the potential field methods. The algorithm solves the local minimum problem and

The paper is organized as follows. Previous work is presented in Section 2. In Section 3 we give a definition of the map representation and how it is used to describe various obstacles situated in the working environment. The path and the obstacle collisions are detected using artificial annealing algorithm. Also, the solution of the local minima problem is described there. In Section 4 we describe the theoretical background and the development of a motion

<sup>1</sup> In the mobile robotics, the term *holonomic* refers to the kinematic constraints of the robot chassis. The holonomic robot has zero nonholonomic kinematic constraints, while the nonholonomic one has one or

algorithms may fail to find a solution even if it exists (Hwang & Ahuja (1992)).

generates optimal path in a relatively small number of calculations.

more nonholonomic kinematic constraints.

**1. Introduction**

*optimal path*, bringing the vehicle to the final position.

**Navigation of Mobile Robots**

*1Department of Electrical & Electronic Engineering,* 

*2Department of Electronics and Information, Zhongyuan* 

Valeri Kroumov1 and Jianli Yu<sup>2</sup>

*University of Technology, Zhengzhou* 

Valeri Kroumov1 and Jianli Yu2

*Okayama University of Science, Okayama* 

*700-0005, Okayama*

*Zhengzhou 450007*

<sup>1</sup>*Japan* <sup>2</sup>*China*

*1Japan 2China* 


### **Neural Networks Based Path Planning and Navigation of Mobile Robots**

Valeri Kroumov1 and Jianli Yu<sup>2</sup> Valeri Kroumov1 and Jianli Yu2

<sup>1</sup>*Department of Electrical & Electronic Engineering, Okayama University of Science, 700-0005, Okayama* <sup>2</sup>*Department of Electronics and Information, Zhongyuan University of Technology, Zhengzhou 450007* <sup>1</sup>*Japan* <sup>2</sup>*China 1Department of Electrical & Electronic Engineering, Okayama University of Science, Okayama 2Department of Electronics and Information, Zhongyuan University of Technology, Zhengzhou 1Japan 2China* 

#### **1. Introduction**

26 Will-be-set-by-IN-TECH

172 Recent Advances in Mobile Robotics

Patsko, V. S.; Pyatko, S. G.; Fedotov, A. A. (2003). Three-dimensional reachability set for a

Patsko, V. S. & Turova, V. L. (2009). From Dubins' car to Reeds and Shepp's mobile robot,

Pecsvaradi, T. (1972). Optimal horizontal guidance law for aircraft in the terminal area, *IEEE*

Pontryagin, L. S.; Boltyanskii, V. G.; Gamkrelidze, R. V.; Mischenko E. F. (1962). *The*

Reeds, J. A. & Shepp, L. A. (1990). Optimal paths for a car that goes both forwards and

Sethian, J. A. (1999) *Level Set Methods and Fast Marching Methods*, Cambridge University Press,

Sou*e*`res, P.; Fourquet, J.-Y.; Laumond, J.-P. (1994). Set of reachable positions for a car, *IEEE*

Sou*e*`res, P. & Laumond, J. P. (1996). Shortest paths synthesis for a car-like robot, *IEEE*

Sussmann, H.J. & Tang, W. (1991). *Shortest paths for the Reeds-Shepp car: a worked out example*

Takei, R. & Tsai, R. (2010). *Optimal trajectories of curvature constrained motion in the Hamilton-Jacobi formulation*, Report 10-67, University of California, Los Angeles.

*of the use of geometric techniques in nonlinear optimal control*, Report SYCON-91-lO,

*Computing and Visualization in Science*, Vol. 12 (No. 7), 345–364.

*Mathematical Theory of Optimal Processes*, Interscience, New York.

*Transactions on Automatic Control* Vol. 17 (No. 6), 763–772.

backwards, *Pacific Journal of Mathematics*, Vol. 145, 367–393.

*Transactions on Automatic Control*, Vol. 39 (No. 8), 1626–1630.

*Transactions on Automatic Control*, Vol. 41 (No. 5), 672–688.

(No. 3), 320–328.

Cambridge, UK.

Rutgers University.

nonlinear control system, *Journal of Computer and Systems Sciences International*, Vol. 42

The path planning for mobile robots is a fundamental issue in the field of unmanned vehicles control. The purpose of the path planner is to compute a path from the start position of the vehicle to the goal to be reached. The primary concern of path planning is to compute *collision-free* paths. Another, equally important issue is to compute a *realizable* and, if possible, *optimal path*, bringing the vehicle to the final position.

Although humans have the superb capability to plan motions of their body and limbs effortlessly, the motion planning turns out to be a very complex problem. The best known algorithm has a complexity that is exponential to the number of degrees of freedom and polynomial in the geometric complexities of the robot and the obstacles in the environment (Chen & Hwang (1998)). Even for motion planning problems in the 2-dimensional space, existing complete algorithms that guarantee a solution often need large amount of memory and in some cases may take long computational time. On the other hand, fast heuristic algorithms may fail to find a solution even if it exists (Hwang & Ahuja (1992)).

In this paper we present a fast algorithm for solving the path planning problem for differential drive (holonomic)<sup>1</sup> robots. The algorithm can be applied to free-flying and snake type robots, too. Generally, we treat the two-dimensional known environment, where the obstacles are stationary polygons or ovals, but the algorithm can easily be extended for the three-dimensional case (Kroumov et al. (2010)). The proposed algorithm is, in general, based on the potential field methods. The algorithm solves the local minimum problem and generates optimal path in a relatively small number of calculations.

The paper is organized as follows. Previous work is presented in Section 2. In Section 3 we give a definition of the map representation and how it is used to describe various obstacles situated in the working environment. The path and the obstacle collisions are detected using artificial annealing algorithm. Also, the solution of the local minima problem is described there. In Section 4 we describe the theoretical background and the development of a motion

<sup>1</sup> In the mobile robotics, the term *holonomic* refers to the kinematic constraints of the robot chassis. The holonomic robot has zero nonholonomic kinematic constraints, while the nonholonomic one has one or more nonholonomic kinematic constraints.

with minimum sized leaf quadrants along the edges of the polygon, but the quadtree is large and the number of leaves in the tree is proportional to the polygon's perimeter and this makes

Neural Networks Based Path Planning and Navigation of Mobile Robots 175

In the subgoal-graph approach, subgoals represent *key* configurations expected to be useful for finding collision-free paths. A graph of subgoals is generated and maintained by a global planner, and a simple local planner is used to determine the reachability among subgoals. This two level planning approach is first reported by Faverjon & Tournassoud (1987) and has

The potential field methods and their applications to path planning for autonomous mobile robots have been extensively studied in the past two decades (Khatib (1986), Warren (1989), Rimon & Doditschek (1992), Hwang & Ahuja (1992), Lee & Kardaras (1997a), Lee & Kardaras (1997b), Chen & Hwang (1998), Ge & Cui (2000), Yu et al. (2002), Kroumov et al. (2004)). The basic concept of the potential field methods is to fill the workspace with an artificial potential field in which the robot is attracted to the goal position being at the same time repulsed away from the obstacles (Latombe (1991)). It is well known that the strength of potential field methods is that, with some limited engineering, it is possible to construct quite efficient and relatively reliable motion planners (Latombe (1991)). But the potential field methods are usually incomplete and may fail to find a free path, even if one exists, because they can get trapped in a local minimum (Khosla & Volpe (1988); Rimon & Doditschek (1992); Sun et al. (1997)). Another problem with the existing potential field methods is that they are not so suitable to generate optimal path: adding optimization elements in the algorithm, usually, makes it quite costly from computational point of view (Zelinsky (1992)). Ideally, the potential

1. The magnitude of the potential field should be unbounded near obstacle boundaries and

3. The equipotential surface near an obstacle should have a shape similar to that of the

• the obstacle descriptions are implemented directly in the simulated annealing neural network—there is no need to use approximations by nonlinear equations (Lee & Kardaras

• there is no need to perform a learning of the workspace *off-line* using backpropagation

• there is no need to perform additional calculations and processing when an obstacle is

• a simple solution of the local minimum problem is developed and the generated paths are conditionally optimized in the sense that they are piecewise linear with directions

The algorithm allows parallel calculation (Sun et al. (1997)) which improves the computational time. The experimental results show that the calculation time of the presented algorithm depends linearly on the total number of obstacles' vertices—a feature

4. The potential, its gradient and their effects on the path must be spatially continuous. The proposed in this paper algorithm is partially inspired by the results presented by Sun et al.

2. The potential should have a spherical symmetry far away from the obstacle.

(1997) and by Lee & Kardaras (1997a), but our planner has several advantages:

turned out to be one of the most effective path planning methods.

field should have the following properties (Khosla & Volpe (1988)):

should decrease with range.

obstacle surface.

(1997b));

(Tsankova (2010));

added or removed;

changing at the corners of the obstacles.

it memory consuming.

planner for a differential drive vehicle. Section 5 presents the algorithm of the path planner. In Section 6 the calculation time is discussed and the effectiveness of the proposed algorithm is proven by presenting several simulation results. In the last section discussions, conclusions, and plans for further developments are presented.

#### **2. Previous work**

Comprehensive reviews on the work on the motion planning can be found in Latombe (1991). In this section we concentrate on motion planning for moving car-like robots in a known environment.

Motion planners can be classified in general as:


Complete motion planners can potentially require long computation times but they can either find a solution if there is one, or prove that there is none. Heuristic motion planners are fast but they often fail to find a solution even if it exists.

To date motion planners can be classified in four categories (Latombe (1991)):


In the skeleton approach the free space is represented by a network of one-dimensional paths called a *skeleton* and the solution is found by first moving the robot onto a point on the skeleton from the start configuration and from the goal, and by connecting the two points via paths on the skeleton. The approach is intuitive for two-dimensional problems, but becomes harder to implement for higher degrees of freedom problems.

Algorithms based on the visibility graph (VGRAPH) (Lozano-Pèrez & Wesley (1979)), the Voronoi diagram (O'Dúnlaing & Yap (1982)), and the *silhouette* (Canny (1988)) (projection of obstacle boundaries) are examples of the skeleton approach. In the VGRAPH algorithm the path planning is accomplished by finding a path through a graph connecting vertices of the forbidden regions (obstacles) and the generated path is near optimal. The drawback of the VGRAPH algorithm is that the description of all the possible paths is quite complicated. Actually, the number of the edges of the VGRAPH is proportional to the squared total number of the obstacles vertices and when this number increases, the calculation time becomes longer. Another drawback is that the algorithm deals only with polygonal objects. Yamamoto et al. (1998) have proposed a near-time-optimal trajectory planning for car-like robots, where the connectivity graph is generated in a fashion very similar to that of Lozano-Pèrez & Wesley (1979). They have proposed optimization of the speed of the robot for the generated trajectory, but the optimization of the trajectory itself is not enough treated.

In the cell decomposition approach (Paden et al. (1989)) the free space is represented as a union of cells, and a sequence of cells comprises a solution path. For efficiency, hierarchical trees, e.g. octree, are often used. The path planning algorithm proposed by Zelinsky (1992) is quite reliable and combines some advantages of the above algorithms. It makes use of quadtree data structure to model the environment and uses the distance transform methodology to generate paths. The obstacles are polygonal-shaped which yields a quadtree 2 Will-be-set-by-IN-TECH

planner for a differential drive vehicle. Section 5 presents the algorithm of the path planner. In Section 6 the calculation time is discussed and the effectiveness of the proposed algorithm is proven by presenting several simulation results. In the last section discussions, conclusions,

Comprehensive reviews on the work on the motion planning can be found in Latombe (1991). In this section we concentrate on motion planning for moving car-like robots in a known

Complete motion planners can potentially require long computation times but they can either find a solution if there is one, or prove that there is none. Heuristic motion planners are fast

In the skeleton approach the free space is represented by a network of one-dimensional paths called a *skeleton* and the solution is found by first moving the robot onto a point on the skeleton from the start configuration and from the goal, and by connecting the two points via paths on the skeleton. The approach is intuitive for two-dimensional problems, but becomes harder to

Algorithms based on the visibility graph (VGRAPH) (Lozano-Pèrez & Wesley (1979)), the Voronoi diagram (O'Dúnlaing & Yap (1982)), and the *silhouette* (Canny (1988)) (projection of obstacle boundaries) are examples of the skeleton approach. In the VGRAPH algorithm the path planning is accomplished by finding a path through a graph connecting vertices of the forbidden regions (obstacles) and the generated path is near optimal. The drawback of the VGRAPH algorithm is that the description of all the possible paths is quite complicated. Actually, the number of the edges of the VGRAPH is proportional to the squared total number of the obstacles vertices and when this number increases, the calculation time becomes longer. Another drawback is that the algorithm deals only with polygonal objects. Yamamoto et al. (1998) have proposed a near-time-optimal trajectory planning for car-like robots, where the connectivity graph is generated in a fashion very similar to that of Lozano-Pèrez & Wesley (1979). They have proposed optimization of the speed of the robot for the generated trajectory,

In the cell decomposition approach (Paden et al. (1989)) the free space is represented as a union of cells, and a sequence of cells comprises a solution path. For efficiency, hierarchical trees, e.g. octree, are often used. The path planning algorithm proposed by Zelinsky (1992) is quite reliable and combines some advantages of the above algorithms. It makes use of quadtree data structure to model the environment and uses the distance transform methodology to generate paths. The obstacles are polygonal-shaped which yields a quadtree

To date motion planners can be classified in four categories (Latombe (1991)):

and plans for further developments are presented.

Motion planners can be classified in general as:

but they often fail to find a solution even if it exists.

implement for higher degrees of freedom problems.

but the optimization of the trajectory itself is not enough treated.

**2. Previous work**

environment.

**1)** complete; **2)** heuristic.

**1)** skeleton;

**2)** cell decomposition; **3)** subgoal graph; **4)** potential field.

with minimum sized leaf quadrants along the edges of the polygon, but the quadtree is large and the number of leaves in the tree is proportional to the polygon's perimeter and this makes it memory consuming.

In the subgoal-graph approach, subgoals represent *key* configurations expected to be useful for finding collision-free paths. A graph of subgoals is generated and maintained by a global planner, and a simple local planner is used to determine the reachability among subgoals. This two level planning approach is first reported by Faverjon & Tournassoud (1987) and has turned out to be one of the most effective path planning methods.

The potential field methods and their applications to path planning for autonomous mobile robots have been extensively studied in the past two decades (Khatib (1986), Warren (1989), Rimon & Doditschek (1992), Hwang & Ahuja (1992), Lee & Kardaras (1997a), Lee & Kardaras (1997b), Chen & Hwang (1998), Ge & Cui (2000), Yu et al. (2002), Kroumov et al. (2004)). The basic concept of the potential field methods is to fill the workspace with an artificial potential field in which the robot is attracted to the goal position being at the same time repulsed away from the obstacles (Latombe (1991)). It is well known that the strength of potential field methods is that, with some limited engineering, it is possible to construct quite efficient and relatively reliable motion planners (Latombe (1991)). But the potential field methods are usually incomplete and may fail to find a free path, even if one exists, because they can get trapped in a local minimum (Khosla & Volpe (1988); Rimon & Doditschek (1992); Sun et al. (1997)). Another problem with the existing potential field methods is that they are not so suitable to generate optimal path: adding optimization elements in the algorithm, usually, makes it quite costly from computational point of view (Zelinsky (1992)). Ideally, the potential field should have the following properties (Khosla & Volpe (1988)):


The proposed in this paper algorithm is partially inspired by the results presented by Sun et al. (1997) and by Lee & Kardaras (1997a), but our planner has several advantages:


The algorithm allows parallel calculation (Sun et al. (1997)) which improves the computational time. The experimental results show that the calculation time of the presented algorithm depends linearly on the total number of obstacles' vertices—a feature

Fig. 2. The annealing network for a rectangular obstacle

Finally, *IHm* is given by the activating function

The following examples show descriptions of simple objects.

The area inside the obstacle can be described with the following equations:

**Example: 1.** *Description of polygonal obstacle (see Fig. 2).*

From these equations and eq. (5):

by:

obstacle.

where *IHm* is the weighted input of the *m*-th neuron of the middle layer and has a role of induced local field of the neuron function. The neuron activation function *f*(·) has the form:

Neural Networks Based Path Planning and Navigation of Mobile Robots 177

where *T* is the pseudotemperature and the induced local field (*x*) of the neuron is equal to *I*<sup>0</sup> for eq. (1) or equal to *IHm* in the case of eq. (2). The pseudotemperature decreasing is given

where *xi* and *yi* are the coordinates of *i*-th point of the path, *wxm* and *wym* are weights, and *θHm* is a bias, which is equal to the free element in the equation expressing the shape of the

> *x* − 0.2 > 0; −*x* + 1.4 > 0 *y* − 0.2 > 0; −*y* + 1.0 > 0.

*wx*<sup>1</sup> = 1 *wy*<sup>1</sup> = 0 *θH*<sup>1</sup> = −0.2 *wx*<sup>2</sup> = −1 *wy*<sup>2</sup> = 0 *θH*<sup>2</sup> = 1.4 *wx*<sup>3</sup> = 0 *wy*<sup>3</sup> = 1 *θH*<sup>3</sup> = −0.2 *wx*<sup>4</sup> = 0 *wy*<sup>4</sup> = −1 *θH*<sup>4</sup> = 1

log(1 + *t*)

<sup>1</sup> <sup>+</sup> *<sup>e</sup>*−*x*/*<sup>T</sup>* , (3)

*IHm* = *wxmxi* + *wymyi* + *θHm* , (5)

. (4)

*<sup>f</sup>*(*x*) = <sup>1</sup>

*<sup>T</sup>*(*t*) = *<sup>β</sup>*<sup>0</sup>

Fig. 1. Obstacle description neural network

which places it among the fastest ones. The only assumptions made in this work are that there are a finite number of stationary oval or polygonal obstacles with a finite number of vertices, and that the robot has a cylindrical shape. The obstacles can be any combination of polygons and ovals, as well. In order to reduce the problem of path planning to that of navigating a point, the obstacles are enlarged by the robot's polygon dimensions to yield a new set of polygonal obstacles. This "enlargement" of the obstacles is a well known method introduced formally by Lozano-Pèrez & Wesley (1979).

#### **3. Environment description**

#### **3.1 Obstacles**

Every obstacle is described by a neural network as shown in Fig. 1. The inputs of the networks are the coordinates of the points of the path. The output neuron is described by the following expression, which is called *a repulsive penalty function (RPF)* and has a role of repulsive potential:

$$\mathcal{C} = f(I\_0) = f\left(\sum\_{m=1}^{M} O\_{H\_m} - \theta\_T\right),\tag{1}$$

where *I*<sup>0</sup> takes a role of the induced local field of the neuron function *f*(·), *θ<sup>T</sup>* is a bias, equal to the number of the vertices of the obstacle decreased by 0.5. The number of the neurons in the hidden layer is equal to the number of the vertices of the obstacle. *OHm* in eq. (1) is the output of the *m*-th neuron of the middle layer:

$$O\_{H\_m} = f(I\_{H\_m}), \quad m = 1, \ldots, M,\tag{2}$$

4 Will-be-set-by-IN-TECH

which places it among the fastest ones. The only assumptions made in this work are that there are a finite number of stationary oval or polygonal obstacles with a finite number of vertices, and that the robot has a cylindrical shape. The obstacles can be any combination of polygons and ovals, as well. In order to reduce the problem of path planning to that of navigating a point, the obstacles are enlarged by the robot's polygon dimensions to yield a new set of polygonal obstacles. This "enlargement" of the obstacles is a well known method

Every obstacle is described by a neural network as shown in Fig. 1. The inputs of the networks are the coordinates of the points of the path. The output neuron is described by the following expression, which is called *a repulsive penalty function (RPF)* and has a role of

> *M* ∑ *m*=1

where *I*<sup>0</sup> takes a role of the induced local field of the neuron function *f*(·), *θ<sup>T</sup>* is a bias, equal to the number of the vertices of the obstacle decreased by 0.5. The number of the neurons in the hidden layer is equal to the number of the vertices of the obstacle. *OHm* in eq. (1) is the

*OHm* − *θ<sup>T</sup>*

*OHm* = *f*(*IHm* ), *m* = 1, . . . , *M*, (2)

, (1)

*C* = *f*(*I*0) = *f*

Fig. 1. Obstacle description neural network

**3. Environment description**

**3.1 Obstacles**

repulsive potential:

introduced formally by Lozano-Pèrez & Wesley (1979).

output of the *m*-th neuron of the middle layer:

Fig. 2. The annealing network for a rectangular obstacle

where *IHm* is the weighted input of the *m*-th neuron of the middle layer and has a role of induced local field of the neuron function. The neuron activation function *f*(·) has the form:

$$f(\mathbf{x}) = \frac{1}{1 + e^{-\mathbf{x}/T}} \tag{3}$$

where *T* is the pseudotemperature and the induced local field (*x*) of the neuron is equal to *I*<sup>0</sup> for eq. (1) or equal to *IHm* in the case of eq. (2). The pseudotemperature decreasing is given by:

$$T(t) = \frac{\beta\_0}{\log(1+t)}.\tag{4}$$

Finally, *IHm* is given by the activating function

$$I\_{H\_m} = w\_{\rm xm} \mathbf{x}\_{\rm i} + w\_{\rm ym} y\_{\rm i} + \theta\_{H\_{m'}} \tag{5}$$

where *xi* and *yi* are the coordinates of *i*-th point of the path, *wxm* and *wym* are weights, and *θHm* is a bias, which is equal to the free element in the equation expressing the shape of the obstacle.

The following examples show descriptions of simple objects.

**Example: 1.** *Description of polygonal obstacle (see Fig. 2).*

The area inside the obstacle can be described with the following equations:

$$\begin{aligned} \mathfrak{x} - 0.2 &> 0; \qquad -\mathfrak{x} + 1.4 &> 0 \\ \mathfrak{y} - 0.2 &> 0; \qquad -\mathfrak{y} + 1.0 &> 0. \end{aligned}$$

From these equations and eq. (5):

$$\begin{aligned} w\_{\ge 1} &= 1 & w\_{y1} &= 0 & \theta\_{H\_1} &= -0.2 \\ w\_{\ge 2} &= -1 & w\_{y2} &= 0 & \theta\_{H\_2} &= 1.4 \\ w\_{\ge 3} &= 0 & w\_{y3} &= 1 & \theta\_{H\_3} &= -0.2 \\ w\_{\ge 4} &= 0 & w\_{y4} &= -1 & \theta\_{H\_4} &= 1 \end{aligned}$$

addressed by many researchers (see e.g. Latombe (1991); Lozano-Pèrez et al. (1994) and the

Neural Networks Based Path Planning and Navigation of Mobile Robots 179

In the proposed in this chapter algorithm, the local minima problem is addressed in a simple

1. After setting the coordinates of the start and the goal points respectively (Fig. 3(a)), the polygonal obstacles are scanned in order to detect concavity (Fig. 3(b)). In the process of scanning, every two other vertices of a given obstacle are connected by a straight line and

2. If the goal (or the start) point lies inside a concavity, then a new, temporary, goal (start) lying outside the concavity is set (point g' in Fig. 3(b)), and the initial path between the original goal (start) and the new one is set as a straight line. The temporary goal (start) is

3. Every detected concavity is temporarily filled, i.e. the obstacle shape is changed from a concave to a nonconcave one (see Fig. 3(c)). After finishing the current path-planning task, the original shapes are retained so that the next task could be planned correctly, and the temporary goal (start) is connected to the original one by a straight line (Fig. 3(d)).

s

g

The above allows isolating the local minima caused by concavities and increases the reliability

g

s

g'

(b) Scanning result

s '

(d) Final path

*E* = *wlEl* + *wcEc*, (8)

if the line lies outside the obstacle, then a concavity exists.

g

(a) The original shape

s '

(c) The filled concavity

The state of the path is described by the following energy function.

Fig. 3. Concave obstacle (the local minima solution)

set at the nearest to the "original" start (goal) vertex.

references there).

and efficient fashion:

s

g

of the path planning.

**4. Optimal path planner**

i.e. in the middle layer of the network, the activating functions become

$$\begin{aligned} I\_{H\_1} &= w\_{\ge 1} \mathbf{x}\_i + w\_{y1} y\_i + \theta\_{H\_1} = \mathbf{x}\_i - 0.2 \\ I\_{H\_2} &= w\_{\ge 2} \mathbf{x}\_i + w\_{y2} y\_i + \theta\_{H\_2} = -\mathbf{x}\_i + 1.4 \\ I\_{H\_3} &= w\_{\ge 3} \mathbf{x}\_i + w\_{y3} y\_i + \theta\_{H\_3} = y\_i - 0.2 \\ I\_{H\_4} &= w\_{\ge 4} \mathbf{x}\_i + w\_{y4} y\_i + \theta\_{H\_4} = -y\_i + 1.0 \end{aligned}$$

Hence, the free elements in the equations describing the obstacle are represented by the biases *θHi* and the weights in the equations for *IHi* are the coefficients in the respective equations.

**Example: 2.** *Circular shape obstacle.*

When an obstacle has a circular shape, *IHm* is expressed as:

$$I\_H = R^2 - (\mathbf{x}\_i - P)^2 - (y\_i - Q)^2,\tag{6}$$

where *R* is the radius and (*P*, *Q*) are the coordinates of the centre.

**Example: 3.** *Description of elliptical obstacles.*

When the obstacle has elliptic (circular) shape, *IHm* is expressed as:

$$I\_H = a^2 b^2 - (X - x\_i)^2 b^2 + (Y - y\_i)^2 a^2 \,\text{.}\tag{7}$$

which comes directly from the standard equation of the ellipse

$$\frac{(X-x\_i)^2}{a^2} + \frac{(Y-y\_i)^2}{b^2} = 1\_A$$

with *xy*-coordinates of the foci (− <sup>√</sup>*a*<sup>2</sup> <sup>−</sup> *<sup>b</sup>*<sup>2</sup> <sup>+</sup> *<sup>X</sup>*, *<sup>Y</sup>*) and (√*a*<sup>2</sup> <sup>−</sup> *<sup>b</sup>*<sup>2</sup> <sup>+</sup> *<sup>X</sup>*,*Y*) respectively, and the description network has two neurons in the middle layer.

**Note 1.** *The oval shaped obstacles are represented by neural network having two neurons (see the above Example 2 and Example 3) in the middle layer.*

It is obvious from the above that any shape which can be expressed mathematically can be represented by the description neural network. This, of course, includes configurations which are a combination of elementary shapes.

The description of the obstacles by the shown here network has the advantage that it can be used for parallel computation of the path, which can increase the speed of path generation. As it will be shown later, this description of the obstacles has the superiority that the calculation time depends only on the total number of the obstacles' vertices.

#### **3.2 The local minima problem**

The local minima remain an important cause of inefficiency for potential field methods. Hence, dealing with local minima is the major issue that one has to face in designing a planner based on this approach. This issue can be addressed at two levels (Latombe (1991)): (1) definition of the potential function, by attempting to specify a function with no or few local minima, and (2) in the design of the search algorithm, by including appropriate techniques for escaping from local minima. However, it is not easy to construct an "ideal" potential function with no local minima in a general configuration. The second level is more realistic and is 6 Will-be-set-by-IN-TECH

*IH*<sup>1</sup> = *wx*1*xi* + *wy*1*yi* + *θH*<sup>1</sup> = *xi* − 0.2 *IH*<sup>2</sup> = *wx*2*xi* + *wy*2*yi* + *θH*<sup>2</sup> = −*xi* + 1.4 *IH*<sup>3</sup> = *wx*3*xi* + *wy*3*yi* + *θH*<sup>3</sup> = *yi* − 0.2 *IH*<sup>4</sup> = *wx*4*xi* + *wy*4*yi* + *θH*<sup>4</sup> = −*yi* + 1.0. Hence, the free elements in the equations describing the obstacle are represented by the biases *θHi* and the weights in the equations for *IHi* are the coefficients in the respective equations.

*IH* <sup>=</sup> *<sup>R</sup>*<sup>2</sup> <sup>−</sup> (*xi* <sup>−</sup> *<sup>P</sup>*)<sup>2</sup> <sup>−</sup> (*yi* <sup>−</sup> *<sup>Q</sup>*)2, (6)

<sup>√</sup>*a*<sup>2</sup> <sup>−</sup> *<sup>b</sup>*<sup>2</sup> <sup>+</sup> *<sup>X</sup>*, *<sup>Y</sup>*) and (√*a*<sup>2</sup> <sup>−</sup> *<sup>b</sup>*<sup>2</sup> <sup>+</sup> *<sup>X</sup>*,*Y*) respectively, and the

<sup>2</sup>*a*2, (7)

<sup>2</sup>*b*<sup>2</sup> + (*<sup>Y</sup>* <sup>−</sup> *yi*)

*<sup>b</sup>*<sup>2</sup> <sup>=</sup> 1,

i.e. in the middle layer of the network, the activating functions become

When an obstacle has a circular shape, *IHm* is expressed as:

where *R* is the radius and (*P*, *Q*) are the coordinates of the centre.

When the obstacle has elliptic (circular) shape, *IHm* is expressed as:

which comes directly from the standard equation of the ellipse

description network has two neurons in the middle layer.

time depends only on the total number of the obstacles' vertices.

*IH* <sup>=</sup> *<sup>a</sup>*2*b*<sup>2</sup> <sup>−</sup> (*<sup>X</sup>* <sup>−</sup> *xi*)

(*<sup>X</sup>* <sup>−</sup> *xi*)<sup>2</sup>

*<sup>a</sup>*<sup>2</sup> <sup>+</sup> (*<sup>Y</sup>* <sup>−</sup> *yi*)<sup>2</sup>

**Note 1.** *The oval shaped obstacles are represented by neural network having two neurons (see the above*

It is obvious from the above that any shape which can be expressed mathematically can be represented by the description neural network. This, of course, includes configurations which

The description of the obstacles by the shown here network has the advantage that it can be used for parallel computation of the path, which can increase the speed of path generation. As it will be shown later, this description of the obstacles has the superiority that the calculation

The local minima remain an important cause of inefficiency for potential field methods. Hence, dealing with local minima is the major issue that one has to face in designing a planner based on this approach. This issue can be addressed at two levels (Latombe (1991)): (1) definition of the potential function, by attempting to specify a function with no or few local minima, and (2) in the design of the search algorithm, by including appropriate techniques for escaping from local minima. However, it is not easy to construct an "ideal" potential function with no local minima in a general configuration. The second level is more realistic and is

**Example: 2.** *Circular shape obstacle.*

with *xy*-coordinates of the foci (−

*Example 2 and Example 3) in the middle layer.*

are a combination of elementary shapes.

**3.2 The local minima problem**

**Example: 3.** *Description of elliptical obstacles.*

addressed by many researchers (see e.g. Latombe (1991); Lozano-Pèrez et al. (1994) and the references there).

In the proposed in this chapter algorithm, the local minima problem is addressed in a simple and efficient fashion:


Fig. 3. Concave obstacle (the local minima solution)

The above allows isolating the local minima caused by concavities and increases the reliability of the path planning.

#### **4. Optimal path planner**

The state of the path is described by the following energy function.

$$E = w\_l E\_l + w\_c E\_{\mathbb{C}} \tag{8}$$

and

*∂C<sup>k</sup> i ∂xi*

*∂C<sup>k</sup> i ∂yi*

This leads to the final form of the function:

<sup>=</sup> *<sup>∂</sup>C<sup>k</sup> i ∂*(*I*0) *k i*

= *f* � ((*I*0) *k i* ) *M* ∑ *m*=1 *f* � *Hm* ((*IHm* )

= *f* � ((*I*0) *k i* ) *M* ∑ *m*=1 *f* � *Hm* ((*IHm* )

*<sup>x</sup>*˙*<sup>i</sup>* = −2*ηwl*(2*xi* − *xi*−<sup>1</sup> − *xi*+1) − *<sup>η</sup>wc*

*<sup>y</sup>*˙*<sup>i</sup>* = −2*ηwl*(2*yi* − *yi*−<sup>1</sup> − *yi*<sup>+</sup>1) − *<sup>η</sup>wc*

*f* � (·) = <sup>1</sup>

*Hm* (·) = <sup>1</sup>

*f* �

but the computational cost increases dramatically (Yu et al. (2003)).

where *f* � is given by the following expressions:

one is for the obstacle avoidance.

**5. The path-planning algorithm**

2, 3, . . . , *N* − 1) are assigned as

**Step 1:** Initial step

position.

<sup>=</sup> *<sup>∂</sup>C<sup>k</sup> i ∂*(*I*0) *k i*

*∂*(*I*0) *k i ∂xi*

*∂*(*I*0) *k i ∂yi*

<sup>=</sup> *<sup>∂</sup>C<sup>k</sup> i ∂*(*I*0) *k i*

Neural Networks Based Path Planning and Navigation of Mobile Robots 181

<sup>=</sup> *<sup>∂</sup>C<sup>k</sup> i ∂*(*I*0) *k i*

 *M* ∑ *m*=1

> *k <sup>i</sup>* )*w<sup>k</sup> xm* ;

 *M* ∑ *m*=1

> *k <sup>i</sup>* )*w<sup>k</sup> ym*

*K* ∑ *k*=1 *f* � ((*I*0) *k i* ) *M* ∑ *m*=1 *f* � *Hm* ((*IHm* )

*K* ∑ *k*=1 *f* � ((*I*0) *k i* ) *M* ∑ *m*=1 *f* � *Hm* ((*IHm* )

*<sup>T</sup> <sup>f</sup>*(·)[<sup>1</sup> <sup>−</sup> *<sup>f</sup>*(·)]

In eq. (16) the first member in the right side is for the path length optimization and the second

One of the important advantages of the above path-planning is that it allows parallelism in the calculations of the neural network outputs (see Section 6), which leads to decreasing the computational time. The generated semi-optimal path can be optimized further by applying evolutionary methods (e.g. genetic algorithm). Such approach leads to an optimal solution

In this section an algorithm based on the background given in Sections 3 and 4 is proposed.

1. Let the start position of the robot is (*x*1, *y*1), and the goal position is denoted as (*xN*, *yN*). 2. Check for concavities (see Section 3.2.) and, if necessary, reassign the goal (start)

3. At *t* = 0 the coordinates of the points of the initial path (straight line) (*xi*, *yi*; *i* =

*yi* = (*yN* − *y*1)(*xi* − *x*1)/(*xN* − *x*1) + *y*0, (18)

*xi* = *x*<sup>0</sup> + *i*(*xN* − *x*1)/(*N* − 1),

The calculations for the path are conceptually composed by the following steps:

*∂*(*OHm* )*<sup>k</sup> i*

*∂*(*IHm* ) *k i ∂xi*

*∂*(*IHm* ) *k i ∂xi*

*<sup>T</sup> fHm* (·)[<sup>1</sup> <sup>−</sup> *fHm* (·)]. (17)

. (15)

*k <sup>i</sup>* )*w<sup>k</sup> xm* ;

*k <sup>i</sup>* )*w<sup>k</sup> ym*

, (16)

*∂*(*IHm* ) *k i*

*∂*(*OHm* )*<sup>k</sup> i*

*∂*(*IHm* ) *k i*

where *wl* and *wc* are weights (*wl* + *wc* = 1), *El* depicts the squared length of the path:

$$E\_l = \sum\_{i=1}^{N-1} L\_i^2 = \sum\_{i=1}^{N-1} \left[ (\mathbf{x}\_{i+1} - \mathbf{x}\_i)^2 + (y\_{i+1} - y\_i)^2 \right] \tag{9}$$

and *Ec* is given by the expression.

$$E\_{\mathbb{C}} = \sum\_{i=1}^{N} \sum\_{k=1}^{K} \mathbb{C}\_{i}^{k} \tag{10}$$

where *N* is the number of the points between the start and the goal, *K* is the number of the obstacles, and *C* is obtained through eq. (1).

Minimizing eq. (8) will lead to obtaining an optimal in length path that does not collide with any of the obstacles. In order to minimize (8) the classical function analysis methods are applied. First, we find the time derivative of *E*:

$$\frac{dE}{dt} = \sum\_{i=1}^{N} \left[ w\_l \left( \frac{\partial L\_i^2}{\partial \mathbf{x}\_i} + \frac{\partial L\_{i-1}^2}{\partial \mathbf{x}\_i} \right) + w\_c \sum\_{k=1}^{K} \frac{\partial \mathbf{C}\_i^k}{\partial \mathbf{x}\_i} \right] \dot{\mathbf{x}}\_i$$

$$+ \sum\_{i=1}^{N} \left[ w\_l \left( \frac{\partial L\_i^2}{\partial y\_i} + \frac{\partial L\_{i-1}^2}{\partial y\_i} \right) + w\_c \sum\_{k=1}^{K} \frac{\partial \mathbf{C}\_i^k}{\partial y\_i} \right] \dot{y}\_i \tag{11}$$

Setting

$$\dot{\mathbf{x}}\_{i} = -\eta \left[ w\_{l} \left( \frac{\partial L\_{i}^{2}}{\partial \mathbf{x}\_{i}} + \frac{\partial L\_{i-1}^{2}}{\partial \mathbf{x}\_{i}} \right) + w\_{c} \sum\_{k=1}^{K} \frac{\partial \mathbf{C}\_{i}^{k}}{\partial \mathbf{x}\_{i}} \right],$$

$$\dot{y}\_{i} = -\eta \left[ w\_{l} \left( \frac{\partial L\_{i}^{2}}{\partial y\_{i}} + \frac{\partial L\_{i-1}^{2}}{\partial y\_{i}} \right) + w\_{c} \sum\_{k=1}^{K} \frac{\partial \mathbf{C}\_{i}^{k}}{\partial y\_{i}} \right] \tag{12}$$

we can rewrite the above equation as

$$\frac{dE}{dt} = -\frac{1}{\eta} \sum\_{i=1}^{N} (\dot{\mathbf{x}}\_i^2 + \dot{y}\_i^2) < 0 \tag{13}$$

where *η* is an adaptation gain. It is obvious that, when *x*˙*<sup>i</sup>* → 0 and *y*˙*<sup>i</sup>* → 0, *E* converges to its minimum. In other words, when all points of the path almost stop moving, there is no collision and the path is the optimal one, i.e. eq. (13) can be used as a condition for termination of the calculation iterations.

Now, from equations (12),

$$\frac{\partial L\_i^2}{\partial \mathbf{x}\_i} + \frac{\partial L\_{i-1}^2}{\partial \mathbf{x}\_i} = -2\mathbf{x}\_{i+1} + 4\mathbf{x}\_i - 2\mathbf{x}\_{i-1}\prime$$

$$\frac{\partial L\_i^2}{\partial y\_i} + \frac{\partial L\_{i-1}^2}{\partial y\_i} = -2y\_{i+1} + 4y\_i - 2y\_{i-1}\prime\tag{14}$$

and

8 Will-be-set-by-IN-TECH

[(*xi*<sup>+</sup><sup>1</sup> − *xi*)

*K* ∑ *k*=1 *Ck*

*N* ∑ *i*=1

where *N* is the number of the points between the start and the goal, *K* is the number of the

Minimizing eq. (8) will lead to obtaining an optimal in length path that does not collide with any of the obstacles. In order to minimize (8) the classical function analysis methods are

> + *wc*

> > + *wc*

 + *wc*

 + *wc*

= −2*xi*<sup>+</sup><sup>1</sup> + <sup>4</sup>*xi* − <sup>2</sup>*xi*−1,

*K* ∑ *k*=1 *∂C<sup>k</sup> i ∂xi*

*K* ∑ *k*=1

*K* ∑ *k*=1

*K* ∑ *k*=1  *x*˙*i*

*∂C<sup>k</sup> i ∂yi*

*∂C<sup>k</sup> i ∂xi*

*∂C<sup>k</sup> i ∂yi*

 ,

= −2*yi*+<sup>1</sup> + <sup>4</sup>*yi* − <sup>2</sup>*yi*−<sup>1</sup> (14)

*<sup>i</sup>* ) < 0 (13)

*y*˙*<sup>i</sup>* (11)

(12)

<sup>2</sup> + (*yi*<sup>+</sup><sup>1</sup> <sup>−</sup> *yi*)2], (9)

*<sup>i</sup>* , (10)

where *wl* and *wc* are weights (*wl* + *wc* = 1), *El* depicts the squared length of the path:

*N*−1 ∑ *i*=1

*Ec* =

*El* =

obstacles, and *C* is obtained through eq. (1).

applied. First, we find the time derivative of *E*:

*dE dt* <sup>=</sup>

*N* ∑ *i*=1

+ *N* ∑ *i*=1

*x*˙*<sup>i</sup>* = −*η*

*y*˙*<sup>i</sup>* = −*η*

*∂L*<sup>2</sup> *i ∂xi* + *∂L*<sup>2</sup> *i*−1 *∂xi*

*∂L*<sup>2</sup> *i ∂yi* + *∂L*<sup>2</sup> *i*−1 *∂yi*

we can rewrite the above equation as

of the calculation iterations. Now, from equations (12),

 *wl ∂L*<sup>2</sup> *i ∂xi* + *∂L*<sup>2</sup> *i*−1 *∂xi*

> *wl ∂L*<sup>2</sup> *i ∂yi* + *∂L*<sup>2</sup> *i*−1 *∂yi*

 *wl ∂L*<sup>2</sup> *i ∂xi* + *∂L*<sup>2</sup> *i*−1 *∂xi*

 *wl*

> *dE dt* <sup>=</sup> <sup>−</sup> <sup>1</sup> *η*

 *<sup>∂</sup>L*<sup>2</sup> *i ∂yi* + *∂L*<sup>2</sup> *i*−1 *∂yi*

> *N* ∑ *i*=1 (*x*˙ 2 *<sup>i</sup>* + *y*˙ 2

where *η* is an adaptation gain. It is obvious that, when *x*˙*<sup>i</sup>* → 0 and *y*˙*<sup>i</sup>* → 0, *E* converges to its minimum. In other words, when all points of the path almost stop moving, there is no collision and the path is the optimal one, i.e. eq. (13) can be used as a condition for termination

and *Ec* is given by the expression.

Setting

*N*−1 ∑ *i*=1 *L*2 *<sup>i</sup>* =

$$\frac{\partial \overline{C}\_{i}^{k}}{\partial \mathbf{x}\_{i}} = \frac{\partial \overline{C}\_{i}^{k}}{\partial (I\_{0})\_{i}^{k}} \frac{\partial (I\_{0})\_{i}^{k}}{\partial \mathbf{x}\_{i}} = \frac{\partial \overline{C}\_{i}^{k}}{\partial (I\_{0})\_{i}^{k}} \left( \sum\_{m=1}^{M} \frac{\partial (O\_{H\_{m}})\_{i}^{k}}{\partial (I\_{H\_{m}})\_{i}^{k}} \frac{\partial (I\_{H\_{m}})\_{i}^{k}}{\partial \mathbf{x}\_{i}} \right)$$

$$= f'((I\_{0})\_{i}^{k}) \left( \sum\_{m=1}^{M} f\_{H\_{m}}^{\prime}((I\_{H\_{m}})\_{i}^{k}) w\_{xm}^{k} \right);$$

$$\frac{\partial \underline{C}\_{i}^{k}}{\partial y\_{i}} = \frac{\partial \underline{C}\_{i}^{k}}{\partial (I\_{0})\_{i}^{k}} \frac{\partial (I\_{0})\_{i}^{k}}{\partial y\_{i}} = \frac{\partial \underline{C}\_{i}^{k}}{\partial (I\_{0})\_{i}^{k}} \left( \sum\_{m=1}^{M} \frac{\partial (O\_{H\_{m}})\_{i}^{k}}{\partial (I\_{H\_{m}})\_{i}^{k}} \frac{\partial (I\_{H\_{m}})\_{i}^{k}}{\partial \mathbf{x}\_{i}} \right)$$

$$= f'((I\_{0})\_{i}^{k}) \left( \sum\_{m=1}^{M} f\_{H\_{m}}^{\prime}((I\_{H\_{m}})\_{i}^{k}) w\_{ym}^{k} \right). \tag{15}$$

This leads to the final form of the function:

$$\begin{split} \dot{\mathbf{x}}\_{i} &= -2\eta w\_{l} (2\mathbf{x}\_{i} - \mathbf{x}\_{i-1} - \mathbf{x}\_{i+1}) - \eta w\_{c} \sum\_{k=1}^{K} f'( (I\_{0})\_{i}^{k} ) \left( \sum\_{m=1}^{M} f'\_{H\_{m}}( (I\_{H\_{m}})\_{i}^{k} ) w\_{xm}^{k} \right); \\ \dot{y}\_{i} &= -2\eta w\_{l} (2y\_{i} - y\_{i-1} - y\_{i+1}) - \eta w\_{c} \sum\_{k=1}^{K} f'( (I\_{0})\_{i}^{k} ) \left( \sum\_{m=1}^{M} f'\_{H\_{m}}( (I\_{H\_{m}})\_{i}^{k} ) w\_{ym}^{k} \right), \end{split} \tag{16}$$

where *f* � is given by the following expressions:

$$f'(\cdot) = \frac{1}{T} f(\cdot)[1 - f(\cdot)]$$

$$f'\_{H\_m}(\cdot) = \frac{1}{T} f\_{H\_m}(\cdot)[1 - f\_{H\_m}(\cdot)].\tag{17}$$

In eq. (16) the first member in the right side is for the path length optimization and the second one is for the obstacle avoidance.

One of the important advantages of the above path-planning is that it allows parallelism in the calculations of the neural network outputs (see Section 6), which leads to decreasing the computational time. The generated semi-optimal path can be optimized further by applying evolutionary methods (e.g. genetic algorithm). Such approach leads to an optimal solution but the computational cost increases dramatically (Yu et al. (2003)).

#### **5. The path-planning algorithm**

In this section an algorithm based on the background given in Sections 3 and 4 is proposed. The calculations for the path are conceptually composed by the following steps:

**Step 1:** Initial step


$$\begin{aligned} \mathbf{x}\_i &= \mathbf{x}\_0 + i(\mathbf{x}\_N - \mathbf{x}\_1)/(N-1), \\ \mathbf{y}\_i &= (y\_N - y\_1)(\mathbf{x}\_i - \mathbf{x}\_1)/(\mathbf{x}\_N - \mathbf{x}\_1) + y\_{0\prime} \end{aligned} \tag{18}$$

i. e. the distance between the *x* and *y* coordinates of every two neighboring points of the path is equal.

**Note 2.** *It is assumed that the obstacles dimensions are enlarged by the robot's polygon dimensions (Lozano-Pèrez & Wesley (1979)).*

**Step2:** 1. For the points (*xi*, *yi*) of the path which lie inside some obstacle, the iterations are performed according to the following equations:

$$\begin{split} \dot{\mathbf{x}}\_{i} &= -2\eta\_{1}w\_{l}(2\mathbf{x}\_{i} - \mathbf{x}\_{i-1} - \mathbf{x}\_{i+1}) - \eta\_{1}w\_{c} \sum\_{k=1}^{K} f'(\left(I\_{0}\right)\_{i}^{k}) \left(\sum\_{m=1}^{M} f'\_{H\_{m}}(\left(I\_{H\_{m}}\right)\_{i}^{k}) w\_{xm}^{k}\right); \\ \dot{y}\_{i} &= -2\eta\_{1}w\_{l}(2y\_{i} - y\_{i-1} - y\_{i+1}) - \eta\_{1}w\_{c} \sum\_{k=1}^{K} f'(\left(I\_{0}\right)\_{i}^{k}) \left(\sum\_{m=1}^{M} f'\_{H\_{m}}(\left(I\_{H\_{m}}\right)\_{i}^{k}) w\_{ym}^{k}\right) \\ &\quad \quad \quad i = 2,3,\ldots,N-1. \end{split} \tag{19}$$

2. For the points (*xi*, *yi*) situated outside the obstacles, instead of eq. (19) use the following equations:

$$\begin{aligned} \dot{x}\_i &= -\eta\_2 w\_l (2x\_i - x\_{i-1} - x\_{i+1}); \\ \dot{y}\_i &= -\eta\_2 w\_l (2y\_i - y\_{i-1} - y\_{i+1}), \end{aligned} \tag{20}$$

i.e. for the points of the path lying outside obstacles, we continue the calculation with the goal to minimize only the length of the path.


Calculate the difference *d* of the path lengths at time *t* and time (*t* + *p*), i.e.

$$d = \sum\_{i=2}^{N-1} \left\{ [\mathbf{x}\_i(t+p) - \mathbf{x}\_i(t)]^2 + [y\_i(t+p) - y\_i(t)]^2 \right\}^{1/2}. \tag{21}$$


Here eq. (19) is almost the same as eq. (16) with the difference that instead of only one RPF, different functions, as explained below, are used for every layer of the neural network. Every obstacle is described using a neural network as shown in Fig. 1. The output neuron is described by eq. (1), the neuron function *f*(·) is the same as in eq. (3) and the pseudotemperature is as in eq. (4).

The hidden layer inputs are as in eq. (1) but the outputs *OHm* now become

$$O\_{H\_m} = f\_{H\_m}(I\_{H\_m}), \quad m = 1, \ldots, M \tag{22}$$

Fig. 4. Block diagram of the algorithm

*THm* (*t*) = *<sup>β</sup><sup>m</sup>*

Finally, *IHm* is given by the activating function (5). Equations (3) and (23) include different values of pseudotemperatures, which is one of the important conditions for convergence of

Neural Networks Based Path Planning and Navigation of Mobile Robots 183

log(<sup>1</sup> <sup>+</sup> *<sup>t</sup>*) (24)

and

with *IHm* becoming the induced local field of the neuron function *fHm* :

$$f\_{H\_m}(I\_{H\_m}) = \frac{1}{1 + e^{-I\_{H\_m}/T\_{H\_m}(t)}}\tag{23}$$

Fig. 4. Block diagram of the algorithm

and

10 Will-be-set-by-IN-TECH

the path is equal.

equations:

**Step 4:** Test for convergence

*(Lozano-Pèrez & Wesley (1979)).*

performed according to the following equations:

*<sup>x</sup>*˙*<sup>i</sup>* = −2*η*1*wl*(2*xi* − *xi*−<sup>1</sup> − *xi*<sup>+</sup>1) − *<sup>η</sup>*1*wc*

*<sup>y</sup>*˙*<sup>i</sup>* = −2*η*1*wl*(2*yi* − *yi*−<sup>1</sup> − *yi*<sup>+</sup>1) − *<sup>η</sup>*1*wc*

the goal to minimize only the length of the path.

1), where *p* is any suitable number, say *p* = 100.

*N*−1 ∑ *i*=2

*d* =

• If *d* ≥ *ε*, then GO TO step 2.

pseudotemperature is as in eq. (4).

*i* = 2, 3, . . . , *N* − 1.

i. e. the distance between the *x* and *y* coordinates of every two neighboring points of

*K* ∑ *k*=1 *f* � ((*I*0) *k i* ) *M* ∑ *m*=1 *f* � *Hm* ((*IHm* )

*K* ∑ *k*=1 *f* � ((*I*0) *k i*) *M* ∑ *m*=1 *f* � *Hm* ((*IHm* )

2. For the points (*xi*, *yi*) situated outside the obstacles, instead of eq. (19) use the following

*<sup>x</sup>*˙*<sup>i</sup>* = −*η*2*wl*(2*xi* − *xi*−<sup>1</sup> − *xi*+1);

i.e. for the points of the path lying outside obstacles, we continue the calculation with

**Step 3:** Perform *p* times the calculations of step 2, i.e. find *xi*(*t* + *p*), *yi*(*t* + *p*) (*i* = 2, 3, . . . , *N* −

• If *d* < *ε* then the algorithm terminates with the conclusion that the goal is reached via

Here eq. (19) is almost the same as eq. (16) with the difference that instead of only one RPF, different functions, as explained below, are used for every layer of the neural network. Every obstacle is described using a neural network as shown in Fig. 1. The output neuron is described by eq. (1), the neuron function *f*(·) is the same as in eq. (3) and the

Calculate the difference *d* of the path lengths at time *t* and time (*t* + *p*), i.e.

an "optimal" path. Here *ε* is a small constant, say *ε* = 0.1.

The hidden layer inputs are as in eq. (1) but the outputs *OHm* now become

*fHm* (*IHm* ) = <sup>1</sup>

with *IHm* becoming the induced local field of the neuron function *fHm* :

*<sup>y</sup>*˙*<sup>i</sup>* = −*η*2*wl*(2*yi* − *yi*−<sup>1</sup> − *yi*+1), (20)

{[*xi*(*<sup>t</sup>* <sup>+</sup> *<sup>p</sup>*) <sup>−</sup> *xi*(*t*)]<sup>2</sup> + [*yi*(*<sup>t</sup>* <sup>+</sup> *<sup>p</sup>*) <sup>−</sup> *yi*(*t*)]2}1/2. (21)

*OHm* = *fHm* (*IHm* ), *m* = 1, . . . , *M* (22)

<sup>1</sup> <sup>+</sup> *<sup>e</sup>*−*IHm* /*THm* (*t*) (23)

*k <sup>i</sup>* )*w<sup>k</sup> xm* ;

*k i*)*w<sup>k</sup> ym* 

(19)

**Note 2.** *It is assumed that the obstacles dimensions are enlarged by the robot's polygon dimensions*

**Step2:** 1. For the points (*xi*, *yi*) of the path which lie inside some obstacle, the iterations are

$$T\_{H\_m}(t) = \frac{\beta\_m}{\log(1+t)}\tag{24}$$

Finally, *IHm* is given by the activating function (5). Equations (3) and (23) include different values of pseudotemperatures, which is one of the important conditions for convergence of

0

the vertices (20–120)

0

speed about three times (see Fig. 8):

number of vertices

5

10

15

Time [s]

20

25

100 200 300 400 500 600 700 800 900 1000

Neural Networks Based Path Planning and Navigation of Mobile Robots 185

120

100

80 60

40

20

CPU1 CPU1+CPU2 CPU1+CPU2+CPU3

Number of loops

20 40 60 80 100 120

Number of vertices

It is possible to further increase the calculation speed by introducing adaptive adjustment of the parameters *η*<sup>1</sup> and *η*2. For example, the following adaptive adjusting law increases the

Fig. 7. Run-time *t* for different number of processors for 1000 loops depending on the

Fig. 6. Computation time depending on the number of the loops (100–1000) and number of

5

10

15

Time [s]

20

25

the algorithm and for generation of almost optimal path. The block diagram of the algorithm is shown in Fig. 4.

#### **6. Experimental results**

To show the effectiveness of the proposed in this paper algorithm, several simulation examples are given in this section.

#### **6.1 Calculation speed evaluation**

The simulations were run on ASUS A7V motherboards rated with 328 base (352 peak) SPEC CFP2000 and 409 base (458 peak) SPEC CINT2000 on the CPU2000 benchmark. The environment shown in Fig. 5 has been chosen for the benchmark tests. In the course of the speed measurements the number of the vertices was increased from 20 to 120 with step of 20 vertices. The final configuration of the obstacles inside the benchmark environment with three generated paths is shown in Fig. 5. To compare the run-times, 12 path generations have been performed randomly. After deleting the lowest and the highest path planning times, the average of the remaining 10 values has been adopted.

Fig. 5. The benchmark environment with three generated paths

Figure 6 shows the speed change depending on the total number of vertices of the obstacles. It is clear that the speed changes linearly with increasing the number of the vertices (cf. Lozano-Pèrez & Wesley (1979)).

As it was explained at the end of Section 4 the algorithm allows parallelizing the path computation. To calculate the speedups we ran parallel simulations for the same benchmark environment on one, two, and three processors. Figure 7 shows how the speed increases with increasing the number of the processors. For the case of two processors the simulation was run on two ASUS A7V boards, and for the case of three processors a Gigabyte 440BX board was added as a third processor. The communication during the parallel calculations was established by an Ethernet bus based network between the computers.

12 Will-be-set-by-IN-TECH

the algorithm and for generation of almost optimal path. The block diagram of the algorithm

To show the effectiveness of the proposed in this paper algorithm, several simulation

The simulations were run on ASUS A7V motherboards rated with 328 base (352 peak) SPEC CFP2000 and 409 base (458 peak) SPEC CINT2000 on the CPU2000 benchmark. The environment shown in Fig. 5 has been chosen for the benchmark tests. In the course of the speed measurements the number of the vertices was increased from 20 to 120 with step of 20 vertices. The final configuration of the obstacles inside the benchmark environment with three generated paths is shown in Fig. 5. To compare the run-times, 12 path generations have been performed randomly. After deleting the lowest and the highest path planning times, the

0 0.5 1 1.5 2 2.5

Figure 6 shows the speed change depending on the total number of vertices of the obstacles. It is clear that the speed changes linearly with increasing the number of the vertices (cf.

As it was explained at the end of Section 4 the algorithm allows parallelizing the path computation. To calculate the speedups we ran parallel simulations for the same benchmark environment on one, two, and three processors. Figure 7 shows how the speed increases with increasing the number of the processors. For the case of two processors the simulation was run on two ASUS A7V boards, and for the case of three processors a Gigabyte 440BX board was added as a third processor. The communication during the parallel calculations was

is shown in Fig. 4.

**6. Experimental results**

examples are given in this section.

**6.1 Calculation speed evaluation**

0

Lozano-Pèrez & Wesley (1979)).

0.2

0.4

0.6

0.8

1

1.2

average of the remaining 10 values has been adopted.

Fig. 5. The benchmark environment with three generated paths

established by an Ethernet bus based network between the computers.

Fig. 6. Computation time depending on the number of the loops (100–1000) and number of the vertices (20–120)

Fig. 7. Run-time *t* for different number of processors for 1000 loops depending on the number of vertices

It is possible to further increase the calculation speed by introducing adaptive adjustment of the parameters *η*<sup>1</sup> and *η*2. For example, the following adaptive adjusting law increases the speed about three times (see Fig. 8):

0


in the course of a simulation.

 0 0.5 1 z



Fig. 10. Shape of the RPF in the course of simulation

0

 0.1 0.2 <sup>x</sup>

0.5 1 1.5 2 2.5 3

Neural Networks Based Path Planning and Navigation of Mobile Robots 187


the simulations the paths were successfully generated and the time for the calculations agreed with the calculation speed explained above. Figure 10 shows the potential fields of obstacles

We have developed software with a graphic user interface (GUI) for the simulation and control of differential drive robots in a 2D environment (See Fig. 11). The environment allows the user to easily build configuration spaces and perform simulations. This environment is available at *http://shiwasu.ee.ous.ac.jp/planner/planner.zip*. The algorithm was successfully applied to the


0

0.2

y

0.4

Fig. 9. Simulation example (environment including concave obstacles)

0.2

0.4

0.6

0.8

1

1.2

1.4

1.6

Fig. 8. Convergence speed of the algorithm depending on the way of setting *η*<sup>1</sup> and *η*2: the slowly converging line is for the case when constant values are used, and the fast converging one depicts the convergence when adaptive adjustment is applied

$$\begin{aligned} \dot{\eta}\_1(n+1) &= -\eta\_1(n) + \delta \operatorname{sgn}[(d(n) - d(n-1))(d(n-1) - d(n-2))], \\ \dot{\eta}\_2(n+1) &= -\eta\_2(n) + \delta \operatorname{sgn}[(d(n) - d(n-1))(d(n-1) - d(n-2))] \end{aligned} \tag{25}$$

where *δ* is a small constant.

In the course of path calculations, a limited number of obstacles has impact on path generation—mostly the obstacles the initial, straight line path intersects with, contribute to path forming. Then, while performing *Step 1* of the algorithm, if all obstacles which do not collide with the initial path are ignored and excluded from the calculations, it is possible to drastically decrease the calculation time. A good example is the path in vertical direction in Fig. 5. When all obstacles are considered, the total number of vertices is 135 and RPF for all obstacles would be calculated. However, if only the four obstacles which collide with the initial straight line are used, the number of vertices decreases to only 17 and the expected calculation time will be approximately 8 times shorter. After finishing the path generation for the decreased number of obstacles, the original environment should be retained and a final correction of the path shape should be performed. Naturally, this final correction would require only few additional calculation loops.

#### **6.2 Simulation results**

Figure 9 shows additional results of path planning simulation in an environment with concaved obstacles. It can be seen that the local minimums do not cause problems in the path generation. In these simulations the number of the points between the start position and the goal was set to 200. In both examples the values of the coefficients in equations of the algorithm were not changed. In fact, the authors have performed many simulations using different environments, in which the obstacles were situated in many different ways. In all

14 Will-be-set-by-IN-TECH

0 1000 2000 3000 4000 5000

*η*˙2(*n* + 1) = −*η*2(*n*) + *δ* sgn[(*d*(*n*) − *d*(*n* − 1))(*d*(*n* − 1) − *d*(*n* − 2))] (25)

Number of loops

Fig. 8. Convergence speed of the algorithm depending on the way of setting *η*<sup>1</sup> and *η*2: the slowly converging line is for the case when constant values are used, and the fast converging

*η*˙1(*n* + 1) = −*η*1(*n*) + *δ* sgn[(*d*(*n*) − *d*(*n* − 1))(*d*(*n* − 1) − *d*(*n* − 2))],

In the course of path calculations, a limited number of obstacles has impact on path generation—mostly the obstacles the initial, straight line path intersects with, contribute to path forming. Then, while performing *Step 1* of the algorithm, if all obstacles which do not collide with the initial path are ignored and excluded from the calculations, it is possible to drastically decrease the calculation time. A good example is the path in vertical direction in Fig. 5. When all obstacles are considered, the total number of vertices is 135 and RPF for all obstacles would be calculated. However, if only the four obstacles which collide with the initial straight line are used, the number of vertices decreases to only 17 and the expected calculation time will be approximately 8 times shorter. After finishing the path generation for the decreased number of obstacles, the original environment should be retained and a final correction of the path shape should be performed. Naturally, this final correction would

Figure 9 shows additional results of path planning simulation in an environment with concaved obstacles. It can be seen that the local minimums do not cause problems in the path generation. In these simulations the number of the points between the start position and the goal was set to 200. In both examples the values of the coefficients in equations of the algorithm were not changed. In fact, the authors have performed many simulations using different environments, in which the obstacles were situated in many different ways. In all

one depicts the convergence when adaptive adjustment is applied

2.5

where *δ* is a small constant.

**6.2 Simulation results**

require only few additional calculation loops.

2.55

2.6

2.65

2.7

2.75

Path lenght

2.8

2.85

2.9

2.95

Fig. 9. Simulation example (environment including concave obstacles)

Fig. 10. Shape of the RPF in the course of simulation

the simulations the paths were successfully generated and the time for the calculations agreed with the calculation speed explained above. Figure 10 shows the potential fields of obstacles in the course of a simulation.

We have developed software with a graphic user interface (GUI) for the simulation and control of differential drive robots in a 2D environment (See Fig. 11). The environment allows the user to easily build configuration spaces and perform simulations. This environment is available at *http://shiwasu.ee.ous.ac.jp/planner/planner.zip*. The algorithm was successfully applied to the

good approximations for the initial values of the *βm* coefficients. Strict solution of this problem

Neural Networks Based Path Planning and Navigation of Mobile Robots 189

The proposed algorithm can be easily extended for the 3-D case (Kroumov et al. (2010)). The extension can be done by adding an additional input for the *z* (height of the obstacles) dimension to the obstacle description neural network. Adding of *z* coordinate and employing a technique for path planning, similar to that proposed in Henrich et al. (1998), would allow further adoption of the algorithm for planning of paths for industrial robots. Development of

This work is partially supported by the Graduate School of Okayama University of Science,

The authors would like to thank Mr. Yoshiro Kobayashi, a graduate student at the Graduate School of Okayama University of Science, for his help in preparing part of the figures. We are

Canny, J. F. (1988). *The Complexity of Robot Motion Planning*, The MIT Press, Cambridge, MA,

Chen, P. C. & Hwang, Y. K. (1998). Sandros: A dynamic graph search algorithm for motion

Faverjon, B. & Tournassoud, P. (1987). A local approach for path planning of manipulators

*on Robotics & Automation*, Raleigh, New Caledonia, U. S. A., pp. 1152–1159. Ge, S. S. & Cui, Y. J. (2000). New potential functions for mobile robot path planning, *IEEE*

Henrich, D., Wurll, C. & Wörn, H. (1998). 6 dof path planning in dynamic environments—a

Hwang, Y. K. & Ahuja, K. (1992). Potential field approach to path planning, *IEEE Transactins*

Khatib, O. (1986). Real-time obstacle avoidance for manipulators and mobile robots,

Khosla, P. & Volpe, R. (1988). Superquadratic artificial potentials for obstacle avoidance

Kroumov, V., Yu, J. & Negishi, H. (2004). Path planning algorithm for car-like robot and its application, *Journal of Systemics, Cybernetics and Informatics* Vol. 2(No. 3): 7. Kroumov, V., Yu, J. & Shibayama, K. (2010). 3d path planning for mobile robots using

Lee, S. & Kardaras, G. (1997a). Collision-free path planning with neural networks, *Proceedings of 1997 IEEE International Conference on Robotics & Automation*, Vol. 4, pp. 3565–3570.

Latombe, J. C. (1991). *Robot Motion Planning*, Kluwer Academic Publishers, Norwell.

with a high number of degrees of freedom, *Proceedings of IEEE International Conference*

parallel on-line approach, *Proceedings of the 1998 IEEE Conference on Robotics &*

and approach, *Proceedings of IEEE International Conference on Robotics & Automation*,

simulated annealing neural network, *International Journal on Innovative Computing,*

planning, *IEEE Transactins on Robotics & Automation* 14(3): 390–403.

*Transactins on Robotics & Automation* 16(5): 615–620.

*Automation*, Leuven, Belgium, pp. 330–335.

*on Robotics & Automation* Vol. 8(No. 1): 23–32.

Leuven, Belgium, pp. 1778–1784.

*Information and Control* 6(7): 2885–2899.

*International Journal of Robotics Research* 5(1): 90–98.

thankful to Ms Z. Krasteva for proofreading the final version of the manuscript.

is one of our next goals.

**8. Acknowledgement**

Okayama, Japan.

**9. References**

U. S. A.

such application is another subject of our future work.

Fig. 11. GUI environment for simulation and control of differential drive robots

control and navigation of "Khepera" (Mondada et al. (1993)) and "Pioneer P3-DX" robots.

#### **7. Discussion and conclusions**

In this paper we have proposed an algorithm which guarantees planning of near optimal in length path for wheeled robots moving in an *a priori* known environment. We are considering wheel placements which impose *no* kinematic constraints on the robot chassis: castor wheel, Swedish wheel, and spherical wheel.

The proposed algorithm is a significant improvement of the potential field algorithms because it finds an almost optimal path without being trapped in local minima and the calculation speed for the proposed algorithm is reasonably fast. Because the only assumptions made are that the obstacles are stationary polygons and ovals, the algorithm can solve the navigation problem in very complex environments. For description of the obstacles the algorithm uses only the Cartesian coordinates of their vertices and does not need memory consuming obstacle transformations (cf. Zelinsky (1992); Zelinsky & Yuta (1993)). The consumed memory space is equal to twice the number of the coordinates of the path points and some additional memory for keeping the parameters of the description networks.

Because the generated paths are piecewise linear with changing directions at the corners of the obstacles, the inverse kinematics problems for the case of differential drive robots are simply solved: to drive the robot to some goal pose (*x*, *y*, *θ*), the robot can be spun in place until it is aimed at (*x*, *y*), then driven forward until it is at (*x*, *y*), and then spun in place until the required goal orientation *θ* is met.

It becomes clear from the course of the above explanations that the effectiveness of the proposed algorithm depends on the development of some mechanism for choosing the initial values of the pseudotemperatures (*βm*) or properly adjusting the pseudotemperatures in order to generate potential fields satisfying the properties given in Section 2. Applying of an adaptive adjusting procedure to the coefficients *β<sup>m</sup>* similar to that for adjusting of *η*<sup>1</sup> and *η*<sup>2</sup> (see. eq. (25)) is one possible solution. Moreover, there is no need to perform continuous adjustment throughout the whole path generation—adjustment in the first few steps gives good approximations for the initial values of the *βm* coefficients. Strict solution of this problem is one of our next goals.

The proposed algorithm can be easily extended for the 3-D case (Kroumov et al. (2010)). The extension can be done by adding an additional input for the *z* (height of the obstacles) dimension to the obstacle description neural network. Adding of *z* coordinate and employing a technique for path planning, similar to that proposed in Henrich et al. (1998), would allow further adoption of the algorithm for planning of paths for industrial robots. Development of such application is another subject of our future work.

#### **8. Acknowledgement**

16 Will-be-set-by-IN-TECH

Fig. 11. GUI environment for simulation and control of differential drive robots

**7. Discussion and conclusions**

Swedish wheel, and spherical wheel.

required goal orientation *θ* is met.

for keeping the parameters of the description networks.

control and navigation of "Khepera" (Mondada et al. (1993)) and "Pioneer P3-DX" robots.

In this paper we have proposed an algorithm which guarantees planning of near optimal in length path for wheeled robots moving in an *a priori* known environment. We are considering wheel placements which impose *no* kinematic constraints on the robot chassis: castor wheel,

The proposed algorithm is a significant improvement of the potential field algorithms because it finds an almost optimal path without being trapped in local minima and the calculation speed for the proposed algorithm is reasonably fast. Because the only assumptions made are that the obstacles are stationary polygons and ovals, the algorithm can solve the navigation problem in very complex environments. For description of the obstacles the algorithm uses only the Cartesian coordinates of their vertices and does not need memory consuming obstacle transformations (cf. Zelinsky (1992); Zelinsky & Yuta (1993)). The consumed memory space is equal to twice the number of the coordinates of the path points and some additional memory

Because the generated paths are piecewise linear with changing directions at the corners of the obstacles, the inverse kinematics problems for the case of differential drive robots are simply solved: to drive the robot to some goal pose (*x*, *y*, *θ*), the robot can be spun in place until it is aimed at (*x*, *y*), then driven forward until it is at (*x*, *y*), and then spun in place until the

It becomes clear from the course of the above explanations that the effectiveness of the proposed algorithm depends on the development of some mechanism for choosing the initial values of the pseudotemperatures (*βm*) or properly adjusting the pseudotemperatures in order to generate potential fields satisfying the properties given in Section 2. Applying of an adaptive adjusting procedure to the coefficients *β<sup>m</sup>* similar to that for adjusting of *η*<sup>1</sup> and *η*<sup>2</sup> (see. eq. (25)) is one possible solution. Moreover, there is no need to perform continuous adjustment throughout the whole path generation—adjustment in the first few steps gives This work is partially supported by the Graduate School of Okayama University of Science, Okayama, Japan.

The authors would like to thank Mr. Yoshiro Kobayashi, a graduate student at the Graduate School of Okayama University of Science, for his help in preparing part of the figures. We are thankful to Ms Z. Krasteva for proofreading the final version of the manuscript.

#### **9. References**


**10** 

*Taiwan* 

**Path Searching Algorithms of Multiple Robot** 

Chinese chess game [1] is one of the most popular games. A two-player game with a complexity level is similar to Western chess. In the recent, the Chinese chess game has gradually attracted many researcher's attention. The most researchers of the fields are belong to expert knowledge and artificial intelligent. There are many evolutionary algorithms to be proposed. Darwen and Yao proposed the co-evolutionary algorithm to solve problems where an object measure to guide the searching process is extremely difficult to device [2]. Yong proposed multi-agent systems to share the rewards and penalties of successes and failures [3]. Almost all the chess game can be described by game tree. Game tree presents the possible movements and lists all situations for the Chinese chesses. We want to use the multi-robots system to present the scenario of the chess movement for the Chinese chess game, and play the Chinese chess game according to the real-time image

The application of co-evolutionary models is to learn Chinese chess strategies, and uses alpha-beta search algorithm, quiescence searching and move ordering [4]. Wang used adaptive genetic algorithm (AGA) to solve the problems of computer Chinese chess [5]. Lee and Liu take such an approach to develop a software framework for rapidly online chess games [6]. Zhou and Zhang present the iterative sort searching techniques based on percentage evaluation and integrate percentage evaluation and iterative sort into problem of Chinese chess computer game [7]. Su and Shiau developed smart mobile robots to speak real-time status using voice module, and program the motion trajectories for multiple

With the robotic technologies development with each passing day, robot system has been widely employed in many applications. Recently, more and more researchers are interest in the robot which can helps people in our daily life, such as entertaining robots, museum docent robots, educational robots, medical robots, service robots, office robots, security robots, home robots, and so on. In the future, we believe that intelligent robots will play an important role in our daily life. In the past literatures, many experts researched in the mobile robot, and proposed many methods to enhance the functions of the mobile robot [9]. So far, developing a big sized mobile robot to be equipped with many functions to become

feedback to the supervised computer via wireless image system.

**1. Introduction** 

mobile robot system [8].

**System Applying in Chinese Chess Game** 

Jr-Hung Guo1, Kuo-Lan Su2 and Sheng-Ven Shiau1

*1Graduate school Engineering Science and technology, National Yunlin University of Science & Technology* 

*National Yunlin University of Science & Technology* 

*2Department of Electrical Engineering,* 


### **Path Searching Algorithms of Multiple Robot System Applying in Chinese Chess Game**

Jr-Hung Guo1, Kuo-Lan Su2 and Sheng-Ven Shiau1

*1Graduate school Engineering Science and technology, National Yunlin University of Science & Technology 2Department of Electrical Engineering, National Yunlin University of Science & Technology Taiwan* 

#### **1. Introduction**

18 Will-be-set-by-IN-TECH

190 Recent Advances in Mobile Robotics

Lee, S. & Kardaras, G. (1997b). Elastic string based global path planning using

Lozano-Pèrez, T., Mason, M. T. & Taylor, R. H. (1994). Automatic synthesis of fine motion strategies for robots, *International Journal of Robotics Research* 3(1): 3–24. Lozano-Pèrez, T. & Wesley, M. A. (1979). An algorithm for planning collision-free paths among polyhedral obstacles, *Communications of the ICM* Vol. 22(No. 10): 560–570. Mondada, F., Franzi, E. & Ienne, P. (1993). Mobile robot miniaturization: a tool for investigation in control algorithms, *Proceedings of ISER3*, Kyoto, Japan. O'Dúnlaing, C. & Yap, C. K. (1982). A retraction method for planning the motion of a disk,

Paden, B., Mees, A. & Fisher, M. (1989). Path planning using a jacobian-based freespace

Rimon, E. & Doditschek, D. E. (1992). Exact robot navigation using artificial potential fields,

Sun, Z. Q., Zhang, Z. X. & Deng, Z. D. (1997). *Intelligent Control Systems*, Tsinghua University

Tsankova, D. (2010). Neural networks based navigation and control of a mobile robot in a

Warren, C. W. (1989). Global path planning using artificial potential fields, *Proceedings of IEEE*

Yamamoto, M., Iwamura, M. & Mohri, A. (1998). Near-time-optimal trajectory planning

constraints and obstacles, *Journal of the Robotics Society of Japan* 16(8): 95–102. Yu, J., Kroumov, V. & Narihisa, H. (2002). Path planning algorithm for car-like robot and its application, *Chinese Quarterly Journal of Mathematics* Vol. 17(No. 3): 98–104. Yu, J., Kroumov, V. & Negishi, H. (2003). Optimal path planner for mobile robot in 2d

Zelinsky, A. (1992). A mobile robot exploration algorithm, *IEEE Transactins on Robotics &*

Zelinsky, A. & Yuta, S. (1993). Reactive planning for mobile robots using numeric potential fields, *Proceedings of Intelligent Autonomous Systems 3 (IAS3)*, pp. 84–93.

generation algorithm, *Proceedings of IEEE International Conference on Robotics &*

partially known environment, *in* A. Barrera (ed.), *Mobile Robots Navigation*, InTech,

for mobile robots with two independently driven wheels considering dynamical

environment, *Proceedings of the 7th World Multiconference on Systemics, Cybernetics and*

*Intelligence in Robotics and Automation (CIRA'97)*, pp. 108–114.

*Automation*, Scottsdale, Arizona, U. S. A., pp. 1732–1737.

*IEEE Transactins on Robotics & Automation* Vol. 8(No. 5): 501–518.

*International Conference on Robotics & Automation*, pp. 316–321.

*Informatics*, Vol. 3, Orlando, Florida, U. S. A., pp. 235–240.

*Automation* Vol. 8(No. 6): 707–717.

*Journal of Algorithms* Vol. 6: 104–111.

Press.

pp. 197–222.

neural networks, *Proceedings of 1997 IEEE International Symposium on Computational*

Chinese chess game [1] is one of the most popular games. A two-player game with a complexity level is similar to Western chess. In the recent, the Chinese chess game has gradually attracted many researcher's attention. The most researchers of the fields are belong to expert knowledge and artificial intelligent. There are many evolutionary algorithms to be proposed. Darwen and Yao proposed the co-evolutionary algorithm to solve problems where an object measure to guide the searching process is extremely difficult to device [2]. Yong proposed multi-agent systems to share the rewards and penalties of successes and failures [3]. Almost all the chess game can be described by game tree. Game tree presents the possible movements and lists all situations for the Chinese chesses. We want to use the multi-robots system to present the scenario of the chess movement for the Chinese chess game, and play the Chinese chess game according to the real-time image feedback to the supervised computer via wireless image system.

The application of co-evolutionary models is to learn Chinese chess strategies, and uses alpha-beta search algorithm, quiescence searching and move ordering [4]. Wang used adaptive genetic algorithm (AGA) to solve the problems of computer Chinese chess [5]. Lee and Liu take such an approach to develop a software framework for rapidly online chess games [6]. Zhou and Zhang present the iterative sort searching techniques based on percentage evaluation and integrate percentage evaluation and iterative sort into problem of Chinese chess computer game [7]. Su and Shiau developed smart mobile robots to speak real-time status using voice module, and program the motion trajectories for multiple mobile robot system [8].

With the robotic technologies development with each passing day, robot system has been widely employed in many applications. Recently, more and more researchers are interest in the robot which can helps people in our daily life, such as entertaining robots, museum docent robots, educational robots, medical robots, service robots, office robots, security robots, home robots, and so on. In the future, we believe that intelligent robots will play an important role in our daily life. In the past literatures, many experts researched in the mobile robot, and proposed many methods to enhance the functions of the mobile robot [9]. So far, developing a big sized mobile robot to be equipped with many functions to become

Path Searching Algorithms of Multiple Robot System Applying in Chinese Chess Game 193

The system architecture of the multiple mobile robots based Chinese chess game system is shown in Fig 1. The system contains a supervised computer, a image system, some wireless RF modules, a remote supervised computer and thirty-two mobile robots. Mobile robots are classified two sides (red side and black side) belong two players. There are sixteen mobile robots in each side. The supervised computer can transmit the command signals to control mobile robots, and receives the status of the mobile robots via wireless RF interface. These signals contain ID code, orientation and displacement of mobile robots from the supervised computer to mobile robots. The feedback signals of mobile robots contain ID code, position (X axis and Y axis) to the supervised computer. Each robot is arranged an ID code to classify the attribute of the chess. Users move the chess on the user interface using the mouse. The supervised computer can controls the mobile robot moving the target position according the command. The mobile robot transmits the ending code to the supervised computer after

Fig. 1. The architecture of the mobile robots based Chinese chess game system.

The Chinese chess game system can transmit the real-time image to the supervised computer via image system. Users can play the Chinese chess game with others on the supervised computer using the mouse, or play the game on the remote supervised computer via wireless Internet. The player can move the chess piece using the mouse according to the rules of the Chinese chess game. Mobile robots receive the status from the supervised computer via wireless RF interface. There are two sides of the game system. One is the red side; the other is black side. Each side has sixteen chesses. There are one "king" chess, two "advisor" chesses, two "elephant" chesses, two "horse" chesses, two "rook" chesses, two "cannon" chesses and five "pawn" chesses. The definition of the chessboard is shown in Fig. 2. Then we define the initial position all chesses. Such as the position of "red king" is (4,0),

The basic rules of the Chinese chess are found easily on the Internet. Before we want to control the multiple mobile robots based Chinese chess game. A chess engine needs to be

**2. System architecture** 

finishing the task via wireless RF interface.

and "black king" is (4,9)…etc.

complex and huge, and the development period is too long. Thus, recently small-sized mobile robot system has been investigated for a specific task, and program the optimal motion path on the dynamic environment [18].

There is a growing in multi-robot cooperation research in recent year. Compare to single mobile robot, cooperation multiple mobile robots can lead to faster task completion, higher quality solution, as well as increase robustness owing its ability adjust to robot failure [10]. Grabowski and Navarro-serment [11] suggested multiple mobile robots in which each mobile platform has a specific sensor for some purpose and therefore the system's task can be distributed to each mobile platform during surveillance. The feature of this system is that each mobile robot has a common motion platform, but has different sensors. Chung et al. [12] composed of one ship and four small-sized search robots for team work in hazardous environment. Balch et al. [13] and Alami et al. [14] investigated cooperation algorithm of multiple robot system.

Some papers consider the problem of the multiple robot system working together. The multiple mobile robot system has more advantages than one single robot system [15]. The multiple mobile robots have the potential to finish some tasks faster than a single robot using ant colony system [16]. Multiple mobile robots therefore can be expected more fault tolerant than only one robot. Another advantage of multiple mobile robots is due to merging of overlapping information, which can help compensate for sensor uncertainty [17]. We have developed multiple small-size robot system to be applied in Chinese chess [8]. We extend the application field of the multiple mobile robot system, and program the shortest path moving on the chessboard platform using A\* searching algorithm. The A\* heuristic function are introduced to improve local searching ability and to estimate the forgotten value [19].

We present the searching algorithms based Chinese chess game, and use multiple mobile robots to present the scenario on the chessboard platform. The mobile robot has the shape of cylinder and its diameter, height and weight is 8cm, 15cm and 1.5kg. The controller of the mobile robot is MCS-51 chip, and acquires the detection signals from sensors through I/O pins, and receives the command from the supervised compute via wireless RF interface. The chesses (mobile robots) can speak Chinese language for real-time status using voice module. We develop the user interface of multiple mobile robots according to the basic rules of Chinese chess game on the supervised computer. The mobile robot receives the command from the supervised computer, and calculates the displacement using the encoder module The supervised computer program the motion trajectories using evaluation algorithm and A\* searching algorithm for the mobile robot to play the Chinese chess game. The A\* searching algorithm can solve shortest path problem of mobile robots from the start point to the target point on the chessboard platform.

The simulation results can found the shortest motion path for mobile robots (chesses) moving to target points from start points in a collision-free environment. The two points are selected by two players according to the rules of the Chinese chess game. In the experimental results, we test some functions of the mobile robot to obey the moving rules of the game on the chessboard, and implement the simulation results on the chessboard platform using mobile robots. Users can play the Chinese chess game on the supervised computer using the mouse. Mobile robots can receive the command from the supervised computer, and move the next position according to the attribute of the chess. The chess (mobile robot) moving scenario of the Chinese chess game feedback to the user interface using wireless image system.

#### **2. System architecture**

192 Recent Advances in Mobile Robotics

complex and huge, and the development period is too long. Thus, recently small-sized mobile robot system has been investigated for a specific task, and program the optimal

There is a growing in multi-robot cooperation research in recent year. Compare to single mobile robot, cooperation multiple mobile robots can lead to faster task completion, higher quality solution, as well as increase robustness owing its ability adjust to robot failure [10]. Grabowski and Navarro-serment [11] suggested multiple mobile robots in which each mobile platform has a specific sensor for some purpose and therefore the system's task can be distributed to each mobile platform during surveillance. The feature of this system is that each mobile robot has a common motion platform, but has different sensors. Chung et al. [12] composed of one ship and four small-sized search robots for team work in hazardous environment. Balch et al. [13] and Alami et al. [14] investigated cooperation algorithm of

Some papers consider the problem of the multiple robot system working together. The multiple mobile robot system has more advantages than one single robot system [15]. The multiple mobile robots have the potential to finish some tasks faster than a single robot using ant colony system [16]. Multiple mobile robots therefore can be expected more fault tolerant than only one robot. Another advantage of multiple mobile robots is due to merging of overlapping information, which can help compensate for sensor uncertainty [17]. We have developed multiple small-size robot system to be applied in Chinese chess [8]. We extend the application field of the multiple mobile robot system, and program the shortest path moving on the chessboard platform using A\* searching algorithm. The A\* heuristic function are introduced to improve local searching ability and to estimate the forgotten

We present the searching algorithms based Chinese chess game, and use multiple mobile robots to present the scenario on the chessboard platform. The mobile robot has the shape of cylinder and its diameter, height and weight is 8cm, 15cm and 1.5kg. The controller of the mobile robot is MCS-51 chip, and acquires the detection signals from sensors through I/O pins, and receives the command from the supervised compute via wireless RF interface. The chesses (mobile robots) can speak Chinese language for real-time status using voice module. We develop the user interface of multiple mobile robots according to the basic rules of Chinese chess game on the supervised computer. The mobile robot receives the command from the supervised computer, and calculates the displacement using the encoder module The supervised computer program the motion trajectories using evaluation algorithm and A\* searching algorithm for the mobile robot to play the Chinese chess game. The A\* searching algorithm can solve shortest path problem of mobile robots from the start point to

The simulation results can found the shortest motion path for mobile robots (chesses) moving to target points from start points in a collision-free environment. The two points are selected by two players according to the rules of the Chinese chess game. In the experimental results, we test some functions of the mobile robot to obey the moving rules of the game on the chessboard, and implement the simulation results on the chessboard platform using mobile robots. Users can play the Chinese chess game on the supervised computer using the mouse. Mobile robots can receive the command from the supervised computer, and move the next position according to the attribute of the chess. The chess (mobile robot) moving scenario of the Chinese chess game feedback to the user interface

motion path on the dynamic environment [18].

the target point on the chessboard platform.

using wireless image system.

multiple robot system.

value [19].

The system architecture of the multiple mobile robots based Chinese chess game system is shown in Fig 1. The system contains a supervised computer, a image system, some wireless RF modules, a remote supervised computer and thirty-two mobile robots. Mobile robots are classified two sides (red side and black side) belong two players. There are sixteen mobile robots in each side. The supervised computer can transmit the command signals to control mobile robots, and receives the status of the mobile robots via wireless RF interface. These signals contain ID code, orientation and displacement of mobile robots from the supervised computer to mobile robots. The feedback signals of mobile robots contain ID code, position (X axis and Y axis) to the supervised computer. Each robot is arranged an ID code to classify the attribute of the chess. Users move the chess on the user interface using the mouse. The supervised computer can controls the mobile robot moving the target position according the command. The mobile robot transmits the ending code to the supervised computer after finishing the task via wireless RF interface.

Fig. 1. The architecture of the mobile robots based Chinese chess game system.

The Chinese chess game system can transmit the real-time image to the supervised computer via image system. Users can play the Chinese chess game with others on the supervised computer using the mouse, or play the game on the remote supervised computer via wireless Internet. The player can move the chess piece using the mouse according to the rules of the Chinese chess game. Mobile robots receive the status from the supervised computer via wireless RF interface. There are two sides of the game system. One is the red side; the other is black side. Each side has sixteen chesses. There are one "king" chess, two "advisor" chesses, two "elephant" chesses, two "horse" chesses, two "rook" chesses, two "cannon" chesses and five "pawn" chesses. The definition of the chessboard is shown in Fig. 2. Then we define the initial position all chesses. Such as the position of "red king" is (4,0), and "black king" is (4,9)…etc.

The basic rules of the Chinese chess are found easily on the Internet. Before we want to control the multiple mobile robots based Chinese chess game. A chess engine needs to be

Path Searching Algorithms of Multiple Robot System Applying in Chinese Chess Game 195

Robot ID code

Definition Start byte ID code Robot ID code X axis of robot Y axis of robot

We develop two module based mobile robot (MBR-I and MBR-II) for the Chinese chess game. The mobile robot (module based robot-I, MBR-I) has the shape of cylinder, and it's equipped with a microchip (MCS-51) as the main controller, two DC motors and driver circuits, a reflect IR sensor circuits, a voice module, an encoder module, three Li batteries and some wireless RF modules. Meanwhile, the mobile robot has four wheels to provide the

The mobile robot has some hardware circuits to be shown in Fig. 3. The encoder module uses two reflect IR sensors to calculate the pulse signals from the two wheels of the mobile robot, and calculates the moving distance on the chessboard platform. The power of the mobile robot is three Li batteries, and connects with parallel arrangement. The driver circuits can control two DC motors to execute the displacement command through PWM signal from the controller. The controller of the mobile robot can acquires the detection signals from sensors through I/O pins, and receives the control command via wireless RF interface. The switch input can turns on the power of the mobile robot, and selects power input to be Li batteries or adapter. The voice module can speak the real-time status of the environment for mobile robot moving on the chessboard. In the encoder module, we plot the white line and black line on the wheel of the mobile robot, and use two reflect IR sensors to calculate the pulse signals from the two wheels of the mobile robot. We can set the pulse number for per revolution to be *P* , and the mobile robot moves pulse number to be *B* . We

can calculate the movement displacement *D* of the mobile robot using the equation.

4.25 *<sup>B</sup> <sup>D</sup> <sup>P</sup>* = ×π

The diameter of the wheel is 4.25 cm. We design the new chessboard using grid based platform to be shown in Fig. 4. The arrangement of the chess board is 11 grids on the horizontal direction (X axis), and is 12 grids on the vertical direction (Y axis). The distance is 30cm between the center of corridor on the X axis and Y axis of the chessboard. The width of

(1)

Obstacle status Orientation of robot No use No use Checksum

X axis of start position

Orientation No use Checksum

Y axis of start position

Byte 0 1 2 3 4

5 6 7 8 9

Byte 0 1 2 3 4

5 6 7 8 9

ID code

Table 1. The communication protocol of the supervised computer.

Y axis of target position

Table 2. The communication protocol of the mobile robot.

Definition Start

X axis of target position

**3. Mobile robot** 

capability of autonomous mobility.

byte

designed and tested to ensure that all chess displacement and game rules are strictly adhered. We proposed the engine to be simple programmed basic rules of the games. The chessboard, denoted a 9 by 10 matrix, is the most important information that determine a players next moving position. We use axis position to define all chesses. First, the chessboard, denoted a axis position (*x*,*y*) from (0,0) to (8,9).

Fig. 2. The positions of the chesses.

We plot the possible motion trajectories using thick lines for the chesses on the chessboard. Then we define the game tree, and move the chess to the target position. Such as the chess "black horse" can moves to the position (8,7), (6,7) or (5,8). But the chess can't moves to the position (5,8) according the rules of the Chinese chess game. The chess "black horse" has an obstacle (black elephant) on the right side, and can't walk over the chessboard. We plot the possible motion trajectory for the chess piece on the chessboard. Then we define the game tree, and move the chess to the assigned position. Such as the chess "red horse" can moves to the position (0,2) or (2,2). But the chess can't moves to the position (3,1) according to the rules of the Chinese chess game.

The communication protocol of the multiple mobile robot system is 10 bytes. It contains one start byte, eight data bytes and one checksum byte. The supervised computer transmits 10 bytes to control the mobile robot, and the communication protocol of the control command is listed in Table 1. The mobile robot receives the command to discriminate the robot ID code to be right, and moves to the target point step by step, and transmits the environment status to the supervised computer on real-time. The communication protocol of the feedback data from the mobile robot is listed in Table 2. The byte 3 and 4 represents the positions of the mobile robot on X axis and Y axis.


Table 1. The communication protocol of the supervised computer.


Table 2. The communication protocol of the mobile robot.

#### **3. Mobile robot**

194 Recent Advances in Mobile Robotics

designed and tested to ensure that all chess displacement and game rules are strictly adhered. We proposed the engine to be simple programmed basic rules of the games. The chessboard, denoted a 9 by 10 matrix, is the most important information that determine a players next moving position. We use axis position to define all chesses. First, the

We plot the possible motion trajectories using thick lines for the chesses on the chessboard. Then we define the game tree, and move the chess to the target position. Such as the chess "black horse" can moves to the position (8,7), (6,7) or (5,8). But the chess can't moves to the position (5,8) according the rules of the Chinese chess game. The chess "black horse" has an obstacle (black elephant) on the right side, and can't walk over the chessboard. We plot the possible motion trajectory for the chess piece on the chessboard. Then we define the game tree, and move the chess to the assigned position. Such as the chess "red horse" can moves to the position (0,2) or (2,2). But the chess can't moves to the position (3,1) according to the

The communication protocol of the multiple mobile robot system is 10 bytes. It contains one start byte, eight data bytes and one checksum byte. The supervised computer transmits 10 bytes to control the mobile robot, and the communication protocol of the control command is listed in Table 1. The mobile robot receives the command to discriminate the robot ID code to be right, and moves to the target point step by step, and transmits the environment status to the supervised computer on real-time. The communication protocol of the feedback data from the mobile robot is listed in Table 2. The byte 3 and 4 represents the positions of

chessboard, denoted a axis position (*x*,*y*) from (0,0) to (8,9).

Fig. 2. The positions of the chesses.

rules of the Chinese chess game.

the mobile robot on X axis and Y axis.

We develop two module based mobile robot (MBR-I and MBR-II) for the Chinese chess game. The mobile robot (module based robot-I, MBR-I) has the shape of cylinder, and it's equipped with a microchip (MCS-51) as the main controller, two DC motors and driver circuits, a reflect IR sensor circuits, a voice module, an encoder module, three Li batteries and some wireless RF modules. Meanwhile, the mobile robot has four wheels to provide the capability of autonomous mobility.

The mobile robot has some hardware circuits to be shown in Fig. 3. The encoder module uses two reflect IR sensors to calculate the pulse signals from the two wheels of the mobile robot, and calculates the moving distance on the chessboard platform. The power of the mobile robot is three Li batteries, and connects with parallel arrangement. The driver circuits can control two DC motors to execute the displacement command through PWM signal from the controller. The controller of the mobile robot can acquires the detection signals from sensors through I/O pins, and receives the control command via wireless RF interface. The switch input can turns on the power of the mobile robot, and selects power input to be Li batteries or adapter. The voice module can speak the real-time status of the environment for mobile robot moving on the chessboard. In the encoder module, we plot the white line and black line on the wheel of the mobile robot, and use two reflect IR sensors to calculate the pulse signals from the two wheels of the mobile robot. We can set the pulse number for per revolution to be *P* , and the mobile robot moves pulse number to be *B* . We can calculate the movement displacement *D* of the mobile robot using the equation.

$$D = 4.25 \times \pi \frac{B}{P} \tag{1}$$

The diameter of the wheel is 4.25 cm. We design the new chessboard using grid based platform to be shown in Fig. 4. The arrangement of the chess board is 11 grids on the horizontal direction (X axis), and is 12 grids on the vertical direction (Y axis). The distance is 30cm between the center of corridor on the X axis and Y axis of the chessboard. The width of

Path Searching Algorithms of Multiple Robot System Applying in Chinese Chess Game 197

The mobile robot (module based robot-II, MBR-II) has the shape of cylinder to be equipped with a microchip (MCS-51) as the main controller, and contains two DC servomotors and driver devices, some sensor circuits, a voice module, a compass module, three Li batteries, a wireless RF interface and three reflect IR sensor modules. Meanwhile, the mobile robot has four wheels to provide the capability of autonomous mobility. The structure of the mobile

The controller calculates the orientation of the mobile robot from the compass module. The compass module has the error range to be 50, and transmits the measured values (*x* and *y*) in X axis and Y axis, and transmits to the controller of the mobile robot. The mobile robot

The distance is 30cm between the center of corridor on the X axis and Y axis, and the width of the corridor is 12cm. The mobile robot uses three IR sensors to detect the signals on the front side, right side and left side of the mobile robot, and decides the position of the obstacles, and detect the cross points. The definition of the orientation is east (X- axis), west (X+ Axis), south (Y+ axis) and north (Y- axis) on the motion platform. We set measurement range on the orientation of the mobile robot. For example, the measurement value of the orientation between 2260 and 3150, and define the direction of the mobile robot is west.

( ) <sup>1</sup>

*Tan y x* / <sup>−</sup> = − (2)

θis

θ

Fig. 5. The structure of the MBR-II.

can calculates the orientation angle

robot is shown in Fig. 5.

the corridor is 12cm. The mobile robot uses IR sensors to detect obstacles, and decides the cross points of the chess board. The game only uses 9 grids (horizontal) by 10 grids (vertical). We release two grids on each direction (horizontal and vertical) to arrange the leaving chesses of red side and black side. We put the mobile robots that are eaten moving around the platform. We improve the mobile robot (MBR-I) to be implemented in the new chessboard platform, and design new driver device using DC servomotor.

Fig. 3. The structure of the MBR-I.

Fig. 4. The chessboard of the Chinese chess game.

Fig. 5. The structure of the MBR-II.

196 Recent Advances in Mobile Robotics

the corridor is 12cm. The mobile robot uses IR sensors to detect obstacles, and decides the cross points of the chess board. The game only uses 9 grids (horizontal) by 10 grids (vertical). We release two grids on each direction (horizontal and vertical) to arrange the leaving chesses of red side and black side. We put the mobile robots that are eaten moving around the platform. We improve the mobile robot (MBR-I) to be implemented in the new

chessboard platform, and design new driver device using DC servomotor.

Fig. 3. The structure of the MBR-I.

Fig. 4. The chessboard of the Chinese chess game.

The mobile robot (module based robot-II, MBR-II) has the shape of cylinder to be equipped with a microchip (MCS-51) as the main controller, and contains two DC servomotors and driver devices, some sensor circuits, a voice module, a compass module, three Li batteries, a wireless RF interface and three reflect IR sensor modules. Meanwhile, the mobile robot has four wheels to provide the capability of autonomous mobility. The structure of the mobile robot is shown in Fig. 5.

The controller calculates the orientation of the mobile robot from the compass module. The compass module has the error range to be 50, and transmits the measured values (*x* and *y*) in X axis and Y axis, and transmits to the controller of the mobile robot. The mobile robot can calculates the orientation angle θis

$$
\theta = \text{Tan}^{-1}\left(-y \mid \mathbf{x}\right) \tag{2}
$$

The distance is 30cm between the center of corridor on the X axis and Y axis, and the width of the corridor is 12cm. The mobile robot uses three IR sensors to detect the signals on the front side, right side and left side of the mobile robot, and decides the position of the obstacles, and detect the cross points. The definition of the orientation is east (X- axis), west (X+ Axis), south (Y+ axis) and north (Y- axis) on the motion platform. We set measurement range on the orientation of the mobile robot. For example, the measurement value of the orientation between 2260 and 3150, and define the direction of the mobile robot is west.

Path Searching Algorithms of Multiple Robot System Applying in Chinese Chess Game 199

Fig. 7. The operation interface of moving the chess "black elephant".

Fig. 8. The operation interface of moving the chess "red horse".

Fig. 6. The mobile robot tuning method.

The mobile robot moves on the corridor of the motion platform, and detects the location to be leaving the center line using the three reflect IR sensors. It tunes the motion path moving to the center line of the corridor. For example, the mobile robot leaves the center line, and moves on the left side of the center line. The mobile robot turns right θ (degree) to be set by users, and is calculated by the encoder module. Then it moves the displacement *S* according to Eq. (3), and moves to the center line. Finally, the mobile robot turns left θ to face the center line. The overview of the tuning schedule is shown in Fig. 6.

$$S = \frac{R\pi\theta}{180} \tag{3}$$

The parameter *R* (=4.25cm) is the diameter of the wheel. Furthermore, the mobile robot moves on the right side of the centre line. The tuning processing is the same as the previous method.

#### **4. User interface**

The operation interface of the multiple robot based Chinese chess game system is shown in Fig. 7. There are two regions in the operation interface. The chessboard of the Chinese chess is the main monitor of the user interface. Next we explain the chessboard of the Chinese chess game, and describe how to use the interface. It can displays "communication port", "communication protocol" and "axis position" on the bottom side of the operation interface. We set the communication port is 2, and Baud rate is 9600. The start position and target position of each chess displays on the left-bottom side of the operation interface. We make an example to explain the operation interface to be shown in Fig. 7. Players can move "black elephant"to the target position (5,8) from the start position (3,10). The bottom side of the interface can displays the start position (3,10) and the target position (5,8). We develop the operation interface using Visual Basic language for the Chinese chess game system.

We make other example to explain the Chinese chess game interface to be shown in Fig. 8. We can move the chess "red horse" from the start position (2,1) to the next position (3,3) using the mouse. The supervised computer control "red horse" chess moving to the next position (3,3). The scenario of the operation interface is shown in Fig. 8. The bottom side of the operation interface can displays the start position (2,1) and the next position (3,3).

198 Recent Advances in Mobile Robotics

moves on the left side of the center line. The mobile robot turns right

face the center line. The overview of the tuning schedule is shown in Fig. 6.

The mobile robot moves on the corridor of the motion platform, and detects the location to be leaving the center line using the three reflect IR sensors. It tunes the motion path moving to the center line of the corridor. For example, the mobile robot leaves the center line, and

users, and is calculated by the encoder module. Then it moves the displacement *S* according to Eq. (3), and moves to the center line. Finally, the mobile robot turns left

> 180 *<sup>R</sup> <sup>S</sup>* πθ

The parameter *R* (=4.25cm) is the diameter of the wheel. Furthermore, the mobile robot moves on the right side of the centre line. The tuning processing is the same as the previous

The operation interface of the multiple robot based Chinese chess game system is shown in Fig. 7. There are two regions in the operation interface. The chessboard of the Chinese chess is the main monitor of the user interface. Next we explain the chessboard of the Chinese chess game, and describe how to use the interface. It can displays "communication port", "communication protocol" and "axis position" on the bottom side of the operation interface. We set the communication port is 2, and Baud rate is 9600. The start position and target position of each chess displays on the left-bottom side of the operation interface. We make an example to explain the operation interface to be shown in Fig. 7. Players can move "black elephant"to the target position (5,8) from the start position (3,10). The bottom side of the interface can displays the start position (3,10) and the target position (5,8). We develop the operation interface using Visual Basic language for the

We make other example to explain the Chinese chess game interface to be shown in Fig. 8. We can move the chess "red horse" from the start position (2,1) to the next position (3,3) using the mouse. The supervised computer control "red horse" chess moving to the next position (3,3). The scenario of the operation interface is shown in Fig. 8. The bottom side of

the operation interface can displays the start position (2,1) and the next position (3,3).

θ

= (3)

(degree) to be set by

θto

Fig. 6. The mobile robot tuning method.

method.

**4. User interface** 

Chinese chess game system.

Fig. 7. The operation interface of moving the chess "black elephant".

Fig. 8. The operation interface of moving the chess "red horse".

Path Searching Algorithms of Multiple Robot System Applying in Chinese Chess Game 201

0 2, 0 2 *x xn y ym* ≤ ≤ ≤±≤ ≤≤ ≤± ≤

7 9, 0 9 *x xn y ym* ≤ ≤ ≤±≤ ≤ ≤ ≤± ≤

0 2, 0 2 *x xn y yn* ≤ ≤ ≤±≤ ≤ ≤ ≤±≤

7 9, 0 9 *x xn y yn* ≤ ≤ ≤±≤ ≤ ≤ ≤±≤

0 4, 0 4 *x xn y yn* ≤ ≤ ≤±≤ ≤ ≤ ≤±≤

0 9, 0 9 *x xn y ym* ≤ ≤ ≤±≤ ≤ ≤ ≤± ≤

3 4, 3 4

≤ ≤ ≤± ≤

5 9, 5 9 *x xn y ym* ≤ ≤ ≤±≤ ≤ ≤ ≤+ ≤

5 6, 5 6

≤ ≤ ≤± ≤

*y ym*

0 4, 0 4 *x xn y ym* ≤ ≤ ±≤ ≤ ≤ ≤− ≤

*y ym*

*x*

*x*

*f* (*n*) = + *g*(*n hn* ) ( ) (4)

≤ ≤

≤ ≤

Red king (*x ny m n m* ± , , 0 1, 0 1 ± ≤≤ ≤ ≤ ) 3 5, 3 5,

Black king (*x ny m n m* ± , , 0 1, 0 1 ± ≤≤ ≤ ≤ ) 3 5, 0 5,

Red advisor (*x ny n n* ± , , 1 ± = ) 3 5, 0 5,

Black advisor (*x ny n n* ± , , 1 ± = ) 3 5, 0 5,

Red elephant (*x ny n n* ± , , 2 ± = ) 0 8, 0 8,

Red horse, Black horse (*x ny m n m* ± , , 3 ± += ) 0 8, 0 8,

Red pawn (*x y*, 1 + ) 0 8,

Black pawn (*x y*, 1 − ) 0 8,

nodes travel system. The formula of A\* searching algorithm is following

Black elephant (*x ny n n* ± , , 2 ± = ) 0 , 8, 5 , 9 ≤ *xx n yy n* ±≤ ≤ ±≤

(*x ny m n m* ± , , 0 or 0 ±= = ) 0 , 8, 5 , 9 ≤ *xx n yy m* ±≤ ≤ ± ≤

(*x ny m n m* ± , , 1 + += ) 0 8, 0 8,

(*x ny m n m* ± , , 1 − += ) 0 8, 8,

Mobile robots move on the chessboard to obey the rules of Chinese chess game. The user interface knows the start position and the target position, and programs the shortest path of the chess (mobile robot) moving to the target position from the start position. We use A\* searching algorithm to find the shortest path on the Chinese chess game. A\* searching algorithm is proposed by Hart in 1968, and solved the shortest path problem of multiple

The core part of an intelligent searching algorithm is the definition of a proper heuristic function *f* (*n*) . *g*(*n*) is the exact cost at sample time *n* from start point to the target point. *h n*( ) is an estimate of the minimum cost from the start point to the target point. In this

Chess Target position Rules

Red rook, Red cannon Black rook, Black

Table 3. The rules of Chinese chesses.

cannon

The user interface of the multiple mobile robot based Chinese chess game system has four parts to be shown in left side of the Fig. 9. Players can start or close the game in the part "I". The part "II" can displays the communication protocol on the bottom of the monitor, and the communication port is 2, and Baud rate setting is 9600. The supervised computer transmits the control command to the mobile robot, and receives the status of the mobile robot, and displays the position of the mobile robot moving on the chessboard platform simultaneously to be shown in the part "III".

The part "IV" is chessboard platform. It can displays the motion path that is computed by evaluation algorithm and A\* searching algorithm. The operation interface of the multiple robot based Chinese chess game system is shown in right side of the Fig. 9. There are two regions in the user interface. One is real-time image from the image system, and is equipped in the mobile robot based chessboard. The other is the chessboard of the Chinese chess game, and displays on the right side or the monitor. The players can move the chess using the mouse according the rules of the Chinese chess game. Mobile robots receive the status from the supervised computer via wireless RF interface, and move to the next position.

#### **5. Evaluation method**

The evaluation algorithm of the Chinese chess game uses the rule based method. Players can move the chess to the target position on the user interface. The position of the chess is not abided by the proposed rules. The user interface can't execute the command, and can't control the mobile robot moving to the target position. Players must renew the operation on the step, and obey the exact rules of the Chinese chess game. We can define the initial position of the chess piece is (*x*,*y*) , and define the movement rules of all chesses as following. *n* is movement displacement on the *x* axis, and *m* is movement displacement on the *y* axis. *n* and *m* of the target position must be plus integer. The positions of all chesses must obey the rules that are listed in the Table 3 at any time.


Table 3. The rules of Chinese chesses.

200 Recent Advances in Mobile Robotics

The user interface of the multiple mobile robot based Chinese chess game system has four parts to be shown in left side of the Fig. 9. Players can start or close the game in the part "I". The part "II" can displays the communication protocol on the bottom of the monitor, and the communication port is 2, and Baud rate setting is 9600. The supervised computer transmits the control command to the mobile robot, and receives the status of the mobile robot, and displays the position of the mobile robot moving on the chessboard platform

The part "IV" is chessboard platform. It can displays the motion path that is computed by evaluation algorithm and A\* searching algorithm. The operation interface of the multiple robot based Chinese chess game system is shown in right side of the Fig. 9. There are two regions in the user interface. One is real-time image from the image system, and is equipped in the mobile robot based chessboard. The other is the chessboard of the Chinese chess game, and displays on the right side or the monitor. The players can move the chess using the mouse according the rules of the Chinese chess game. Mobile robots receive the status from the supervised computer via wireless RF interface, and move to

The evaluation algorithm of the Chinese chess game uses the rule based method. Players can move the chess to the target position on the user interface. The position of the chess is not abided by the proposed rules. The user interface can't execute the command, and can't control the mobile robot moving to the target position. Players must renew the operation on the step, and obey the exact rules of the Chinese chess game. We can define the initial position of the chess piece is (*x*,*y*) , and define the movement rules of all chesses as following. *n* is movement displacement on the *x* axis, and *m* is movement displacement on the *y* axis. *n* and *m* of the target position must be plus integer. The positions of all

chesses must obey the rules that are listed in the Table 3 at any time.

simultaneously to be shown in the part "III".

Fig. 9. The user interface of the game.

the next position.

**5. Evaluation method** 

Mobile robots move on the chessboard to obey the rules of Chinese chess game. The user interface knows the start position and the target position, and programs the shortest path of the chess (mobile robot) moving to the target position from the start position. We use A\* searching algorithm to find the shortest path on the Chinese chess game. A\* searching algorithm is proposed by Hart in 1968, and solved the shortest path problem of multiple nodes travel system. The formula of A\* searching algorithm is following

$$f\left(n\right) = \lg\left(n\right) + h\left(n\right) \tag{4}$$

The core part of an intelligent searching algorithm is the definition of a proper heuristic function *f* (*n*) . *g*(*n*) is the exact cost at sample time *n* from start point to the target point. *h n*( ) is an estimate of the minimum cost from the start point to the target point. In this

Path Searching Algorithms of Multiple Robot System Applying in Chinese Chess Game 203

We execute some experimental scenarios using the MBR-I robot on the chessboard for the Chinese chess game system, and test the movement functions of some chesses (mobile robots). The two players are located by two sides, and move the chess one time each other. One moves red chesses; the other moves black chesses. The first experimental scenario is "red king". The player moves forward the chess "red king" using the mouse to be shown in Fig. 12 (a). The chess "red king" moves one grid to the target position on the user interface to be shown in Fig. 12 (b). The supervised computer orders the command to the mobile robot "red king" moving forward to the target position via wireless RF interface. The mobile robot can calculates the movement displacement using the encoder module, and speaks the movement status using the voice module. The experimental result is

The second experimental scenario is "red rook". The player moves forward chess "red rook" using the mouse to be shown in Fig. 13 (a). The supervised computer orders the command

Fig. 11. The searching result for case 2.

**6. Experimental results** 

shown in Fig. 12 (c).

study, *n* is reschedules as *n*′ to generate an approximate minimum cost schedule for the next point. The equation (5) can be rewritten as follows:

$$f\left(n\right) = \lg\left(n\right) + h\left(n'\right) \tag{5}$$

We make an example to explain algorithm. Such as a mobile robot move to the target point "T" from the start point "S". The position of the start point is (2,6), and the target position is (2,9). We set some obstacle on the platform. The white rectangle is unknown obstacle. The black rectangle (obstacle) is detected by the mobile robot using A\* searching algorithm. We construct two labels (Open list and Close list) in the right side of Fig. 8. The neighbour points of the start point fill in the "Open list". The "Close list" fills the start point and evaluation points. We construct label on the first searching result to be shown in Fig. 8. We calculate the values of *f* (*n n* ), g( ) and *h n*( ) function according to the pulse numbers by the encoder of the DC servomotor, and use the proposed method to compare the values of the function. We select the minimum value of the function *f* (*n*) to be stored in "Close list". We can find the target point on the final searching result to be shown in Fig. 10, and we can decide a shortest path to control the mobile robot moving to the target point.

The total distance of the shortest path *Cst* can be calculated as

$$C\_{st} = \sum\_{n=1}^{m} G\_n \left( m = t - 1 \right) = 2043 \tag{6}$$

In the other condition, we rebuild the positions of the obstacles on the platform to be shown in Fig. 11. The mobile robot can't find the path *Cst* moving to the target position using the proposed method. The total distance *Cst* as

$$\mathbf{C}\_{st} = \mathbf{c} \tag{7}$$

Fig. 10. The searching result for case 1.


Fig. 11. The searching result for case 2.

#### **6. Experimental results**

202 Recent Advances in Mobile Robotics

study, *n* is reschedules as *n*′ to generate an approximate minimum cost schedule for the

We make an example to explain algorithm. Such as a mobile robot move to the target point "T" from the start point "S". The position of the start point is (2,6), and the target position is (2,9). We set some obstacle on the platform. The white rectangle is unknown obstacle. The black rectangle (obstacle) is detected by the mobile robot using A\* searching algorithm. We construct two labels (Open list and Close list) in the right side of Fig. 8. The neighbour points of the start point fill in the "Open list". The "Close list" fills the start point and evaluation points. We construct label on the first searching result to be shown in Fig. 8. We calculate the values of *f* (*n n* ), g( ) and *h n*( ) function according to the pulse numbers by the encoder of the DC servomotor, and use the proposed method to compare the values of the function. We select the minimum value of the function *f* (*n*) to be stored in "Close list". We can find the target point on the final searching result to be shown in Fig. 10, and we can

( )

In the other condition, we rebuild the positions of the obstacles on the platform to be shown in Fig. 11. The mobile robot can't find the path *Cst* moving to the target position using the

1 2043

= =− = ∑ (6)

*Cst* = ∞ (7)

decide a shortest path to control the mobile robot moving to the target point.

1

*m st n n C Gmt* =

The total distance of the shortest path *Cst* can be calculated as

proposed method. The total distance *Cst* as

Fig. 10. The searching result for case 1.

*f* (*n*) = + *g*(*n hn* ) ( ′) (5)

next point. The equation (5) can be rewritten as follows:

We execute some experimental scenarios using the MBR-I robot on the chessboard for the Chinese chess game system, and test the movement functions of some chesses (mobile robots). The two players are located by two sides, and move the chess one time each other. One moves red chesses; the other moves black chesses. The first experimental scenario is "red king". The player moves forward the chess "red king" using the mouse to be shown in Fig. 12 (a). The chess "red king" moves one grid to the target position on the user interface to be shown in Fig. 12 (b). The supervised computer orders the command to the mobile robot "red king" moving forward to the target position via wireless RF interface. The mobile robot can calculates the movement displacement using the encoder module, and speaks the movement status using the voice module. The experimental result is shown in Fig. 12 (c).

The second experimental scenario is "red rook". The player moves forward chess "red rook" using the mouse to be shown in Fig. 13 (a). The supervised computer orders the command

Path Searching Algorithms of Multiple Robot System Applying in Chinese Chess Game 205

(c) (d)

We implement the functions of the mobile robot to obey the movement rules of Chinese chess game. Then we execute experimental scenarios of the multiple mobile robots based Chinese chess game system. The first experimental scenario is "red elephant" that is moved by the player (red side). The player moves the chess "red elephant" to the target position (4,2) using the mouse to be shown in Fig. 15 (a) and (b). The supervised computer orders the command to the mobile robot "red elephant" via wireless RF interface. The start position of the mobile robot is (6,0). The movement orientation of the mobile robot is west-north. The mobile robot receives the command to turn left on 900, and moves forward to the target position according to the Chinese chess rules. The mobile robot speaks the movement status using voice module, and calculates the movement displacement using the encoder module, and moves to the

(a) (b)

(c) (d)

target position (4,2). The experimental results are shown in Fig. 15 (c) and (d).

Fig. 14. The experimental result for "red cannon".

Fig. 15. The experimental result for "red elephant".

Fig. 12. The experimental result for "red king".

Fig. 13. The experimental result for "red rook".

to the mobile robot "red rook". The mobile robot "red rook" moves to the target position (8,2) from the start position (8,0). The mobile robot can calculates the movement displacement using the encoder module, and stops at the target position. The experimental result is shown in Fig. 13 (b) and (c).erwe

The third experimental scenario is "red cannon". The player moves the chess "red cannon" to right side using the mouse. The chess "red cannon" moves to the target position (4,2) from the start position (1,2) to be shown in Fig. 14 (a) and (b). The supervised computer orders the command to the mobile robot "red cannon" moving to the target position. The mobile robot receives the command to turn right, and moves to the target position. Then it can turn left and face to the black chess. The mobile robot calculates the movement displacement and the orientation using the encoder module, and speaks the movement status using the voice module. The experimental results are shown in Fig. 14 (c) and (d).

Fig. 14. (Continued)

204 Recent Advances in Mobile Robotics

(a) (b) (c)

to the mobile robot "red rook". The mobile robot "red rook" moves to the target position (8,2) from the start position (8,0). The mobile robot can calculates the movement displacement using the encoder module, and stops at the target position. The experimental

The third experimental scenario is "red cannon". The player moves the chess "red cannon" to right side using the mouse. The chess "red cannon" moves to the target position (4,2) from the start position (1,2) to be shown in Fig. 14 (a) and (b). The supervised computer orders the command to the mobile robot "red cannon" moving to the target position. The mobile robot receives the command to turn right, and moves to the target position. Then it can turn left and face to the black chess. The mobile robot calculates the movement displacement and the orientation using the encoder module, and speaks the movement status using the voice module. The experimental results are shown in Fig. 14 (c) and (d).

(a) (b)

(a) (b) (c)

Fig. 12. The experimental result for "red king".

Fig. 13. The experimental result for "red rook".

result is shown in Fig. 13 (b) and (c).erwe

Fig. 14. (Continued)

We implement the functions of the mobile robot to obey the movement rules of Chinese chess game. Then we execute experimental scenarios of the multiple mobile robots based Chinese chess game system. The first experimental scenario is "red elephant" that is moved by the player (red side). The player moves the chess "red elephant" to the target position (4,2) using the mouse to be shown in Fig. 15 (a) and (b). The supervised computer orders the command to the mobile robot "red elephant" via wireless RF interface. The start position of the mobile robot is (6,0). The movement orientation of the mobile robot is west-north. The mobile robot receives the command to turn left on 900, and moves forward to the target position according to the Chinese chess rules. The mobile robot speaks the movement status using voice module, and calculates the movement displacement using the encoder module, and moves to the target position (4,2). The experimental results are shown in Fig. 15 (c) and (d).

Fig. 15. The experimental result for "red elephant".

Path Searching Algorithms of Multiple Robot System Applying in Chinese Chess Game 207

(a)

(b) (c)

(d) (e)

The second experimental scenario is "red cannon". The user moves the chess "red cannon" using the mouse to be shown in Fig. 18 (a). The start position of the "red cannon" chess is (1,2), and the target point is (1,6). The mobile robot "red cannon" can't moves forward to the target position (6,1). The motion path of the mobile robot has an obstacle (chess) at the position (1,5). The supervised computer reconstructs all possible paths using A\* searching

Fig. 17. The experimental results for the chess "red elephant".

The second experimental scenario is "black advisor" that is moved by the other player (black side). The player moves the chess "black advisor" using the mouse to be shown in Fig. 16 (a). The start position of the "black advisor" chess is (3,9). The supervised computer orders the command to the mobile robot "black advisor". The chess turn left on 900, and moves to the target position (4,8) and stop. The mobile robot can calculates the movement displacement using the encoder module, and speak the movement status using voice module, too. The experimental result is shown in Fig. 16 (b). The next step must moves red chess by the player (red side). The players (red side and black side) move one chess step by step on the chessboard each other. Finally, the user interface can decides which player to be a winner, and closes the game.

Fig. 16. The experimental result for "black advisor".

The controller of the mobile robots calculates the movement displacement to be error, or loses pulse numbers from the encoder module. Mobile robots can't stay at the cross point of two black lines on the chessboard for a long time. We design the grid based chessboard platform to improve the weakness, and implement some experimental results using the multiple mobile robots (MBR-II) based Chinese chess game system. The first experimental scenario is "red elephant".

The user moves the chess "red elephant" using the mouse to be shown in Fig. 17 (a). The start position of the "red elephant" chess is (6,0). The supervised computer orders the command to the mobile robot "red elephant". The chess moves forward two grids, and turn left angle to be 900. Then the mobile robot moves two grids to the next position (4,2), and turn right angle 900 to face the black side and stop. The user interface of the Chinese chess game system uses multiple mobile robots to be shown in Fig. 17(a). The mobile robot can calculates the movement displacement via encoder of DC servomotor, and speaks the movement status of the mobile robot using voice module, too. The experimental scenarios use the mobile robot to execute the motion path of the chess "red elephant" to be shown in Fig. 17 (b)-(e). There is not obstacle on the motion path of the chess. We program the shortest path using A\* searching algorithm.

206 Recent Advances in Mobile Robotics

The second experimental scenario is "black advisor" that is moved by the other player (black side). The player moves the chess "black advisor" using the mouse to be shown in Fig. 16 (a). The start position of the "black advisor" chess is (3,9). The supervised computer orders the command to the mobile robot "black advisor". The chess turn left on 900, and moves to the target position (4,8) and stop. The mobile robot can calculates the movement displacement using the encoder module, and speak the movement status using voice module, too. The experimental result is shown in Fig. 16 (b). The next step must moves red chess by the player (red side). The players (red side and black side) move one chess step by step on the chessboard each other. Finally, the user interface can decides which player to be

(a) (b)

The controller of the mobile robots calculates the movement displacement to be error, or loses pulse numbers from the encoder module. Mobile robots can't stay at the cross point of two black lines on the chessboard for a long time. We design the grid based chessboard platform to improve the weakness, and implement some experimental results using the multiple mobile robots (MBR-II) based Chinese chess game system. The first experimental

The user moves the chess "red elephant" using the mouse to be shown in Fig. 17 (a). The start position of the "red elephant" chess is (6,0). The supervised computer orders the command to the mobile robot "red elephant". The chess moves forward two grids, and turn left angle to be 900. Then the mobile robot moves two grids to the next position (4,2), and turn right angle 900 to face the black side and stop. The user interface of the Chinese chess game system uses multiple mobile robots to be shown in Fig. 17(a). The mobile robot can calculates the movement displacement via encoder of DC servomotor, and speaks the movement status of the mobile robot using voice module, too. The experimental scenarios use the mobile robot to execute the motion path of the chess "red elephant" to be shown in Fig. 17 (b)-(e). There is not obstacle on the motion path of the chess. We program the shortest

a winner, and closes the game.

Fig. 16. The experimental result for "black advisor".

scenario is "red elephant".

path using A\* searching algorithm.

(a)

Fig. 17. The experimental results for the chess "red elephant".

The second experimental scenario is "red cannon". The user moves the chess "red cannon" using the mouse to be shown in Fig. 18 (a). The start position of the "red cannon" chess is (1,2), and the target point is (1,6). The mobile robot "red cannon" can't moves forward to the target position (6,1). The motion path of the mobile robot has an obstacle (chess) at the position (1,5). The supervised computer reconstructs all possible paths using A\* searching

Path Searching Algorithms of Multiple Robot System Applying in Chinese Chess Game 209

The supervised computer orders the command to the mobile robot "red cannon". The chess piece moves forward two grids, and turn left angle to be 900. Then the mobile robot moves one grid to, and turn right angle 900. Then the mobile robot moves two grids, and turn right angle 900. Finally the mobile robot moves one grid to the target position (1,6), and turn left angle 900 to face the black side and stop. Finally, the experiment scenarios are shown in Fig.

We have presented the experimental scenario of the Chinese chess game system using multiple mobile robots. The system contains a supervised computer, a image system, some wireless RF modules and thirty-two mobile robots. Mobile robots are classified red side and black side. We design two types' mobile robots (MBR-I and MBR-II) for Chinese chess game. The two mobile robots have the shape of cylinder and its diameter, height and weights is 8cm, 15cm and 1.5kg, and execute the chess attribute using two interfaces. One is wireless RF interface, and the other is voice interface. We develop the user interface on the supervised computer for the Chinese chess game. The supervised computer can programs the motion paths of the mobile robots using evaluation algorithm according to the rule of Chinese chess game, and receive the status of mobile robots via wireless RF interface. The MBR-I calculates the movement displacement using the pulse numbers from reflect IR sensors of the encoder module. Then we develop the MBR-II robot to implement scenario of the Chinese chess game on the grid based chessboard platform. The MBR-II robot calculates the movement displacement using encoder of DC servomotor. The chess (mobile robot) uses A\* searching algorithm to program the shortest path moving to the target point. The supervised computer can controls mobile robots, and receives the status of mobile robots via wireless RF interface. Players can move the chess piece using the mouse on the supervised computer, and obey the game rules. The supervised computer can controls the mobile robot moving to the target position. The mobile robot speaks Chinese language for the movement

This work was supported by the project "Development of an education robot", under

S. J. Yen, J. C. Chen, T. N. Yang and S. C. Hsu, "Computer Chinese Chess," ICGA Journal,

P. Darwen and X. Yao, "Coevolution in Iterated Prisoner's Dilemma with Intermediate

C. S. Ong, H. Y. Quek, K. C. Tan and A. Tay, "Discovering Chinese Chess Strategies

Computational Intelligence Applications, Vol. 2, No. 1, pp.83-107, 2002. C. H. Yong and R. Miikkulainen, "Cooperative Coevolution of Multi-agent Systems,"

University of Texas, Austin, USA, Tech. Rep. AI01-287,2001.

Levels of Cooperation: Application to Missile Defense," International Journal of

Through Coevolutionary Approaches," IEEE Symposium on Computational

National Science Council of Taiwan, (NSC 99-2622-E-224-012-CC3).

Vol.27, No. 1, pp.3-18, Mar, 2004.

Intelligent and Games, pp.360-367, 2007.

18(b)-(e).

status.

**8. Acknowledgment** 

**9. References** 

**7. Conclusion** 

algorithm for red cannon step by step. Finally, we can find out the shortest path to avoid the obstacle (chess). The path can displays on the interface using red line. The supervised computer controls the mobile robot moving to the target point from the start point using the shortest path to avoid the obstacle. The supervised computer calculates the cost *f(n)* to be listed on the left side of the Fig. 18 (a).

Fig. 18. The experimental results for the chess "red cannon".

The supervised computer orders the command to the mobile robot "red cannon". The chess piece moves forward two grids, and turn left angle to be 900. Then the mobile robot moves one grid to, and turn right angle 900. Then the mobile robot moves two grids, and turn right angle 900. Finally the mobile robot moves one grid to the target position (1,6), and turn left angle 900 to face the black side and stop. Finally, the experiment scenarios are shown in Fig. 18(b)-(e).

### **7. Conclusion**

208 Recent Advances in Mobile Robotics

algorithm for red cannon step by step. Finally, we can find out the shortest path to avoid the obstacle (chess). The path can displays on the interface using red line. The supervised computer controls the mobile robot moving to the target point from the start point using the shortest path to avoid the obstacle. The supervised computer calculates the cost *f(n)* to be

(a)

(b) (c)

(d) (e)

Fig. 18. The experimental results for the chess "red cannon".

listed on the left side of the Fig. 18 (a).

We have presented the experimental scenario of the Chinese chess game system using multiple mobile robots. The system contains a supervised computer, a image system, some wireless RF modules and thirty-two mobile robots. Mobile robots are classified red side and black side. We design two types' mobile robots (MBR-I and MBR-II) for Chinese chess game. The two mobile robots have the shape of cylinder and its diameter, height and weights is 8cm, 15cm and 1.5kg, and execute the chess attribute using two interfaces. One is wireless RF interface, and the other is voice interface. We develop the user interface on the supervised computer for the Chinese chess game. The supervised computer can programs the motion paths of the mobile robots using evaluation algorithm according to the rule of Chinese chess game, and receive the status of mobile robots via wireless RF interface. The MBR-I calculates the movement displacement using the pulse numbers from reflect IR sensors of the encoder module. Then we develop the MBR-II robot to implement scenario of the Chinese chess game on the grid based chessboard platform. The MBR-II robot calculates the movement displacement using encoder of DC servomotor. The chess (mobile robot) uses A\* searching algorithm to program the shortest path moving to the target point. The supervised computer can controls mobile robots, and receives the status of mobile robots via wireless RF interface. Players can move the chess piece using the mouse on the supervised computer, and obey the game rules. The supervised computer can controls the mobile robot moving to the target position. The mobile robot speaks Chinese language for the movement status.

### **8. Acknowledgment**

This work was supported by the project "Development of an education robot", under National Science Council of Taiwan, (NSC 99-2622-E-224-012-CC3).

#### **9. References**


**0**

**11**

*U.S.A*

Jr. and Oscar Chuy<sup>2</sup>

*FAMU-FSU College of Engineering*

**Motion Planning for Mobile Robots Via**

<sup>1</sup>*Naval Surface Warfare Center - Panama City Division*

<sup>2</sup>*Center for Intelligent Systems, Control and Robotics (CISCOR)*

**Sampling-Based Model Predictive Optimization**

Path planning is a method that determines a path, consecutive states, between a start state and goal state, LaValle (2006). However, in motion planning that path must be parameterized by time to create a trajectory. Consequently, not only is the path determined, but the time the vehicle moves along the path. To be successful at motion planning, a vehicle model must be incorporated into the trajectory computation. The motivation in utilizing a vehicle model is to provide the opportunity to predict the vehicle's motion resulting from a variety of system inputs. The kinematic model enforces the vehicle kinematic constraints (i.e. turn radius, etc.), on the vehicle that limit the output space (state space). However, the kinematic model is limited because it does not take into account the forces acting on the vehicle. The dynamic model incorporates more useful information about the vehicle's motion than the kinematic model. It describes the feasible control inputs, velocities, acceleration and vehicle/terrain interaction phenomena. Motion planning that will require the vehicle to perform close to its limits (i.e. extreme terrains, frequent acceleration, etc.) will need the dynamic model. Examples of missions that would benefit from using a dynamic model in the planning are time optimal motion planning, energy efficient motion planning and planning in the presence

Sampling-based methods represent a type of model based motion planning algorithm. These methods incorporate the system model. There are current sampling-based planners that should be discussed: The Rapidly-Exploring Random Tree (RRT) Planner, Randomized *A* (*RA*) algorithm, and the Synergistic Combination of Layers of Planning (SyCLoP) multi-layered planning framework. The Rapidly-Exploring Random Tree Planner was one of the first single-query sampling based planners and serves as a foundation upon which many current algorithms are developed. The RRT Planner is very efficient and has been used in many applications including manipulator path planning, Kuffner & LaValle. (2000), and robot trajectory planning, LaValle & Kuffner (2001). However, the RRT Planner has the major drawback of lacking any sort of optimization other than a bias towards exploring the search space. The *RA* algorithm, which was designed based on the RRT Planner, addresses this drawback by combining the RRT Planner with an *A* algorithm. The SyCLoP framework is

**1. Introduction**

of faults, Yu et al. (2010).

Damion D. Dunlap1, Charmane V. Caldwell2, Emmanuel G. Collins2,


### **Motion Planning for Mobile Robots Via Sampling-Based Model Predictive Optimization**

Damion D. Dunlap1, Charmane V. Caldwell2, Emmanuel G. Collins2, Jr. and Oscar Chuy<sup>2</sup>

> <sup>1</sup>*Naval Surface Warfare Center - Panama City Division* <sup>2</sup>*Center for Intelligent Systems, Control and Robotics (CISCOR) FAMU-FSU College of Engineering U.S.A*

#### **1. Introduction**

210 Recent Advances in Mobile Robotics

J. Wang, Y. H. Luo, D. N. Qiu and X. H. Xu, "Adaptive Genetic Algorithm's Implement on

W. P. Lee, L. J. Liu and J. A. Chiou, "A Component-Based Framework to Rapidly Prototype

W. Zhou, J. L. Zhang and Y. Z. Wang, "The Iterative Sort Search Techniques Based on

Kuo-Lan Su, Sheng-Ven Shiau, Jr-Hom Guo and Chih-Wei Shiau, "Mobile Robot Based

C. Buiu and N. Popescu, Aesthetic emotions in human-robot interaction implications on

T. Song, X. Yan, A Liang and K. Chen, A distributed bidirectional auction algorithm for

R. Grabowski and L. Navarro-Serment, C. Paredis, and P. Khosla, Heterogeneous teams of

E. J. Chung, Y. S. Kwon, J. T. Seo, J. J. Jeon, andH. Y. Lee, Development of a multiple mobile

T. Balch, and R. Arkin, Behavior-based formation control for multirobot teams, IEEE Trans.

R. Alami, S. Fleury, M. Herrb, F. Ingrand, and F. Robert, Multi-robot cooperation in the

Y. Cao *et al*, Cooperative mobile robotics: antecedents and directions, Autonomous Robots,

J. H. Guo and K. L. Su, Ant system based multi-robot path planning, ICIC Express Letters,

W. Burgard, M. Moors *et al*, Coordinated Multi-Robot Exploration, IEEE Transaction on

K. L. Su, C. Y. Chung, Y. L. Liuao and J. H. Guo, A\* searching algorithm based path

Y. Saber and T. Senjyu (2007) Memory-bounded ant colony optimization with dynamic

planning of mobile robots, ICIC Express Letters, Part B: Applications, Vol. 2, No. 1,

programming and A\* local search for generator planning, IEEE Trans. on Power

on Robotics and Automation, Vol.14, No. 6, pp.926-939, 1998.

Part B: Applications, Vol. 2, No. 2, pp. 493-498, 2011.

Robotics, Vol. 21, No. 3, pp.376-386, 2005.

System, Vol.22, No. 4, pp.1965-1973

System, Man and Cybernetics, pp.4011-4016, 2006.

Computing, Information and Control, pp.63, 2009.

Computer Science, pp.145-148, 2009.

Information and Control, Vol.7, No. 3, pp.1097-1107, 2011.

pp.1206-1209.

pp.5263-5266.

pp.293-308, 2000.

4296, 2006.

47, 1998.

Vol.4, No. 1, pp.7-27, 1997.

pp. 273-278, 2011.

Evaluation Function in Computer Chinese Chess," Proceeding of ISCIT 2005,

Online Chess Game for Home Entertainment," IEEE International Conference on

Percentage Evaluation," Chinese Control and Decision Conference (CCDC 2008),

Onlin Chinese Chess Game," The Fourth International Conference on Innovative

interaction design of robotic artists, International Journal of Innovative Computing,

multi-robot coordination, IEEE International Conference on Research Challenges in

modular robots for mapping and exploration, Autonomous Robots, Vol.8, No. 3,

robotic system for team work, SICE-ICASE International Joint Conference, pp.4291-

MARTHA project, IEEE Robotics and Automation Magazine, Vol. 5, No. 1, pp.36-

Path planning is a method that determines a path, consecutive states, between a start state and goal state, LaValle (2006). However, in motion planning that path must be parameterized by time to create a trajectory. Consequently, not only is the path determined, but the time the vehicle moves along the path. To be successful at motion planning, a vehicle model must be incorporated into the trajectory computation. The motivation in utilizing a vehicle model is to provide the opportunity to predict the vehicle's motion resulting from a variety of system inputs. The kinematic model enforces the vehicle kinematic constraints (i.e. turn radius, etc.), on the vehicle that limit the output space (state space). However, the kinematic model is limited because it does not take into account the forces acting on the vehicle. The dynamic model incorporates more useful information about the vehicle's motion than the kinematic model. It describes the feasible control inputs, velocities, acceleration and vehicle/terrain interaction phenomena. Motion planning that will require the vehicle to perform close to its limits (i.e. extreme terrains, frequent acceleration, etc.) will need the dynamic model. Examples of missions that would benefit from using a dynamic model in the planning are time optimal motion planning, energy efficient motion planning and planning in the presence of faults, Yu et al. (2010).

Sampling-based methods represent a type of model based motion planning algorithm. These methods incorporate the system model. There are current sampling-based planners that should be discussed: The Rapidly-Exploring Random Tree (RRT) Planner, Randomized *A* (*RA*) algorithm, and the Synergistic Combination of Layers of Planning (SyCLoP) multi-layered planning framework. The Rapidly-Exploring Random Tree Planner was one of the first single-query sampling based planners and serves as a foundation upon which many current algorithms are developed. The RRT Planner is very efficient and has been used in many applications including manipulator path planning, Kuffner & LaValle. (2000), and robot trajectory planning, LaValle & Kuffner (2001). However, the RRT Planner has the major drawback of lacking any sort of optimization other than a bias towards exploring the search space. The *RA* algorithm, which was designed based on the RRT Planner, addresses this drawback by combining the RRT Planner with an *A* algorithm. The SyCLoP framework is

results of both an AUV and an unmanned ground vehicle (UGV) that perform steep hill climbing. An evaluation of SBMPO tuning parameters on the computation time and cost

Motion Planning for Mobile Robots Via Sampling-Based Model Predictive Optimization 213

This section provides the SBMPC Algorithm and the comparison of SBMPO to other traditional sampling based methods. However, first the variables used in the algorithm are described. The SBMPO algorithm and terms follow closely with Lifelong Planning *A*� (*LPA*�), Koenig et al. (2004). However, the variation is in the Generate Neighbor algorithm which generates the next state by integrating the system model and considering constraint violations. All the components of SBMPC are described in this Section, but the later simulation results in

SBMPC operates on a dynamic directed graph *G* which is a set of all nodes and edges currently in the graph. *SUCC*(*n*) represents the set of successors (children) of node *n* ∈ *G* while *PRED*(*n*) denotes the set of all predecessors (parents) of node *v* ∈ *G*. The cost of traversing

) is denoted by *c*(*n*�

optimization component is called Sampling Based Model Predictive Optimization and is an algorithm that determines the optimal cost (i.e. shortest path, shortest time, least energy, etc.)

The start distance of node *<sup>v</sup>* <sup>∈</sup> *<sup>G</sup>* is given by *<sup>g</sup>*�(*v*) which is the cost of the optimal path from the given start node *vstart* to the current node *v*. SBMPC maintains two estimates of *g*�(*v*). The first estimate *g*(*v*) is essentially the current cost from *vstart* to the node *v* while the

A node *v* is locally consistent iff *g*(*v*) = *rhs*(*v*) and locally inconsistent iff *g*(*v*) � *rhs*(*v*). If all nodes are locally consistent, then *g*(*v*) satisfies (1) for all *v* ∈ *G* and is therefore equal to the start distance. This enables the ability to trace the shortest path from *vstart* to any node *v* by

To facilitate fast re-planning, SBMPC does not make every node locally consistent after an edge cost change and instead uses a heuristic function *h*(*v*, *vgoal*) to focus the search so that it only updates *g*(*v*) for nodes necessary to obtain the optimal cost. The heuristic is used to approximate the goal distances and must follow the triangle inequality: *h*(*vgoal*, *vgoal*) = 0 and

heuristic function along with the start distance estimates to rank the priority queue containing the locally inconsistent nodes and thus all the nodes that need to be updated in order for them to be locally consistent. The priority of a node is determined by a two component key vector:

0, if *v* = *vstart*

) + *c*(*v*�

, *vgoal*) for all nodes *v* ∈ *G* and *v*� ∈ *SUCC*(*s*). SBMPO employs the

min(*g*(*v*),*rhs*(*v*))

min(*g*(*v*),*rhs*(*v*)) + *h*(*v*, *vgoal*)

, *n*), where 0 < *c*(*n*�

, *n*) < ∞. The

) for *v*� ∈ *PRED*(*v*)

, *v*) until *vstart* is

(2)

, *<sup>v</sup>*)), otherwise. (1)

) + *c*(*v*�

is presented in Section 5. Finally, Section 6 concludes and presents future work.

Section 3 and 4 utilize only the SBMPO and Generate Neighbors algorithms.

second estimate, *rhs*(*v*), is a one-step lookahead estimate based on *g*(*v*�

starting at *v* and traversing to any predecessor *v*� that minimizes *g*(*v*�

and provides more information than the estimate *g*(*v*). The *rhs*(*v*) value satisfies

min*v*�∈*PRED*(*v*)(*g*(*v*�

**2. SBMPC algorithm**

**2.1 SBMPC variables**

reached.

*h*(*v*, *vgoal*) ≤ *c*(*v*, *v*�

from node *n*� to node *n* ∈ *SUCC*(*n*�

from a start node *nstart* ∈ *G* to a goal node *ngoal* ∈ *G*.

*rhs*(*v*) =

) + *h*(*v*�

*key*(*v*) = *<sup>k</sup>*1(*v*)

*k*2(*v*)

 =

presented because it not only represents a very current sampling-based planning approach, but the framework is also one of the few algorithms to directly sample the control inputs. Originally, this research began by applying nonlinear model predictive control (NMPC), implemented with sequential programming, to generate a path for an autonomous underwater vehicle (AUV), Caldwell et al. (2006). As depicted in Fig. 1, NMPC was attractive because it is an online optimal control method that incorporates the system model, optimizes a cost function and includes current and future constraints all in the design process. These benefits made planning with NMPC promising, but there were weaknesses of NMPC that had to be addressed. Since MPC must solve the optimization problem online in real-time, the method was limited to slow systems. Additionally, even though models were used in the design process, linear models where typically used in order to avoid the local minima problem that accompany the use of nonlinear models. In order to exploit the benefits of MPC these issues had to be addressed.

Fig. 1. The stages of the MPC algorithm.

Since the robotics and AI communities had the same goal for planning but have different approaches that tend to yield computationally efficient algorithms, it was decided to integrate these various concepts to produce a new enhanced planner called Sampling Based Model Predictive Control (SBMPC). The concept behind SBMPC was first presented in Dunlap et al. (2008). Instead of utilizing traditional numerical methods in the NMPC optimization phase in Fig. 1, Sampling Based Model Predictive Optimization (SBMPO) uses *A* type optimization from the AI community. This type of graph search algorithm results in paths that do not become stuck in local minima. In addition, the idea of using sampling to consider only a finite number of solutions comes from robotic motion planning community. Sampling is the mechanism used to trade performance for computational efficiency. Instead of sampling in the output space as traditional sampling based planning methods, SBMPC follows the view of traditional MPC and SyCLoP, which samples the input space. Thus, SBMPC draws from the control theory, robotics and AI communities.

Section 2 of this chapter will present the novel SBMPC algorithm in detail and compare Sampling Based Model Predictive Optimization and traditional Sampling based methods. Section 3 provides simulation results utilized on an AUV kinematic model. Section 4 presents results of both an AUV and an unmanned ground vehicle (UGV) that perform steep hill climbing. An evaluation of SBMPO tuning parameters on the computation time and cost is presented in Section 5. Finally, Section 6 concludes and presents future work.

#### **2. SBMPC algorithm**

2 Will-be-set-by-IN-TECH

presented because it not only represents a very current sampling-based planning approach, but the framework is also one of the few algorithms to directly sample the control inputs. Originally, this research began by applying nonlinear model predictive control (NMPC), implemented with sequential programming, to generate a path for an autonomous underwater vehicle (AUV), Caldwell et al. (2006). As depicted in Fig. 1, NMPC was attractive because it is an online optimal control method that incorporates the system model, optimizes a cost function and includes current and future constraints all in the design process. These benefits made planning with NMPC promising, but there were weaknesses of NMPC that had to be addressed. Since MPC must solve the optimization problem online in real-time, the method was limited to slow systems. Additionally, even though models were used in the design process, linear models where typically used in order to avoid the local minima problem that accompany the use of nonlinear models. In order to exploit the benefits of MPC

Since the robotics and AI communities had the same goal for planning but have different approaches that tend to yield computationally efficient algorithms, it was decided to integrate these various concepts to produce a new enhanced planner called Sampling Based Model Predictive Control (SBMPC). The concept behind SBMPC was first presented in Dunlap et al. (2008). Instead of utilizing traditional numerical methods in the NMPC optimization phase in Fig. 1, Sampling Based Model Predictive Optimization (SBMPO) uses *A* type optimization from the AI community. This type of graph search algorithm results in paths that do not become stuck in local minima. In addition, the idea of using sampling to consider only a finite number of solutions comes from robotic motion planning community. Sampling is the mechanism used to trade performance for computational efficiency. Instead of sampling in the output space as traditional sampling based planning methods, SBMPC follows the view of traditional MPC and SyCLoP, which samples the input space. Thus, SBMPC draws from

Section 2 of this chapter will present the novel SBMPC algorithm in detail and compare Sampling Based Model Predictive Optimization and traditional Sampling based methods. Section 3 provides simulation results utilized on an AUV kinematic model. Section 4 presents

these issues had to be addressed.

Fig. 1. The stages of the MPC algorithm.

the control theory, robotics and AI communities.

This section provides the SBMPC Algorithm and the comparison of SBMPO to other traditional sampling based methods. However, first the variables used in the algorithm are described. The SBMPO algorithm and terms follow closely with Lifelong Planning *A*� (*LPA*�), Koenig et al. (2004). However, the variation is in the Generate Neighbor algorithm which generates the next state by integrating the system model and considering constraint violations. All the components of SBMPC are described in this Section, but the later simulation results in Section 3 and 4 utilize only the SBMPO and Generate Neighbors algorithms.

#### **2.1 SBMPC variables**

SBMPC operates on a dynamic directed graph *G* which is a set of all nodes and edges currently in the graph. *SUCC*(*n*) represents the set of successors (children) of node *n* ∈ *G* while *PRED*(*n*) denotes the set of all predecessors (parents) of node *v* ∈ *G*. The cost of traversing from node *n*� to node *n* ∈ *SUCC*(*n*� ) is denoted by *c*(*n*� , *n*), where 0 < *c*(*n*� , *n*) < ∞. The optimization component is called Sampling Based Model Predictive Optimization and is an algorithm that determines the optimal cost (i.e. shortest path, shortest time, least energy, etc.) from a start node *nstart* ∈ *G* to a goal node *ngoal* ∈ *G*.

The start distance of node *<sup>v</sup>* <sup>∈</sup> *<sup>G</sup>* is given by *<sup>g</sup>*�(*v*) which is the cost of the optimal path from the given start node *vstart* to the current node *v*. SBMPC maintains two estimates of *g*�(*v*). The first estimate *g*(*v*) is essentially the current cost from *vstart* to the node *v* while the second estimate, *rhs*(*v*), is a one-step lookahead estimate based on *g*(*v*� ) for *v*� ∈ *PRED*(*v*) and provides more information than the estimate *g*(*v*). The *rhs*(*v*) value satisfies

$$rhs(v) = \begin{cases} 0, & \text{if } v = v\_{start} \\ \min\_{v' \in PRED(v)} (g(v') + c(v', v)), & \text{otherwise.} \end{cases} \tag{1}$$

A node *v* is locally consistent iff *g*(*v*) = *rhs*(*v*) and locally inconsistent iff *g*(*v*) � *rhs*(*v*). If all nodes are locally consistent, then *g*(*v*) satisfies (1) for all *v* ∈ *G* and is therefore equal to the start distance. This enables the ability to trace the shortest path from *vstart* to any node *v* by starting at *v* and traversing to any predecessor *v*� that minimizes *g*(*v*� ) + *c*(*v*� , *v*) until *vstart* is reached.

To facilitate fast re-planning, SBMPC does not make every node locally consistent after an edge cost change and instead uses a heuristic function *h*(*v*, *vgoal*) to focus the search so that it only updates *g*(*v*) for nodes necessary to obtain the optimal cost. The heuristic is used to approximate the goal distances and must follow the triangle inequality: *h*(*vgoal*, *vgoal*) = 0 and *h*(*v*, *vgoal*) ≤ *c*(*v*, *v*� ) + *h*(*v*� , *vgoal*) for all nodes *v* ∈ *G* and *v*� ∈ *SUCC*(*s*). SBMPO employs the heuristic function along with the start distance estimates to rank the priority queue containing the locally inconsistent nodes and thus all the nodes that need to be updated in order for them to be locally consistent. The priority of a node is determined by a two component key vector:

$$key(v) = \begin{pmatrix} k\_1(v) \\ k\_2(v) \end{pmatrix} = \begin{pmatrix} \min(g(v), rhs(v)) + h(v, v\_{goal}) \\ \min(g(v), rhs(v)) \end{pmatrix} \tag{2}$$

**Algorithm 2** SBMPO ( )

8: **end for** 9: **else**

13: **end for** 14: **end if** 15: **end while**

10: *vbest*.*g* = ∞

1: **for** *i* = 0 to **B do**

6: **Break** 7: **end if** 8: **end for** 9: *xnew* = *x*(*t*2)

15: **end if** 16: **end for**

2: *vbest* ⇐ *PRIORITY*.*Top*() 3: Generate\_Neighbors ( *vbest*, **B**) 4: **if** *vbest*.*g* > *vbest*.*rhs* **then** 5: *vbest*.*g* = *vbest*.*rhs*

6: **for all** *v* ∈ *SUCCvbest* **do** 7: Update the node, *v*

12: Update the node, *v*

3: **for** *t* = *t*<sup>1</sup> : *dtinteg* : *t*<sup>2</sup> **do**

5: **if** *x*(*t*) �∈ **X***f ree*(*t*) **then**

12: **else if** *xnew* ∈ **X***f ree* **then**

11: **for all** *v* ∈ *SUCC*(*vbest*) ∪ *vbest* **do**

2: Generate sampled input, *<sup>u</sup>* <sup>∈</sup> **<sup>R</sup>***<sup>u</sup>* <sup>∩</sup> **<sup>U</sup>***f ree*

10: **if** *xnew* ∈ *STATE*\_*GRID* and *xnew* ∈ **X***f ree* **then**

performance. It is not possible to cover every variant, but the purpose of this section is to put in perspective how SBMPO is a variant of the traditional sampling-based method.

Examples of traditional sampling based motion planning algorithms include RRTs, LaValle (1998), and probability roadmaps,. A common feature of each of these algorithms is they work in the output space of the robot and employ various strategies for generating samples (i.e., random or pseudo-random points). In essence, as shown in Fig. 2, sampling based motion planning methods work by using sampling to construct a tree that connects the root

Most online sampling based planning algorithms follow this general framework:

4: Evaluate model: *x*(*t*) = *f*(*v*.*x*, *u*)

11: Add *Edge*(*v*.*x*, *xnew*) to graph, **G**

**2.3.1 Traditional sampling based methods**

(initial state) with a goal region.

13: Add *Vertex*(*xnew*) to graph, **G** 14: Add *Edge*(*v*.*x*, *xnew*) to graph, **G**

**Algorithm 3** Generate\_Neighbors ( Vertex v, Branching **B** )

1: **while** *PRIORITY*.*TopKey*() < *vgoal*.*key* � *vgoal*.*rhs* �= *vgoal*.*g* **do**

Motion Planning for Mobile Robots Via Sampling-Based Model Predictive Optimization 215

where the keys are ordered lexicographically with the smaller key values having a higher priority.

#### **2.2 SBMPC algorithm**

The SBMPC algorithm is comprised of three primary methods: Sampling Based Model Predictive Control, Sampling Based Model Predictive Optimization and Generate Neighbor. The main SBMPC algorithm follows the general structure of MPC where SBMPC repeatedly computes the optimal path between the current state *xcurrent* and the goal state *xgoal*. After a single path is generated, *xcurrent* is updated to reflect the implementation of the first control input and the graph *G* is updated to reflect any system changes. These steps are repeated until the goal state is reached.

The second algorithm SBMPO is the optimization phase of SBMPC that provides the prediction paths. SBMPO repeatedly generates the neighbors of locally inconsistent nodes until *vgoal* is locally consistent or the key of the next node in the priority que is not smaller than *key*(*vgoal*). This follows closely with the ComputeShortestPath algorithm of *LPA*� Koenig et al. (2004). The node, *vbest*, with the highest priority (lowest key value) is on top of the priority que. The algorithm then deals with two potential cases based on the consistency of the expanded node *vbest*. If the node is locally overconsistent, *g*(*v*) > *rhs*(*v*), the *g*-value is set to *rhs*(*v*) making the node locally consistent. The successors of *v* are then updated. The update node process includes recalculating *rhs*(*v*) and key values, checking for local consistency and either adding or removing the node from the priority que accordingly. For the case when the node is locally underconsistent, *g*(*v*) < *rhs*(*v*), the *g*-value is set to ∞ making the node either locally consistent or overconsistent. This change can affect the node along with its successors which then go through the node update process.

The Generate Neighbor algorithm determines the successor nodes of the current node. In the input space, a set of quasi-random samples are generated that are then used with a model of the system to predict a set of paths to a new set of outputs (nodes) with *xcurrent* being the initial condition. The branching factor *B* (sampling number) determines the number of paths that will be generated and new successor nodes. The path is represented by a sequence of states *x*(*t*) for *t* = *t*1, *t*<sup>1</sup> + Δ*t*, ··· , *t*2, where Δ*t* is the model step size. The set of states that do not violate any state or obstacle constraints is called **X***f ree*. If *x*(*t*) ∈ **X***f ree*, then the new neighbor node *xnew* and the connecting edge can be added to the directed graph, G. If *xnew* ∈ *STATE*\_*GRID*, then the node currently exists in the graph and only the new path to get to the existing node needs to be added.

**Algorithm 1** Sampling Based Model Predictive Control

```
1: xcurrent ⇐ start
2: repeat
3: SBMPO ( )
4: Update system state, xcurrent
5: Update graph, G
```
6: **until** the goal state is achieved

#### **2.3 Comparison of SBMPO and traditional sampling based methods**

This section discusses the conceptual comparison between SBMPO and traditional Sampling-based methods. Similar to many other planning methods, there have been many variants of the sampling based methods that seek to improve various aspects of their

#### **Algorithm 2** SBMPO ( )

4 Will-be-set-by-IN-TECH

where the keys are ordered lexicographically with the smaller key values having a higher

The SBMPC algorithm is comprised of three primary methods: Sampling Based Model Predictive Control, Sampling Based Model Predictive Optimization and Generate Neighbor. The main SBMPC algorithm follows the general structure of MPC where SBMPC repeatedly computes the optimal path between the current state *xcurrent* and the goal state *xgoal*. After a single path is generated, *xcurrent* is updated to reflect the implementation of the first control input and the graph *G* is updated to reflect any system changes. These steps are repeated until

The second algorithm SBMPO is the optimization phase of SBMPC that provides the prediction paths. SBMPO repeatedly generates the neighbors of locally inconsistent nodes until *vgoal* is locally consistent or the key of the next node in the priority que is not smaller than *key*(*vgoal*). This follows closely with the ComputeShortestPath algorithm of *LPA*� Koenig et al. (2004). The node, *vbest*, with the highest priority (lowest key value) is on top of the priority que. The algorithm then deals with two potential cases based on the consistency of the expanded node *vbest*. If the node is locally overconsistent, *g*(*v*) > *rhs*(*v*), the *g*-value is set to *rhs*(*v*) making the node locally consistent. The successors of *v* are then updated. The update node process includes recalculating *rhs*(*v*) and key values, checking for local consistency and either adding or removing the node from the priority que accordingly. For the case when the node is locally underconsistent, *g*(*v*) < *rhs*(*v*), the *g*-value is set to ∞ making the node either locally consistent or overconsistent. This change can affect the node along with its successors which

The Generate Neighbor algorithm determines the successor nodes of the current node. In the input space, a set of quasi-random samples are generated that are then used with a model of the system to predict a set of paths to a new set of outputs (nodes) with *xcurrent* being the initial condition. The branching factor *B* (sampling number) determines the number of paths that will be generated and new successor nodes. The path is represented by a sequence of states *x*(*t*) for *t* = *t*1, *t*<sup>1</sup> + Δ*t*, ··· , *t*2, where Δ*t* is the model step size. The set of states that do not violate any state or obstacle constraints is called **X***f ree*. If *x*(*t*) ∈ **X***f ree*, then the new neighbor node *xnew* and the connecting edge can be added to the directed graph, G. If *xnew* ∈ *STATE*\_*GRID*, then the node currently exists in the graph and only the new path to

This section discusses the conceptual comparison between SBMPO and traditional Sampling-based methods. Similar to many other planning methods, there have been many variants of the sampling based methods that seek to improve various aspects of their

priority.

**2.2 SBMPC algorithm**

the goal state is reached.

then go through the node update process.

get to the existing node needs to be added.

4: Update system state, *xcurrent*

6: **until** the goal state is achieved

1: *xcurrent* ⇐ **start**

5: Update graph, **G**

2: **repeat** 3: SBMPO ( )

**Algorithm 1** Sampling Based Model Predictive Control

**2.3 Comparison of SBMPO and traditional sampling based methods**

```
1: while PRIORITY.TopKey() < vgoal.key � vgoal.rhs �= vgoal.g do
2: vbest ⇐ PRIORITY.Top()
3: Generate_Neighbors ( vbest, B)
4: if vbest.g > vbest.rhs then
5: vbest.g = vbest.rhs
6: for all v ∈ SUCCvbest do
7: Update the node, v
8: end for
9: else
10: vbest.g = ∞
11: for all v ∈ SUCC(vbest) ∪ vbest do
12: Update the node, v
13: end for
14: end if
15: end while
```
**Algorithm 3** Generate\_Neighbors ( Vertex v, Branching **B** )

1: **for** *i* = 0 to **B do** 2: Generate sampled input, *<sup>u</sup>* <sup>∈</sup> **<sup>R</sup>***<sup>u</sup>* <sup>∩</sup> **<sup>U</sup>***f ree* 3: **for** *t* = *t*<sup>1</sup> : *dtinteg* : *t*<sup>2</sup> **do** 4: Evaluate model: *x*(*t*) = *f*(*v*.*x*, *u*) 5: **if** *x*(*t*) �∈ **X***f ree*(*t*) **then** 6: **Break** 7: **end if** 8: **end for** 9: *xnew* = *x*(*t*2) 10: **if** *xnew* ∈ *STATE*\_*GRID* and *xnew* ∈ **X***f ree* **then** 11: Add *Edge*(*v*.*x*, *xnew*) to graph, **G** 12: **else if** *xnew* ∈ **X***f ree* **then** 13: Add *Vertex*(*xnew*) to graph, **G** 14: Add *Edge*(*v*.*x*, *xnew*) to graph, **G** 15: **end if** 16: **end for**

performance. It is not possible to cover every variant, but the purpose of this section is to put in perspective how SBMPO is a variant of the traditional sampling-based method.

#### **2.3.1 Traditional sampling based methods**

Examples of traditional sampling based motion planning algorithms include RRTs, LaValle (1998), and probability roadmaps,. A common feature of each of these algorithms is they work in the output space of the robot and employ various strategies for generating samples (i.e., random or pseudo-random points). In essence, as shown in Fig. 2, sampling based motion planning methods work by using sampling to construct a tree that connects the root (initial state) with a goal region.

Most online sampling based planning algorithms follow this general framework:

theoretical assurances that if the sampling is dense enough, the sampling algorithm will find

Motion Planning for Mobile Robots Via Sampling-Based Model Predictive Optimization 217

Since SBMPO is the outgrowth of both MPC and graph search algorithms, there are some

There are two primary disadvantages to using output (i.e., configuration space) sampling as is commonly done in traditional sampling based methods. The first limitation lies within the VSM, where the algorithm must determine the most ideal node to expand. This selection is typically made based on the proximity of nodes in the graph to a sampled output node and involves a potentially costly nearest neighbor search. The LPM presents the second and perhaps more troublesome problem, which is determining an input that connects a newly sampled node to the current node. This problem is essentially a two-point boundary value problem (BVP) that connects one output or state to another. There is no guarantee that such an input exists. Also, for systems with complex dynamics, the search itself can be computationally expensive, which leads to a computationally inefficient planner. A solution to the problem is to introduce input sampling. The concept of input sampling is not new and has been integrated into methods like the SyCLoP algorithm, Plaku et al. (2010). When the input space is sampled as proposed in this chapter, the need for a nearest-neighbor search is eliminated, and the LPM is reduced to the integration of a system model, and therefore, only generates outputs that are achievable by the system. Sampling the control inputs directly also prevents the need to determine where to connect new samples to the current graph and

In order to visualize this concept, consider an Ackerman steered vehicle at rest that has position (*x*, *y*) and orientation *θ*, which are the outputs of the kinematic model. The model restricts the attainable outputs. All the dots in Fig. 3 are output nodes obtained from sampling the output space even though only the dots on the mesh surface can physically be obtained by the vehicle. There are a larger number of dots (sampled outputs) in the output space that do not lie in the achievable region (mesh surface). This means those sampled outputs are not physically possible, so traditional sample based methods would have to start the search over. This leads to an inefficient search that can substantially increase the computational time of the planner. The intersection of the grid lines in Fig. 3 correspond to the points in output space generated by a uniform sampling of the model inputs, the left and right wheel velocities. In essence, sampling in the input space leads to more efficient results since each of

Although input sampling avoids two of the primary computational bottle-necks of sampling-based motion planning, there is also a downside of input sampling. Input sampling has not been used in most planning research, because it is seen as being inefficient. This type of sampling can result in highly dense samples in the output space since input sampling does not inherently lead to a uniformly discretized output space, such as a uniform grid. This problem is especially evident when encountering a local minimum problem associated with the *A* algorithm, which can occur when planning in the presence of a large concave obstacle while the goal is on the other side of the obstacle. This situation is considered in depth for discretized 2D path planning in the work of Likhachev & Stentz (2008), which discusses that

a solution when it exists (i.e. it has some type of completeness).

therefore avoid costly nearest-neighbor searches.

the corresponding dots in the output space is allowed by the model.

2.3.3.1 Input sampling

2.3.3.2 Implicit state grid

**2.3.3 Differences of SBMPO and traditional sampling based methods**

fundamental differences in SBMPO and traditional sampling based methods.

Fig. 2. A tree that connects the root with a goal region.


The model is part of the local planning method (LPM), which determines the connection between the newly generated sample and the existing graph. Essentially, it is a two-point value boundary problem.

#### **2.3.2 Similarities of SBMPO and traditional sampling based methods**

There are some similarities that both SBMPO and traditional sampling methods share.

#### 2.3.2.1 Sampling

As its name implies, SBMPC is dependent upon the concept of sampling, which has arisen as one of the major paradigms for robotic motion planning community, LaValle (2006). Sampling is the mechanism used to trade performance for computational efficiency. SBMPO employs quasi-random samples of the input space. Properly designed sampling algorithms provide theoretical assurances that if the sampling is dense enough, the sampling algorithm will find a solution when it exists (i.e. it has some type of completeness).

#### **2.3.3 Differences of SBMPO and traditional sampling based methods**

Since SBMPO is the outgrowth of both MPC and graph search algorithms, there are some fundamental differences in SBMPO and traditional sampling based methods.

#### 2.3.3.1 Input sampling

6 Will-be-set-by-IN-TECH

1. **Initialize:** Let *G*(*V*; *E*) represent a search graph where *V* contains at least one vertex (i.e.,

3. **Local Planning Method (LPM):** For some *unew* ∈ *Cf ree* (free states in the configuration space) and attempt to generate a path *τ<sup>s</sup>* : [0, 1] →: *τ*(0) = *u* and *τ*(1) = *unew*. The path must be checked to ensure that no constraints are violated. If the LPM fails, then go back

4. **Insert an Edge in the Graph:** Insert *τ<sup>s</sup>* into *E*, as an edge from *u* to *unew*. Insert *unew* into *V*

6. **Return to Step 2:** Repeat unless a solution has been found or a failure condition has been

The model is part of the local planning method (LPM), which determines the connection between the newly generated sample and the existing graph. Essentially, it is a two-point

As its name implies, SBMPC is dependent upon the concept of sampling, which has arisen as one of the major paradigms for robotic motion planning community, LaValle (2006). Sampling is the mechanism used to trade performance for computational efficiency. SBMPO employs quasi-random samples of the input space. Properly designed sampling algorithms provide

There are some similarities that both SBMPO and traditional sampling methods share.

Fig. 2. A tree that connects the root with a goal region.

5. **Check for a Solution:** Check *G* for a solution path.

to Step 2.

met.

2.3.2.1 Sampling

if it does not already exist.

value boundary problem.

node), typically the start vertex and *E* does not contain any edges. 2. **Vertex Selection Method (VSM):** Select a vertex *u* in *V* for expansion.

**2.3.2 Similarities of SBMPO and traditional sampling based methods**

There are two primary disadvantages to using output (i.e., configuration space) sampling as is commonly done in traditional sampling based methods. The first limitation lies within the VSM, where the algorithm must determine the most ideal node to expand. This selection is typically made based on the proximity of nodes in the graph to a sampled output node and involves a potentially costly nearest neighbor search. The LPM presents the second and perhaps more troublesome problem, which is determining an input that connects a newly sampled node to the current node. This problem is essentially a two-point boundary value problem (BVP) that connects one output or state to another. There is no guarantee that such an input exists. Also, for systems with complex dynamics, the search itself can be computationally expensive, which leads to a computationally inefficient planner. A solution to the problem is to introduce input sampling. The concept of input sampling is not new and has been integrated into methods like the SyCLoP algorithm, Plaku et al. (2010). When the input space is sampled as proposed in this chapter, the need for a nearest-neighbor search is eliminated, and the LPM is reduced to the integration of a system model, and therefore, only generates outputs that are achievable by the system. Sampling the control inputs directly also prevents the need to determine where to connect new samples to the current graph and therefore avoid costly nearest-neighbor searches.

In order to visualize this concept, consider an Ackerman steered vehicle at rest that has position (*x*, *y*) and orientation *θ*, which are the outputs of the kinematic model. The model restricts the attainable outputs. All the dots in Fig. 3 are output nodes obtained from sampling the output space even though only the dots on the mesh surface can physically be obtained by the vehicle. There are a larger number of dots (sampled outputs) in the output space that do not lie in the achievable region (mesh surface). This means those sampled outputs are not physically possible, so traditional sample based methods would have to start the search over. This leads to an inefficient search that can substantially increase the computational time of the planner. The intersection of the grid lines in Fig. 3 correspond to the points in output space generated by a uniform sampling of the model inputs, the left and right wheel velocities. In essence, sampling in the input space leads to more efficient results since each of the corresponding dots in the output space is allowed by the model.

#### 2.3.3.2 Implicit state grid

Although input sampling avoids two of the primary computational bottle-necks of sampling-based motion planning, there is also a downside of input sampling. Input sampling has not been used in most planning research, because it is seen as being inefficient. This type of sampling can result in highly dense samples in the output space since input sampling does not inherently lead to a uniformly discretized output space, such as a uniform grid. This problem is especially evident when encountering a local minimum problem associated with the *A* algorithm, which can occur when planning in the presence of a large concave obstacle while the goal is on the other side of the obstacle. This situation is considered in depth for discretized 2D path planning in the work of Likhachev & Stentz (2008), which discusses that

algorithms utilize Bellman's optimality principle to improve the path to a particular output by updating the paths through that output when a lower cost alternative is found. This feature is essential to the proper functioning of the algorithm and requires a mechanism to identify when outputs (states) are close enough to be considered the same. The scenario presented in Fig. 5 is a situation for which the lack of this mechanism would generate an inefficient path. In this situation, node *v*<sup>1</sup> is selected for expansion after which the lowest cost node is *v*3. The *implicit state grid* then recognizes that *v*<sup>2</sup> and *v*<sup>3</sup> are close enough to be considered the same

Motion Planning for Mobile Robots Via Sampling-Based Model Predictive Optimization 219

c b

The concept of an *implicit state grid*, Ericson (2005), is introduced as a solution to both of the challenges generated by input sampling. The implicit grid ensures that the graph generated by the SBMPC algorithm is constructed such that only one active output (state) exists in each grid cell, limiting the number of nodes that can exist within any finite region of the output space. In essence, the *implicit state grid* provides a discretized output space. It also allows for the efficient storage of potentially infinite grids by only storing the grid cells that contain nodes, which is increasingly important for higher dimensional problems, Ericson (2005). The resolution of the grid is a significant factor in determining the performance of the algorithm with more fine grids in general requiring more computation time, due to the increased number of outputs, with the benefit being a more optimal solution. Therefore, the grid resolution is a useful tuning tool that enables SBMPC to effectively make the trade off between solution

There is a class of discrete optimization techniques that have their origin in graph theory and have been further developed in the path planning literature. In this study these techniques will be called *goal-directed optimization* and refer to graph search algorithms such as Dijkstra's algorithm and the *A*�, *D*�, and *LPA*� algorithms Koenig et al. (2004); LaValle (2006). Given a graph, these algorithms find a path that optimizes some cost of moving from a start node to

v2 v3

a Goal

and updates the path to their grid cell to be path *c* since *c* < *a* + *b*.

start v1

Fig. 5. Illustration of the necessity of an implicit state grid.

quality and computational performance.

2.3.3.3 Goal directed optimization

Fig. 3. Illustration of the potential inefficiency of sampling in the output space.

the *A* algorithm must explore all the states in the neighborhood of the local minimum, shown as the shaded region of Fig. 4, before progressing to the final solution. The issue that this presents to input sampling methods is that the number of states within the local minimum is infinite because of the lack of a discretized output space.

Fig. 4. Illustration of the necessity of an implicit state grid.

The second challenge resulting from the nature of input sampling as well as the lack of a grid is that the likelihood of two outputs (states) being identical is extremely small. All *A*-like 8 Will-be-set-by-IN-TECH

Fig. 3. Illustration of the potential inefficiency of sampling in the output space.

infinite because of the lack of a discretized output space.

Fig. 4. Illustration of the necessity of an implicit state grid.

the *A* algorithm must explore all the states in the neighborhood of the local minimum, shown as the shaded region of Fig. 4, before progressing to the final solution. The issue that this presents to input sampling methods is that the number of states within the local minimum is

The second challenge resulting from the nature of input sampling as well as the lack of a grid is that the likelihood of two outputs (states) being identical is extremely small. All *A*-like

Goal

algorithms utilize Bellman's optimality principle to improve the path to a particular output by updating the paths through that output when a lower cost alternative is found. This feature is essential to the proper functioning of the algorithm and requires a mechanism to identify when outputs (states) are close enough to be considered the same. The scenario presented in Fig. 5 is a situation for which the lack of this mechanism would generate an inefficient path. In this situation, node *v*<sup>1</sup> is selected for expansion after which the lowest cost node is *v*3. The *implicit state grid* then recognizes that *v*<sup>2</sup> and *v*<sup>3</sup> are close enough to be considered the same and updates the path to their grid cell to be path *c* since *c* < *a* + *b*.

Fig. 5. Illustration of the necessity of an implicit state grid.

The concept of an *implicit state grid*, Ericson (2005), is introduced as a solution to both of the challenges generated by input sampling. The implicit grid ensures that the graph generated by the SBMPC algorithm is constructed such that only one active output (state) exists in each grid cell, limiting the number of nodes that can exist within any finite region of the output space. In essence, the *implicit state grid* provides a discretized output space. It also allows for the efficient storage of potentially infinite grids by only storing the grid cells that contain nodes, which is increasingly important for higher dimensional problems, Ericson (2005). The resolution of the grid is a significant factor in determining the performance of the algorithm with more fine grids in general requiring more computation time, due to the increased number of outputs, with the benefit being a more optimal solution. Therefore, the grid resolution is a useful tuning tool that enables SBMPC to effectively make the trade off between solution quality and computational performance.

#### 2.3.3.3 Goal directed optimization

There is a class of discrete optimization techniques that have their origin in graph theory and have been further developed in the path planning literature. In this study these techniques will be called *goal-directed optimization* and refer to graph search algorithms such as Dijkstra's algorithm and the *A*�, *D*�, and *LPA*� algorithms Koenig et al. (2004); LaValle (2006). Given a graph, these algorithms find a path that optimizes some cost of moving from a start node to

Inputs min max States min max u 0 m/s 2 m/s x -5 m 30 m v -0.1 m/s 0.1 m/s y -5 m 30 m w -0.1 m/s 0.1 m/s z -20 m 0 m p −5◦/*s* 5◦/*s φ* −15◦ 15◦ q −5◦/*s* 5◦/*s θ* −15◦ 15◦ r −15◦/*s* 15◦/*s ψ* −360◦ 360◦

Motion Planning for Mobile Robots Via Sampling-Based Model Predictive Optimization 221

can be presented with that has a set of concave obstacles in front of the goal. Note that whenever a vehicle is behind an obstacle or group of obstacles and has to increase its distance

The simulations were run with a sampling number of 25 and grid resolution of 0.1m. The vehicle has a start posture of (5*m*, 0*m*, −10*m*, 0◦) and a goal position of (5*m*, 10*m*, −10*m*). As shown in Fig. 6, SBMPO does not get stuck behind the obstacles, but successfully determines a trajectory in 0.59s. The successful traversal is largely due to the optimization method used in SBMPO. The goal-directed optimization allows a more promising node (lower cost) to replace a higher cost node as shown in the the example in Fig 7. Goal-directed optimization can accomplish this because they compute each predicted control input separately and backs up when needed as illustrated by the iterations corresponding to the 3rd, 4th, and 5th arrays. This feature enables it to avoid local minima. It converges to the optimal predicted control sequence {*u*∗(*k*)} (denoted by the right-most array), which is the optimal solution subject to the sampling, whereas a nonlinear programming method may get stuck at a local minimum. In addition, these results show that SBMPO's implementation of the implicit state grid helps prevent the issues with input sampling discussed in Section 2.3.3.2. Since the implicit grid is applied, it does not require significant time to explore the area around the concave obstacles.

In Section 3.1, there was one local minimum in the scenario. However, some underwater environments will require the AUV to navigate around multiple cluttered obstacles. This can produce a more complex situation because now there are multiple local minima . The simulations in this section assume there were random start, goal and obstacle locations in order to represent 100 different multiple obstacle underwater environment configurations. The start locations *X*,*Y*, *Z* and *ψ* were chosen randomly in the respective ranges [0 20]*m*, [0 1]*m*, [−12 − 8]*m*, [30◦ 150◦], and the goal was chosen randomly in the respective ranges [0 20]*m*, [19 20]*m*, [−12 − 8]*m*. In addition, there were 40 randomly generated obstacles. The 100 simulation runs had a sampling number of 25. Fig. 8 exemplifies one random scenarios generated. In the scenarios SBMPO was capable of allowing the AUV to maneuver in the

For a vehicle to be truly autonomous, it must be capable of determining a trajectory that will allow it to successfully reach the goal without colliding with an obstacle. In these simulations SBMPO was 100% successful in assuring the vehicle accomplished this task. It is important to consider both SBMPO's mean computation time of 0.43*s* and median computation time of 0.15*s* to compute these trajectories. Since the scenarios generated were random, there were a few scenarios created that were more cluttered which caused a larger CPU time. This is the

reason for the discrepancy between the mean and median computation times.

Table 1. Simulation Constraints for the 3D Kinematic Model

**3.2 AUV cluttered multiple obstacles**

cluttered environment successfully reaching the goal.

from the goal to achieve the goal, it is in a local minimum position.

some given goal. In contrast to discrete optimization algorithms such as branch-and-bound optimization Nocedal & Wright (1999), which "relaxes" continuous optimization problems, the goal-directed optimization methods are inherently discrete, and have often been used for real-time path planning.

Generally, sampling based methods such as RRTs do not incorporate any optimization and terminate when an initial feasible solution is determined. In essence, instead of determining an optimal trajectory, traditional sampling based methods only attempt to find feasible trajectories. To remedy these problems, the Randomized *A* (*RA*) algorithm was introduced in Diankov & Kuffner. (2007), as a hybrid between RRTs and the *A* search algorithm. Similar to *RA*, SBMPO incorporates a goal directed optimization to ensure the trajectory is optimal subject to the sampling.

Although not commonly recognized, goal-directed optimization approaches are capable of solving control theory problems for which the ultimate objective is to plan an optimal trajectory and control inputs to reach a goal (or set point) while optimizing a cost function. Hence, graph search algorithms can be applied to terminal constraint optimization problems and set point control problems. To observe this, consider the tree graph of Fig. 2. Each node of this tree can correspond to a system state, and the entire tree may be generated by integrating sampled inputs to a system model. Assume that the cost of a trajectory is given by the sum of the cost of the corresponding edges (i.e., branches), where the cost of each edge is dependent not only on the states it connects but also the inputs that are used to connect those states. The use of the system model can be viewed simply as a means to generate the directed graph and associated edge costs.

#### **3. 3D motion planning with kinematic model**

In order to demonstrate SBMPO capabilities, two local minima scenarios will be considered: 1) a concave obstacle and 2) a highly cluttered area. The purpose is to test how SBMPO handles these types of local minima environments. In this section, the kinematic model of an AUV is used for the motion planning simulations.

$$
\begin{bmatrix}
\dot{\mathbf{x}} \\
\dot{y} \\
\dot{z}
\end{bmatrix} = \begin{bmatrix}
c\theta c\psi \ s\phi s\theta c\psi - c\phi s\psi \ c\phi s\theta c\psi - s\phi s\psi \\
c\theta s\psi \ s\phi s\theta s\psi - c\phi c\psi \ c\phi s\theta s\psi - s\phi c\psi \\
s\theta & s\phi c\theta & c\phi c\theta
\end{bmatrix} \begin{bmatrix} u \\ v \\ w \end{bmatrix}
$$

$$
\begin{bmatrix}
\dot{\phi} \\
\dot{\theta} \\
\dot{\psi}
\end{bmatrix} = \begin{bmatrix}
1\ s\phi t\theta \ c\phi t\theta \\
0 \ c\phi - s\phi \\
0 \ s\phi s\theta \ c\phi s\theta
\end{bmatrix} \begin{bmatrix}
p \\ q \\ r
\end{bmatrix} \tag{3}
$$

where *u*, *v*, *w* are linear velocities in the local body fixed frame along the *x*, *y*, *z* axes, respectively and *p*, *q*,*r* are the angular velocities in the local body fixed frame along the *x*, *y*, *z* axes, respectively. The AUV posture can be defined by six coordinates, three representing the position *x*<sup>1</sup> = (*x*, *y*, *z*)*<sup>T</sup>* and three corresponding to the orientation *x*<sup>2</sup> = (*φ*, *θ*, *ψ*)*T*, all with respect to the world frame. The constraints for the vehicle is given in Table 1.

The basic problem in each of these scenarios is to use the kinematic model to plan a minimum distance trajectory for the AUV from a start posture to a goal position while avoiding the obstacles. A 2.93 GHz Intel Core 2 Duo desktop was used for simulations in this Section.

#### **3.1 AUV concave obstacle**

As previously stated, SBMPO can handle local minimum problems that other path planning methods have difficulties handling. A local minima problem is a possible scenario a vehicle


Table 1. Simulation Constraints for the 3D Kinematic Model

can be presented with that has a set of concave obstacles in front of the goal. Note that whenever a vehicle is behind an obstacle or group of obstacles and has to increase its distance from the goal to achieve the goal, it is in a local minimum position.

The simulations were run with a sampling number of 25 and grid resolution of 0.1m. The vehicle has a start posture of (5*m*, 0*m*, −10*m*, 0◦) and a goal position of (5*m*, 10*m*, −10*m*). As shown in Fig. 6, SBMPO does not get stuck behind the obstacles, but successfully determines a trajectory in 0.59s. The successful traversal is largely due to the optimization method used in SBMPO. The goal-directed optimization allows a more promising node (lower cost) to replace a higher cost node as shown in the the example in Fig 7. Goal-directed optimization can accomplish this because they compute each predicted control input separately and backs up when needed as illustrated by the iterations corresponding to the 3rd, 4th, and 5th arrays. This feature enables it to avoid local minima. It converges to the optimal predicted control sequence {*u*∗(*k*)} (denoted by the right-most array), which is the optimal solution subject to the sampling, whereas a nonlinear programming method may get stuck at a local minimum. In addition, these results show that SBMPO's implementation of the implicit state grid helps prevent the issues with input sampling discussed in Section 2.3.3.2. Since the implicit grid is applied, it does not require significant time to explore the area around the concave obstacles.

#### **3.2 AUV cluttered multiple obstacles**

10 Will-be-set-by-IN-TECH

some given goal. In contrast to discrete optimization algorithms such as branch-and-bound optimization Nocedal & Wright (1999), which "relaxes" continuous optimization problems, the goal-directed optimization methods are inherently discrete, and have often been used for

Generally, sampling based methods such as RRTs do not incorporate any optimization and terminate when an initial feasible solution is determined. In essence, instead of determining an optimal trajectory, traditional sampling based methods only attempt to find feasible trajectories. To remedy these problems, the Randomized *A* (*RA*) algorithm was introduced in Diankov & Kuffner. (2007), as a hybrid between RRTs and the *A* search algorithm. Similar to *RA*, SBMPO incorporates a goal directed optimization to ensure the trajectory is optimal

Although not commonly recognized, goal-directed optimization approaches are capable of solving control theory problems for which the ultimate objective is to plan an optimal trajectory and control inputs to reach a goal (or set point) while optimizing a cost function. Hence, graph search algorithms can be applied to terminal constraint optimization problems and set point control problems. To observe this, consider the tree graph of Fig. 2. Each node of this tree can correspond to a system state, and the entire tree may be generated by integrating sampled inputs to a system model. Assume that the cost of a trajectory is given by the sum of the cost of the corresponding edges (i.e., branches), where the cost of each edge is dependent not only on the states it connects but also the inputs that are used to connect those states. The use of the system model can be viewed simply as a means to generate the directed graph and

In order to demonstrate SBMPO capabilities, two local minima scenarios will be considered: 1) a concave obstacle and 2) a highly cluttered area. The purpose is to test how SBMPO handles these types of local minima environments. In this section, the kinematic model of an AUV is

> *cθcψ sφsθcψ* − *cφsψ cφsθcψ* − *sφsψ cθsψ sφsθsψ* − *cφcψ cφsθsψ* − *sφcψ sθ sφcθ cφcθ*

> > 1 *sφtθ cφtθ* 0 *cφ* −*sφ* 0 *sφsθ cφsθ*

where *u*, *v*, *w* are linear velocities in the local body fixed frame along the *x*, *y*, *z* axes, respectively and *p*, *q*,*r* are the angular velocities in the local body fixed frame along the *x*, *y*, *z* axes, respectively. The AUV posture can be defined by six coordinates, three representing the position *x*<sup>1</sup> = (*x*, *y*, *z*)*<sup>T</sup>* and three corresponding to the orientation *x*<sup>2</sup> = (*φ*, *θ*, *ψ*)*T*, all with

The basic problem in each of these scenarios is to use the kinematic model to plan a minimum distance trajectory for the AUV from a start posture to a goal position while avoiding the obstacles. A 2.93 GHz Intel Core 2 Duo desktop was used for simulations in this Section.

As previously stated, SBMPO can handle local minimum problems that other path planning methods have difficulties handling. A local minima problem is a possible scenario a vehicle

⎤ ⎦ ⎡ ⎣ *p q r*

⎤

⎤ ⎦ ⎡ ⎣ *u v w* ⎤ ⎦

⎦ , (3)

real-time path planning.

subject to the sampling.

associated edge costs.

**3.1 AUV concave obstacle**

**3. 3D motion planning with kinematic model**

⎤ ⎦ = ⎡ ⎣

> ⎡ ⎣ *φ*˙ ˙ *θ ψ*˙

⎤ ⎦ =

respect to the world frame. The constraints for the vehicle is given in Table 1.

⎡ ⎣

used for the motion planning simulations. ⎡ ⎣ *x*˙ *y*˙ *z*˙

In Section 3.1, there was one local minimum in the scenario. However, some underwater environments will require the AUV to navigate around multiple cluttered obstacles. This can produce a more complex situation because now there are multiple local minima . The simulations in this section assume there were random start, goal and obstacle locations in order to represent 100 different multiple obstacle underwater environment configurations. The start locations *X*,*Y*, *Z* and *ψ* were chosen randomly in the respective ranges [0 20]*m*, [0 1]*m*, [−12 − 8]*m*, [30◦ 150◦], and the goal was chosen randomly in the respective ranges [0 20]*m*, [19 20]*m*, [−12 − 8]*m*. In addition, there were 40 randomly generated obstacles. The 100 simulation runs had a sampling number of 25. Fig. 8 exemplifies one random scenarios generated. In the scenarios SBMPO was capable of allowing the AUV to maneuver in the cluttered environment successfully reaching the goal.

For a vehicle to be truly autonomous, it must be capable of determining a trajectory that will allow it to successfully reach the goal without colliding with an obstacle. In these simulations SBMPO was 100% successful in assuring the vehicle accomplished this task. It is important to consider both SBMPO's mean computation time of 0.43*s* and median computation time of 0.15*s* to compute these trajectories. Since the scenarios generated were random, there were a few scenarios created that were more cluttered which caused a larger CPU time. This is the reason for the discrepancy between the mean and median computation times.

Fig. 8. A random start, goal and obstacle scenario.

Table 2. The simulation constraints for the AUV dynamic model.

Table 3. The simulation parameters for the AUV dynamic model.

hill was constructed utilizing 6 sphere obstacles constraints stacked to give a peak at 8*m*. The

Motion Planning for Mobile Robots Via Sampling-Based Model Predictive Optimization 223

States min max States min max Inputs min max x -30 m 130 m u 0 m/s 2 m/s *δ<sup>r</sup>* −22◦ 22◦ y -30 m/s 130 m v -2 m/s 2 m/s *δ<sup>s</sup>* −22◦ 22◦ z -20 m 5 m w -2 m/s 2 m/s *δ<sup>b</sup>* −22◦ 22◦ *φ* −15◦ 15◦ p −5◦/*s* 5◦/*s δbp* −22◦ 22◦ *θ* −85◦ 85◦ q −5◦/*s* 5◦/*s δbs* −22◦ 22◦ *ψ* −360◦ 360◦ r −15◦/*s* 15◦/*s* n 0 rpm 1500 rpm

> Model Time Steps 0.5s Control updates 10s No. of Input Samples 20 Grid Resolution 0.5

A dynamic model was utilized to determine the path over a steep hill. The AUV was not able to determine a path because the vehicle starts too close to the steep hill to gain momentum. As depicted in Fig. 9, the dynamic model was able to predict that there was not enough momentum to overcome the hill in such a short distance. Note the vehicle constraints do not allow this type of AUV to have a negative velocity which would allow the vehicle to be able to reverse in order to acquire enough momentum. As a result of the vehicle constraint, Fig 9 shows the unsuccessful path. It is not the SBMPO algorithm that cannot successfully

AUV's start location was (−4*m* 17*m* − 18*m* 0◦ 0◦ 0◦) and goal was (19*m* 7*m* − 14*m*).

Fig. 6. A local minima scenario.

Fig. 7. Example of goal-directed optimization.

#### **4. 3D motion planning with dynamic model**

In some scenarios it is sufficient to plan using the kinematic model. However, in cases where the vehicle is pushed to an extreme, it is necessary to consider the vehicles dynamic model when planning.

#### **4.1 Steep hill climbing**

In this section, two different types of vehicles, an AUV and an UGV, consider steep hill motion planning using their respective dynamic model. The vehicle must be capable of acquiring a certain amount of momentum to successfully traverse the hill. In order to determine if the vehicle can produce the correct amount of momentum, a dynamic model is not physically capable of traversing.

#### **4.1.1 AUV**

The AUV dynamic model used for these simulations can be found in Healey & Lienard (1993). The model constraints are given in Table 2. The SBMPO parameters are in Table 3. The steep

Fig. 8. A random start, goal and obstacle scenario.

12 Will-be-set-by-IN-TECH

In some scenarios it is sufficient to plan using the kinematic model. However, in cases where the vehicle is pushed to an extreme, it is necessary to consider the vehicles dynamic model

In this section, two different types of vehicles, an AUV and an UGV, consider steep hill motion planning using their respective dynamic model. The vehicle must be capable of acquiring a certain amount of momentum to successfully traverse the hill. In order to determine if the vehicle can produce the correct amount of momentum, a dynamic model is not physically

The AUV dynamic model used for these simulations can be found in Healey & Lienard (1993). The model constraints are given in Table 2. The SBMPO parameters are in Table 3. The steep

Fig. 6. A local minima scenario.

when planning.

**4.1 Steep hill climbing**

capable of traversing.

**4.1.1 AUV**

Fig. 7. Example of goal-directed optimization.

**4. 3D motion planning with dynamic model**

hill was constructed utilizing 6 sphere obstacles constraints stacked to give a peak at 8*m*. The AUV's start location was (−4*m* 17*m* − 18*m* 0◦ 0◦ 0◦) and goal was (19*m* 7*m* − 14*m*).


Table 2. The simulation constraints for the AUV dynamic model.


Table 3. The simulation parameters for the AUV dynamic model.

A dynamic model was utilized to determine the path over a steep hill. The AUV was not able to determine a path because the vehicle starts too close to the steep hill to gain momentum. As depicted in Fig. 9, the dynamic model was able to predict that there was not enough momentum to overcome the hill in such a short distance. Note the vehicle constraints do not allow this type of AUV to have a negative velocity which would allow the vehicle to be able to reverse in order to acquire enough momentum. As a result of the vehicle constraint, Fig 9 shows the unsuccessful path. It is not the SBMPO algorithm that cannot successfully

Fig. 10. The AUV kinematic model steep hill scenario.

up to gain momentum and leads to a successful climb.

(a) (b) Fig. 11. A steep hill climbing scenario for a UGV (a) the UGV rushes to the top of the hill without enough momentum and torque and leads to unsuccessful climb (b) the UGV backs

Motion Planning for Mobile Robots Via Sampling-Based Model Predictive Optimization 225

A minimum time cost function is used to implement steep hill climbing with zero velocity at

Δ

where *q*<sup>1</sup> = *q* and *q*<sup>2</sup> = *q*˙. It is desired to find the minimum time needed to transfer the system

for transferring the system from (*q*1,0, *q*2,0) to the origin (0, 0) is easily extended to the more

general case by a simple change of variable, for ease of exposition it is assumed that

*q*¨ = *u*; *q*(0) = *q*0, *q*˙(0) = *ω*0, (4)

Δ

*q*1, *<sup>f</sup>* = 0. (6)

Δ

= *q*2,0, (5)

= *q <sup>f</sup>* . Since the solution

= *q*1,0, *q*2(0) = *ω*<sup>0</sup>

the goal. To formulate the minimum time, consider a system described by

*q*˙1 = *q*2, *q*˙2 = *u*; *q*1(0) = *q*<sup>0</sup>

from the original state (*q*1,0, *q*2,0) to the final state (*q*1, *<sup>f</sup>* , 0), where *q*1, *<sup>f</sup>*

where *u* is bounded by −*a* ≤ *u* ≤ *b*. The state space description of (4) is given by

determine a path, but the vehicle constraint (dynamic model) that predicts there was not enough momentum to overcome the hill in such a short distance. In order to demonstrate this consider the same scenario using the kinematic model in Fig 10. SBMPO does determine a path, but this is only because the kinematic model utilized does not provide all the vehicle information to correctly predict the vehicle motion. This further shows the importance of using the proper model when motion planning. The trajectory determined by the planner is only as accurate as the model used.

Fig. 9. The AUV dynamic model steep hill scenario.

#### **4.1.2 UGV**

This section discusses momentum-based motion planning applied to UGV steep hill climbing. Note that steep hill climbing capability of UGVs is very important to aid the completion of assigned tasks or missions. As an additional requirement, the motion planning is constrained such that the UGV has a zero velocity at the goal (e.g. top of the hill) and this has a unique application, such as reconnaissance, where UGV needs to climb and stop at the top of the hill to gather information. In this section, the momentum-based motion planning is implemented using SBMPO with UGV's dynamic model and a minimum time cost function. The minimum time cost function is employed to achieve zero velocity at the goal.

Figure 11 shows a scenario where a UGV is at the bottom of a steep hill and the task is to climb to the top of the hill. The general approach is to rush to the top of the hill. However, if the torque of the UGV and the momentum are not enough, it is highly possible that the UGV will fail to climb as shown in Fig. 11(a). An alternative approach for the UGV is to back up to gain more momentum and rush to the top of the hill as shown in Fig 11(b). The aforementioned approaches can be done using SBMPO with UGV's dynamic model. SBMPO can generate a trajectory for successful steep hill climbing, and it can also determine if the UGV needs to back up or how far the UGV needs to back up to successfully climb the hill.

Fig. 10. The AUV kinematic model steep hill scenario.

14 Will-be-set-by-IN-TECH

determine a path, but the vehicle constraint (dynamic model) that predicts there was not enough momentum to overcome the hill in such a short distance. In order to demonstrate this consider the same scenario using the kinematic model in Fig 10. SBMPO does determine a path, but this is only because the kinematic model utilized does not provide all the vehicle information to correctly predict the vehicle motion. This further shows the importance of using the proper model when motion planning. The trajectory determined by the planner is

This section discusses momentum-based motion planning applied to UGV steep hill climbing. Note that steep hill climbing capability of UGVs is very important to aid the completion of assigned tasks or missions. As an additional requirement, the motion planning is constrained such that the UGV has a zero velocity at the goal (e.g. top of the hill) and this has a unique application, such as reconnaissance, where UGV needs to climb and stop at the top of the hill to gather information. In this section, the momentum-based motion planning is implemented using SBMPO with UGV's dynamic model and a minimum time cost function. The minimum

Figure 11 shows a scenario where a UGV is at the bottom of a steep hill and the task is to climb to the top of the hill. The general approach is to rush to the top of the hill. However, if the torque of the UGV and the momentum are not enough, it is highly possible that the UGV will fail to climb as shown in Fig. 11(a). An alternative approach for the UGV is to back up to gain more momentum and rush to the top of the hill as shown in Fig 11(b). The aforementioned approaches can be done using SBMPO with UGV's dynamic model. SBMPO can generate a trajectory for successful steep hill climbing, and it can also determine if the UGV needs to back

only as accurate as the model used.

Fig. 9. The AUV dynamic model steep hill scenario.

time cost function is employed to achieve zero velocity at the goal.

up or how far the UGV needs to back up to successfully climb the hill.

**4.1.2 UGV**

Fig. 11. A steep hill climbing scenario for a UGV (a) the UGV rushes to the top of the hill without enough momentum and torque and leads to unsuccessful climb (b) the UGV backs up to gain momentum and leads to a successful climb.

A minimum time cost function is used to implement steep hill climbing with zero velocity at the goal. To formulate the minimum time, consider a system described by

$$
\ddot{q} = \mu; \ q(0) = q\_{0\prime} \cdot \dot{q}(0) = \omega\_{0\prime} \tag{4}
$$

where *u* is bounded by −*a* ≤ *u* ≤ *b*. The state space description of (4) is given by

$$\not q\_1 = q\_{2\prime} \not q\_2 = \mu \not q\_1(0) = q\_0 \stackrel{\Delta}{=} q\_{1,0\prime} \not q\_2(0) = \omega\_0 \stackrel{\Delta}{=} q\_{2,0\prime} \tag{5}$$

where *q*<sup>1</sup> = *q* and *q*<sup>2</sup> = *q*˙. It is desired to find the minimum time needed to transfer the system from the original state (*q*1,0, *q*2,0) to the final state (*q*1, *<sup>f</sup>* , 0), where *q*1, *<sup>f</sup>* Δ = *q <sup>f</sup>* . Since the solution for transferring the system from (*q*1,0, *q*2,0) to the origin (0, 0) is easily extended to the more general case by a simple change of variable, for ease of exposition it is assumed that

$$q\_{1,f} = 0.\tag{6}$$

Fig. 13. The UGV steep hill parameters.

done by SBMPO.

**5. Tuning parameters**

**5.1 Sampling number**

completely true.

The results of the motion planning using SBMPO with the UGV's dynamic model and minimum time cost function are shown in Fig. 14. Fig. 14(a) shows the desired X-Z position of the UGV and Figs. 14(b)-(d) show respectively the desired wheel angular position, velocity, and acceleration, which are the trajectory components of the UGV's. In practice, the resulting trajectory is fed to the UGV's low-level controller for tracking. In Fig. 14(b), the desired wheel angular position starts at zero, and it goes negative (UGV backs up) before it proceeds to the goal. Fig. 14(c) shows the desired angular velocity of the wheel, and it is negative before the UGV accelerates to climb the hill. It also shows that the angular velocity at the goal is zero. The results clearly show that the UGV backs up to increase momentum, which is automatically

Motion Planning for Mobile Robots Via Sampling-Based Model Predictive Optimization 227

Similar to other algorithms, SBMPO has parameters that have to be tuned to guarantee optimal results. SBMPO has two main tuning parameters, the sampling number (branching factor) and grid resolution (size). Each tuning parameter has an effect on the computation time and cost. In this Section, one of the random scenarios from Section 3.2 was investigated.

The sample number is the number of samples that are selected to span the input space. In order to determine how the sample number effects the computation time of SBMPO the grid resolution was held constant at 0.1m, and the sample number was varied from 10 to 40 by increments of 2. Fig. 15 shows that the effect of the sampling number is nonlinear, so there is no direct relationship between the sample number and computation time. Originally it was thought that an increase in sample number would cause an increase in computation time because there would be more nodes to evaluate. However, as shown in Fig. 15 this is not

The reason for the nonlinear trend is threefold. First as shown in Fig. 15 by samples 10 and 12 , when there are not enough samples (the sample number is too low) to span the space, it can

The minimum time control problem described above can be solved by forming the Hamiltonian and applying the "Minimum Principle" (often referred to as "Pontryagin's Maximum Principle") as described in Bryson & Ho (1975). In fact, the above problem is solved in Bryson & Ho (1975) for the case when the parameters *a* and *b* are given by *a* = *b* = 1. Generalizing these results yields that the minimum time is the solution t*<sup>f</sup>* of

$$\begin{aligned} \mathfrak{L}\_f^2 - \frac{2q\_{2,0}}{a} \mathfrak{t}\_f &= \frac{q\_{2,0}^2 + 2(a+b)q\_{1,0}}{ab}, \text{ if } q\_{1,0} + \frac{q\_{2,0}|q\_{2,0}|}{2b} < 0, \\\\ \mathfrak{L}\_f^2 + \frac{2q\_{2,0}}{b} \mathfrak{t}\_f &= \frac{q\_{2,0}^2 - 2(a+b)q\_{1,0}}{ab}, \text{ if } q\_{1,0} + \frac{q\_{2,0}|q\_{2,0}|}{2a} > 0. \end{aligned} \tag{7}$$

The minimum time (t*<sup>f</sup>* ) computed using (7) corresponds to a "bang-bang" optimal controller illustrated by Fig. 12, which shows switching curves that take the system to the origin using either the minimum or maximum control input (i.e., *u* = −*a* or *u* = *b*). Depending on the initial conditions, the system uses either the minimum or maximum control input to take the system to the appropriate switching curve. For example, if (*q*1,0, *q*2,0) corresponds to point *p*<sup>1</sup> in Fig. 12, then the control input should be *u* = −*a* until the system reaches point *p*<sup>2</sup> on the switching curve corresponding to *u* = *b*. At this point the control is switched to *u* = *b*, which will take the system to the origin.

Fig. 12. Illustration of bang-bang minimum time optimal control which yields the minimum time solution t*<sup>f</sup>* of (7).

To demonstrate steep hill climbing, the UGV starts at (0,0,0)[m] and the goal is located at (2.5,0,0.76)[m]. As shown in Fig. 13, the hill is described with the following parameters: *R* = 1*m*, *l* = 0.75*m*, *d* = 0.4*m* and *θ* = 30*o*. A UGV dynamic model discussed in Yu et al. (2010) is used and it is given by

$$M\ddot{q} + \mathbb{C}(\dot{q}, q) + \mathbb{G}(q) = \tau,\tag{8}$$

where

$$-\mathfrak{T}\_{\text{max}} < \mathfrak{T} < \mathfrak{T}\_{\text{max}} \tag{9}$$

and *τmax* = 10*Nm*. *q*¨, *q*˙, and *q* are respectively the wheel angular acceleration, velocity, and position, *M* is the inertia, *C*(*q*˙, *q*) is the friction term, and *G*(*q*) is the gravity term. Based on the parameters of the hill and the UGV, the maximum required torque to climb quasi-statically the hill is 14.95Nm. This clearly shows that the UGV cannot climb without using momentum.

The results of the motion planning using SBMPO with the UGV's dynamic model and minimum time cost function are shown in Fig. 14. Fig. 14(a) shows the desired X-Z position of the UGV and Figs. 14(b)-(d) show respectively the desired wheel angular position, velocity, and acceleration, which are the trajectory components of the UGV's. In practice, the resulting trajectory is fed to the UGV's low-level controller for tracking. In Fig. 14(b), the desired wheel angular position starts at zero, and it goes negative (UGV backs up) before it proceeds to the goal. Fig. 14(c) shows the desired angular velocity of the wheel, and it is negative before the UGV accelerates to climb the hill. It also shows that the angular velocity at the goal is zero. The results clearly show that the UGV backs up to increase momentum, which is automatically done by SBMPO.

#### **5. Tuning parameters**

16 Will-be-set-by-IN-TECH

The minimum time control problem described above can be solved by forming the Hamiltonian and applying the "Minimum Principle" (often referred to as "Pontryagin's Maximum Principle") as described in Bryson & Ho (1975). In fact, the above problem is solved in Bryson & Ho (1975) for the case when the parameters *a* and *b* are given by *a* = *b* = 1.

*ab* , if *<sup>q</sup>*1,0 <sup>+</sup> *<sup>q</sup>*2,0|*q*2,0<sup>|</sup>

*ab* , if *<sup>q</sup>*1,0 <sup>+</sup> *<sup>q</sup>*2,0|*q*2,0<sup>|</sup>

<sup>2</sup>*<sup>b</sup>* < 0,

(7)

<sup>2</sup>*<sup>a</sup>* > 0.

Generalizing these results yields that the minimum time is the solution t*<sup>f</sup>* of

2,0+2(*a*+*b*)*q*1,0

2,0−2(*a*+*b*)*q*1,0

The minimum time (t*<sup>f</sup>* ) computed using (7) corresponds to a "bang-bang" optimal controller illustrated by Fig. 12, which shows switching curves that take the system to the origin using either the minimum or maximum control input (i.e., *u* = −*a* or *u* = *b*). Depending on the initial conditions, the system uses either the minimum or maximum control input to take the system to the appropriate switching curve. For example, if (*q*1,0, *q*2,0) corresponds to point *p*<sup>1</sup> in Fig. 12, then the control input should be *u* = −*a* until the system reaches point *p*<sup>2</sup> on the switching curve corresponding to *u* = *b*. At this point the control is switched to *u* = *b*, which

Fig. 12. Illustration of bang-bang minimum time optimal control which yields the minimum

To demonstrate steep hill climbing, the UGV starts at (0,0,0)[m] and the goal is located at (2.5,0,0.76)[m]. As shown in Fig. 13, the hill is described with the following parameters: *R* = 1*m*, *l* = 0.75*m*, *d* = 0.4*m* and *θ* = 30*o*. A UGV dynamic model discussed in Yu et al.

and *τmax* = 10*Nm*. *q*¨, *q*˙, and *q* are respectively the wheel angular acceleration, velocity, and position, *M* is the inertia, *C*(*q*˙, *q*) is the friction term, and *G*(*q*) is the gravity term. Based on the parameters of the hill and the UGV, the maximum required torque to climb quasi-statically the hill is 14.95Nm. This clearly shows that the UGV cannot climb without using momentum.

*Mq*¨ + *C*(*q*˙, *q*) + *G*(*q*) = *τ*, (8)

− *τmax* < *τ* < *τmax* (9)

*<sup>a</sup>* <sup>t</sup>*<sup>f</sup>* <sup>=</sup> *<sup>q</sup>*<sup>2</sup>

*<sup>b</sup>* <sup>t</sup>*<sup>f</sup>* <sup>=</sup> *<sup>q</sup>*<sup>2</sup>

t 2 *<sup>f</sup>* <sup>−</sup> <sup>2</sup>*q*2,0

t 2 *<sup>f</sup>* <sup>+</sup> <sup>2</sup>*q*2,0

will take the system to the origin.

time solution t*<sup>f</sup>* of (7).

where

(2010) is used and it is given by

Similar to other algorithms, SBMPO has parameters that have to be tuned to guarantee optimal results. SBMPO has two main tuning parameters, the sampling number (branching factor) and grid resolution (size). Each tuning parameter has an effect on the computation time and cost. In this Section, one of the random scenarios from Section 3.2 was investigated.

#### **5.1 Sampling number**

The sample number is the number of samples that are selected to span the input space. In order to determine how the sample number effects the computation time of SBMPO the grid resolution was held constant at 0.1m, and the sample number was varied from 10 to 40 by increments of 2. Fig. 15 shows that the effect of the sampling number is nonlinear, so there is no direct relationship between the sample number and computation time. Originally it was thought that an increase in sample number would cause an increase in computation time because there would be more nodes to evaluate. However, as shown in Fig. 15 this is not completely true.

The reason for the nonlinear trend is threefold. First as shown in Fig. 15 by samples 10 and 12 , when there are not enough samples (the sample number is too low) to span the space, it can

Fig. 15. The effect of sample size on computation time.

which leads to more of an optimal solution.

**5.2 Grid size**

(a) (b)

Motion Planning for Mobile Robots Via Sampling-Based Model Predictive Optimization 229

Fig. 17 depicts how varying the sample number effects the cost (i.e. distance). The cost is larger in the smaller sample numbers 10 and 12. Afterwards, the variation in the cost is small,

The grid size is the resolution of the implicit state grid. To evaluate how this tuning parameter effects the computation time, the sampling number was held constant at 25, and the grid resolution was varied between 0.02 to 0.5. Again this tuning parameter is not monotonic with respect to the computation time as depicted in Fig. 18. This shows the importance of properly tuning the algorithm. It may be thought that increasing the grid size would cause

Fig. 16. (a) Scenario with sample number = 26, (b) Scenario with sample number = 36.

Fig. 14. (a) X-Z position of the UGV (b) wheel angular position (c) wheel angular velocity (d) wheel angular acceleration.

also increase the CPU time, because it takes more iterations (i.e. steps in SBMPO) to determine the solution. A good tuning of the parameter occurs at 14 samples which results in a smaller computation time. The second trend, as shown in Fig. 15 between samples 14 and 22, is that after a good tuning of the parameter, increasing the number of samples also increases the computation times which corresponds to the original hypothesis that an increase in sample number will result in an increase in CPU time. Lastly, a factor that contributes to the effect of the sample number on the computation time is the path produced by SBMPO. It is possible for a larger sample number to have a lower computation time when the path SBMPO generates to the goal encounters a smaller cluster of obstacles. Figs. 16a and 16b show the paths generated respectively using 26 and 36 samples. The path of Fig. 16a which has the lower sampling number takes the AUV through a cluster of obstacles, whereas the path of Fig. 16b which has the larger sample number takes a path that largely avoids the obstacles. Even though Fig. 16b corresponds to a sample number of 36, referring to Fig. 15, its computation time of 0.29*s* is smaller than that for Fig. 16a, which corresponds to a sample number of 26 and has a computation time of 0.5*s*.

Fig. 15. The effect of sample size on computation time.

Fig. 16. (a) Scenario with sample number = 26, (b) Scenario with sample number = 36.

Fig. 17 depicts how varying the sample number effects the cost (i.e. distance). The cost is larger in the smaller sample numbers 10 and 12. Afterwards, the variation in the cost is small, which leads to more of an optimal solution.

#### **5.2 Grid size**

18 Will-be-set-by-IN-TECH

(a) (b)

(c) (d) Fig. 14. (a) X-Z position of the UGV (b) wheel angular position (c) wheel angular velocity (d)

also increase the CPU time, because it takes more iterations (i.e. steps in SBMPO) to determine the solution. A good tuning of the parameter occurs at 14 samples which results in a smaller computation time. The second trend, as shown in Fig. 15 between samples 14 and 22, is that after a good tuning of the parameter, increasing the number of samples also increases the computation times which corresponds to the original hypothesis that an increase in sample number will result in an increase in CPU time. Lastly, a factor that contributes to the effect of the sample number on the computation time is the path produced by SBMPO. It is possible for a larger sample number to have a lower computation time when the path SBMPO generates to the goal encounters a smaller cluster of obstacles. Figs. 16a and 16b show the paths generated respectively using 26 and 36 samples. The path of Fig. 16a which has the lower sampling number takes the AUV through a cluster of obstacles, whereas the path of Fig. 16b which has the larger sample number takes a path that largely avoids the obstacles. Even though Fig. 16b corresponds to a sample number of 36, referring to Fig. 15, its computation time of 0.29*s* is smaller than that for Fig. 16a, which corresponds to a sample number of 26 and has a

wheel angular acceleration.

computation time of 0.5*s*.

The grid size is the resolution of the implicit state grid. To evaluate how this tuning parameter effects the computation time, the sampling number was held constant at 25, and the grid resolution was varied between 0.02 to 0.5. Again this tuning parameter is not monotonic with respect to the computation time as depicted in Fig. 18. This shows the importance of properly tuning the algorithm. It may be thought that increasing the grid size would cause

Fig. 19. The effect of grid size on path cost.

system or systems subject to nonlinear constraints.

the correct model is important.

SBMPO is a NMPC method that exploits sampling-based concepts from the robotics literature along with the *LPA* incremental optimization algorithm from the AI literature to achieve the goal of quickly and simultaneously determining the control updates and paths while avoiding local minima. The SBMPO solution is globally optimal *subject to the sampling*. Sampling Based Model Predictive Optimization has been shown to effectively generate paths in the presence of nonlinear constraints and when vehicles are pushed to extreme limits. It was determined that SBMPO is only as good as the model supplied to predict the vehicle's movement. Selecting

Motion Planning for Mobile Robots Via Sampling-Based Model Predictive Optimization 231

The future work is to develop the replanning feature of SBMPC. Currently, SBMPO is applied, but the ability to replan is essential to the SBMPC algorithm. SBMPC utilizes *LPA* which allows quick replanning of the path without having to completely restart the planning process when new information is obtained or changes in the environment occur. Only the nodes that are affected by a change in the environment must be reevaluated. This reduces the computation time and aids the method in achieving fast computation times. Once the replanning feature of SBMPC is in place, scenarios that include disturbance, model mismatch, unknown obstacles and moving obstacles can be examined to test more realistic situations. The algorithm will also be in a framework that is more comparable to traditional NMPC that only takes the first input and replans at every time step to create a more robust controller. Then SBMPC can be considered a general fast NMPC method that is useful for any nonlinear

Bryson, A. & Ho, Y. (1975). *Applied Optimal Control Optimization, Estimation, and Control*, HPC,

Caldwell, C., Collins, E. & Palanki, S. (2006). Integrated guidance and control of AUVs using

shrinking horizon model predictive control, *OCEANS Conference* .

**6. Conclusion**

**7. References**

New York.

Fig. 17. The effect of sample size on path cost.

less computation. However, the opposite is true. The larger the grid size, the higher the possibility that two nodes are considered as the same state, which leads to the need for more sampling of the input space and an increased computation time. When choosing the grid resolution, it is important to recognize that increasing the grid size tends to lead to higher cost solutions as depicted in Fig. 19.

Fig. 18. The effect of grid size on computation time.

Fig. 19. The effect of grid size on path cost.

#### **6. Conclusion**

20 Will-be-set-by-IN-TECH

less computation. However, the opposite is true. The larger the grid size, the higher the possibility that two nodes are considered as the same state, which leads to the need for more sampling of the input space and an increased computation time. When choosing the grid resolution, it is important to recognize that increasing the grid size tends to lead to higher cost

Fig. 17. The effect of sample size on path cost.

Fig. 18. The effect of grid size on computation time.

solutions as depicted in Fig. 19.

SBMPO is a NMPC method that exploits sampling-based concepts from the robotics literature along with the *LPA* incremental optimization algorithm from the AI literature to achieve the goal of quickly and simultaneously determining the control updates and paths while avoiding local minima. The SBMPO solution is globally optimal *subject to the sampling*. Sampling Based Model Predictive Optimization has been shown to effectively generate paths in the presence of nonlinear constraints and when vehicles are pushed to extreme limits. It was determined that SBMPO is only as good as the model supplied to predict the vehicle's movement. Selecting the correct model is important.

The future work is to develop the replanning feature of SBMPC. Currently, SBMPO is applied, but the ability to replan is essential to the SBMPC algorithm. SBMPC utilizes *LPA* which allows quick replanning of the path without having to completely restart the planning process when new information is obtained or changes in the environment occur. Only the nodes that are affected by a change in the environment must be reevaluated. This reduces the computation time and aids the method in achieving fast computation times. Once the replanning feature of SBMPC is in place, scenarios that include disturbance, model mismatch, unknown obstacles and moving obstacles can be examined to test more realistic situations. The algorithm will also be in a framework that is more comparable to traditional NMPC that only takes the first input and replans at every time step to create a more robust controller. Then SBMPC can be considered a general fast NMPC method that is useful for any nonlinear system or systems subject to nonlinear constraints.

#### **7. References**

Bryson, A. & Ho, Y. (1975). *Applied Optimal Control Optimization, Estimation, and Control*, HPC, New York.

Caldwell, C., Collins, E. & Palanki, S. (2006). Integrated guidance and control of AUVs using shrinking horizon model predictive control, *OCEANS Conference* .

**Part 3** 

**Mobile Robots Navigation** 


## **Part 3**

**Mobile Robots Navigation** 

22 Will-be-set-by-IN-TECH

232 Recent Advances in Mobile Robotics

Diankov, R. & Kuffner., J. (2007). Randomized statistical path planning, *Conference on Intelligent*

Dunlap, D. D., E. G. Collins, J. & Caldwell, C. V. (2008). Sampling based model predictive

Healey, A. & Lienard, D. (1993). Multivariable sliding-mode control for autonomous diving

LaValle, S. M. & Kuffner, J. J. (2001). Randomized kinodynamic planning, *International Journal*

Likhachev, M. & Stentz, A. (2008). R search, *Proceedings of the National Conference on Artificial*

Plaku, E., Kavraki, L. & Vardi, M. (2010). Motion planning with dynamics by synergistic combination of layers of planning, *IEEE Transaction on Robotics* pp. 469–482. Yu, W., Jr., O. C., Jr., E. C. & Hollis, P. (2010). Analysis and experimental verification for

dynamic modeling of a skid-steered wheeled vehicle, *IEEE Transactions on Robotics*

Koenig, S., Likhachev, M. & Furcy, D. (2004). Lifelong planning A, *Artificial Intelligence* . Kuffner, J. J. & LaValle., S. M. (2000). Rrt-connect: An efficient approach to single-query path planning, *IEEE International Conference on Robotics and Automation* p. 9951001. LaValle, S. (1998). Rapidly-exploring random trees: A new tool for path planning, *Technical*

LaValle, S. M. (2006). *Planning Algorithms*, Cambridge University Press.

Nocedal, J. & Wright, S. (1999). *Numerical Optimization*, Springer, New York.

control with application to autonomous vehicle guidance, *Florida Conference on Recent*

and steering for unmanned underwater vehicle, *IEEE Journal of Oceanic Engineering*

*Robots and Systems* .

*Advances in Robotics* .

*report*, Iowa State University.

*Intelligence (AAAI)* pp. 1–7.

*of Robotics Research* 20(8): 378–400.

18(3): 327–338.

pp. 340 – 353.

Ericson, C. (2005). *Real–Time Collision Detection*, Elsevier.

**0**

**12**

*Serbia*

**Fictitious Fuzzy-Magnet Concept in Solving**

A human driver can successfully execute different vehicle navigation tasks, such as: forward garaging, reverse garaging, parallel parking, diagonal parking, following a given trajectory, stopping at a given point, avoiding one or more obstacles, and the like. These processes are either too difficult to model accurately or too complex to model at all, while experienced drivers are able to successfully execute the tasks without any knowledge of the vehicle's mathematical model, even when they switch vehicles. Existing knowledge about how such problems are solved and the ability to express this knowledge, are benefits which can be put to good use when designing controllers for wheeled mobile robot (WMR) navigation tasks. It is well known that fuzzy logic is the most effective tool for solving a problem for which there is a human solution (Zadeh, 2001), because it offers a mathematical apparatus in the background of an inaccurate, qualitative description, which projects input variables into output variables through several stages. However, describing the tasks executed by a driver in spoken language is not a simple process. When we drive a car, we rely on our driving skills and prefer to use our hands and feet, rather than our brain. It is safe to assume that compiling

In view of the above, this chapter presents a new approach to the modeling of driver skills, based on a fuzzy model and an original virtual fuzzy-magnet concept. Driver experience is used to solve simple tasks (turn left or right, slow down or accelerate), while the entire problem is solved by specifying an appropriate number of fictitious fuzzy magnets and defining their tasks. Each fictitious fuzzy magnet is defined by its position and a set (or subset) of fuzzy rules which determine its action. For example, to solve a target navigation problem in open space requires only one fuzzy magnet which attracts the mobile robot to the target, while a more complex problem, such as bidirectional garaging of a mobile robot, requires two fuzzy magnets: one immediately in front of the garage and the other inside the garage. The first point is used to approach the garage and while the vehicle is entering the garage, it serves as a reference point for proper orientation, similar to human driving skills. The second point is both a target and a reference point. Compared to other algorithms which address this type of problem, the proposed algorithms are very simple; they are not based on the WMR model. The proposed fuzzy controllers are of the Takagi-Sugeno (T-S) type; they are manually

**1. Introduction**

rules based on driver skills will not be an easy task.

**Mobile–Robot Target Navigation, Obstacle**

**Avoidance and Garaging Problems**

Srdan T. Mitrovi´ ¯ c1 and Željko M. Ðurovi´c2

<sup>2</sup>*University of Belgrade, School of Electrical Engineering*

<sup>1</sup>*Defense University, Military Academy*

### **Fictitious Fuzzy-Magnet Concept in Solving Mobile–Robot Target Navigation, Obstacle Avoidance and Garaging Problems**

Srdan T. Mitrovi´ ¯ c1 and Željko M. Ðurovi´c2 <sup>1</sup>*Defense University, Military Academy* <sup>2</sup>*University of Belgrade, School of Electrical Engineering Serbia*

#### **1. Introduction**

A human driver can successfully execute different vehicle navigation tasks, such as: forward garaging, reverse garaging, parallel parking, diagonal parking, following a given trajectory, stopping at a given point, avoiding one or more obstacles, and the like. These processes are either too difficult to model accurately or too complex to model at all, while experienced drivers are able to successfully execute the tasks without any knowledge of the vehicle's mathematical model, even when they switch vehicles. Existing knowledge about how such problems are solved and the ability to express this knowledge, are benefits which can be put to good use when designing controllers for wheeled mobile robot (WMR) navigation tasks. It is well known that fuzzy logic is the most effective tool for solving a problem for which there is a human solution (Zadeh, 2001), because it offers a mathematical apparatus in the background of an inaccurate, qualitative description, which projects input variables into output variables through several stages. However, describing the tasks executed by a driver in spoken language is not a simple process. When we drive a car, we rely on our driving skills and prefer to use our hands and feet, rather than our brain. It is safe to assume that compiling rules based on driver skills will not be an easy task.

In view of the above, this chapter presents a new approach to the modeling of driver skills, based on a fuzzy model and an original virtual fuzzy-magnet concept. Driver experience is used to solve simple tasks (turn left or right, slow down or accelerate), while the entire problem is solved by specifying an appropriate number of fictitious fuzzy magnets and defining their tasks. Each fictitious fuzzy magnet is defined by its position and a set (or subset) of fuzzy rules which determine its action. For example, to solve a target navigation problem in open space requires only one fuzzy magnet which attracts the mobile robot to the target, while a more complex problem, such as bidirectional garaging of a mobile robot, requires two fuzzy magnets: one immediately in front of the garage and the other inside the garage. The first point is used to approach the garage and while the vehicle is entering the garage, it serves as a reference point for proper orientation, similar to human driving skills. The second point is both a target and a reference point. Compared to other algorithms which address this type of problem, the proposed algorithms are very simple; they are not based on the WMR model. The proposed fuzzy controllers are of the Takagi-Sugeno (T-S) type; they are manually

*α<sup>A</sup>*

Target Navigation, Obstacle Avoidance and Garaging Problems

*dA*

*dA*(*k*) =

*Control Rule i* : If *dA*(*k*) is *μi*<sup>1</sup> and *αA*(*k*) is *μi*<sup>2</sup>

Fig. 1. Mutual spatial positions of wheeled mobile robot and fictitious fuzzy magnet.

longitudinal line of symmetry of the WMR, *SR*, and the segment which connects the points (*xR*, *yR*) and *A* is denoted by *αA*. If the coordinates of the WMR center are denoted by (*xR*(*k*), *yR*(*k*)) at the sample time *k*, the distance between the WMR and the fictitious fuzzy

<sup>237</sup> Fictitious Fuzzy-Magnet Concept in Solving Mobile–Robot

The position of the WMR at the sample time *k*, relative to the point *A*, is unequivocally determined by its orientation relative to the point *A* – angle *αA*(*k*) and the distance *dA*(*k*), which will be adopted as input variables of the *FRsS*. In the general case, we will state that the output velocity variables are *v*<sup>1</sup> and *v*2. The *FRsS* of *r* rules for the discrete T-S fuzzy

then *v*1(*k*) = *Ci*<sup>1</sup> and *v*2(*k*) = *Ci*<sup>2</sup>

where *dA*(*k*) and *αA*(*k*) are premise variables. The membership function which corresponds

where *μij*(*xj*(*k*)) is the degree of membership of *xj*(*k*) in *μij*. As is well known, the control of a differential drive mobile robot can be designed in two ways: using the speed of the left and right wheels, or using the linear and angular velocities of the robot. Consequently, the general definition of the *FRsS* can be written more precisely in two ways. The navigation of a differential drive mobile robot was designed in (Mitrovi´c & Ðurovi´c, 2010a) based on wheel speed control, such that the output variables of the *FRsS* are the speed of the left wheel – *vL*

then *vL*(*k*) = *Ci*<sup>1</sup> and *vR*(*k*) = *Ci*<sup>2</sup>

*μi*,1(*dA*(*k*)) · *μi*,2(*αA*(*k*)) · [*Ci*,1 *Ci*,2]

*μi*,1(*dA*(*k*)) · *μi*,2(*αA*(*k*))

*θR*

*xA*

*i* = 1, 2, . . . ,*r* (3)

*i* = 1, 2, . . . ,*r* (5)

(*xA* − *xR*(*k*))<sup>2</sup> + (*yA* − *yR*(*k*))2. (2)

*th* premise variable is denoted by *μij*, and *Cij* are constants.

*T*

*A*

*x*

(4)

*xR*

magnet, *dA*(*k*), is:

system is defined as:

*th* control rule and the *j*

 *v*1(*k*) *v*2(*k*)

System outputs are *v*1(*k*) and *v*2(*k*), obtained from:

 =

and the speed of the right wheel – *vR*, such that (3-4) become:

*Control Rule i* : If *dA*(*k*) is *μi*<sup>1</sup> and *αA*(*k*) is *μi*<sup>2</sup>

*r* ∑ *i*=1

> *r* ∑ *i*=1

to the *i*

*SR <sup>y</sup>*

*yR*

*yA*

generated (in a manner similar to human driving skills), and have clearly-defined physical parameters.

The first section of this chapter describes the fictitious fuzzy-magnet concept and its application in solving target navigation problems in an obstacle-free environment. It further analyzes the application of a pair of fuzzy magnets in solving a garaging problem involving a differential-drive mobile robot. The fictitious fuzzy magnet concept allows navigation to the target in a single maneuver, without changing the direction of WMR travel. The symmetry of the differential-drive WMR is utilized fully, such that the algorithm provides a bidirectional solution to the WMR garaging problem. The robot is automatically parked from the end of the robot that is closer to the garage entrance. The algorithm can be applied when the control variable is of the discrete type and where there are relatively few quantization levels. The efficiency and shortfalls of the proposed algorithm are analyzed by means of both detailed simulations and multiple re-runs of a real experiment. Special attention is devoted to the analysis of different initial robot configurations and the effect of an error in the estimation of the current position of the robot on garaging efficiency.

The third section of this chapter analyzes the possibility of applying the fictitious fuzzy-magnet concept to navigate a mobile robot to a target in a workspace which contains one or more obstacles. At the preprocessing stage, the left and right side of the obstacle in the robot's line of sight are determined based on the mutual positions of the robot and the obstacle. Since controller inputs are based on the relative dimensions of the obstacle, the controller can be applied to obstacles of different shapes and sizes. Following the design of the above-described controllers, and the successful testing of navigation efficiency to the goal in a workspace which includes a single stationary obstacle, the algorithm was applied, without modification, to avoid an obstacle moving along a straight line at a constant speed equal to one-third of the maximum speed of the robot. Here the proposed algorithm demonstrated limited efficiency. The final section of the chapter proposes a method for a simple but efficient generalization of the algorithm to support a group of obstacles, using parallel data processing, illustrated by several computer simulations of WMR navigation to the target point through a group of obstacles. The disadvantages of the proposed algorithm when applied in complex environments are discussed at the end of the chapter.

#### **2. Fictitious fuzzy magnets concept**

A given point *A*(*xA*, *yA*), is assumed to be in the Cartesian coordinate system and to represent the position of the fictitious fuzzy magnet *FM*. The *FM* is defined as an arranged pair comprised of its position *A* and an added sub-set of fuzzy rules, *FRsS* (Fuzzy Rules subSet):

$$FM = \begin{pmatrix} A, FRsS \end{pmatrix} \tag{1}$$

The *FRsS* enables the determination of the zone of influence of the fuzzy magnet, as well as the definition of the action it causes. To establish the structure of the fuzzy rules, it is necessary to adopt input and output variables (i.e., premise variables and conclusion variables). Figure 1 shows the position of the WMR relative to the point *A*, which represents the position of the fictitious fuzzy magnet, *FM*.

Point (*xR*, *yR*) is the center of the WMR, and its orientation relative to the *x* axis is denoted by *θR*, such that the configuration of the WMR *qR* is unequivocally defined by three coordinates: *qR*(*xR*, *yR*, *θR*). The bold line denotes the front end of the robot. The angle formed by the 2 Mobile Robots / Book 1

generated (in a manner similar to human driving skills), and have clearly-defined physical

The first section of this chapter describes the fictitious fuzzy-magnet concept and its application in solving target navigation problems in an obstacle-free environment. It further analyzes the application of a pair of fuzzy magnets in solving a garaging problem involving a differential-drive mobile robot. The fictitious fuzzy magnet concept allows navigation to the target in a single maneuver, without changing the direction of WMR travel. The symmetry of the differential-drive WMR is utilized fully, such that the algorithm provides a bidirectional solution to the WMR garaging problem. The robot is automatically parked from the end of the robot that is closer to the garage entrance. The algorithm can be applied when the control variable is of the discrete type and where there are relatively few quantization levels. The efficiency and shortfalls of the proposed algorithm are analyzed by means of both detailed simulations and multiple re-runs of a real experiment. Special attention is devoted to the analysis of different initial robot configurations and the effect of an error in the estimation of

The third section of this chapter analyzes the possibility of applying the fictitious fuzzy-magnet concept to navigate a mobile robot to a target in a workspace which contains one or more obstacles. At the preprocessing stage, the left and right side of the obstacle in the robot's line of sight are determined based on the mutual positions of the robot and the obstacle. Since controller inputs are based on the relative dimensions of the obstacle, the controller can be applied to obstacles of different shapes and sizes. Following the design of the above-described controllers, and the successful testing of navigation efficiency to the goal in a workspace which includes a single stationary obstacle, the algorithm was applied, without modification, to avoid an obstacle moving along a straight line at a constant speed equal to one-third of the maximum speed of the robot. Here the proposed algorithm demonstrated limited efficiency. The final section of the chapter proposes a method for a simple but efficient generalization of the algorithm to support a group of obstacles, using parallel data processing, illustrated by several computer simulations of WMR navigation to the target point through a group of obstacles. The disadvantages of the proposed algorithm when applied in complex

A given point *A*(*xA*, *yA*), is assumed to be in the Cartesian coordinate system and to represent the position of the fictitious fuzzy magnet *FM*. The *FM* is defined as an arranged pair comprised of its position *A* and an added sub-set of fuzzy rules, *FRsS* (Fuzzy Rules subSet):

The *FRsS* enables the determination of the zone of influence of the fuzzy magnet, as well as the definition of the action it causes. To establish the structure of the fuzzy rules, it is necessary to adopt input and output variables (i.e., premise variables and conclusion variables). Figure 1 shows the position of the WMR relative to the point *A*, which represents the position of the

Point (*xR*, *yR*) is the center of the WMR, and its orientation relative to the *x* axis is denoted by *θR*, such that the configuration of the WMR *qR* is unequivocally defined by three coordinates: *qR*(*xR*, *yR*, *θR*). The bold line denotes the front end of the robot. The angle formed by the

*FM* = (*A*, *FRsS*) (1)

the current position of the robot on garaging efficiency.

environments are discussed at the end of the chapter.

**2. Fictitious fuzzy magnets concept**

fictitious fuzzy magnet, *FM*.

parameters.

Fig. 1. Mutual spatial positions of wheeled mobile robot and fictitious fuzzy magnet.

longitudinal line of symmetry of the WMR, *SR*, and the segment which connects the points (*xR*, *yR*) and *A* is denoted by *αA*. If the coordinates of the WMR center are denoted by (*xR*(*k*), *yR*(*k*)) at the sample time *k*, the distance between the WMR and the fictitious fuzzy magnet, *dA*(*k*), is:

$$d\_A(k) = \sqrt{(x\_A - x\_R(k))^2 + (y\_A - y\_R(k))^2}.\tag{2}$$

The position of the WMR at the sample time *k*, relative to the point *A*, is unequivocally determined by its orientation relative to the point *A* – angle *αA*(*k*) and the distance *dA*(*k*), which will be adopted as input variables of the *FRsS*. In the general case, we will state that the output velocity variables are *v*<sup>1</sup> and *v*2. The *FRsS* of *r* rules for the discrete T-S fuzzy system is defined as:

$$\begin{array}{ll}\text{Control Rule } i: \text{ If } d\_A(k) \text{ is } \mu\_{i1} \text{ and } a\_A(k) \text{ is } \mu\_{i2} \\ \text{then } v\_1(k) = \mathbb{C}\_{i1} \text{ and } v\_2(k) = \mathbb{C}\_{i2} \end{array} \quad i = 1, 2, \ldots, r \tag{3}$$

where *dA*(*k*) and *αA*(*k*) are premise variables. The membership function which corresponds to the *i th* control rule and the *j th* premise variable is denoted by *μij*, and *Cij* are constants. System outputs are *v*1(*k*) and *v*2(*k*), obtained from:

$$
\begin{bmatrix} v\_1(k) \\ v\_2(k) \end{bmatrix} = \frac{\sum\_{i=1}^r \mu\_{i,1}(d\_A(k)) \cdot \mu\_{i,2}(\alpha\_A(k)) \cdot [\mathbb{C}\_{i,1}\mathbb{C}\_{i,2}]^T}{\sum\_{i=1}^r \mu\_{i,1}(d\_A(k)) \cdot \mu\_{i,2}(\alpha\_A(k))} \tag{4}
$$

where *μij*(*xj*(*k*)) is the degree of membership of *xj*(*k*) in *μij*. As is well known, the control of a differential drive mobile robot can be designed in two ways: using the speed of the left and right wheels, or using the linear and angular velocities of the robot. Consequently, the general definition of the *FRsS* can be written more precisely in two ways. The navigation of a differential drive mobile robot was designed in (Mitrovi´c & Ðurovi´c, 2010a) based on wheel speed control, such that the output variables of the *FRsS* are the speed of the left wheel – *vL* and the speed of the right wheel – *vR*, such that (3-4) become:

$$\begin{array}{l}\text{Control Rule } i:\text{ If } d\_A(k) \text{ is } \mu\_{i1} \text{ and } a\_A(k) \text{ is } \mu\_{i2} \\\text{then } v\_L(k) = \mathbb{C}\_{i1} \text{ and } v\_R(k) = \mathbb{C}\_{i2} \end{array} \quad i = 1, 2, \ldots, r \tag{5}$$

and the angles related to orientation at the rear {*B*, *BL*, *BR*}. The method proposed in this chapter analyzes mobile objects with an equal ability of maneuvering by front and rear pace; therefore, the objective of defined membership functions and fuzzy rules is to provide identical performance in both cases. For this reason, the membership function *B* (*Back*) is

<sup>239</sup> Fictitious Fuzzy-Magnet Concept in Solving Mobile–Robot

*B* = *S*(*B*�

where *S* represents the operator of *S* − *norm*, which corresponds to the fact that the union of sets *B*� and *B*�� produces set *B*. Since the sets *B*� and *B*�� are disjunctive, the calculation of

The distance-to-target linguistic variable *dA* is described by means of a single membership function – *near* (Fig. 2(a)), and its task is to stop the WMR in close proximity to the target. This is ensured by adjustment of free parameters *c*<sup>1</sup> and *c*2. The designed FLC is of the T-S type, with two inputs: the distance between the robot and the target – *dA* and the orientation of the robot relative to the target – *αA*, and two outputs: linear velocity *vR* and angular velocity *ω<sup>R</sup>* of the robot. The fuzzy controller rules for robot navigation to the target in an obstacle-free

*vR* −*Vm* −*Vm*/2 *Vm*/2 *Vm Vm*/2 −*Vm*/2 *ω<sup>R</sup>* 0 *ω<sup>m</sup>* −*ω<sup>m</sup>* 0 *ω<sup>m</sup>* −*ω<sup>m</sup>* Table 1. Fuzzy Logic Controller for navigation in an Obstacle–Free Environment (*FLCOFE*) –

The maximum linear velocity of the analyzed robot is *Vm* = 0.5 m/s and its maximum angular velocity is *ω<sup>m</sup>* = 7.14 rad/s. The approximate dynamic model of a mobile robot (Mitrovi´c, 2006) was used for the simulations, and the distance between robot wheels was 7 cm. Figure 3 shows WMR trajectories to the target (coordinate origin), from three initial configurations. Figure 4 shows controller outputs for the navigation scenarios depicted in Fig. 3. It is apparent from Fig. 4 that during navigation, one of the controller outputs assumes maximum values

The WMR garage parking (garaging) problem can also be viewed as a navigation problem in an environment with an obstacle, where the garage is the obstacle of a specific shape, but is also the target. When navigation problems with obstacles are solved, the goal is to achieve the ultimate configuration without colliding with the obstacle, which involves obstacle avoidance. Numerous obstacle avoidance and mobile robot navigation methods are discussed in literature. The potential field method, originally proposed by Khatib (1986) for obstacle avoidance in real-time by manipulators and mobile robots, plays a major role in solving this type of problem. A robot is navigated in a potential field which represents the sum of attractive forces originating from the target, and repulsive forces originating from the obstacles, while the analytical solution is the negative gradient of a potential function. Additionally, the resulting potential field may contain local minima where navigation ends without reaching the target, which is the greatest disadvantage of this method (Koren & Borenstein, 1991). The most frequent example of a local minimum is the case where obstacles

*B BL FL F FR BR*

, *B*��) (11)

divided into two sub-functions *B*� and *B*��, in the following manner:

*S* − *norm* is not important (Mitrovi´c & Ðurovi´c, 2010a).

Target Navigation, Obstacle Avoidance and Garaging Problems

and this reduces the duration of the navigation process.

environment are shown in Table 1.

**3. Bidirectional garage parking**

fuzzy rules base.

$$
\begin{bmatrix} v\_L(k) \\ v\_R(k) \end{bmatrix} = \frac{\sum\_{i=1}^r \mu\_{i,1}(d\_A(k)) \cdot \mu\_{i,2}(\mathfrak{a}\_A(k)) \cdot [\mathbb{C}\_{i,1} \ \mathbb{C}\_{i,2}]^T}{\sum\_{i=1}^r \mu\_{i,1}(d\_A(k)) \cdot \mu\_{i,2}(\mathfrak{a}\_A(k))} \tag{6}
$$

while in (Mitrovi´c & Ðurovi´c, 2010b) the output variables of the *FRsS* are linear and angular velocities of the robot *vR* and *ωR*, and in this case (3-4), can be written as:

$$\begin{array}{ll}\text{Control Rule } i: \text{ If } d\_A(k) \text{ is } \mu\_{i1} \text{ and } a\_A(k) \text{ is } \mu\_{i2} \\ \text{then } v\_R(k) = \mathbb{C}\_{i1} \text{ and } \omega\_R(k) = \mathbb{C}\_{i2} \end{array} \quad \text{i = 1, 2, \dots, r} \tag{7}$$

$$
\begin{bmatrix} v\_R(k) \\ \omega\_R(k) \end{bmatrix} = \frac{\sum\_{i=1}^r \mu\_{i,1}(d\_A(k)) \cdot \mu\_{i,2}(\alpha\_A(k)) \cdot [\mathbb{C}\_{i,1}\mathbb{C}\_{i,2}]^T}{\sum\_{i=1}^r \mu\_{i,1}(d\_A(k)) \cdot \mu\_{i,2}(\alpha\_A(k))} \tag{8}
$$

#### **2.1 Navigation in an obstacle-free environment**

The application of the proposed concept will be illustrated by an example involving WMR navigation to a target in an obstacle-free environment. The WMR configuration (*xR*, *yR*, *θR*) and the target position (*xA*, *yA*) are shown in Fig. 1. We will assume the robot control inputs (*vR*, *ωR*) to be the outputs of the fuzzy logic controller for navigation in an obstacle free environment (*FLCOFE*). The WMR angle relative to the target *α<sup>A</sup>* and the distance to the target *dA* are fuzzy controller inputs, and the corresponding membership functions are shown in Fig. 2.

Fig. 2. Membership functions of input linguistic variables: (a) Orientation relative to the target – *αA*, (b) Distance relative to the target *dA*.

The linguistic variable *α<sup>A</sup>* is defined through the following membership functions:

$$\mathfrak{a}\_{\mathfrak{a}}\left\{ \text{Front}, \text{Front} \text{Left}, \text{Front} \text{Right}, \text{Back}, \text{Back} \text{Left}, \text{Back}, \text{Right} \right\} \tag{9}$$

or abbreviated as:

$$\propto\_A \left\{ F, FL, FR, B, BL, BR \right\} \tag{10}$$

as shown in Figure 2(b). In order to enable bidirectional navigation, the variable *α<sup>A</sup>* is strictly divided into two groups – the angles related to orientation at the front {*F*, *FL*, *FR*}, 4 Mobile Robots / Book 1

while in (Mitrovi´c & Ðurovi´c, 2010b) the output variables of the *FRsS* are linear and angular

then *vR*(*k*) = *Ci*<sup>1</sup> and *ωR*(*k*) = *Ci*<sup>2</sup>

The application of the proposed concept will be illustrated by an example involving WMR navigation to a target in an obstacle-free environment. The WMR configuration (*xR*, *yR*, *θR*) and the target position (*xA*, *yA*) are shown in Fig. 1. We will assume the robot control inputs (*vR*, *ωR*) to be the outputs of the fuzzy logic controller for navigation in an obstacle free environment (*FLCOFE*). The WMR angle relative to the target *α<sup>A</sup>* and the distance to the target *dA* are fuzzy controller inputs, and the corresponding membership functions are shown

−90

Fig. 2. Membership functions of input linguistic variables: (a) Orientation relative to the

as shown in Figure 2(b). In order to enable bidirectional navigation, the variable *α<sup>A</sup>* is strictly divided into two groups – the angles related to orientation at the front {*F*, *FL*, *FR*},

The linguistic variable *α<sup>A</sup>* is defined through the following membership functions:

*μi*,1(*dA*(*k*)) · *μi*,2(*αA*(*k*)) · [*Ci*,1 *Ci*,2]

*μi*,1(*dA*(*k*)) · *μi*,2(*αA*(*k*))

*μi*,1(*dA*(*k*)) · *μi*,2(*αA*(*k*)) · [*Ci*,1 *Ci*,2]

*μi*,1(*dA*(*k*)) · *μi*,2(*αA*(*k*))

*<sup>B</sup>*� *BL FL <sup>F</sup> FR BR <sup>B</sup>* <sup>1</sup> ��

15

*α<sup>A</sup>* {*F*, *FL*, *FR*, *B*, *BL*, *BR*} (10)

0 −15

(b) Orientation relative to the target

*α<sup>a</sup>* {*Front*, *FrontLef t*, *FrontRight*, *Back*, *BackLef t*, *Back*, *Right*} (9)

90

angle [deg]

165

180

*T*

*T*

*i* = 1, 2, . . . ,*r* (7)

(6)

(8)

 *vL*(*k*) *vR*(*k*)

 *vR*(*k*) *ωR*(*k*)

**2.1 Navigation in an obstacle-free environment**

in Fig. 2.

1

*near*

(a) Distance to the target

*dA* [m]

target – *αA*, (b) Distance relative to the target *dA*.

0

Degree of

membership

−180

−165

0

or abbreviated as:

Degree of

membership

*c*<sup>1</sup> *c*<sup>2</sup>

 =

 =

*r* ∑ *i*=1

velocities of the robot *vR* and *ωR*, and in this case (3-4), can be written as:

*Control Rule i* : If *dA*(*k*) is *μi*<sup>1</sup> and *αA*(*k*) is *μi*<sup>2</sup>

*r* ∑ *i*=1

*r* ∑ *i*=1

*r* ∑ *i*=1 and the angles related to orientation at the rear {*B*, *BL*, *BR*}. The method proposed in this chapter analyzes mobile objects with an equal ability of maneuvering by front and rear pace; therefore, the objective of defined membership functions and fuzzy rules is to provide identical performance in both cases. For this reason, the membership function *B* (*Back*) is divided into two sub-functions *B*� and *B*��, in the following manner:

$$B = \mathcal{S}(\mathcal{B}', \mathcal{B}'') \tag{11}$$

where *S* represents the operator of *S* − *norm*, which corresponds to the fact that the union of sets *B*� and *B*�� produces set *B*. Since the sets *B*� and *B*�� are disjunctive, the calculation of *S* − *norm* is not important (Mitrovi´c & Ðurovi´c, 2010a).

The distance-to-target linguistic variable *dA* is described by means of a single membership function – *near* (Fig. 2(a)), and its task is to stop the WMR in close proximity to the target. This is ensured by adjustment of free parameters *c*<sup>1</sup> and *c*2. The designed FLC is of the T-S type, with two inputs: the distance between the robot and the target – *dA* and the orientation of the robot relative to the target – *αA*, and two outputs: linear velocity *vR* and angular velocity *ω<sup>R</sup>* of the robot. The fuzzy controller rules for robot navigation to the target in an obstacle-free environment are shown in Table 1.


Table 1. Fuzzy Logic Controller for navigation in an Obstacle–Free Environment (*FLCOFE*) – fuzzy rules base.

The maximum linear velocity of the analyzed robot is *Vm* = 0.5 m/s and its maximum angular velocity is *ω<sup>m</sup>* = 7.14 rad/s. The approximate dynamic model of a mobile robot (Mitrovi´c, 2006) was used for the simulations, and the distance between robot wheels was 7 cm. Figure 3 shows WMR trajectories to the target (coordinate origin), from three initial configurations. Figure 4 shows controller outputs for the navigation scenarios depicted in Fig. 3. It is apparent from Fig. 4 that during navigation, one of the controller outputs assumes maximum values and this reduces the duration of the navigation process.

#### **3. Bidirectional garage parking**

The WMR garage parking (garaging) problem can also be viewed as a navigation problem in an environment with an obstacle, where the garage is the obstacle of a specific shape, but is also the target. When navigation problems with obstacles are solved, the goal is to achieve the ultimate configuration without colliding with the obstacle, which involves obstacle avoidance. Numerous obstacle avoidance and mobile robot navigation methods are discussed in literature. The potential field method, originally proposed by Khatib (1986) for obstacle avoidance in real-time by manipulators and mobile robots, plays a major role in solving this type of problem. A robot is navigated in a potential field which represents the sum of attractive forces originating from the target, and repulsive forces originating from the obstacles, while the analytical solution is the negative gradient of a potential function. Additionally, the resulting potential field may contain local minima where navigation ends without reaching the target, which is the greatest disadvantage of this method (Koren & Borenstein, 1991). The most frequent example of a local minimum is the case where obstacles

(*xG*, *yG*) is inside the garage and is set to coincide with the center of the garage *Cm*, such that *Cm*(*xG*, *yG*). In the case of bidirectional garaging, the targeted configuration is not uniquely defined because the objective is for the longitudinal axis of symmetry of the robot to coincide with that of the garage, such that *δ* = 0 or *δ* = *π*, meaning that there are two solutions for the angle *θ<sup>G</sup>* : *θG*<sup>1</sup> = *β*, *θG*<sup>2</sup> = *β* + *π*. For reasons of efficiency, the choice between these two possibilities should provide the shortest travel distance of the mobile robot. The controller proposed in this section does not require *a priori* setting of the angle *θG*, because it has been designed in such a way that the mobile robot initiates the garaging process from the end closer

<sup>241</sup> Fictitious Fuzzy-Magnet Concept in Solving Mobile–Robot

*δ*

*SR* *θR*

**0** *x xG xR*

The bidirectional garaging problem is similar to the stabilization problem, also known as the "parking problem", formulated by Oriolo et al. (2002): "the robot must reach a desired configuration *qG*(*xG*, *yG*, *θG*) starting from a given initial configuration *q*(0)(*xR*(0), *yR*(0), *θR*(0))". The differences are that there are two solutions for the desired configuration in bidirectional garaging problems and that the stabilization problem need not

Although the bidirectional garaging problem is addressed in this chapter as a stabilization problem, the proposed solution can also be used as a garaging trajectory generator because it

From a human control skill perspective, the garaging of a vehicle is comprised of at least two stages: approach to the garage and entry into the garage. The adjustment of the vehicle's position inside the garage might be the third stage, but the need for this stage depends on the success of the second stage (i.e., driver skills). Since there is much less experience in differential drive mobile vehicle control than in car and car-like mobile robot control, and based on recommendations in (Sugeno & Nishida, 1985), the proposed WMR garaging system is a model of operator control actions, designed by means of two fictitious fuzzy magnets, one of which is located immediately in front of the garage and the other inside the garage. The first point is used to approach the garage and while the vehicle is entering the garage, it serves as a reference point for proper orientation, similar to human driving skills. The second point

*LG*

*Cm*

Target Navigation, Obstacle Avoidance and Garaging Problems

Fig. 5. Mutual spatial positions of wheeled mobile robot and garage.

necessarily involve constrains imposed by the presence of the garage.

includes non-holonomic constraints of the mobile robot.

**3.1 System of two fictitious fuzzy magnets**

*SG*

to the garage.

*y*

*yR yG*

*β*

*WG*

Fig. 3. Robot navigation trajectories from three initial postures (*xR*, *yR*, *θR*) to the target (0, 0).

Fig. 4. Graphs for scenarios depicted in Fig. 3: (a) WMR linear velocity, (b) WMR angular velocity.

are deployed in such a way that they form a concave **U** barrier, such that the algorithm enters into an infinite loop or halts at the local minimum (Krishna & Kalra, 2001; Motlagh et al., 2009). Since the garage is a concave obstacle, the main shortfall of the potential field method could also be a solution to the garaging problem; however, analyzed literature does not provide an example of the use of the potential field method to solve mobile robot garaging problems. A probable reason for this is that the goal of garaging is to reach the target configuration in a controlled manner, with limited deviations, while the completion of navigation at the local minimum of the potential field is accompanied by oscillatory behavior of the object being parked, or the entry into an infinite loop (Koren & Borenstein, 1991), which is certainly not a desirable conclusion of the process.

The garaging/parking problem implies that a mobile robot is guided from an initial configuration (*xR*(0), *yR*(0), *θR*(0)) to a desired one (*xG*, *yG*, *θG*), such that it does not collide with the garage. Figure 5 shows the garage and robot parameters; the position of the garage is defined by the coordinates of its center – *Cm*, while its orientation is defined by the angle of the axis of symmetry of the garage – *SG* relative to the *x* axis, identified as *β* in the figure. The width of the garage is denoted by *WG* and the length by *LG*, and it is understood that garage dimensions enable the garaging of the observed object. The implication is that the goal 6 Mobile Robots / Book 1

0

*qB*

0.2 0.4 0.6 0.8 1 *time*[s]

*qB*(0, 0.2, *π*/12)

0.1

*qC*

0.2

*qA*

Fig. 4. Graphs for scenarios depicted in Fig. 3: (a) WMR linear velocity, (b) WMR angular

are deployed in such a way that they form a concave **U** barrier, such that the algorithm enters into an infinite loop or halts at the local minimum (Krishna & Kalra, 2001; Motlagh et al., 2009). Since the garage is a concave obstacle, the main shortfall of the potential field method could also be a solution to the garaging problem; however, analyzed literature does not provide an example of the use of the potential field method to solve mobile robot garaging problems. A probable reason for this is that the goal of garaging is to reach the target configuration in a controlled manner, with limited deviations, while the completion of navigation at the local minimum of the potential field is accompanied by oscillatory behavior of the object being parked, or the entry into an infinite loop (Koren & Borenstein, 1991), which is certainly not a

The garaging/parking problem implies that a mobile robot is guided from an initial configuration (*xR*(0), *yR*(0), *θR*(0)) to a desired one (*xG*, *yG*, *θG*), such that it does not collide with the garage. Figure 5 shows the garage and robot parameters; the position of the garage is defined by the coordinates of its center – *Cm*, while its orientation is defined by the angle of the axis of symmetry of the garage – *SG* relative to the *x* axis, identified as *β* in the figure. The width of the garage is denoted by *WG* and the length by *LG*, and it is understood that garage dimensions enable the garaging of the observed object. The implication is that the goal

(a)

Fig. 3. Robot navigation trajectories from three initial postures (*xR*, *yR*, *θR*) to the target (0, 0).

0.3

0

0

−*ω<sup>m</sup>*

*ωm*

0.4 0.5 [m]

*qB*

(b)

0.2 0.4 0.6 0.8 1 *time*[s]

*qA*

*qC*

*qA*(0.5, 0, *π*/2)

*qC*(0.3, 0.3, −*π*/3)

0

0

desirable conclusion of the process.

0

−*Vm*

velocity.

*Vm*

0.1

0.2

0.3

[m]

(*xG*, *yG*) is inside the garage and is set to coincide with the center of the garage *Cm*, such that *Cm*(*xG*, *yG*). In the case of bidirectional garaging, the targeted configuration is not uniquely defined because the objective is for the longitudinal axis of symmetry of the robot to coincide with that of the garage, such that *δ* = 0 or *δ* = *π*, meaning that there are two solutions for the angle *θ<sup>G</sup>* : *θG*<sup>1</sup> = *β*, *θG*<sup>2</sup> = *β* + *π*. For reasons of efficiency, the choice between these two possibilities should provide the shortest travel distance of the mobile robot. The controller proposed in this section does not require *a priori* setting of the angle *θG*, because it has been designed in such a way that the mobile robot initiates the garaging process from the end closer to the garage.

Fig. 5. Mutual spatial positions of wheeled mobile robot and garage.

The bidirectional garaging problem is similar to the stabilization problem, also known as the "parking problem", formulated by Oriolo et al. (2002): "the robot must reach a desired configuration *qG*(*xG*, *yG*, *θG*) starting from a given initial configuration *q*(0)(*xR*(0), *yR*(0), *θR*(0))". The differences are that there are two solutions for the desired configuration in bidirectional garaging problems and that the stabilization problem need not necessarily involve constrains imposed by the presence of the garage.

Although the bidirectional garaging problem is addressed in this chapter as a stabilization problem, the proposed solution can also be used as a garaging trajectory generator because it includes non-holonomic constraints of the mobile robot.

#### **3.1 System of two fictitious fuzzy magnets**

From a human control skill perspective, the garaging of a vehicle is comprised of at least two stages: approach to the garage and entry into the garage. The adjustment of the vehicle's position inside the garage might be the third stage, but the need for this stage depends on the success of the second stage (i.e., driver skills). Since there is much less experience in differential drive mobile vehicle control than in car and car-like mobile robot control, and based on recommendations in (Sugeno & Nishida, 1985), the proposed WMR garaging system is a model of operator control actions, designed by means of two fictitious fuzzy magnets, one of which is located immediately in front of the garage and the other inside the garage. The first point is used to approach the garage and while the vehicle is entering the garage, it serves as a reference point for proper orientation, similar to human driving skills. The second point

0

1

0 −

Degree of

membership

*π*

−

**3.2 Definition of fuzzy rules**

−

*π*

−

Fig. 8. Membership functions of linguistic variable *Direction*.

−

−

*N*

*M*

*π*/2

+

*M*

*π*

+

*N*

*μf ar*(*dFm*)

1 *f ar*

*F*<sup>1</sup> *F*<sup>2</sup>

*dFm*

Fig. 7. Definition of membership functions for distances: (a) *dFm*, (b) *dCm*.

the selection of parameters and fuzzy rules will be presented later in the text.

0

of attraction of at least one fictitious fuzzy magnet at all times. A more detailed procedure for

The next step in the definition of fuzzy rules subsets is the definition of the second input variable, associated with the WMR orientation relative to the fictitious fuzzy magnets. The angles are denoted by *αFm* and *αCm* in Fig. 6. If angle *αFm* is close to zero, then the vehicle's front end is oriented toward the fictitious fuzzy magnet *Fm*, and if the value of angle *αFm* is close to *π*, the vehicle's rear end is oriented toward *Fm*. This fact is important, as will be shown later in the text, from the perspective that the objective of vehicle control will be to reduce the angles *αFm* and *αCm* to a zero or *π* level, depending on initial positioning conditions. Linguistic variables *αFm* and *αCm* are together identified as *Direction* and are described by membership functions which should point to the orientation of the WMR relative to the fictitious fuzzy magnets according to (9) and (10), whose form is shown in Fig. 8.

*B*� *BL FL F FR BR B*��

0

Since the WMR bidirectional garaging system is comprised of two fictitious fuzzy magnets, *FMFm* and *FMCm* , the fuzzy rules base is created from the rules subsets of each fuzzy magnet. It follows from the definition of the concept (3) that a fuzzy rules subset is determined by two inputs (WMR distance and orientation relative to fuzzy magnet position), meaning that the number of fuzzy controller inputs is equal to *K* × 2, where *K* is the number of fuzzy magnets which make up the system, such that the number of inputs into a fuzzy system comprised of two fuzzy magnets is four. This concept is convenient because inherence rules for each fuzzy magnet are generated from inputs related to that magnet, such that the addition of a fuzzy magnet increases the number of rules in the controller base only in its subset of fuzzy rules. We will now specifically define two fuzzy rules subsets, *FRsSFm* and *FRsSCm* . Since the

*N*

*M*

*π*/2

*π*

*π*

−

*N*

*π*

−

*M*

1

<sup>243</sup> Fictitious Fuzzy-Magnet Concept in Solving Mobile–Robot

*C*1

*μnear*(*dCm*)

*near*

*C*<sup>2</sup> *C*<sup>3</sup> *C*<sup>4</sup>

*dCm*

(b)

(a)

Target Navigation, Obstacle Avoidance and Garaging Problems

is both the target and a reference point for the WMR when it enters the garage and when it completes the garaging process. Contrary to the well-known point-to-point navigation approach, navigation aided by a system of two fuzzy magnets is executed in a single stage since the garaging process requires the robot to approach the area in front of the garage, but not to also reach the point which is the center of this area. The fictitious fuzzy magnets are denoted by *FMFm* (*Forward magnet*) and *FMCm* (*Central magnet*), and created according to (1):

$$F M\_{Fm} = (F\_{m\nu} F R s S\_{Fm})\_{\prime} \tag{12}$$

$$F M\_{\mathbb{C}\_m} = (\mathbb{C}\_m F R s S\_{\mathbb{C}m}).\tag{13}$$

In Fig. 6, their positions are denoted by *Cm* and *Fm*. The point *Fm* lies on the garage axis of symmetry *SG* at a distance *dF* from the front line. Let us imagine that these points are fictitious fuzzy magnets with attraction regions around them and if the mobile object/vehicle finds itself in that region, the attraction force will act on it. The activity of these fictitious fuzzy magnets will be neutralized in point *Cm*, thus finishing the garaging process.

Fig. 6. Mutual spatial positions of wheeled mobile robot and fictitious fuzzy magnet.

The implementation of this concept requires the definition of the interaction between the fictitious fuzzy magnets and the vehicle, consistent with the definition of the fictitious fuzzy magnets. Figure 6 also shows the parameters which define the relationship between the fictitious fuzzy magnets and the vehicle. The orientation of the WMR relative to the point *Fm* is denoted by *αFm*, and relative to the point *Cm* by *αCm*. The distance between the WMR and the fictitious fuzzy magnet *Cm* is *dCm*, while the distance to the fictitious magnet *Fm* is *dFm*. The key step in the design of such an approach is the observation of these two distances as fuzzy sets which need to be attributed appropriate membership functions. The first task of the *FMFm* is to guide the WMR to the area immediately in front of the garage, such that the linguistic variable *dFm* is assigned a single membership function, denoted by *f ar* in Fig. 7(a). The membership function was not assigned to the linguistic variable *μf ar*(*dFm*) in the standard manner because the entire universe of discourse is not covered (since *F*<sup>1</sup> > 0). This practically means that *FMFm* will have no effect on the robot when *dFm* < *F*1, but *FMCm* will; the membership function for the distance to its position *Cm* is shown in Fig. 7(b). The membership function *μnear*(*dCm*) allows the action of fuzzy magnet *Cm* to affect the WMR only when the WMR is near *FMCm*, or when the distance of *dCm* is less than *C*4. Even though no linguistic variable related to distance covers the entire set of possible values, the WMR is in the region 8 Mobile Robots / Book 1

is both the target and a reference point for the WMR when it enters the garage and when it completes the garaging process. Contrary to the well-known point-to-point navigation approach, navigation aided by a system of two fuzzy magnets is executed in a single stage since the garaging process requires the robot to approach the area in front of the garage, but not to also reach the point which is the center of this area. The fictitious fuzzy magnets are denoted by *FMFm* (*Forward magnet*) and *FMCm* (*Central magnet*), and created according to (1):

In Fig. 6, their positions are denoted by *Cm* and *Fm*. The point *Fm* lies on the garage axis of symmetry *SG* at a distance *dF* from the front line. Let us imagine that these points are fictitious fuzzy magnets with attraction regions around them and if the mobile object/vehicle finds itself in that region, the attraction force will act on it. The activity of these fictitious fuzzy

*αCm*

Fig. 6. Mutual spatial positions of wheeled mobile robot and fictitious fuzzy magnet.

The implementation of this concept requires the definition of the interaction between the fictitious fuzzy magnets and the vehicle, consistent with the definition of the fictitious fuzzy magnets. Figure 6 also shows the parameters which define the relationship between the fictitious fuzzy magnets and the vehicle. The orientation of the WMR relative to the point *Fm* is denoted by *αFm*, and relative to the point *Cm* by *αCm*. The distance between the WMR and the fictitious fuzzy magnet *Cm* is *dCm*, while the distance to the fictitious magnet *Fm* is *dFm*. The key step in the design of such an approach is the observation of these two distances as fuzzy sets which need to be attributed appropriate membership functions. The first task of the *FMFm* is to guide the WMR to the area immediately in front of the garage, such that the linguistic variable *dFm* is assigned a single membership function, denoted by *f ar* in Fig. 7(a). The membership function was not assigned to the linguistic variable *μf ar*(*dFm*) in the standard manner because the entire universe of discourse is not covered (since *F*<sup>1</sup> > 0). This practically means that *FMFm* will have no effect on the robot when *dFm* < *F*1, but *FMCm* will; the membership function for the distance to its position *Cm* is shown in Fig. 7(b). The membership function *μnear*(*dCm*) allows the action of fuzzy magnet *Cm* to affect the WMR only when the WMR is near *FMCm*, or when the distance of *dCm* is less than *C*4. Even though no linguistic variable related to distance covers the entire set of possible values, the WMR is in the region

*dFm*

magnets will be neutralized in point *Cm*, thus finishing the garaging process.

*dCm*

*αFm*

*SG*

*Fm*

*dF*

*Cm*

*FMFm* = (*Fm*, *FRsSFm*), (12)

*FMCm* = (*Cm*, *FRsSCm*). (13)

*SR*

Fig. 7. Definition of membership functions for distances: (a) *dFm*, (b) *dCm*.

of attraction of at least one fictitious fuzzy magnet at all times. A more detailed procedure for the selection of parameters and fuzzy rules will be presented later in the text.

The next step in the definition of fuzzy rules subsets is the definition of the second input variable, associated with the WMR orientation relative to the fictitious fuzzy magnets. The angles are denoted by *αFm* and *αCm* in Fig. 6. If angle *αFm* is close to zero, then the vehicle's front end is oriented toward the fictitious fuzzy magnet *Fm*, and if the value of angle *αFm* is close to *π*, the vehicle's rear end is oriented toward *Fm*. This fact is important, as will be shown later in the text, from the perspective that the objective of vehicle control will be to reduce the angles *αFm* and *αCm* to a zero or *π* level, depending on initial positioning conditions. Linguistic variables *αFm* and *αCm* are together identified as *Direction* and are described by membership functions which should point to the orientation of the WMR relative to the fictitious fuzzy magnets according to (9) and (10), whose form is shown in Fig. 8.

Fig. 8. Membership functions of linguistic variable *Direction*.

#### **3.2 Definition of fuzzy rules**

Since the WMR bidirectional garaging system is comprised of two fictitious fuzzy magnets, *FMFm* and *FMCm* , the fuzzy rules base is created from the rules subsets of each fuzzy magnet. It follows from the definition of the concept (3) that a fuzzy rules subset is determined by two inputs (WMR distance and orientation relative to fuzzy magnet position), meaning that the number of fuzzy controller inputs is equal to *K* × 2, where *K* is the number of fuzzy magnets which make up the system, such that the number of inputs into a fuzzy system comprised of two fuzzy magnets is four. This concept is convenient because inherence rules for each fuzzy magnet are generated from inputs related to that magnet, such that the addition of a fuzzy magnet increases the number of rules in the controller base only in its subset of fuzzy rules. We will now specifically define two fuzzy rules subsets, *FRsSFm* and *FRsSCm* . Since the

**3.3 Selection of fuzzy controller parameters**

Target Navigation, Obstacle Avoidance and Garaging Problems

the first iteration, observing the following limitation:

*dF* > 

*C*1, *C*2, *C*3, and *C*4.

**3.3.1 Parameter** *dF*

**3.3.2 Parameters** *M* **and** *N*

**3.3.3 Coefficients** *C*3, *C*4, *F*1**, and** *F*<sup>2</sup>

However, with regard to the endless set of possible combinations of input variables, the selection of fuzzy rules does not guarantee that the vehicle will stop at the desired position. It is therefore, necessary to pay close attention to the selection of parameters *dF*, *M*, *N*, *F*1, *F*2,

<sup>245</sup> Fictitious Fuzzy-Magnet Concept in Solving Mobile–Robot

*FMFm* is described by (12) and, before the parameters which define the membership functions of the linguistic variables of *FRsSFm* are determined, the position *Fm* of *FMFm* needs to be established. In Fig. 6, the point *Fm* is located on the longitudinal axis of symmetry of the garage and its distance from the garage door is *dF*. The parameter *dF* should be adopted in

where *WR* is the robot width, and *LR* is the robot length, with the recommendation *dF* ≈ *WR*.

The selection of parameters *M* and *N*, in their geometrical sense as presented in Fig. 8, influences the nature of the maneuver (curve and speed of turning) performed by the vehicle during garaging, especially at the beginning of the garaging process when the robot is at some distance from the garage. When the values of *M* and *N* are low, the vehicle rotates with a very small curve diameter; as these values increase, the arches circumscribed by the vehicle also increase (Mitrovi´c & Ðurovi´c, 2010a). During the selection of these values, a compromise must be made to maintain the maneuvering capabilities of the vehicle, as well as the constraints imposed by geometry, namely, the ratio between the vehicle dimensions and the width of the garage. Generally, these parameters are adjusted in such a way that the vehicle circumscribes larger arches when distant from the garage, whereas more vivid maneuvers are needed in the vicinity of the garage. As such, in the second iteration, parameters *M* and *N* related to input *αFm* are adjusted, followed by those related to the input *αCm*, with the following constraints:

The trapezoidal membership function *f ar* of the linguistic variable *dFm* (Fig. 7(a)) is determined by the coefficients *F*<sup>1</sup> and *F*2, while the membership function *near* of the linguistic variable *dCm* is determined by the coefficients *C*1, *C*2, *C*<sup>3</sup> and *C*<sup>4</sup> (Fig. 7(a)). The coefficients *C*3, *C*4, *F*<sup>1</sup> and *F*<sup>2</sup> enable gradual activation of fictitious magnet *Cm* and the deactivation of fictitious magnet *Fm*, as the robot approaches its destination. The selection of these parameters has a critical impact on the performance of the entire fuzzy controller. Inequalities (17) and (18) guarantee the form of the membership functions shown in Fig. 7(a) and Fig. 7(b), respectively.

Figure 9(a) illustrates the action of *FMFm* . Outside the circle of radius *F*<sup>2</sup> , with its center at *Fm* (dark gray in the figure), this fuzzy magnet exhibits maximum activity. When the WMR is

(*WR*/2)<sup>2</sup> + (*LR*/2)2, (15)

*π*/2 > *M* ≥ *N* > 0. (16)

*C*<sup>4</sup> > *C*<sup>3</sup> > 0 (17)

*F*<sup>2</sup> > *F*<sup>1</sup> ≥ 0 (18)

fuzzy rules subsets are created for the same object, the output variables are defined equally, regardless of whether concept (5-6) or (7-8) is applied.

The definition of output variables of the FLC requires a clear definition of the object of control. The algorithm described in this chapter is primarily intended for the control of two-wheeled, differential drive mobile robots. The wheels of this type of vehicle are excited independently and their speeds are defined autonomously. Guidance is controlled by the wheel speed difference, and the fuzzy rules subsets will be generated in the form of (5-6). We will assume that the physical characteristics of the actuators of both wheels are identical, and that the object has identical front- and back-drive capabilities. Since the inherence rules will be defined in general, we will assume certain general characteristics of the object of control, which will not affect the practical application of the concept. The maximum wheel speed will be denoted by the positive constant *Vm*, where −*Vm* means that the wheel is rotating at maximum velocity which induces the object to move in reverse. We will also assume a nonnegative constant *V*1, which satisfies the condition:

$$V\_1 \le V\_m.\tag{14}$$

Fuzzy rules are defined based on the following principles (Mitrovi´c & Ðurovi´c, 2010a):


We will identify the fuzzy logic controller for bidirectional garaging using the fictitious fuzzy magnet concept as *FLCBG*. The fuzzy rules base of the *FLCBG* is comprised of two subsets: the *FRsSFm* subset, which corresponds to the fictitious fuzzy magnet *FMFm*, shown in Table 2(a), and the *FRsSCm* subset shown in Table 2(b).




(b) Fuzzy rules subset *FRsSCm*

Table 2. Fuzzy rules base for bidirectional garaging fuzzy controller – *FLCBG*.

Membership functions of input variables *dFm* and *dCm* independently activate particular rules (Table 2), an action which results in a considerable reduction in the number of rules. The rule which produces zero commands on both wheels does not exist; this might lead to the wrong conclusion that the vehicle never stops and does not take a final position. Since the points *Cm* and *Fm* are on the garage axis of symmetry, when the vehicle finds itself near its final position the difference between orientation angles *αFm* and *αCm* is close to ±*π*. In this case, at least two rules generating opposite commands are activated, and their influence becomes annulled (Mitrovi´c & Ðurovi´c, 2010a).

#### **3.3 Selection of fuzzy controller parameters**

However, with regard to the endless set of possible combinations of input variables, the selection of fuzzy rules does not guarantee that the vehicle will stop at the desired position. It is therefore, necessary to pay close attention to the selection of parameters *dF*, *M*, *N*, *F*1, *F*2, *C*1, *C*2, *C*3, and *C*4.

#### **3.3.1 Parameter** *dF*

10 Mobile Robots / Book 1

fuzzy rules subsets are created for the same object, the output variables are defined equally,

The definition of output variables of the FLC requires a clear definition of the object of control. The algorithm described in this chapter is primarily intended for the control of two-wheeled, differential drive mobile robots. The wheels of this type of vehicle are excited independently and their speeds are defined autonomously. Guidance is controlled by the wheel speed difference, and the fuzzy rules subsets will be generated in the form of (5-6). We will assume that the physical characteristics of the actuators of both wheels are identical, and that the object has identical front- and back-drive capabilities. Since the inherence rules will be defined in general, we will assume certain general characteristics of the object of control, which will not affect the practical application of the concept. The maximum wheel speed will be denoted by the positive constant *Vm*, where −*Vm* means that the wheel is rotating at maximum velocity which induces the object to move in reverse. We will also assume a nonnegative constant *V*1,

Fuzzy rules are defined based on the following principles (Mitrovi´c & Ðurovi´c, 2010a): • As the distance from the vehicle to the garage grows, the total speed of the vehicle should

• As the distance from the vehicle to the garage shrinks, the total speed of the vehicle should

• The difference between the wheel speeds causes turning, which must depend on the

We will identify the fuzzy logic controller for bidirectional garaging using the fictitious fuzzy magnet concept as *FLCBG*. The fuzzy rules base of the *FLCBG* is comprised of two subsets: the *FRsSFm* subset, which corresponds to the fictitious fuzzy magnet *FMFm*, shown in Table 2(a),

*V*<sup>1</sup> ≤ *Vm*. (14)

Rule *dCm αCm vL vR* 7. *near B* −*Vm* −*Vm* 8. *near BL* 0 −*V*<sup>1</sup> 9. *near FL* 0 *V*<sup>1</sup> 10. *near F Vm Vm* 11. *near FR V*<sup>1</sup> 0 12. *near BR* −*V*<sup>1</sup> 0 (b) Fuzzy rules subset *FRsSCm*

regardless of whether concept (5-6) or (7-8) is applied.

which satisfies the condition:

robot's orientation toward the garage;

and the *FRsSCm* subset shown in Table 2(b).

Rule *dFm αFm vL vR* 1. *f ar B* −*Vm* −*Vm* 2. *f ar BL* 0 −*Vm* 3. *f ar FL* 0 *Vm* 4. *f ar F Vm Vm* 5. *f ar FR Vm* 0 6. *f ar BR* −*Vm* 0 (a) Fuzzy rules subset *FRsSFm*

(Mitrovi´c & Ðurovi´c, 2010a).

• In the case of good orientation, the robot speed may be maximal.

Table 2. Fuzzy rules base for bidirectional garaging fuzzy controller – *FLCBG*.

Membership functions of input variables *dFm* and *dCm* independently activate particular rules (Table 2), an action which results in a considerable reduction in the number of rules. The rule which produces zero commands on both wheels does not exist; this might lead to the wrong conclusion that the vehicle never stops and does not take a final position. Since the points *Cm* and *Fm* are on the garage axis of symmetry, when the vehicle finds itself near its final position the difference between orientation angles *αFm* and *αCm* is close to ±*π*. In this case, at least two rules generating opposite commands are activated, and their influence becomes annulled

increase;

drop;

*FMFm* is described by (12) and, before the parameters which define the membership functions of the linguistic variables of *FRsSFm* are determined, the position *Fm* of *FMFm* needs to be established. In Fig. 6, the point *Fm* is located on the longitudinal axis of symmetry of the garage and its distance from the garage door is *dF*. The parameter *dF* should be adopted in the first iteration, observing the following limitation:

$$d\_{\rm F} > \sqrt{(W\_{\rm R}/2)^2 + (L\_{\rm R}/2)^2},\tag{15}$$

where *WR* is the robot width, and *LR* is the robot length, with the recommendation *dF* ≈ *WR*.

#### **3.3.2 Parameters** *M* **and** *N*

The selection of parameters *M* and *N*, in their geometrical sense as presented in Fig. 8, influences the nature of the maneuver (curve and speed of turning) performed by the vehicle during garaging, especially at the beginning of the garaging process when the robot is at some distance from the garage. When the values of *M* and *N* are low, the vehicle rotates with a very small curve diameter; as these values increase, the arches circumscribed by the vehicle also increase (Mitrovi´c & Ðurovi´c, 2010a). During the selection of these values, a compromise must be made to maintain the maneuvering capabilities of the vehicle, as well as the constraints imposed by geometry, namely, the ratio between the vehicle dimensions and the width of the garage. Generally, these parameters are adjusted in such a way that the vehicle circumscribes larger arches when distant from the garage, whereas more vivid maneuvers are needed in the vicinity of the garage. As such, in the second iteration, parameters *M* and *N* related to input *αFm* are adjusted, followed by those related to the input *αCm*, with the following constraints:

$$
\pi/2 > M \ge N > 0.\tag{16}
$$

#### **3.3.3 Coefficients** *C*3, *C*4, *F*1**, and** *F*<sup>2</sup>

The trapezoidal membership function *f ar* of the linguistic variable *dFm* (Fig. 7(a)) is determined by the coefficients *F*<sup>1</sup> and *F*2, while the membership function *near* of the linguistic variable *dCm* is determined by the coefficients *C*1, *C*2, *C*<sup>3</sup> and *C*<sup>4</sup> (Fig. 7(a)). The coefficients *C*3, *C*4, *F*<sup>1</sup> and *F*<sup>2</sup> enable gradual activation of fictitious magnet *Cm* and the deactivation of fictitious magnet *Fm*, as the robot approaches its destination. The selection of these parameters has a critical impact on the performance of the entire fuzzy controller. Inequalities (17) and (18) guarantee the form of the membership functions shown in Fig. 7(a) and Fig. 7(b), respectively.

$$\mathsf{C}\_{4} > \mathsf{C}\_{3} > 0 \tag{17}$$

$$F\_2 > F\_1 \ge 0 \tag{18}$$

Figure 9(a) illustrates the action of *FMFm* . Outside the circle of radius *F*<sup>2</sup> , with its center at *Fm* (dark gray in the figure), this fuzzy magnet exhibits maximum activity. When the WMR is

**3.3.4 Coefficients** *C*<sup>1</sup> **and** *C*<sup>2</sup>

**3.4 Simulation results**

robot, are presented in Table 4.

concept.

experimentally, observing the following constraints:

Target Navigation, Obstacle Avoidance and Garaging Problems

same sign, which in turn causes oscillations around the point *Cm*.

domain of membership functions attributed to the output variables.

The selection of the above parameters might enable vehicle stopping near point *Cm*. The simultaneous adjustment of the garaging path and stopping at the target position, due to a large number of free parameters, requires a compromise which may have a considerable impact on the quality of the controller. The introduction of parameters *C*<sup>1</sup> and *C*<sup>2</sup> enables unharnessed adjustment of vehicle stopping. The selection of parameters *C*<sup>1</sup> and *C*2, enabled by theoretically exact stopping of vehicles at point *Cm*, will cause oscillations in movement around point *Cm* due to the dynamics of the vehicle, a factor which was neglected in the process of controller design. Accordingly, the selected desired final point of garaging is in the immediate vicinity of the fictitious magnet *Cm* location. Coefficients *C*<sup>1</sup> and *C*<sup>2</sup> take into account the neglected dynamics of the vehicle, and their adjustment is performed

<sup>247</sup> Fictitious Fuzzy-Magnet Concept in Solving Mobile–Robot

The above relation ensures that at least two opposed rule-generating commands are always activated around the point *Cm*. If the variable *dFm* assumes a value larger than *LG*/2 + *dF*, meaning that the vehicle has practically surpassed the target point, the angles *αCm* and *αFm* will become nearly equal, and results in the activation of rules that generate commands of the

An approximate dynamic model of the *Hemisson* robot (Mitrovi´c, 2006) was used for the simulations. This is a mobile vehicle of symmetrical shape, with two wheels and a differential drive. Wheels are independently excited, and their speeds may be defined independently from each other. Guidance is controlled by the difference in wheel speeds. Each of these speeds can be set as one of the integer values in the interval [−9, 9], (Table 3), where a negative value means a change in the direction of wheel rotation. The dependence of the command on wheel speed is not exactly linear causing certain additional problems in the process of vehicle control (Miškovi´c et al., 2002). Specifically, for the subject case, the selected output variables of the FLC are the speed commands of the left and right wheels: *VL* and *VR*, respectively. Accordingly, this discrete set of integer values from the interval [−9, 9] is selected as the

Wheel Speed Command 0 1 2 3 4 5 6 7 8 9 Wheel Speed [ mm/s] 0 3 18 42 64 86 109 134 156 184

The sampling period of the FLC was *Ts* = 0.2 s. The size of the garage was 16 cm × 20 cm, and the size of the robot was 10 cm ×12 cm. The *FLCBG* was designed to conform to the constraints defined in the previous section, with the objective of ensuring successful garaging. The experimentally-determined values of these parameters, for the examined case of the Hemisson

Figure 10 shows WMR garaging trajectories for a set of 17 different initial conditions. Depending on the initial configuration, the WMR is guided to the target by forward or back drive, which illustrates the bidirectional feature of the proposed fictitious fuzzy magnets

Table 3. Wheel speed of the Hemisson robot as a function of wheel speed command.

*dCm* < *LG*/2 ⇒ *dFm* ≤ *LG*/2 + *dF*. (21)

within this region, the distance between the robot and the point *Fm*, which was earlier denoted by *dFm* is greater than *F*<sup>2</sup> and *μf ar*(*dFm* > *F*2) = 1, as shown in Fig. 7(a).

(a) Activity of fictitious fuzzy magnet *FMFm*. (b) Activity of fictitious fuzzy magnet *FMCm*.

Fig. 9. Illustration of fictitious fuzzy magnet activity.

The activity of *FMFm* declines as the position of the robot approaches the point *Fm*, through to the edge of the circle of radius *F*1, inside which the activity of this fuzzy magnet equals zero (i.e., *μf ar*(*dFm* ≤ *F*1) = 0). To facilitate adjustment of the overlap region of the two fuzzy magnets, it is convenient for the point *Cm*, which determines both the position of *FMCm* and the position of the center of the garage, to lie within the circle of radius *F*2, which can be expressed, based on Fig. 9(a), by the following inequality:

$$\rm{\_{L}F\_{2}} \ge L\_{G}/2 + d\_{F}.\tag{19}$$

The coefficients *C*<sup>1</sup> and *C*<sup>2</sup> (Fig. 7(b)) should initially be set at *C*<sup>1</sup> = 1 and *C*<sup>2</sup> = 0. Figure 9(b) illustrates the activity of *FMCm*. Outside the circle of radius *C*4, whose center is at *Cm*, the activity of this fuzzy magnet is equal to zero (i.e., *μnear*(*dCm* > *C*4) = 0). The activity of *FMCm* increases as the robot approaches the point *Cm*, through to the edge of the circle of radius *C*3, within which the activity of this fuzzy magnet is maximal (i.e., *μnear*(*dCm* ≤ *C*3) = 1), shown in dark gray in the figure). To ensure that at least one fuzzy magnet is active at any point in time, the following constraint must strictly be observed:

$$\mathbb{C}\_{4} > L\_{\mathbb{G}}/2 + d\_{\mathbb{F}} + F\_{\mathbb{I}}.\tag{20}$$

This constraint ensures that the circle of radius *F*<sup>1</sup> is included in the circle of radius *C*4, which practically means that the vehicle is within the region of attraction of at least one fuzzy magnet at all times. The dominant impact on vehicle garaging is obtained by overlapping fictitious fuzzy magnet attraction regions, and proper vehicle garaging is achieved through the adjustment of the region in which both fictitious fuzzy magnets are active.

#### **3.3.4 Coefficients** *C*<sup>1</sup> **and** *C*<sup>2</sup>

12 Mobile Robots / Book 1

within this region, the distance between the robot and the point *Fm*, which was earlier denoted

The activity of *FMFm* declines as the position of the robot approaches the point *Fm*, through to the edge of the circle of radius *F*1, inside which the activity of this fuzzy magnet equals zero (i.e., *μf ar*(*dFm* ≤ *F*1) = 0). To facilitate adjustment of the overlap region of the two fuzzy magnets, it is convenient for the point *Cm*, which determines both the position of *FMCm* and the position of the center of the garage, to lie within the circle of radius *F*2, which can be

The coefficients *C*<sup>1</sup> and *C*<sup>2</sup> (Fig. 7(b)) should initially be set at *C*<sup>1</sup> = 1 and *C*<sup>2</sup> = 0. Figure 9(b) illustrates the activity of *FMCm*. Outside the circle of radius *C*4, whose center is at *Cm*, the activity of this fuzzy magnet is equal to zero (i.e., *μnear*(*dCm* > *C*4) = 0). The activity of *FMCm* increases as the robot approaches the point *Cm*, through to the edge of the circle of radius *C*3, within which the activity of this fuzzy magnet is maximal (i.e., *μnear*(*dCm* ≤ *C*3) = 1), shown in dark gray in the figure). To ensure that at least one fuzzy magnet is active at any point in

This constraint ensures that the circle of radius *F*<sup>1</sup> is included in the circle of radius *C*4, which practically means that the vehicle is within the region of attraction of at least one fuzzy magnet at all times. The dominant impact on vehicle garaging is obtained by overlapping fictitious fuzzy magnet attraction regions, and proper vehicle garaging is achieved through

the adjustment of the region in which both fictitious fuzzy magnets are active.

*Cm Fm*

*C*3

*F*1

(b) Activity of fictitious fuzzy magnet *FMCm*.

*F*<sup>2</sup> ≥ *LG*/2 + *dF*. (19)

*C*<sup>4</sup> > *LG*/2 + *dF* + *F*1. (20)

*C*4

*F*2

by *dFm* is greater than *F*<sup>2</sup> and *μf ar*(*dFm* > *F*2) = 1, as shown in Fig. 7(a).

*F*2

*Cm Fm*

*C*3

*F*1

(a) Activity of fictitious fuzzy magnet *FMFm*.

Fig. 9. Illustration of fictitious fuzzy magnet activity.

expressed, based on Fig. 9(a), by the following inequality:

time, the following constraint must strictly be observed:

*C*4

The selection of the above parameters might enable vehicle stopping near point *Cm*. The simultaneous adjustment of the garaging path and stopping at the target position, due to a large number of free parameters, requires a compromise which may have a considerable impact on the quality of the controller. The introduction of parameters *C*<sup>1</sup> and *C*<sup>2</sup> enables unharnessed adjustment of vehicle stopping. The selection of parameters *C*<sup>1</sup> and *C*2, enabled by theoretically exact stopping of vehicles at point *Cm*, will cause oscillations in movement around point *Cm* due to the dynamics of the vehicle, a factor which was neglected in the process of controller design. Accordingly, the selected desired final point of garaging is in the immediate vicinity of the fictitious magnet *Cm* location. Coefficients *C*<sup>1</sup> and *C*<sup>2</sup> take into account the neglected dynamics of the vehicle, and their adjustment is performed experimentally, observing the following constraints:

$$d\_{\mathbb{C}m} < L\_{\mathbb{G}}/2 \Rightarrow d\_{\mathbb{F}m} \leq L\_{\mathbb{G}}/2 + d\_{\mathbb{F}}.\tag{21}$$

The above relation ensures that at least two opposed rule-generating commands are always activated around the point *Cm*. If the variable *dFm* assumes a value larger than *LG*/2 + *dF*, meaning that the vehicle has practically surpassed the target point, the angles *αCm* and *αFm* will become nearly equal, and results in the activation of rules that generate commands of the same sign, which in turn causes oscillations around the point *Cm*.

#### **3.4 Simulation results**

An approximate dynamic model of the *Hemisson* robot (Mitrovi´c, 2006) was used for the simulations. This is a mobile vehicle of symmetrical shape, with two wheels and a differential drive. Wheels are independently excited, and their speeds may be defined independently from each other. Guidance is controlled by the difference in wheel speeds. Each of these speeds can be set as one of the integer values in the interval [−9, 9], (Table 3), where a negative value means a change in the direction of wheel rotation. The dependence of the command on wheel speed is not exactly linear causing certain additional problems in the process of vehicle control (Miškovi´c et al., 2002). Specifically, for the subject case, the selected output variables of the FLC are the speed commands of the left and right wheels: *VL* and *VR*, respectively. Accordingly, this discrete set of integer values from the interval [−9, 9] is selected as the domain of membership functions attributed to the output variables.


Table 3. Wheel speed of the Hemisson robot as a function of wheel speed command.

The sampling period of the FLC was *Ts* = 0.2 s. The size of the garage was 16 cm × 20 cm, and the size of the robot was 10 cm ×12 cm. The *FLCBG* was designed to conform to the constraints defined in the previous section, with the objective of ensuring successful garaging. The experimentally-determined values of these parameters, for the examined case of the Hemisson robot, are presented in Table 4.

Figure 10 shows WMR garaging trajectories for a set of 17 different initial conditions. Depending on the initial configuration, the WMR is guided to the target by forward or back drive, which illustrates the bidirectional feature of the proposed fictitious fuzzy magnets concept.

position

*N*[%]

**0**

conditions.

5 57 203

420

454

441

362

(a) Histograms of WMR distances from final position

0.3 0.6 0.9 1.2 1.5 1.8 2.1 2.4 2.7

than those seen in the experiment illustrated in Fig. 11.

0.6 1.2 1.8 2.4 3 3.6 4.2 4.8

127

59

35

25

283

328

(a) Histograms of WMR distances from final

**3.5 Robustness of the system**

1609

*dCm*[ cm] <sup>283</sup>

Target Navigation, Obstacle Avoidance and Garaging Problems

<sup>183</sup> <sup>66</sup> <sup>14</sup> <sup>1</sup>

Fig. 11. Results of simulations with *FLCBG*, for the set of initial conditions *Nd*.

orientation

Since the system was designed for use with a real vehicle, its efficiency needed to be examined under less-than-ideal conditions. To operate the system, the positions and orientations of the robot for each selection period must be determined, whereby the robot position and orientation need not necessarily coincide with the real position and orientation. Since inputs into the system consist of two distances and two angles whose accuracy depends directly on the accuracy of the determination of the position of the robot during the garaging process, the effect of inaccuracies of the vehicle coordinates on the efficiency of the garaging process were analyzed. The set of experiments was repeated for the *FLCBG* with *Nd* initial conditions, and the WMR coordinate determination error was modeled by noise with uniform distribution within the range [−1 cm, 1 cm]. Figure 12 shows histograms of distance and orientation deviations from the targeted configuration, under the conditions of simulated sensor noise. It was found that the system retained its functionality but that the deviations were greater

*dCm*[ cm]

1208

849

348

65

*N*[%]

**0**

final orientation

1 2 3 4 5 6 7 8 9 10

(b) Histograms of WMR deviations from

5 3 222

*δ*[ deg]

9 2 2

Fig. 12. Results of simulations with *FLCBG* with simulated sensor noise, for the *Nd* initial

0.5 1 1.5 2 2.5

(b) Histograms of WMR deviations from final

1022

440

*<sup>δ</sup>*[ deg] <sup>57</sup>

186

779

*N*[%]

<sup>249</sup> Fictitious Fuzzy-Magnet Concept in Solving Mobile–Robot

*N*[%]


Table 4. *FLCBG* – coefficient values.

Fig. 10. Illustrative trajectories for front- and back-drive garaging

To test the efficiency and applicability of the proposed method using an extended set of initial configurations, a new test set of *N* initial conditions was created. This set of initial configurations (*xRi*,(*t*=0), *yRi*,(*t*=0), and *θRi*,(*t*=0)), *i* = 1 : *N*, was formed such that the robot is placed at equidistant points on the *x* and *y* axes spaced 1 cm apart, while the angle *θRi*,(*t*=0) was a uniform-distribution random variable in the interval [−*π*/2, *π*/2]. The test configuration set was divided into two sub-sets: the first satisfies the condition that the robot in its initial configuration is at an adequate distance from the garage and contains *Nd* elements, while the other includes initial configurations near the garage and contains *Nc* = *N* − *Nd* elements. Garaging performance was measured by the distance of the robot from the target position *dCm* (Fig. 6), and the angle *δ* (Fig. 5), which showed that the robot and garage axes of symmetry did not coincide:

$$\delta = |(\beta - \theta\_{\mathbb{R}}) \bmod \pi| \tag{22}$$

The results of simulations with the *FLCBG*, for the set of initial conditions *Nd*, are shown in Fig. 11. In all *Nd* cases, the garaging process was completed with no collision occurring between the robot and the garage. In view of the non-linear setting of the speeds and the long sampling period, the conclusion is that the results of garaging were satisfactory. The average deviation in *Nd* cases was *dCm* = 1.1 cm, while the average angle error was *δ* = 1.4◦.

position orientation

Fig. 11. Results of simulations with *FLCBG*, for the set of initial conditions *Nd*.

#### **3.5 Robustness of the system**

14 Mobile Robots / Book 1

Coeff. Value Coeff. Value *M* (*α*1) 7*π*/36 *C*<sup>1</sup> 20/63 *N* (*α*1) *π*/12 *C*<sup>2</sup> 4.3 cm *M* (*α*2) *π*/4 *C*<sup>3</sup> 16 cm *N* (*α*2) *π*/18 *C*<sup>4</sup> 40.0 cm *F*<sup>1</sup> 4.0 cm *dF* 11.0 cm *F*<sup>2</sup> 44.5 cm *V*<sup>1</sup> Command 3

Table 4. *FLCBG* – coefficient values.

symmetry did not coincide:

Fig. 10. Illustrative trajectories for front- and back-drive garaging

To test the efficiency and applicability of the proposed method using an extended set of initial configurations, a new test set of *N* initial conditions was created. This set of initial configurations (*xRi*,(*t*=0), *yRi*,(*t*=0), and *θRi*,(*t*=0)), *i* = 1 : *N*, was formed such that the robot is placed at equidistant points on the *x* and *y* axes spaced 1 cm apart, while the angle *θRi*,(*t*=0) was a uniform-distribution random variable in the interval [−*π*/2, *π*/2]. The test configuration set was divided into two sub-sets: the first satisfies the condition that the robot in its initial configuration is at an adequate distance from the garage and contains *Nd* elements, while the other includes initial configurations near the garage and contains *Nc* = *N* − *Nd* elements. Garaging performance was measured by the distance of the robot from the target position *dCm* (Fig. 6), and the angle *δ* (Fig. 5), which showed that the robot and garage axes of

The results of simulations with the *FLCBG*, for the set of initial conditions *Nd*, are shown in Fig. 11. In all *Nd* cases, the garaging process was completed with no collision occurring between the robot and the garage. In view of the non-linear setting of the speeds and the long sampling period, the conclusion is that the results of garaging were satisfactory. The average

deviation in *Nd* cases was *dCm* = 1.1 cm, while the average angle error was *δ* = 1.4◦.

*δ* = |(*β* − *θR*) mod *π*| (22)

Since the system was designed for use with a real vehicle, its efficiency needed to be examined under less-than-ideal conditions. To operate the system, the positions and orientations of the robot for each selection period must be determined, whereby the robot position and orientation need not necessarily coincide with the real position and orientation. Since inputs into the system consist of two distances and two angles whose accuracy depends directly on the accuracy of the determination of the position of the robot during the garaging process, the effect of inaccuracies of the vehicle coordinates on the efficiency of the garaging process were analyzed. The set of experiments was repeated for the *FLCBG* with *Nd* initial conditions, and the WMR coordinate determination error was modeled by noise with uniform distribution within the range [−1 cm, 1 cm]. Figure 12 shows histograms of distance and orientation deviations from the targeted configuration, under the conditions of simulated sensor noise. It was found that the system retained its functionality but that the deviations were greater than those seen in the experiment illustrated in Fig. 11.

Fig. 12. Results of simulations with *FLCBG* with simulated sensor noise, for the *Nd* initial conditions.

*qG*, *qR*(*t*=0)

<sup>251</sup> Fictitious Fuzzy-Magnet Concept in Solving Mobile–Robot

*dCm*, *αCm*, *dFm*, *αFm*

*FLCBG*

*qR*(*t*) estimation

*VL* = 0 & *VR* = 0

Fig. 14. Block diagram of real-time experiment: (*A*) Determining of initial conditions; (*B*) Calculation of: *dCm*, *dFm*, *αCm* and *αFm*; (*C*) Fuzzy logic controller for bidirectional garaging; (*D*) Bluetooth interface; (*E*) Hemisson mobile robot; (*F*) Web camera; (*G*) Estimation of the

A personal computer with a Bluetooth interface (*D*) and web camera (*F*) – resolution 640 × 480 pixels, were used during the experiment. The initial conditions were determined prior to the initiation of the garaging process, namely (*A*): the position and orientation of the garage *qG*, and the posture (position and orientation) of the robot *qR*(*t*=0). Based on the posture of the robot and the position and orientation of the garage, in block (*B*), input variables *dCm*, *dFm*, *αCm* and *αFm* were calculated for the *FLCBG* (*C*). FLC outputs were wheel speed commands for the Hemisson robot (*VL* and *VR*), which were issued to the robot via the Bluetooth interface. The web camera performed a successive acquisition of frames in real time, with a 0.2 s repetition time, which dictated the sampling period for the entire fuzzy controller. In block (*G*), web camera frames were used to estimate the position of the robot and compute its orientation (Ðurovi´c & Kovaˇcevi´c, 1995; Kovaˇcevi´c et al., 1992). During the garaging process, wheel speed commands were different from zero. When both commands became equal to zero, garaging was completed and block (*H*) halted the execution of the

Figures 15(a) and 15(b) show 12 typical trajectories obtained during the process of garaging of a Hemisson robot in a real experiment. The illustrative trajectories shown in Fig. 15(a) deviate from expected trajectories and are mildly oscillatory in nature, which is a result of robot wheel eccentricity and the ultimate resolution of the camera used to estimate the position and orientation of the robot. The system which tracks the WMR to the target can be upgraded by a non-linear algorithm (Ðurovi´c & Kovaˇcevi´c, 2008) or instantaneous acquisition by two sensors (Ðurovi´c et al., 2009). Figure 15(b) shows trajectories obtained during the course of garaging from "difficult" initial positions (initial angle between the axis of symmetry of the robot and the axis of symmetry of the garage near *π*/2 ). These trajectories were obtained

True STOP

WEB camera

(*F*) (*E*)

(*A*)

Target Navigation, Obstacle Avoidance and Garaging Problems

(*B*)

(*C*)

(*D*)

(*G*)

(*H*)

WMR posture; (*H*) Examination of *VL* and *VR* values.

algorithm.

*VL*, *VR*

Table 5 shows extreme and mean values derived from the garaging experiments. The subscript *N* denotes that the simulations were conducted under simulated sensor noise conditions. The sensor noise mostly affected the maximum distances from the target position *dCm* max; the mean values of angle error *δ* were unexpectedly lower under simulated sensor noise conditions.


Table 5. Extreme and mean values derived from garaging experiments.

#### **3.6 Limitations of the proposed FLC**

Simulations were conducted for the sub-set *Nc* of initial conditions, under which the robot was not at a sufficient distance from the garage and was placed at the points of an equidistant grid, spaced 0.25 cm apart, where the initial orientation of the robot *θRi*,(*t*=0) was a uniform-distribution random variable in the interval [−*π*/2, *π*/2]. Figure 13 shows the regions of initial conditions where the probability of collision of the robot with the garage is greater than zero. Full lines identify the limits of the region under ideal conditions, while dotted lines denote simulated sensor noise conditions. Figure 13 shows that sensor noise has little effect on the restricted area of initial conditions, which is an indicator of the robustness of the system to sensor noise.

Fig. 13. Limits of the system – region of initial configurations with probable collision of the WMR with the garage: full line – ideal conditions (*FLCBG*); dotted line – with sensor noise conditions (*FLCBGN* ).

#### **3.7 Experimental results**

An analogous experiment was performed with a real mobile vehicle (Hemisson mobile robot) and a real garage whose dimensions are 16 cm × 20 cm. A block diagram of the garaging experiment is shown in Fig. 14.

16 Mobile Robots / Book 1

Table 5 shows extreme and mean values derived from the garaging experiments. The subscript *N* denotes that the simulations were conducted under simulated sensor noise conditions. The sensor noise mostly affected the maximum distances from the target position *dCm* max; the mean values of angle error *δ* were unexpectedly lower under simulated sensor noise

> *FLCBG* 1.37 2.35 1.10 2.41 *FLCBGN* 1.19 9.52 1.93 4.80

Simulations were conducted for the sub-set *Nc* of initial conditions, under which the robot was not at a sufficient distance from the garage and was placed at the points of an equidistant grid, spaced 0.25 cm apart, where the initial orientation of the robot *θRi*,(*t*=0) was a uniform-distribution random variable in the interval [−*π*/2, *π*/2]. Figure 13 shows the regions of initial conditions where the probability of collision of the robot with the garage is greater than zero. Full lines identify the limits of the region under ideal conditions, while dotted lines denote simulated sensor noise conditions. Figure 13 shows that sensor noise has little effect on the restricted area of initial conditions, which is an indicator of the robustness

0

70 60 50 40 30 30 40 50 60

An analogous experiment was performed with a real mobile vehicle (Hemisson mobile robot) and a real garage whose dimensions are 16 cm × 20 cm. A block diagram of the garaging

Fig. 13. Limits of the system – region of initial configurations with probable collision of the WMR with the garage: full line – ideal conditions (*FLCBG*); dotted line – with sensor noise

10 10

*Cm*

*FLCBG*

[ cm]

20 20

◦] *dCm*[ cm] *dCm*max[ cm]

*δ* [

**3.6 Limitations of the proposed FLC**

of the system to sensor noise.

0

10

−

−

20

30

conditions (*FLCBGN* ).

**3.7 Experimental results**

experiment is shown in Fig. 14.

<sup>−</sup> −−− <sup>−</sup> <sup>−</sup> <sup>−</sup> <sup>−</sup>

*FLCBGN*

10

20

30

[ cm]

◦] *δ*max [

Table 5. Extreme and mean values derived from garaging experiments.

conditions.

Fig. 14. Block diagram of real-time experiment: (*A*) Determining of initial conditions; (*B*) Calculation of: *dCm*, *dFm*, *αCm* and *αFm*; (*C*) Fuzzy logic controller for bidirectional garaging; (*D*) Bluetooth interface; (*E*) Hemisson mobile robot; (*F*) Web camera; (*G*) Estimation of the WMR posture; (*H*) Examination of *VL* and *VR* values.

A personal computer with a Bluetooth interface (*D*) and web camera (*F*) – resolution 640 × 480 pixels, were used during the experiment. The initial conditions were determined prior to the initiation of the garaging process, namely (*A*): the position and orientation of the garage *qG*, and the posture (position and orientation) of the robot *qR*(*t*=0). Based on the posture of the robot and the position and orientation of the garage, in block (*B*), input variables *dCm*, *dFm*, *αCm* and *αFm* were calculated for the *FLCBG* (*C*). FLC outputs were wheel speed commands for the Hemisson robot (*VL* and *VR*), which were issued to the robot via the Bluetooth interface. The web camera performed a successive acquisition of frames in real time, with a 0.2 s repetition time, which dictated the sampling period for the entire fuzzy controller. In block (*G*), web camera frames were used to estimate the position of the robot and compute its orientation (Ðurovi´c & Kovaˇcevi´c, 1995; Kovaˇcevi´c et al., 1992). During the garaging process, wheel speed commands were different from zero. When both commands became equal to zero, garaging was completed and block (*H*) halted the execution of the algorithm.

Figures 15(a) and 15(b) show 12 typical trajectories obtained during the process of garaging of a Hemisson robot in a real experiment. The illustrative trajectories shown in Fig. 15(a) deviate from expected trajectories and are mildly oscillatory in nature, which is a result of robot wheel eccentricity and the ultimate resolution of the camera used to estimate the position and orientation of the robot. The system which tracks the WMR to the target can be upgraded by a non-linear algorithm (Ðurovi´c & Kovaˇcevi´c, 2008) or instantaneous acquisition by two sensors (Ðurovi´c et al., 2009). Figure 15(b) shows trajectories obtained during the course of garaging from "difficult" initial positions (initial angle between the axis of symmetry of the robot and the axis of symmetry of the garage near *π*/2 ). These trajectories were obtained

based on data from 24 ultrasonic sensors. Abdessemed et al. (2004) proposed a reactive controller inspired by the potential field method, with a rule base optimized by evolutionary techniques. Hui & Pratihar (2009) compared the potential field method with soft computing

<sup>253</sup> Fictitious Fuzzy-Magnet Concept in Solving Mobile–Robot

This section proposes a new methodology for robot navigation in a workspace which contains one or more obstacles. The approach is based on the fictitious fuzzy magnet concept but, contrary to the system described in the previous section, the rules are generated such that the action of the fuzzy magnet is to repel the vehicle. It should be noted that the inputs into the controller based on the fuzzy magnet concept are the distance and orientation relative to the fuzzy magnet position, which is a point, while the obstacles have dimensions which must be taken into account at the controller design stage. The controller proposed in this section uses information about the relative dimensions of the obstacle. At the preprocessing stage, the left and right side of the obstacle in the robot's line of sight are determined based on the mutual positions of the robot and the obstacle. Since controller inputs are based on the relative dimensions of the obstacle, the controller can be applied to obstacles of different shapes and

In general, WMR navigation to a target in a workspace with obstacles is comprised of two tasks: avoidance of obstacles and navigation to the target. The controller may be designed to integrate these two tasks, or the tasks can be performed independently but in parallel. Parallel processing of target and obstacle data, contrary to integrated approaches, offers a number of advantages, ranging from fewer FLC inputs to parallel distribute compensation discussed by Wang et al. (1996). To allow for the algorithm to be used for navigation in an environment which contains an unknown number of obstacles, the parallel fuzzy controller data processing approach was selected. The *FLCOFE* controller discussed in Section 2.1 was used to execute a navigation task to a target in an obstacle-free environment. Instead of *vR* and *ωR*, in this section its outputs will be denoted by *vOFE* and *ωOFE*, respectively, since notations with *R* are

Variables which will reflect the mutual positions of the robot and the obstacle need to be defined and efficient navigation in real-time enabled, regardless of the position and size of the obstacle relative to the target and the current configuration of the robot. Figure 16 shows the WMR and obstacle parameters. Since we address the WMR navigation problem in planar form, we will assume that the obstacle in the robot's workspace is a circle defined by its center coordinates and radius: *O*(*xO*, *yO*,*rO*). Figure 16 shows a real obstacle whose radius is *rr*, in light gray. Because of the finite dimensions of the robot, it is extended by a circular ring of width *rcl* (dark gray). Hereafter we will use *rO* for the radius of the obstacle, computed from

In Fig. 16, the distance between the WMR center and the obstacle is denoted by *dRO*. Point *PC* lies at the intersection of the WMR course line and the obstacle's radius which form a right angle and represent a virtual collision point. When this intersection is an empty set, the obstacle does not lie on the WMR trajectory and point *PC* is not defined. The portion of the radius which is located to the left of the point *PC* relative to the WMR is denoted by *lL*, while the portion of the radius to the right of *PC* is denoted by *lR*. We will use variables *lL* and *lR* to define the obstacle avoidance algorithm. Since *lL* and *lR* are dimensional lengths and since

*rO* = *rr* + *rcl*, *rcl* > *WG*/2. (23)

methods used to solve moving obstacle avoidance problems.

Target Navigation, Obstacle Avoidance and Garaging Problems

**4.1 Navigation in a single-obstacle environment**

reserved for outputs from the entire navigation system.

sizes.

the equation:

equally under front- and back-drive conditions (the front of the robot is identified by a thicker line). All of the illustrated trajectories are indicative of good performance of the proposed garaging algorithm.

Fig. 15. Real-time garaging experiment results.

#### **4. Obstacle avoidance**

The navigation of a mobile robot in a workspace with obstacles is a current problem which has been extensively researched over the past few decades. Depending on the definition of the problem, the literature offers several approaches based on different methodologies. Relative to the environment, these approaches can be classified depending on whether the vehicle moves in a known (Kermiche et al., 2006), partially known (Rusu et al., 2003), or totally unknown environment (Pradhan et al., 2009). Further, the environment can be stationary or dynamic – with moving obstacles (Hui & Pratihar, 2009), or a moving target (Glasius et al., 1995). With regard to the ultimate target, the mobile robot is navigated to a given point (Mitrovi´c et al., 2009), or a given configuration – a point and a given orientation at that point (Mitrovi´c & Ðurovi´c, 2010a), or navigated to reach and follow a given trajectory (Tanaka & Sano, 1995). The problems are solved both in real-time and off-line.

Numerous obstacle avoidance methods are discussed in literature. The potential field method (Khatib, 1986) plays a major role in solving this type of problem. The potential function has to be derived for each individual obstacle and this can be a drawback from a real-time perspective if there are a large number of obstacles. Additionally, the resulting potential field may contain local minima where navigation ends without reaching the goal (Koren & Borenstein, 1991).

Fuzzy-logic-based controllers are widely applied to address obstacle avoidance and mobile robot navigation problems, and they range from reactive fuzzy controllers (which do not include a planning stage and generate outputs based on real-time sensor signals) to controllers which resolve the trajectory planning problem of a WMR through the synthesis of fuzzy logic with genetic algorithms, neural networks, or the potential field approach. Reignier (1994) proposed a reactive FLC for WMR navigation through a workspace with obstacles, 18 Mobile Robots / Book 1

equally under front- and back-drive conditions (the front of the robot is identified by a thicker line). All of the illustrated trajectories are indicative of good performance of the proposed

(b)

20 40 60 80 100 120 140 [ cm]

(a)

garaging algorithm.

0 0

20

**4. Obstacle avoidance**

Borenstein, 1991).

40

60

Fig. 15. Real-time garaging experiment results.

80

The problems are solved both in real-time and off-line.

[ cm] 0

The navigation of a mobile robot in a workspace with obstacles is a current problem which has been extensively researched over the past few decades. Depending on the definition of the problem, the literature offers several approaches based on different methodologies. Relative to the environment, these approaches can be classified depending on whether the vehicle moves in a known (Kermiche et al., 2006), partially known (Rusu et al., 2003), or totally unknown environment (Pradhan et al., 2009). Further, the environment can be stationary or dynamic – with moving obstacles (Hui & Pratihar, 2009), or a moving target (Glasius et al., 1995). With regard to the ultimate target, the mobile robot is navigated to a given point (Mitrovi´c et al., 2009), or a given configuration – a point and a given orientation at that point (Mitrovi´c & Ðurovi´c, 2010a), or navigated to reach and follow a given trajectory (Tanaka & Sano, 1995).

Numerous obstacle avoidance methods are discussed in literature. The potential field method (Khatib, 1986) plays a major role in solving this type of problem. The potential function has to be derived for each individual obstacle and this can be a drawback from a real-time perspective if there are a large number of obstacles. Additionally, the resulting potential field may contain local minima where navigation ends without reaching the goal (Koren &

Fuzzy-logic-based controllers are widely applied to address obstacle avoidance and mobile robot navigation problems, and they range from reactive fuzzy controllers (which do not include a planning stage and generate outputs based on real-time sensor signals) to controllers which resolve the trajectory planning problem of a WMR through the synthesis of fuzzy logic with genetic algorithms, neural networks, or the potential field approach. Reignier (1994) proposed a reactive FLC for WMR navigation through a workspace with obstacles,

20

40

60

80

100

120 [ cm] based on data from 24 ultrasonic sensors. Abdessemed et al. (2004) proposed a reactive controller inspired by the potential field method, with a rule base optimized by evolutionary techniques. Hui & Pratihar (2009) compared the potential field method with soft computing methods used to solve moving obstacle avoidance problems.

This section proposes a new methodology for robot navigation in a workspace which contains one or more obstacles. The approach is based on the fictitious fuzzy magnet concept but, contrary to the system described in the previous section, the rules are generated such that the action of the fuzzy magnet is to repel the vehicle. It should be noted that the inputs into the controller based on the fuzzy magnet concept are the distance and orientation relative to the fuzzy magnet position, which is a point, while the obstacles have dimensions which must be taken into account at the controller design stage. The controller proposed in this section uses information about the relative dimensions of the obstacle. At the preprocessing stage, the left and right side of the obstacle in the robot's line of sight are determined based on the mutual positions of the robot and the obstacle. Since controller inputs are based on the relative dimensions of the obstacle, the controller can be applied to obstacles of different shapes and sizes.

#### **4.1 Navigation in a single-obstacle environment**

In general, WMR navigation to a target in a workspace with obstacles is comprised of two tasks: avoidance of obstacles and navigation to the target. The controller may be designed to integrate these two tasks, or the tasks can be performed independently but in parallel. Parallel processing of target and obstacle data, contrary to integrated approaches, offers a number of advantages, ranging from fewer FLC inputs to parallel distribute compensation discussed by Wang et al. (1996). To allow for the algorithm to be used for navigation in an environment which contains an unknown number of obstacles, the parallel fuzzy controller data processing approach was selected. The *FLCOFE* controller discussed in Section 2.1 was used to execute a navigation task to a target in an obstacle-free environment. Instead of *vR* and *ωR*, in this section its outputs will be denoted by *vOFE* and *ωOFE*, respectively, since notations with *R* are reserved for outputs from the entire navigation system.

Variables which will reflect the mutual positions of the robot and the obstacle need to be defined and efficient navigation in real-time enabled, regardless of the position and size of the obstacle relative to the target and the current configuration of the robot. Figure 16 shows the WMR and obstacle parameters. Since we address the WMR navigation problem in planar form, we will assume that the obstacle in the robot's workspace is a circle defined by its center coordinates and radius: *O*(*xO*, *yO*,*rO*). Figure 16 shows a real obstacle whose radius is *rr*, in light gray. Because of the finite dimensions of the robot, it is extended by a circular ring of width *rcl* (dark gray). Hereafter we will use *rO* for the radius of the obstacle, computed from the equation:

$$r\_{\rm{O}} = r\_{r} + r\_{\rm{cl}}, \qquad r\_{\rm{cl}} > \mathcal{W}\_{\rm{G}}/2. \tag{23}$$

In Fig. 16, the distance between the WMR center and the obstacle is denoted by *dRO*. Point *PC* lies at the intersection of the WMR course line and the obstacle's radius which form a right angle and represent a virtual collision point. When this intersection is an empty set, the obstacle does not lie on the WMR trajectory and point *PC* is not defined. The portion of the radius which is located to the left of the point *PC* relative to the WMR is denoted by *lL*, while the portion of the radius to the right of *PC* is denoted by *lR*. We will use variables *lL* and *lR* to define the obstacle avoidance algorithm. Since *lL* and *lR* are dimensional lengths and since

Degree of membership 1

−1 −0.5 0 0.5

Table 6. *FLCOA* – fuzzy rules base.

0.25, 0), and *qD*(0.6, 0.2, −2*π*/3).

**4.1.1 Moving obstacle avoidance**

obstacle at an angle.

*ω<sup>R</sup>* =

from the following relation:

Ðurovi´c, 2010b):

(a) Membership functions of linguistic variable *Side*.

*Lef tB RightB RightF Lef tF*

Target Navigation, Obstacle Avoidance and Garaging Problems

1

<sup>255</sup> Fictitious Fuzzy-Magnet Concept in Solving Mobile–Robot

Fig. 17. Membership functions of obstacle avoidance fuzzy controller *FLCOA*.

*ωOFE* if *ωOA* = 0

*<sup>ω</sup>OA* if *<sup>ω</sup>OA* �<sup>=</sup> <sup>0</sup> ; *vR* <sup>=</sup>

To decrease the duration of the navigation process, the controller output *vR* was post-processed to ensure maximum linear velocity *vRmax* of the robot based on (Mitrovi´c &

where *D* is the distance between the robot wheels. The maximum linear velocity of the robot is a function of the angular velocity *ω<sup>R</sup>* and the physical constraints of the robot. Figure 18 illustrates the trajectories followed by the WMR to avoid two obstacles: *O*1(−0.4, 0, 0.15) and *O*2(0.3, 0.1, 0.05), from four initial positions: *qA*(−0.8, 0, *π*), *qB*(−0.7, 0.2, *π*/4), *qC*(0.45,

The proposed algorithm, without modification, was applied in a single moving obstacle avoidance scenario. It was assumed that the obstacle travels along a straight line at a constant speed *vobs*, which is equal to one-third of the maximum speed of the robot. The outcome was successful when the robot overtook or directly passed by the obstacle. However, if it attempted to pass by at an angle, the robot collided with the obstacle. This was as expected, since the controller does not take the speed of the obstacle into account. Figure 19 illustrates the scenarios in which the robot overtakes, directly passes by, and meets with the moving

Rule *Side dOR vO A ωO A* 1. *Lef tF near Vm*/2 −*ω<sup>m</sup>* 2. *Lef tB near* −*Vm*/2 *ω<sup>m</sup>* 3. *RightF near Vm*/2 *ω<sup>m</sup>* 4. *RightB near* −*Vm*/2 −*ω<sup>m</sup>* 5. *Lef tF f ar Vm* −*ωm*/2 6. *Lef tB f ar* −*Vm ωm*/2 7. *RightF f ar Vm ωm*/2 8. *RightB f ar* −*Vm* −*ωm*/2

1

0 *dRO*[m]

*vOFE* if *vOA* = 0

*vRmax*(*ωR*, *vR*) = *sgn*(*vR*) · (*Vm* − |*ωR*| · *D*/2), (27)

*vOA* if *vOA* �<sup>=</sup> <sup>0</sup> . (26)

(b) Membership functions of linguistic variable distance to obstacle – *dRO*.

0.1 0.2 0.4

*near f ar*

Fig. 16. Relative positions of obstacle and mobile robot.

their values depend on the WMR position and obstacle dimensions, we will normalize them based on the equation:

$$\begin{aligned} L\_L &= l\_L / (2 \cdot r\_O)\_\prime \\ L\_R &= l\_R / (2 \cdot r\_O)\_\prime \end{aligned} \tag{24}$$

where *LL* and *LR* are the left and right side of the obstacle, respectively, and can assume values from the interval [0, 1], regardless of obstacle dimensions, since *LL* + *LR* = 1. If *LL* or *LR* is equal to 1, the robot will follow a course which will bypass the obstacle. If point *PC* is not defined, we will assume that *LL* and *LR* are equal to zero, meaning that the obstacle does not lie on the robot's course. Based on the position of the virtual collision point *PC* and the linear velocity of the robot *vR*, we will generate the obstacle avoidance controller input using:

$$Side = \begin{cases} [-1, -0.5) \text{ if } L\_L \in (0.5, 1] \text{ and } v\_R \le 0\\ [-0.5, 0) \quad \text{if } L\_R \in [0.5, 1] \text{ and } v\_R \le 0\\ (0, 0.5] \qquad \text{if } L\_R \in [0.5, 1] \text{ and } v\_R > 0\\ (0.5, 1] \qquad \text{if } L\_L \in (0.5, 1] \text{ and } v\_R > 0\\ 0 \qquad \text{if } L\_R = 0 \qquad \text{and } L\_L = 0 \text{ .} \end{cases} \tag{25}$$

The variable *Side* provides information about the position of the obstacle relative to the robot and is equal to zero if the obstacle does not lie on the robot's course. The corresponding membership functions are shown in Fig. 17(a). In the name of the membership function, subscript *B* stands for *Backward* and *F* for *Forward*. To ensure efficient bypassing of the obstacle, in addition to the position of the obstacle relative to the WMR, the controller has to be provided with the distance between the WMR and the obstacle, denoted by *dRO* in Fig. 16, whose membership functions are shown in Fig. 17(b).

The outputs of the obstacle avoidance controller are the linear velocity *vOA* and the angular velocity *ωOA* of the robot. The respective fuzzy rules are shown in Table 6.

It is important to note that the outputs of the obstacle avoidance controller are equal to zero when the obstacle does not lie on the robot's course. For this reason the algorithm for WMR navigation to the target point in a single-obstacle environment must integrate *FLCOFE* and *FLCOA* solutions. The linear velocity *vR* and the angular velocity *ω<sup>R</sup>* of the robot are computed

(a) Membership functions of linguistic variable *Side*.

(b) Membership functions of linguistic variable distance to obstacle – *dRO*.

Fig. 17. Membership functions of obstacle avoidance fuzzy controller *FLCOA*.


Table 6. *FLCOA* – fuzzy rules base.

from the following relation:

20 Mobile Robots / Book 1

*θR*

*dRO*

> *lR*

their values depend on the WMR position and obstacle dimensions, we will normalize them

*LL* = *lL*/(2 · *rO*),

where *LL* and *LR* are the left and right side of the obstacle, respectively, and can assume values from the interval [0, 1], regardless of obstacle dimensions, since *LL* + *LR* = 1. If *LL* or *LR* is equal to 1, the robot will follow a course which will bypass the obstacle. If point *PC* is not defined, we will assume that *LL* and *LR* are equal to zero, meaning that the obstacle does not lie on the robot's course. Based on the position of the virtual collision point *PC* and the linear velocity of the robot *vR*, we will generate the obstacle avoidance controller input using:

> [−1, −0.5) if *LL* ∈ (0.5, 1] and *vR* ≤ 0 [−0.5, 0) if *LR* ∈ [0.5, 1] and *vR* ≤ 0 (0, 0.5] if *LR* ∈ [0.5, 1] and *vR* > 0 (0.5, 1] if *LL* ∈ (0.5, 1] and *vR* > 0 0 if *LR* = 0 and *LL* = 0 .

The variable *Side* provides information about the position of the obstacle relative to the robot and is equal to zero if the obstacle does not lie on the robot's course. The corresponding membership functions are shown in Fig. 17(a). In the name of the membership function, subscript *B* stands for *Backward* and *F* for *Forward*. To ensure efficient bypassing of the obstacle, in addition to the position of the obstacle relative to the WMR, the controller has to be provided with the distance between the WMR and the obstacle, denoted by *dRO* in Fig. 16,

The outputs of the obstacle avoidance controller are the linear velocity *vOA* and the angular

It is important to note that the outputs of the obstacle avoidance controller are equal to zero when the obstacle does not lie on the robot's course. For this reason the algorithm for WMR navigation to the target point in a single-obstacle environment must integrate *FLCOFE* and *FLCOA* solutions. The linear velocity *vR* and the angular velocity *ω<sup>R</sup>* of the robot are computed

velocity *ωOA* of the robot. The respective fuzzy rules are shown in Table 6.

*PC* (*x*0, *y*0)

*lL*

*x*

(25)

*rcl*

*rr*

*LR* <sup>=</sup> *lR*/(<sup>2</sup> · *rO*), (24)

*rO*

(*xR*

based on the equation:

, *yR*, *θR*)

Fig. 16. Relative positions of obstacle and mobile robot.

*Side* =

whose membership functions are shown in Fig. 17(b).

⎧ ⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎩

$$
\omega\_R = \begin{cases}
\omega\_{OFE} \text{ if } \omega\_{OA} = 0 \\
\omega\_{OA} \text{ if } \omega\_{OA} \neq 0
\end{cases}; \quad v\_R = \begin{cases}
v\_{OFE} \text{ if } v\_{OA} = 0 \\
v\_{OA} \text{ if } v\_{OA} \neq 0
\end{cases}.\tag{26}
$$

To decrease the duration of the navigation process, the controller output *vR* was post-processed to ensure maximum linear velocity *vRmax* of the robot based on (Mitrovi´c & Ðurovi´c, 2010b):

$$\upsilon\_{\text{Rmax}}(\omega\_{\text{R}}, \upsilon\_{\text{R}}) = \text{sgn}(\upsilon\_{\text{R}}) \cdot (V\_{\text{m}} - |\omega\_{\text{R}}| \cdot D/2), \tag{27}$$

where *D* is the distance between the robot wheels. The maximum linear velocity of the robot is a function of the angular velocity *ω<sup>R</sup>* and the physical constraints of the robot. Figure 18 illustrates the trajectories followed by the WMR to avoid two obstacles: *O*1(−0.4, 0, 0.15) and *O*2(0.3, 0.1, 0.05), from four initial positions: *qA*(−0.8, 0, *π*), *qB*(−0.7, 0.2, *π*/4), *qC*(0.45, 0.25, 0), and *qD*(0.6, 0.2, −2*π*/3).

#### **4.1.1 Moving obstacle avoidance**

The proposed algorithm, without modification, was applied in a single moving obstacle avoidance scenario. It was assumed that the obstacle travels along a straight line at a constant speed *vobs*, which is equal to one-third of the maximum speed of the robot. The outcome was successful when the robot overtook or directly passed by the obstacle. However, if it attempted to pass by at an angle, the robot collided with the obstacle. This was as expected, since the controller does not take the speed of the obstacle into account. Figure 19 illustrates the scenarios in which the robot overtakes, directly passes by, and meets with the moving obstacle at an angle.

follows:

to (28), from:

0

1

0.5

1.5

[m]

0

**4.2.1 Constraints of the system**

*Ck* =

*vOA* =

navigation through an area which includes obstacles.

 *N* ∑ *k*=1

Target Navigation, Obstacle Avoidance and Garaging Problems

*Ck* · *vOAk* · *<sup>d</sup>*−<sup>1</sup>

Fig. 20. Navigation to the goal (coordinate origin) through a group of obstacles.

Although the positions of the obstacles and the target are known, a closer analysis of the proposed approach leads to the conclusion that navigation to the target takes place in an unknown environment. The position of the target is known in all of the illustrated scenarios, but obstacles are taken into account only after they have triggered appropriate FLC rules, that is, when the distance between the robot and the obstacle is not greater than 40 cm (universe of

0 if *Sidek* = 0

We will derive the speed for the system of *N* obstacle avoidance controllers in a manner similar

<sup>257</sup> Fictitious Fuzzy-Magnet Concept in Solving Mobile–Robot

where *vOAk* is the *FLCOA* output for the *kth* obstacle. Outputs from the system of *N FLCOA* and *FLCOFE* controllers are integrated by means of (26), and the speed of the robot is maximized based on (27). Figure 20 shows a scenario with several randomly-distributed obstacles. Initial positions and the target (coordinate origin) are beyond the obstacle area. It is apparent from the figure that the proposed methodology efficiently solves the problem of

*ROk* · *N* ∑ *k*=1

1 if *Sidek* � <sup>0</sup> . (29)

*ROk*<sup>−</sup><sup>1</sup>

, (30)

[m]

*Ck* · *<sup>d</sup>*−<sup>1</sup>

1 2 3

Fig. 18. Bypass trajectories of the robot from four initial positions around two obstacles of different dimensions.

Fig. 19. Avoidance of moving obstacles: *SI* – overtaking, *SI I* – directly passing, *SIII* – meeting at an angle.

#### **4.2 Navigation trough a group of obstacles**

We will now address the scenario in which there is a group of *N* obstacles in the workspace of the robot, where *N* > 1. To benefit from the approach described in the previous section, we will use the single-obstacle *FLCOA* for each individual obstacle and thus obtain *N* values for the robot's linear and angular velocities. We will then compute the final angular velocity from the following equation:

$$
\omega\_{OA} = \left(\sum\_{k=1}^{N} \mathbf{C}\_k \cdot \omega\_{OAk} \cdot d\_{ROk}^{-1}\right) \cdot \left(\sum\_{k=1}^{N} \mathbf{C}\_k \cdot d\_{ROk}^{-1}\right)^{-1} \tag{28}
$$

where *ωOAk* is the *FLCOA* output for the *kth* obstacle and *dROk* is the distance between the robot and the *kth* obstacle. As the distance from the vehicle to the *kth* obstacle shrinks, its influence to the controller outputs should increase. Using the coefficient *Ck*, we will take into account only those obstacles which lie on the robot's course. It is computed using (25), as follows:

22 Mobile Robots / Book 1

0

*SIII*

4

8

0.2

*Ck* · *<sup>d</sup>*−<sup>1</sup> *ROk*

Fig. 18. Bypass trajectories of the robot from four initial positions around two obstacles of

*SI SI I*

1

2

3

4

5

6

6

9

5

1 1

7 6

8 7

0

9 8

10

10 9

2 1

*Ck* · *<sup>ω</sup>OAk* · *<sup>d</sup>*−<sup>1</sup>

8

3 2

4 3

9

5 4

6 5

−0.8 −0.6 −0.4 −0.2 0.6 [ m]

Fig. 19. Avoidance of moving obstacles: *SI* – overtaking, *SI I* – directly passing, *SIII* – meeting

We will now address the scenario in which there is a group of *N* obstacles in the workspace of the robot, where *N* > 1. To benefit from the approach described in the previous section, we will use the single-obstacle *FLCOA* for each individual obstacle and thus obtain *N* values for the robot's linear and angular velocities. We will then compute the final angular velocity from

> *ROk* · *N* ∑ *k*=1

where *ωOAk* is the *FLCOA* output for the *kth* obstacle and *dROk* is the distance between the robot and the *kth* obstacle. As the distance from the vehicle to the *kth* obstacle shrinks, its influence to the controller outputs should increase. Using the coefficient *Ck*, we will take into account only those obstacles which lie on the robot's course. It is computed using (25), as

*O*<sup>2</sup>

0.2 0.4

1

2

6

3

7

[m]

2

, (28)

3

4

1011

5

0.4

<sup>−</sup><sup>1</sup>

*qC qD*

*O*<sup>1</sup>

−0.8 −0.6 −0.4 −0.2

1

**4.2 Navigation trough a group of obstacles**

*ωOA* =

 *N* ∑ *k*=1

4

5

6

7

2

3

[m]

0

−0.1

−0.2

0

at an angle.

the following equation:

0.2

0.4

[ m]

0.1

0.2

*qA*

different dimensions.

*qB*

$$\mathbf{C}\_{k} = \begin{cases} 0 & \text{if } Side\_{k} = 0 \\ 1 & \text{if } Side\_{k} \neq 0 \end{cases} . \tag{29}$$

We will derive the speed for the system of *N* obstacle avoidance controllers in a manner similar to (28), from:

$$v\_{OA} = \left(\sum\_{k=1}^{N} \mathbb{C}\_k \cdot v\_{OAk} \cdot d\_{ROk}^{-1}\right) \cdot \left(\sum\_{k=1}^{N} \mathbb{C}\_k \cdot d\_{ROk}^{-1}\right)^{-1} \tag{30}$$

where *vOAk* is the *FLCOA* output for the *kth* obstacle. Outputs from the system of *N FLCOA* and *FLCOFE* controllers are integrated by means of (26), and the speed of the robot is maximized based on (27). Figure 20 shows a scenario with several randomly-distributed obstacles. Initial positions and the target (coordinate origin) are beyond the obstacle area. It is apparent from the figure that the proposed methodology efficiently solves the problem of navigation through an area which includes obstacles.

Fig. 20. Navigation to the goal (coordinate origin) through a group of obstacles.

#### **4.2.1 Constraints of the system**

Although the positions of the obstacles and the target are known, a closer analysis of the proposed approach leads to the conclusion that navigation to the target takes place in an unknown environment. The position of the target is known in all of the illustrated scenarios, but obstacles are taken into account only after they have triggered appropriate FLC rules, that is, when the distance between the robot and the obstacle is not greater than 40 cm (universe of

Ðurovi´c, Ž. & Kovaˇcevi´c, B. (2008). A sequential LQG approach to nonlinear tracking problem,

<sup>259</sup> Fictitious Fuzzy-Magnet Concept in Solving Mobile–Robot

Ðurovi´c, Ž., Kovaˇcevi´c, B. & Diki´c, G. (2009). Target tracking with two passive infrared

Glasius, R., Komoda, A. & Gielen, S. (1995). Neural network dynamics for path planning and

Hui, N. & Pratihar, D. (2009). A comparative study on some navigation schemes of a

Kermiche, S., Saidi, M., Abbassi, H. & Ghodbane, H. (2006). Takagi–Sugeno based controller for mobile robot navigation, *Journal of Applied Sciences* 6(8): 1838–1844. Khatib, O. (1986). Real-time obstacle avoidance for manipulators and mobile robots, *The*

Koren, Y. & Borenstein, J. (1991). Potential field methods and their inherent limitations for

Kovaˇcevi´c, B., Ðurovi´c, Ž. & Glavaški, S. (1992). On robust Kalman filtering, *International*

Krishna, K. & Kalra, P. (2001). Perception and remembrance of the environment during real-time navigation of a mobile robot, *Robotics and Autonomous Systems* 37: 25–51. Miškovi´c, Lj., Ðurovi´c, Ž. & Kovaˇcevi´c, B. (2002). Application of the minimum state error

Mitrovi´c, S. (2006). Design of fuzzy logic controller for autonomous garaging of mobile robot,

Mitrovi´c, S. & Ðurovi´c, Ž. (2010a). Fuzzy logic controller for bidirectional garaging of a differential drive mobile robot, *Advanced Robotics* 24(8–9): 1291–1311. Mitrovi´c, S. & Ðurovi´c, Ž. (2010b). Fuzzy–based controller for differential drive mobile robot

Motlagh, O., Hong, T. & Ismail, N. (2009). Development of a new minimum avoidance system for a behavior-based mobile robot, *Fuzzy Sets and Systems* 160: 1929–1945. Oriolo, G., Luca, A. & Vendittell, M. (2002). WMR control via dynamic feedback linearization:

Pradhan, S., Parhi, D. & Panda, A. (2009). Fuzzy logic techniques for navigation of several

Reignier, P. (1994). Fuzzy logic techniques for mobile robot obstacle avoidance, *Robotics and*

Rusu, P., Petriu, E., Whalen, T., Cornell, A. & Spoelder, H. (2003). Behavior–based neuro–fuzzy

Sugeno, M. & Nishida, M. (1985). Fuzzy control of model car, *Fuzzy Sets and Systems*

mobile robots, *Applied Soft Computing Journal* 9(1): 290–304.

*Intelligent Autonomous Vehicle*, Vol. 7, IFAC–PapersOnLine, Lecce, Italy. Mitrovi´c, S., Ðurovi´c, Ž. & Aleksi´c, M. (2009). One approach for mobile robot navigation

real robot tackling moving obstacles, *Robotics and Computer Integrated Manufacturing*

mobile robot navigation, *Proceedings of the IEEE Conference on Robotics and Automation*,

variance approach to nonlinear system control, *International Journal of Systems Science*

obstacle avoidance, *in* G. Indiveri & A. M. Pascoal (eds), *7th IFAC Symposium on*

throw area with obstacles, *eProc. of LIII ETRAN Conference*, Vol. 53, Vrnjaˇcka Banja,

Design, implementation, and experimental validation, *IEEE Trans. Control Systems*

controller for mobile robot navigation, *IEEE Transactions on Instrumentation and*

*International Journal of Systems Science* 39(4): 371–382.

Target Navigation, Obstacle Avoidance and Garaging Problems

obstacle avoidance, *Neural Networks* 8(1): 125–133.

*International Journal of Robotics Research* 5(1): 90–98.

25(4-5): 810–828.

33(5): 359–368.

Serbia, pp. AU3.5 1–4.

*Technology* 10: 835–852.

*Autonomous Systems* 12(3-4): 143–153.

*Measurement* 52(4): 1335–1340.

16(2): 103–113.

Sacramento, pp. 1398–404.

*Journal of Control* 56(3): 547–562.

*Journal of Automatic Control* 16: 13–16.

non-imaging sensors, *IET Signal Processing* 3(3): 177–188.

discourse of the linguistic variable *dRO*, Fig. 17(b)). This basically means that the robot takes into account obstacles within a 40 cm radius.

It is well-known that a basic FLC cannot solve navigation through a workspace with arbitrarily distributed obstacles, if prior information about them is not available. A special problem is encountered when the obstacles are deployed such that they create a concave, **U**, barrier, in which case the robot enters an infinite loop or local minima (Krishna & Kalra, 2001; Motlagh et al., 2009).

#### **5. Conclusion**

A fictitious fuzzy magnet concept was introduced and its application analyzed in this chapter, using different navigation problems of differential drive mobile robots. The concept allows for navigation to the target in a single maneuver, without changing the direction of travel of the mobile robot. The symmetricity of the differential drive mobile robot is fully utilized, such that approaches based on the fictitious fuzzy magnet concept provide bidirectional solutions to mobile robot navigation problems. For example, during garaging, the mobile robot is automatically parked from the end of the robot which is closer to the garage entrance. The algorithm can be applied when the control variable is of the discrete type and where there are relatively few quantization levels.

When applied to a mobile robot garaging problem, a detailed analysis of simulation and experimental results illustrates the efficiency of the proposed algorithm and its robustness in the case of a random or systematic WMR position estimation error, as well as its limitations. The most significant shortfall of the proposed algorithm is that it does not provide a solution which will ensure that regardless of initial conditions the garage parking process is completed with no collision with the garage. The geometrical position of the initial conditions which lead to a collision of the robot with the garage is a compact, finite area which is discussed in the chapter. Some of the constraints mentioned here could be overcome in further research aimed at improving the proposed algorithm through a higher level of FLC complexity. Additionally, if a larger number of fictitious fuzzy magnets are introduced, the concept could be used to perform more intricate garage parking tasks. Re-configuration of controller outputs would render the proposed algorithm applicable for garage parking of a broader class of car-like mobile robots.

Additionally, it was shown that a larger number of fictitious fuzzy magnets can be introduced to execute tasks more complex than garage parking. Specifically, the feasibility was explored of applying the fictitious fuzzy magnet concept to execute the task of mobile robot navigation to a target in an environment which contains one or more obstacles. It was shown that the algorithm designed to avoid a single stationary obstacle can be successfully generalized to a multiple obstacles avoidance problem, preserving the bidirectional capability. The proposed algorithm can be extended by fuzzification of the coefficients *Ck*, taking into account the probability of collision with obstacles which lie on the robot's course.

#### **6. References**

Abdessemed, F., Benmahammed, K. & Monacelli, E. (2004). A fuzzy-based reactive controller for a non-holonomic mobile robot, *Robotics and Autonomous Systems* 47(1): 31–46.

Ðurovi´c, Ž. & Kovaˇcevi´c, B. (1995). QQ–plot approach to robust Kalman filtering, *International Journal of Control* 61(4): 837–857.

24 Mobile Robots / Book 1

discourse of the linguistic variable *dRO*, Fig. 17(b)). This basically means that the robot takes

It is well-known that a basic FLC cannot solve navigation through a workspace with arbitrarily distributed obstacles, if prior information about them is not available. A special problem is encountered when the obstacles are deployed such that they create a concave, **U**, barrier, in which case the robot enters an infinite loop or local minima (Krishna & Kalra, 2001; Motlagh

A fictitious fuzzy magnet concept was introduced and its application analyzed in this chapter, using different navigation problems of differential drive mobile robots. The concept allows for navigation to the target in a single maneuver, without changing the direction of travel of the mobile robot. The symmetricity of the differential drive mobile robot is fully utilized, such that approaches based on the fictitious fuzzy magnet concept provide bidirectional solutions to mobile robot navigation problems. For example, during garaging, the mobile robot is automatically parked from the end of the robot which is closer to the garage entrance. The algorithm can be applied when the control variable is of the discrete type and where there are

When applied to a mobile robot garaging problem, a detailed analysis of simulation and experimental results illustrates the efficiency of the proposed algorithm and its robustness in the case of a random or systematic WMR position estimation error, as well as its limitations. The most significant shortfall of the proposed algorithm is that it does not provide a solution which will ensure that regardless of initial conditions the garage parking process is completed with no collision with the garage. The geometrical position of the initial conditions which lead to a collision of the robot with the garage is a compact, finite area which is discussed in the chapter. Some of the constraints mentioned here could be overcome in further research aimed at improving the proposed algorithm through a higher level of FLC complexity. Additionally, if a larger number of fictitious fuzzy magnets are introduced, the concept could be used to perform more intricate garage parking tasks. Re-configuration of controller outputs would render the proposed algorithm applicable for garage parking of a broader class of car-like

Additionally, it was shown that a larger number of fictitious fuzzy magnets can be introduced to execute tasks more complex than garage parking. Specifically, the feasibility was explored of applying the fictitious fuzzy magnet concept to execute the task of mobile robot navigation to a target in an environment which contains one or more obstacles. It was shown that the algorithm designed to avoid a single stationary obstacle can be successfully generalized to a multiple obstacles avoidance problem, preserving the bidirectional capability. The proposed algorithm can be extended by fuzzification of the coefficients *Ck*, taking into account the

Abdessemed, F., Benmahammed, K. & Monacelli, E. (2004). A fuzzy-based reactive controller for a non-holonomic mobile robot, *Robotics and Autonomous Systems* 47(1): 31–46. Ðurovi´c, Ž. & Kovaˇcevi´c, B. (1995). QQ–plot approach to robust Kalman filtering, *International*

probability of collision with obstacles which lie on the robot's course.

*Journal of Control* 61(4): 837–857.

into account obstacles within a 40 cm radius.

et al., 2009).

**5. Conclusion**

mobile robots.

**6. References**

relatively few quantization levels.


**1. Introduction**

example, transportation) in indoor environments.

Pirjanian (1999) for reviews of such methods).

the term used here.

Long-term robustness is a crucial property of robots intended for real-world tasks such as, for example, transportation in indoor environments (e.g. warehouses, industries, hospitals, airports etc.). In order to be useful, such robots must be able to operate over long distances without much human supervision, something that sets high demands both on the actual robot (hardware) and its artificial brain<sup>1</sup> (software). This paper is focused on the development of artificial robotic brains for reliable long-term navigation (for use in, for

*Department of Applied Mechanics, Chalmers University of Technology, Göteborg* 

**Reliable Long-Term Navigation** 

Mattias Wahde, David Sandberg and Krister Wolff

**in Indoor Environments** 

*Sweden* 

**13**

Reliable decision-making is an essential tool for achieving long-term robustness. The ability to make correct decisions, in real-time and often based on incomplete and noisy information, is important not only for mobile robots but also for animals, including humans (McFarland, 1998; Prescott et al., 2007). One may argue that the entire sub-field of behavior-based robotics emerged, at least in part, as a result of the perceived failure of classical artificial intelligence to address real-time decision-making based on a robot's imperfect knowledge of the world. Starting with the *subsumption method* (Brooks, 1986), many different methods for decision-making, often referred to as methods for *behavior selection* or *action selection*, have been suggested in the literature on behavior-based robotics (see, for example, Bryson (2007);

In actual applications, a common approach is to combine a *reactive layer* of decision-making, using mainly behavior-based concepts, with a *deliberative layer* using, for example, concepts from classical artificial intelligence, such as high-level reasoning) (Arkin, 1998). Several approaches of this kind have been suggested (see, for example, Arkin (1987) and Gat (1991))

In the *utility function* (UF) method for decision-making (Wahde, 2003; 2009), which will be used here, a somewhat different approach is taken in which, for the purposes of decision-making, no distinction is made between reactive and deliberative aspects of the robotic brain. In this method, an artificial robotic brain is built from a repertoire of *brain processes* as well as a single decision-making system responsible for activating and de-activating brain processes

<sup>1</sup> The term *artificial (robotic) brain* is here used instead of the more common *control system* since the latter term, in the authors' view, signifies the low-level parts (such as motor control to achieve a certain speed) of robot intelligence, whereas more high-level parts, such as decision-making are better described by

and applied in different robots (see, for example, Sakagami et al. (2002)).


## **Reliable Long-Term Navigation in Indoor Environments**

Mattias Wahde, David Sandberg and Krister Wolff *Department of Applied Mechanics, Chalmers University of Technology, Göteborg Sweden* 

#### **1. Introduction**

26 Mobile Robots / Book 1

260 Recent Advances in Mobile Robotics

Tanaka, K. & Sano, M. (1995). Trajectory stabilization of a model car via fuzzy control, *Fuzzy*

Wang, H., Tanaka, K. & Griffin, M. (1996). An approach to fuzzy control of nonlinear systems: Stability and design issues, *IEEE Transactions on Fuzzy Systems* 4: 14 – 23. Zadeh, L. (2001). Outline of a computational theory of perceptions based on computing with

*Theory and Applications*, Academic Press, London, pp. 2–33.

words, *in* N. Sinha, M. Gupta & L. Zadeh (eds), *Soft Computing and Intelligent Systems:*

*Sets and Systems* 70(2-3): 155–170.

Long-term robustness is a crucial property of robots intended for real-world tasks such as, for example, transportation in indoor environments (e.g. warehouses, industries, hospitals, airports etc.). In order to be useful, such robots must be able to operate over long distances without much human supervision, something that sets high demands both on the actual robot (hardware) and its artificial brain<sup>1</sup> (software). This paper is focused on the development of artificial robotic brains for reliable long-term navigation (for use in, for example, transportation) in indoor environments.

Reliable decision-making is an essential tool for achieving long-term robustness. The ability to make correct decisions, in real-time and often based on incomplete and noisy information, is important not only for mobile robots but also for animals, including humans (McFarland, 1998; Prescott et al., 2007). One may argue that the entire sub-field of behavior-based robotics emerged, at least in part, as a result of the perceived failure of classical artificial intelligence to address real-time decision-making based on a robot's imperfect knowledge of the world. Starting with the *subsumption method* (Brooks, 1986), many different methods for decision-making, often referred to as methods for *behavior selection* or *action selection*, have been suggested in the literature on behavior-based robotics (see, for example, Bryson (2007); Pirjanian (1999) for reviews of such methods).

In actual applications, a common approach is to combine a *reactive layer* of decision-making, using mainly behavior-based concepts, with a *deliberative layer* using, for example, concepts from classical artificial intelligence, such as high-level reasoning) (Arkin, 1998). Several approaches of this kind have been suggested (see, for example, Arkin (1987) and Gat (1991)) and applied in different robots (see, for example, Sakagami et al. (2002)).

In the *utility function* (UF) method for decision-making (Wahde, 2003; 2009), which will be used here, a somewhat different approach is taken in which, for the purposes of decision-making, no distinction is made between reactive and deliberative aspects of the robotic brain. In this method, an artificial robotic brain is built from a repertoire of *brain processes* as well as a single decision-making system responsible for activating and de-activating brain processes

<sup>1</sup> The term *artificial (robotic) brain* is here used instead of the more common *control system* since the latter term, in the authors' view, signifies the low-level parts (such as motor control to achieve a certain speed) of robot intelligence, whereas more high-level parts, such as decision-making are better described by the term used here.

in Indoor Environments 3

Reliable Long-Term Navigation in Indoor Environments 263

Fig. 1. Left panel: The differentially steered robot considered here. The laser range finder is

to activate and de-activate the various brain processes. Here, only a brief description will be

Brain processes are divided into several types. The two categories used here are *cognitive processes* (that do not make use of the robot's motors) and *motor behaviors* that *do* make use of the motors. In the UF method, any number of cognitive process can be active simultaneously, whereas exactly one motor behavior is active at any time, the rationale being that a robot of the kind considered here (with two wheels as its only actuators) can only carry out one motor

Each brain process (regardless of its type) is associated with a *utility function* whose task it is to determine the merit of running the brain process, in any given situation. The utility functions, in turn, depend on scalar *state variables*. A simple example of a state variable *z* may be the reading of an IR sensor mounted at the front of a robot. The value of *z* is an (admittedly incomplete) measure of the risks involved in moving forward. A more complex example is the *moving obstacle danger level* (see Subsect. 4.4 below), which uses consecutive LRF scans to

obtain a scalar value measuring the risk of collision with moving obstacles.

mounted on top. Right panel: The simulated version of the robot.

given. For a more detailed description, see Wahde (2009).

action at any given time.

(several of which may run in parallel). Two kinds of brain processes are defined, namely *(motor) behaviors*, which make use of the robot's motors and *cognitive processes*, which do not. Both kinds of processes may include both reactive and deliberative aspects. Note also that, regardless of content, the brain processes used in the UF method are all written in a unified manner, described in Sect. 4 below.

As its name implies, the UF method, described in Sect. 3 below, is based on the concept of utility, formalized by von Neumann & Morgenstern (1944). The method is mainly intended for complex motor tasks (e.g. transportation of objects in arenas with moving obstacles) requiring both reactivity and deliberation. While the method has already been tested (both in simulation and in real robots) for applications involving navigation over distances of 10-20 m or less (Wahde, 2009), this paper will present a more challenging test of the method, involving operation over long distances. The task considered here will be a delivery task, in which a robot moves to a sequence of target points in a large arena containing both stationary and moving obstacles. The approach considered here involves long-term simulations, preceded by extensive validation of the simulator, using a real robot.

#### **2. Robot**

The differentially steered, two-wheeled robot (developed in the authors' group) used in this investigation is shown in Fig. 1. The robot has a circular shape with a radius of 0.20 m. Note that an indentation has been made for the wheels. The height (from the ground to the top of the laser range finder (LRF)) is around 0.84 m. The weight is around 14.5 kg. The robot's two actuators are Faulhaber 3863A024C DC motors, each with a Faulhaber 38/1S-14:1 planetary gearhead. Each motor is controlled by a Parallax HB-25 motor controller. The robot is equipped with two 8 Ah power sources, with voltages of 12 V and 7.2 V, respectively. In addition to the two drive wheels, the robot has two Castor wheels, one in the back and one in the front. The front Castor wheel is equipped with a suspension system, in order to avoid situations where the drive wheels lose contact with the ground, due to bumps in the surface on which the robot moves. The wheels are perforated by 24 holes which, together with a Boe-Bot Digital Encoder Kit detector on each wheel, are used for measuring the rotation of the wheels for use in odometry. In addition, the robot is equipped with a Hokuyo URG-04LX LRF with a range of 4 m and a 240 degree sweep angle. For proximity detection, the robot has two forward-pointing Sharp GP2D12 IR sensors (oriented towards ±30 degrees, respectively, from the robot's front direction), and one IR sensor, of the same type, pointing straight backwards. The robot is equipped with two Basic Stamp II microcontrollers (which, although slow, are sufficient for the task considered here), one that reads the wheel encoder signals, and one that (i) sends signals to the motor controllers and (ii) receives signals from the three IR sensors. The two Basic Stamps are, in turn, connected via a USB hub to a laptop that can be placed on top of the robot (under the beam that holds the LRF). The LRF is also connected to the laptop, via the same USB hub.

#### **3. Decision-making structure**

The decision-making structure is based on the UF method, the most recent version of which is described by Wahde (2009). The method is mainly intended for use in tasks such as navigation, transportation, or mapping. In this method, the artificial brain is built from (i) a set of brain processes and (ii) a decision-making system based on the concept of utility, allowing the robot 2 Will-be-set-by-IN-TECH

(several of which may run in parallel). Two kinds of brain processes are defined, namely *(motor) behaviors*, which make use of the robot's motors and *cognitive processes*, which do not. Both kinds of processes may include both reactive and deliberative aspects. Note also that, regardless of content, the brain processes used in the UF method are all written in a unified

As its name implies, the UF method, described in Sect. 3 below, is based on the concept of utility, formalized by von Neumann & Morgenstern (1944). The method is mainly intended for complex motor tasks (e.g. transportation of objects in arenas with moving obstacles) requiring both reactivity and deliberation. While the method has already been tested (both in simulation and in real robots) for applications involving navigation over distances of 10-20 m or less (Wahde, 2009), this paper will present a more challenging test of the method, involving operation over long distances. The task considered here will be a delivery task, in which a robot moves to a sequence of target points in a large arena containing both stationary and moving obstacles. The approach considered here involves long-term simulations, preceded

The differentially steered, two-wheeled robot (developed in the authors' group) used in this investigation is shown in Fig. 1. The robot has a circular shape with a radius of 0.20 m. Note that an indentation has been made for the wheels. The height (from the ground to the top of the laser range finder (LRF)) is around 0.84 m. The weight is around 14.5 kg. The robot's two actuators are Faulhaber 3863A024C DC motors, each with a Faulhaber 38/1S-14:1 planetary gearhead. Each motor is controlled by a Parallax HB-25 motor controller. The robot is equipped with two 8 Ah power sources, with voltages of 12 V and 7.2 V, respectively. In addition to the two drive wheels, the robot has two Castor wheels, one in the back and one in the front. The front Castor wheel is equipped with a suspension system, in order to avoid situations where the drive wheels lose contact with the ground, due to bumps in the surface on which the robot moves. The wheels are perforated by 24 holes which, together with a Boe-Bot Digital Encoder Kit detector on each wheel, are used for measuring the rotation of the wheels for use in odometry. In addition, the robot is equipped with a Hokuyo URG-04LX LRF with a range of 4 m and a 240 degree sweep angle. For proximity detection, the robot has two forward-pointing Sharp GP2D12 IR sensors (oriented towards ±30 degrees, respectively, from the robot's front direction), and one IR sensor, of the same type, pointing straight backwards. The robot is equipped with two Basic Stamp II microcontrollers (which, although slow, are sufficient for the task considered here), one that reads the wheel encoder signals, and one that (i) sends signals to the motor controllers and (ii) receives signals from the three IR sensors. The two Basic Stamps are, in turn, connected via a USB hub to a laptop that can be placed on top of the robot (under the beam that holds the LRF). The LRF is also connected to the laptop,

The decision-making structure is based on the UF method, the most recent version of which is described by Wahde (2009). The method is mainly intended for use in tasks such as navigation, transportation, or mapping. In this method, the artificial brain is built from (i) a set of brain processes and (ii) a decision-making system based on the concept of utility, allowing the robot

manner, described in Sect. 4 below.

**2. Robot**

via the same USB hub.

**3. Decision-making structure**

by extensive validation of the simulator, using a real robot.

Fig. 1. Left panel: The differentially steered robot considered here. The laser range finder is mounted on top. Right panel: The simulated version of the robot.

to activate and de-activate the various brain processes. Here, only a brief description will be given. For a more detailed description, see Wahde (2009).

Brain processes are divided into several types. The two categories used here are *cognitive processes* (that do not make use of the robot's motors) and *motor behaviors* that *do* make use of the motors. In the UF method, any number of cognitive process can be active simultaneously, whereas exactly one motor behavior is active at any time, the rationale being that a robot of the kind considered here (with two wheels as its only actuators) can only carry out one motor action at any given time.

Each brain process (regardless of its type) is associated with a *utility function* whose task it is to determine the merit of running the brain process, in any given situation. The utility functions, in turn, depend on scalar *state variables*. A simple example of a state variable *z* may be the reading of an IR sensor mounted at the front of a robot. The value of *z* is an (admittedly incomplete) measure of the risks involved in moving forward. A more complex example is the *moving obstacle danger level* (see Subsect. 4.4 below), which uses consecutive LRF scans to obtain a scalar value measuring the risk of collision with moving obstacles.

in Indoor Environments 5

Reliable Long-Term Navigation in Indoor Environments 265

user must set these parameters to appropriate values. Note that, for many cognitive processes (e.g. odometry, which normally should be running continuously), all that is needed is for the utility values to be positive at all times, which can easily be arranged by setting all the

In many cases, the remaining parameters (especially those pertaining to the motor behaviors) can be set by hand using trial-and-error. In more complex situations, one can use a stochastic optimization method such as, for example, a genetic algorithm or particle swarm optimization to find appropriate parameters values. In those situations, one must first define a suitable objective function, for example the distance travelled in a given amount of time, subject to the condition that collisions should be avoided. Optimization runs of this kind are normally carried out in the simulator (described in Subsect. 5.1 below) rather than a real robot, since the

Just as the decision-making system, brain processes used in the UF method also have a unified structure; the main part of each brain process is a finite-state machine (FSM) consisting of*states* (in which various computations are carried out and, in the case of motor behaviors, actions are taken) and *conditional transitions* between states. In any brain process, the FSM executes the *current state* until a transition condition forces it to jump to another state, which is then

For the task considered here, namely reliable long-term navigation, six brain processes have been used, namely *Grid navigation* (denoted *B*1), *Odometry* (*B*2), *Odometric calibration* (*B*3), *Moving obstacle detection* (*B*4), *Moving obstacle avoidance* (*B*5), and *Long-term memory* (*B*6). The

The navigation method used here is contained in a brain process called *Grid navigation*. As the name implies, the navigation method (which is intended for indoor, planar environments) relies on a grid. Dividing a given arena into a set of convex cells is a common problem in robotics, and it is typically approached using *Voronoi diagrams* (or their dual, *Delaunay triangulation*) (Okabe et al., 2000) or *Meadow maps* (Singh & Agarwal, 2010). However, these tessellations tend to generate jagged paths, with unnecessary turns that increase the length of the robot's path (a problem that can be overcome using *path relaxation*; see Thorpe (1984)). Nevertheless, in this paper, a different method for automatic grid generation will be used, in which the map of the arena is first contracted to generate a margin for the robot's movement. Next, the arena is divided into convex cells in a preprocessing step similar to that described by Singh & Wagh (1987). Note, however, that the method described in this paper also can handle non-rectangular arenas (and obstacles). Finally, the cells are joined in order to generate larger convex cells in which the robot can move freely. Once such a grid is available, a path is generated using Dijkstra's algorithm (see Sect. 4.1.2 below), optimizing the placement of waypoints (on the edges of the cells) in order to minimize the path length. The method for

It is assumed that a map is available, in the form of a set of closed *map curves* consisting, in turn, of a sequence of connected (i.e. sharing a common point) *map curve segments*, each

*a*-parameters for that process to zero, and the *b*-parameter to any positive value.

number of evaluations needed during optimization can become quite large.

executed etc. An illustration of a brain process FSM is shown in Fig. 8 below.

grid generation, which is summarized in Table 1, will now be described.

**4. Brain processes**

**4.1 Grid navigation**

**4.1.1 Grid generation method**

six brain processes will be described next.

State variable values are measured continuously, but asynchronously (since not all sensors are updated with the same frequency), and the most recent values available are used as inputs to the utility functions from which the utility value *ui* for brain process *i* is obtained using the equation

$$
\tau\_i \dot{\mu}\_i + \mu\_i = \sigma\_i \left( \sum\_{k=1}^m a\_{ik} z\_k + b\_i + \Gamma\_i \right), \ i = 1, \dots, n,\tag{1}
$$

where *n* is the number of brain processes, *τ<sup>i</sup>* are time constants determining the reaction time of the robot (typically set to around 0.1 s), *m* is the number of state variables (denoted *zk*), and *aik* and *bi* are tunable parameters. The squashing function *σi*(*x*) is taken as tanh(*cix*) where *ci* is a positive constant (typically set to 1). Thus, the squashing functions *σ<sup>i</sup>* serve to keep the utility values in the range [−1, 1].

Once the values of the utility functions have been computed, decision-making is simple in the UF method, and works as follows: (i) The motor behavior with largest utility (among all motor behaviors) is active and (ii) any cognitive process with positive utility is active. All other brain processes are inactive.

The parameters Γ*<sup>i</sup>* (referred to as *gamma parameters*), which are normally equal to zero, allow direct activation or de-activation of a brain process. Ideally, the state variables *zk* should provide the robot will the information needed to make an informed decision regarding which brain processes to use in any situation encountered. However, in practice, the parameters Γ*<sup>i</sup>* are needed in certain situations. Consider, for example, a simple case in which the utility function for an obstacle avoidance behavior depends on a single state variable *z* (multiplied by a positive constant *a*) that measures obstacle proximity (using, for example, IR or sonar sensors). Now, if the obstacle avoidance behavior is activated, the robot's first action is commonly to turn away from the obstacle. When this happens, the value *z* will then drop, so that *u* also drops, albeit with a slight delay depending on the value of the time constant in Eq. (1). At this point, the obstacle avoidance behavior is likely to be de-activated again, before actually having properly avoided the obstacle. The gamma parameters, which can be set directly by the brain processes, have been introduced to prevent such problems. Thus, for example, when the obstacle avoidance behavior is activated, it can set its *own* gamma parameter to a positive value, thus normally avoiding de-activation when the state variable *z* drops as described above. Whenever the obstacle avoidance behavior has run its course, it can set the gamma parameter to a large negative value, thus effectively de-activating itself. Note that the decision-making system is active continuously, so that, in the example above, obstacle avoidance *can* be de-activated (even after raising its own gamma parameter) should another brain process reach an even higher utility value. Once a gamma parameter has been set to any value (positive or negative) other than zero, its magnitude falls of exponentially with time, with a time constant (*τ*<sup>Γ</sup> *<sup>i</sup>* ) specific to the brain process at hand. This time constant typically takes a larger value than the time constant in the utility function.

As mentioned above, the UF method allows several (cognitive) brain processes to run in parallel with the (single) active motor behavior. Thus, for example, while moving (using a motor behavior), a robot using the UF method would simultaneously be able to run two cognitive processes, one for generating odometric pose estimates from encoder readings and one for processing laser scans in order to recalibrate its odometric readings, if needed.

As is evident from Eq. (1), for a given set of state variables, the actual decisions taken by the robot will depend on the parameters *τi*, *aij*, *bi*, and *ci* as well as the values of the gamma parameters (if used). Thus, in order to make a robot carry out a specific task correctly, the user must set these parameters to appropriate values. Note that, for many cognitive processes (e.g. odometry, which normally should be running continuously), all that is needed is for the utility values to be positive at all times, which can easily be arranged by setting all the *a*-parameters for that process to zero, and the *b*-parameter to any positive value.

In many cases, the remaining parameters (especially those pertaining to the motor behaviors) can be set by hand using trial-and-error. In more complex situations, one can use a stochastic optimization method such as, for example, a genetic algorithm or particle swarm optimization to find appropriate parameters values. In those situations, one must first define a suitable objective function, for example the distance travelled in a given amount of time, subject to the condition that collisions should be avoided. Optimization runs of this kind are normally carried out in the simulator (described in Subsect. 5.1 below) rather than a real robot, since the number of evaluations needed during optimization can become quite large.

#### **4. Brain processes**

4 Will-be-set-by-IN-TECH

State variable values are measured continuously, but asynchronously (since not all sensors are updated with the same frequency), and the most recent values available are used as inputs to the utility functions from which the utility value *ui* for brain process *i* is obtained using the

*aikzk* + *bi* + Γ*<sup>i</sup>*

where *n* is the number of brain processes, *τ<sup>i</sup>* are time constants determining the reaction time of the robot (typically set to around 0.1 s), *m* is the number of state variables (denoted *zk*), and *aik* and *bi* are tunable parameters. The squashing function *σi*(*x*) is taken as tanh(*cix*) where *ci* is a positive constant (typically set to 1). Thus, the squashing functions *σ<sup>i</sup>* serve to keep the

Once the values of the utility functions have been computed, decision-making is simple in the UF method, and works as follows: (i) The motor behavior with largest utility (among all motor behaviors) is active and (ii) any cognitive process with positive utility is active. All

The parameters Γ*<sup>i</sup>* (referred to as *gamma parameters*), which are normally equal to zero, allow direct activation or de-activation of a brain process. Ideally, the state variables *zk* should provide the robot will the information needed to make an informed decision regarding which brain processes to use in any situation encountered. However, in practice, the parameters Γ*<sup>i</sup>* are needed in certain situations. Consider, for example, a simple case in which the utility function for an obstacle avoidance behavior depends on a single state variable *z* (multiplied by a positive constant *a*) that measures obstacle proximity (using, for example, IR or sonar sensors). Now, if the obstacle avoidance behavior is activated, the robot's first action is commonly to turn away from the obstacle. When this happens, the value *z* will then drop, so that *u* also drops, albeit with a slight delay depending on the value of the time constant in Eq. (1). At this point, the obstacle avoidance behavior is likely to be de-activated again, before actually having properly avoided the obstacle. The gamma parameters, which can be set directly by the brain processes, have been introduced to prevent such problems. Thus, for example, when the obstacle avoidance behavior is activated, it can set its *own* gamma parameter to a positive value, thus normally avoiding de-activation when the state variable *z* drops as described above. Whenever the obstacle avoidance behavior has run its course, it can set the gamma parameter to a large negative value, thus effectively de-activating itself. Note that the decision-making system is active continuously, so that, in the example above, obstacle avoidance *can* be de-activated (even after raising its own gamma parameter) should another brain process reach an even higher utility value. Once a gamma parameter has been set to any value (positive or negative) other than zero, its magnitude falls of exponentially

*<sup>i</sup>* ) specific to the brain process at hand. This time constant

, *i* = 1, . . . , *n*, (1)

*τiu*˙ *<sup>i</sup>* + *ui* = *σ<sup>i</sup>*

utility values in the range [−1, 1].

other brain processes are inactive.

with time, with a time constant (*τ*<sup>Γ</sup>

typically takes a larger value than the time constant in the utility function.

As mentioned above, the UF method allows several (cognitive) brain processes to run in parallel with the (single) active motor behavior. Thus, for example, while moving (using a motor behavior), a robot using the UF method would simultaneously be able to run two cognitive processes, one for generating odometric pose estimates from encoder readings and one for processing laser scans in order to recalibrate its odometric readings, if needed. As is evident from Eq. (1), for a given set of state variables, the actual decisions taken by the robot will depend on the parameters *τi*, *aij*, *bi*, and *ci* as well as the values of the gamma parameters (if used). Thus, in order to make a robot carry out a specific task correctly, the

 *m* ∑ *k*=1

equation

Just as the decision-making system, brain processes used in the UF method also have a unified structure; the main part of each brain process is a finite-state machine (FSM) consisting of*states* (in which various computations are carried out and, in the case of motor behaviors, actions are taken) and *conditional transitions* between states. In any brain process, the FSM executes the *current state* until a transition condition forces it to jump to another state, which is then executed etc. An illustration of a brain process FSM is shown in Fig. 8 below.

For the task considered here, namely reliable long-term navigation, six brain processes have been used, namely *Grid navigation* (denoted *B*1), *Odometry* (*B*2), *Odometric calibration* (*B*3), *Moving obstacle detection* (*B*4), *Moving obstacle avoidance* (*B*5), and *Long-term memory* (*B*6). The six brain processes will be described next.

#### **4.1 Grid navigation**

The navigation method used here is contained in a brain process called *Grid navigation*. As the name implies, the navigation method (which is intended for indoor, planar environments) relies on a grid. Dividing a given arena into a set of convex cells is a common problem in robotics, and it is typically approached using *Voronoi diagrams* (or their dual, *Delaunay triangulation*) (Okabe et al., 2000) or *Meadow maps* (Singh & Agarwal, 2010). However, these tessellations tend to generate jagged paths, with unnecessary turns that increase the length of the robot's path (a problem that can be overcome using *path relaxation*; see Thorpe (1984)). Nevertheless, in this paper, a different method for automatic grid generation will be used, in which the map of the arena is first contracted to generate a margin for the robot's movement. Next, the arena is divided into convex cells in a preprocessing step similar to that described by Singh & Wagh (1987). Note, however, that the method described in this paper also can handle non-rectangular arenas (and obstacles). Finally, the cells are joined in order to generate larger convex cells in which the robot can move freely. Once such a grid is available, a path is generated using Dijkstra's algorithm (see Sect. 4.1.2 below), optimizing the placement of waypoints (on the edges of the cells) in order to minimize the path length. The method for grid generation, which is summarized in Table 1, will now be described.

#### **4.1.1 Grid generation method**

It is assumed that a map is available, in the form of a set of closed *map curves* consisting, in turn, of a sequence of connected (i.e. sharing a common point) *map curve segments*, each

in Indoor Environments 7

Reliable Long-Term Navigation in Indoor Environments 267

Fig. 3. The map contraction procedure. The left panel shows the original map (dotted line), along with the line segments obtained using the procedure described in the main text. In the right panel, the intersection points between those lines have been determined, resulting in

line in the contracted map, it should not touch any wall or other fixed obstacle. The procedure is illustrated in Fig. 3. The algorithm for generating the contracted map is straightforward: Consider a given line segment **p** = (*px*, *py*) = **P***<sup>b</sup>* − **P***<sup>b</sup>* = (*xb* − *xa*, *yb* − *ya*). Let **c** be a vector

> **<sup>c</sup>** <sup>=</sup> *<sup>μ</sup>*<sup>Ω</sup> (−*py*, *px*) *p*2 *<sup>x</sup>* + *p*<sup>2</sup> *y*

Note, however, that these two points are normally not the end points of the contracted map curve segment. In order to determine the end points, one must first carry out the process described above for all map curve segments in a given map curve. Once that process has been completed, the end points of the contracted map curve segments can be determined as the intersection points between the lines passing through the point pairs generated during the contraction process just described. If these lines are extended to infinity, any given line may intersect many other lines. In that case, the points chosen as end points of the contracted map curve segments are taken as those intersection points that are nearest to the points **Q***<sup>a</sup>* and **Q***b*, respectively. The process is illustrated in Fig. 3, where a map consisting of a single map curve,

Note that, in its simplest form, the map contraction method requires that no contracted map *curve* should intersect any of the other contracted map curves. The cases in which this happens

. (3)

**Q***<sup>a</sup>* = **P***<sup>a</sup>* + **c** (4)

**Q***<sup>b</sup>* = **P***<sup>b</sup>* + **c**. (5)

of length *μ* orthogonal to **p** and pointing towards the in-direction. Thus

It is now possible to obtain two points on the contracted map curve segment as

the contracted map (solid line).

with six map curve segments, is contracted.

and

Step 1 Generate the contracted map.

Step 2 Generate the preliminary map curve intersection grid.

Step 3 Process non-axis parallel map curve segments.

Step 4 Generate the map curve intersection grid by removing cells outside the grid.

Step 5 Generate the convex navigation grid by joining cells.

Table 1. The algorithm for generating the convex navigation grid. See the main text for a detailed description of the various steps.

Fig. 2. The definition of in-directions in a rectangular arena with a square obstacle. In this simple arena, there are two map curves, each with four enumerated map curve segments. The thick arrow on each map curve segment points from the starting point to the end point of the segment. The thin arrows point towards the in-direction. The corresponding value of Ω (see Eq. (2)) is equal to +1 for the outer map curve (wall) and −1 for the inner map curve (obstacle). Note that the sign of Ω depends on the (arbitrary) choice of enumeration (clockwise or counterclockwise) for the map curve segments.

defined by a starting point **P***<sup>a</sup>* = (*xa*, *ya*) and an end point **P***<sup>b</sup>* = (*xb*, *yb*). Thus, each map curve is a simple (i.e. without self-intersections) polygon. In addition, for each map curve segment, an *in-direction* is defined. The in-direction Ω is given by the sign (either -1 or 1) of the *z*-component of the cross product between the vector **p** = **P***<sup>b</sup>* − **P***<sup>a</sup>* and a unit vector **d** (orthogonal to **p**) pointing towards the side of the map curve segment which is accessible to the robot. Thus,

$$
\Omega = \text{sgn}\left( (\mathbf{p} \times \mathbf{d}) \cdot \mathbf{\mathcal{E}} \right). \tag{2}
$$

The in-direction concept is illustrated in Fig. 2. Note that, for any given map curve, all segments will have the same in-direction.

The method divides the accessible area of a robot's environment into a set of convex grid cells. Due to the convexity of the grid cells the robot can move freely within any grid cell, a property that facilitates path optimization, as described below. The grid generation method operates as follows: First (Step 1 in the algorithm described in Table 1), since the robot has a certain size, the actual map is shrunk to generated a *contracted map*. The margin *μ* (a tunable parameter) used when generating the contracted map should be such that, if the robot is positioned on a

Fig. 3. The map contraction procedure. The left panel shows the original map (dotted line), along with the line segments obtained using the procedure described in the main text. In the right panel, the intersection points between those lines have been determined, resulting in the contracted map (solid line).

line in the contracted map, it should not touch any wall or other fixed obstacle. The procedure is illustrated in Fig. 3. The algorithm for generating the contracted map is straightforward: Consider a given line segment **p** = (*px*, *py*) = **P***<sup>b</sup>* − **P***<sup>b</sup>* = (*xb* − *xa*, *yb* − *ya*). Let **c** be a vector of length *μ* orthogonal to **p** and pointing towards the in-direction. Thus

$$\mathbf{c} = \mu \Omega \frac{(-p\_{y'}, p\_x)}{\sqrt{p\_x^2 + p\_y^2}}.\tag{3}$$

It is now possible to obtain two points on the contracted map curve segment as

$$\mathbf{Q}\_a = \mathbf{P}\_a + \mathbf{c} \tag{4}$$

and

2

3

6 Will-be-set-by-IN-TECH

Step 4 Generate the map curve intersection grid by removing cells outside the grid.

1 = +1

(clockwise or counterclockwise) for the map curve segments.

<sup>1</sup> <sup>2</sup>

Fig. 2. The definition of in-directions in a rectangular arena with a square obstacle. In this simple arena, there are two map curves, each with four enumerated map curve segments. The thick arrow on each map curve segment points from the starting point to the end point of the segment. The thin arrows point towards the in-direction. The corresponding value of Ω (see Eq. (2)) is equal to +1 for the outer map curve (wall) and −1 for the inner map curve (obstacle). Note that the sign of Ω depends on the (arbitrary) choice of enumeration

defined by a starting point **P***<sup>a</sup>* = (*xa*, *ya*) and an end point **P***<sup>b</sup>* = (*xb*, *yb*). Thus, each map curve is a simple (i.e. without self-intersections) polygon. In addition, for each map curve segment, an *in-direction* is defined. The in-direction Ω is given by the sign (either -1 or 1) of the *z*-component of the cross product between the vector **p** = **P***<sup>b</sup>* − **P***<sup>a</sup>* and a unit vector **d** (orthogonal to **p**) pointing towards the side of the map curve segment which is accessible to

The in-direction concept is illustrated in Fig. 2. Note that, for any given map curve, all

The method divides the accessible area of a robot's environment into a set of convex grid cells. Due to the convexity of the grid cells the robot can move freely within any grid cell, a property that facilitates path optimization, as described below. The grid generation method operates as follows: First (Step 1 in the algorithm described in Table 1), since the robot has a certain size, the actual map is shrunk to generated a *contracted map*. The margin *μ* (a tunable parameter) used when generating the contracted map should be such that, if the robot is positioned on a

Ω = sgn ((**p** × **d**) · *z*ˆ). (2)

3 4

Table 1. The algorithm for generating the convex navigation grid. See the main text for a

= -1

Step 1 Generate the contracted map.

detailed description of the various steps.

4

the robot. Thus,

segments will have the same in-direction.

Step 2 Generate the preliminary map curve intersection grid. Step 3 Process non-axis parallel map curve segments.

Step 5 Generate the convex navigation grid by joining cells.

$$\mathbf{Q}\_b = \mathbf{P}\_b + \mathbf{c}.\tag{5}$$

Note, however, that these two points are normally not the end points of the contracted map curve segment. In order to determine the end points, one must first carry out the process described above for all map curve segments in a given map curve. Once that process has been completed, the end points of the contracted map curve segments can be determined as the intersection points between the lines passing through the point pairs generated during the contraction process just described. If these lines are extended to infinity, any given line may intersect many other lines. In that case, the points chosen as end points of the contracted map curve segments are taken as those intersection points that are nearest to the points **Q***<sup>a</sup>* and **Q***b*, respectively. The process is illustrated in Fig. 3, where a map consisting of a single map curve, with six map curve segments, is contracted.

Note that, in its simplest form, the map contraction method requires that no contracted map *curve* should intersect any of the other contracted map curves. The cases in which this happens

in Indoor Environments 9

Reliable Long-Term Navigation in Indoor Environments 269

1

1

 

 

Fig. 5. The grid simplification algorithm, applied to the grid shown in Fig. 4. During grid simplification, polygonal cells are combined to form larger (but still convex) cells. Note that, in each step, the enumeration changes after the removal of the two cells that are joined. The upper left panel shows the initial grid, whereas the upper right panel shows the grid after the first step. In the bottom row, the left panel shows the grid after the second step, and the right

*can* be handled, but the procedure for doing so will not be described here. Instead, we shall limit ourselves to considering maps in which the contracted map curves do not intersect. Once the contracted map has been found, the algorithm (Step 2) proceeds to generate a temporary grid (referred to as the *preliminary map curve intersection grid*), consisting of the intersections between all axis-parallel (horizontal or vertical) map curve segments expanded to infinite length, as illustrated in the first row of Fig. 4. Next (Step 3), all non-axis parallel map curve segments are processed to check for intersections between those lines and the sides of the cells in the preliminary map curve intersection grid. For any intersection found, the

panel shows the final result, after several steps.

Fig. 4. The grid generation algorithm, applied to the contracted map from Fig. 3. See the main text for a detailed description of the various steps.

Will-be-set-by-IN-TECH

Fig. 4. The grid generation algorithm, applied to the contracted map from Fig. 3. See the

main text for a detailed description of the various steps.

Fig. 5. The grid simplification algorithm, applied to the grid shown in Fig. 4. During grid simplification, polygonal cells are combined to form larger (but still convex) cells. Note that, in each step, the enumeration changes after the removal of the two cells that are joined. The upper left panel shows the initial grid, whereas the upper right panel shows the grid after the first step. In the bottom row, the left panel shows the grid after the second step, and the right panel shows the final result, after several steps.

*can* be handled, but the procedure for doing so will not be described here. Instead, we shall limit ourselves to considering maps in which the contracted map curves do not intersect. Once the contracted map has been found, the algorithm (Step 2) proceeds to generate a temporary grid (referred to as the *preliminary map curve intersection grid*), consisting of the intersections between all axis-parallel (horizontal or vertical) map curve segments expanded to infinite length, as illustrated in the first row of Fig. 4. Next (Step 3), all non-axis parallel map curve segments are processed to check for intersections between those lines and the sides of the cells in the preliminary map curve intersection grid. For any intersection found, the

in Indoor Environments 11

Reliable Long-Term Navigation in Indoor Environments 271

Fig. 6. Path planning: The upper left panel shows the convex navigation grid for the arena described in Sect. 6 and the remaining panels show the paths considered during path planning. In this case, the robot was located in the lower left corner of the arena, and its navigation target was located near the upper right corner. The first path considered is shown in the upper middle panel. After some time, the optimization procedure found a shorter path (upper right panel), along a different route. In the remaining steps (lower panels) the path planning algorithm further optimized the path (length minimization) by adjusting the placement of the waypoints (on the edges of the convex grid cells). Note that, in this figure, the actual navigation path is shown, i.e. not only the waypoints on the cell edges, but also all

The process is carried out for all grid cells, since the geometrically shortest path may involve a potential collision with a moving obstacle, as described above, in which case another path must be found, as shown in Fig. 7. A navigation path candidate is obtained once all grid cells

However, as noted above, the exact locations of the waypoints *qc*,*ai* (on the edges of the navigation grid cells) are randomly chosen. Thus, an optimization procedure is applied, during which the entire path generation process is iterated *n* times, using different randomly selected waypoints along the cell sides (see also Fig. 6), and the shortest path thus found is taken as the final path. Once this path has been obtained, the actual *navigation path* is formed by adding interpolated waypoints between the waypoints obtained during path generation, so that the distance between consecutive waypoints is, at most, *d*wp (a user-specified

the intermediate (interpolated) waypoints.

have been considered.

parameter).

corresponding point is added to the grid, as illustrated in the middle row (left panel) of Fig. 4. Lines are then drawn horizontally and vertically from the newly added points, and the points corresponding to any intersections between those two lines and the cells of the preliminary map curve intersection grid are added as well; see the right panel in the middle row of Fig. 4. Some of the cells will be located outside the (contracted) map, as shown in the left panel in the bottom row of Fig. 4. Those cells are removed (Step 4), resulting in the *map curve intersection grid*, shown in the final panel of Fig. 4.

Note that the cells in the map curve intersection grid are convex by construction. In principle, one could just remove the inaccessible cells and use the remaining cells during navigation. However, in a realistic arena (more complex than the simple arena used for illustrating the procedure) the map curve intersection grid typically contains very many cells, thus slowing down the path generation. Furthermore, with a large number of cells, the paths generated will typically have an unwanted zig-zag nature. Thus, in Step 5 of the algorithm, cells in the map curve intersection grid are joined to form larger cells, with the condition that two cells are only joined if the result is again a convex polygon, something that can easily be checked since, in a convex polygon, all cross products between consecutive sides (i.e. sides that share a common point) have the same sign. In the current version of the algorithm, no attempt has been made to generate a minimal (in number) set of convex polygons. Instead, the cells are simply processed in order, removing joined cells and adding newly formed cells, until there are no cells left such that their combination would be convex. The first steps of this process, as well as the final result, are shown in Fig. 5. The result is referred to as the *convex navigation grid*.

#### **4.1.2 Path planning**

The path planning procedure uses the convex navigation grid (which can be generated once and for all for any given (fixed) arena). Given a starting point (i.e. the robot's estimate of its current location) and a desired goal point, a path is generated using Dijkstra's algorithm (Dijkstra, 1959), augmented with a process for selecting navigation waypoints on the edges of the grid cells through which the path passes. As in the standard Dijkstra algorithm, cells are considered in an expanding pattern from the starting cell (i.e. the cell in which the robot is currently located). For any considered cell *c*, all neighbors *ai* of *c* (i.e. those cells that, partially or fully, share a common side with *c*) are considered. For each *ai*, a randomly generated waypoint *qc*,*ai* is selected (and stored) along the common side shared by the two cells (see Fig. 6). The distance from *ai* to the starting point can then be computed and stored.

However, the path planning algorithm should also be able to handle situations in which a moving obstacle (which, of course, would not be included in the convex navigation grid) is found. If, for a line segment *l* connecting *c* to *ai*, an intersection is found between *l* and a circle (with an added margin) around a detected moving obstacle (see below), a large penalty is added in the computation of the distance from *ai* to the starting point, thus effectively rendering that path useless. Note that the robot, of course, has no global knowledge of the positions of moving obstacles: Only those moving obstacles that it can actually *detect* are included in the analysis. Note also that the path planning method will ignore any detected moving obstacle at a distance of at least Δ (a tunable parameter) from the robot. This is so, since there is no point to account for a faraway moving obstacle (which may not even be headed in the robot's direction) while planning the path.

10 Will-be-set-by-IN-TECH

corresponding point is added to the grid, as illustrated in the middle row (left panel) of Fig. 4. Lines are then drawn horizontally and vertically from the newly added points, and the points corresponding to any intersections between those two lines and the cells of the preliminary map curve intersection grid are added as well; see the right panel in the middle row of Fig. 4. Some of the cells will be located outside the (contracted) map, as shown in the left panel in the bottom row of Fig. 4. Those cells are removed (Step 4), resulting in the *map curve intersection*

Note that the cells in the map curve intersection grid are convex by construction. In principle, one could just remove the inaccessible cells and use the remaining cells during navigation. However, in a realistic arena (more complex than the simple arena used for illustrating the procedure) the map curve intersection grid typically contains very many cells, thus slowing down the path generation. Furthermore, with a large number of cells, the paths generated will typically have an unwanted zig-zag nature. Thus, in Step 5 of the algorithm, cells in the map curve intersection grid are joined to form larger cells, with the condition that two cells are only joined if the result is again a convex polygon, something that can easily be checked since, in a convex polygon, all cross products between consecutive sides (i.e. sides that share a common point) have the same sign. In the current version of the algorithm, no attempt has been made to generate a minimal (in number) set of convex polygons. Instead, the cells are simply processed in order, removing joined cells and adding newly formed cells, until there are no cells left such that their combination would be convex. The first steps of this process, as well as the final result, are shown in Fig. 5. The result is referred to as the *convex navigation*

The path planning procedure uses the convex navigation grid (which can be generated once and for all for any given (fixed) arena). Given a starting point (i.e. the robot's estimate of its current location) and a desired goal point, a path is generated using Dijkstra's algorithm (Dijkstra, 1959), augmented with a process for selecting navigation waypoints on the edges of the grid cells through which the path passes. As in the standard Dijkstra algorithm, cells are considered in an expanding pattern from the starting cell (i.e. the cell in which the robot is currently located). For any considered cell *c*, all neighbors *ai* of *c* (i.e. those cells that, partially or fully, share a common side with *c*) are considered. For each *ai*, a randomly generated waypoint *qc*,*ai* is selected (and stored) along the common side shared by the two cells (see Fig. 6). The distance from *ai* to the starting point can then be computed

However, the path planning algorithm should also be able to handle situations in which a moving obstacle (which, of course, would not be included in the convex navigation grid) is found. If, for a line segment *l* connecting *c* to *ai*, an intersection is found between *l* and a circle (with an added margin) around a detected moving obstacle (see below), a large penalty is added in the computation of the distance from *ai* to the starting point, thus effectively rendering that path useless. Note that the robot, of course, has no global knowledge of the positions of moving obstacles: Only those moving obstacles that it can actually *detect* are included in the analysis. Note also that the path planning method will ignore any detected moving obstacle at a distance of at least Δ (a tunable parameter) from the robot. This is so, since there is no point to account for a faraway moving obstacle (which may not even be

headed in the robot's direction) while planning the path.

*grid*, shown in the final panel of Fig. 4.

*grid*.

**4.1.2 Path planning**

and stored.

Fig. 6. Path planning: The upper left panel shows the convex navigation grid for the arena described in Sect. 6 and the remaining panels show the paths considered during path planning. In this case, the robot was located in the lower left corner of the arena, and its navigation target was located near the upper right corner. The first path considered is shown in the upper middle panel. After some time, the optimization procedure found a shorter path (upper right panel), along a different route. In the remaining steps (lower panels) the path planning algorithm further optimized the path (length minimization) by adjusting the placement of the waypoints (on the edges of the convex grid cells). Note that, in this figure, the actual navigation path is shown, i.e. not only the waypoints on the cell edges, but also all the intermediate (interpolated) waypoints.

The process is carried out for all grid cells, since the geometrically shortest path may involve a potential collision with a moving obstacle, as described above, in which case another path must be found, as shown in Fig. 7. A navigation path candidate is obtained once all grid cells have been considered.

However, as noted above, the exact locations of the waypoints *qc*,*ai* (on the edges of the navigation grid cells) are randomly chosen. Thus, an optimization procedure is applied, during which the entire path generation process is iterated *n* times, using different randomly selected waypoints along the cell sides (see also Fig. 6), and the shortest path thus found is taken as the final path. Once this path has been obtained, the actual *navigation path* is formed by adding interpolated waypoints between the waypoints obtained during path generation, so that the distance between consecutive waypoints is, at most, *d*wp (a user-specified parameter).

in Indoor Environments 13

Reliable Long-Term Navigation in Indoor Environments 273

No

not contain any obstacle. If this is so, the robot jumps to State 5 of the FSM, in which it sets the motor signals to equal, positive values, making the robot move forward, essentially in a straight line3. It then jumps back to State 2, again checking if it has reached the grid etc. If instead the robot finds that the path ahead is not clear, it jumps to State 4, in which it sets the motor signals to equal values but with opposite signs, so that it begins rotating without moving its center of mass. It then jumps back to State 3, to check whether the path ahead is

If, in State 2, the robot finds that it is located *inside* the grid, it jumps to State 6, in which it determines the starting point (from the current odometric estimate) and the navigation goal (provided by the user), then generates an optimized path using the procedure described in Subsect. 4.1.2 and, finally, sets the current waypoint as the second waypoint in the path (the first point being its current location). It then jumps to State 7, in which it checks whether (the absolute value of) the difference4 in estimated heading (using odometry) and desired heading (obtained using the direction to the current waypoint) exceeds a threshold *δ*. If so, it begins a pure rotation (as in State 4, see above). Once the difference in heading drops below the threshold, the robot jumps to State 8, in which the left and right motor signals are set as

where Δ*ϕ* is the difference in heading angle, and *Kp* is a constant. While in State 8, the robot continuously checks the difference between estimated and desired heading. If it should

<sup>3</sup> Even though the motor settings are equal, the actual path may differ slightly from a straight line, due

<sup>4</sup> The difference <sup>Δ</sup>*<sup>φ</sup>* between two heading angles *<sup>ϕ</sup>*<sup>1</sup> and *<sup>ϕ</sup>*<sup>2</sup> is defined as *<sup>ϕ</sup>*<sup>1</sup> <sup>−</sup> *<sup>ϕ</sup>*<sup>2</sup> mod 2*π*.

State 4: Rotate

State 3: Clear path ahead? State 5: Move forward

Yes

*v*<sup>L</sup> = *V*nav − Δ*V* (6)

*v*<sup>R</sup> = *V*nav + Δ*V*, (7)

Δ*V* = *KpV*navΔ*ϕ*, (8)

State 6: Generate path and set first waypoint

Heading difference below threshold

Heading difference above threshold

State 8: Navigate State 7: Rotate

State 9: Set waypoint Navigation goal not yet reached

Fig. 8. The FSM implementing grid navigation.

to actuator noise and other similar sources of error.

Yes

No

State 1: Set goal

Navigation goal reached

> Waypoint reached

clear.

and

respectively, where

State 2: Inside grid?

Fig. 7. Path planning with (upper panels) and without (lower panels) moving obstacles present. The moving obstacle is represented as a gray cylinder.

#### **4.1.3 Navigation**

The navigation motor behavior, illustrated in Fig. 8, consists of an FSM with 9 states. In State 1, the robot stops (path planning takes place at standstill), and jumps to State 2 in which the robot determines whether or not (based on its odometric position estimate) it is actually located in the convex navigation grid. This check is necessary, since the robot may have left the grid (slightly) as a result of carrying out moving obstacle avoidance; see below. If the robot finds that it is located *outside* the grid, it jumps to State 3 in which it checks, using the LRF2, whether a certain sector (angular width *γc*, radius *rc*) in front of the robot is clear, i.e. does

<sup>2</sup> It is assumed that the height of the moving obstacles is such that they are detectable by the (two-dimensional) LRF.

Fig. 8. The FSM implementing grid navigation.

not contain any obstacle. If this is so, the robot jumps to State 5 of the FSM, in which it sets the motor signals to equal, positive values, making the robot move forward, essentially in a straight line3. It then jumps back to State 2, again checking if it has reached the grid etc. If instead the robot finds that the path ahead is not clear, it jumps to State 4, in which it sets the motor signals to equal values but with opposite signs, so that it begins rotating without moving its center of mass. It then jumps back to State 3, to check whether the path ahead is clear.

If, in State 2, the robot finds that it is located *inside* the grid, it jumps to State 6, in which it determines the starting point (from the current odometric estimate) and the navigation goal (provided by the user), then generates an optimized path using the procedure described in Subsect. 4.1.2 and, finally, sets the current waypoint as the second waypoint in the path (the first point being its current location). It then jumps to State 7, in which it checks whether (the absolute value of) the difference4 in estimated heading (using odometry) and desired heading (obtained using the direction to the current waypoint) exceeds a threshold *δ*. If so, it begins a pure rotation (as in State 4, see above). Once the difference in heading drops below the threshold, the robot jumps to State 8, in which the left and right motor signals are set as

$$
\sigma\_{\rm L} = V\_{\rm nav} - \Delta V \tag{6}
$$

and

12 Will-be-set-by-IN-TECH

Fig. 7. Path planning with (upper panels) and without (lower panels) moving obstacles

The navigation motor behavior, illustrated in Fig. 8, consists of an FSM with 9 states. In State 1, the robot stops (path planning takes place at standstill), and jumps to State 2 in which the robot determines whether or not (based on its odometric position estimate) it is actually located in the convex navigation grid. This check is necessary, since the robot may have left the grid (slightly) as a result of carrying out moving obstacle avoidance; see below. If the robot finds that it is located *outside* the grid, it jumps to State 3 in which it checks, using the LRF2, whether a certain sector (angular width *γc*, radius *rc*) in front of the robot is clear, i.e. does

<sup>2</sup> It is assumed that the height of the moving obstacles is such that they are detectable by the

present. The moving obstacle is represented as a gray cylinder.

**4.1.3 Navigation**

(two-dimensional) LRF.

$$v\_{\rm R} = V\_{\rm nav} + \Delta V\_{\prime} \tag{7}$$

respectively, where

$$
\Delta V = K\_p V\_{\text{nav}} \Delta \varphi\_\prime \tag{8}
$$

where Δ*ϕ* is the difference in heading angle, and *Kp* is a constant. While in State 8, the robot continuously checks the difference between estimated and desired heading. If it should

<sup>3</sup> Even though the motor settings are equal, the actual path may differ slightly from a straight line, due to actuator noise and other similar sources of error.

<sup>4</sup> The difference <sup>Δ</sup>*<sup>φ</sup>* between two heading angles *<sup>ϕ</sup>*<sup>1</sup> and *<sup>ϕ</sup>*<sup>2</sup> is defined as *<sup>ϕ</sup>*<sup>1</sup> <sup>−</sup> *<sup>ϕ</sup>*<sup>2</sup> mod 2*π*.

in Indoor Environments 15

Reliable Long-Term Navigation in Indoor Environments 275

moving obstacles. It does so by noting that there is an upper limit on the pose errors, since the odometric calibration is executed at least a few times per second. Thus, laser rays for which the discrepancy between the expected and actual reading is too large to be caused by pose

If the scan match error is below a threshold *T�*, the robot concludes that its pose is correct and jumps back to State 1. However, if the error exceeds the threshold, a search procedure is initiated in State 3 of the FSM. Here, the robot carries out a search in a region of size *Lx* × *Ly* × *L<sup>ϕ</sup>* in pose space, around the estimated pose (obtained from odometry) recorded when

Since the search procedure takes some time (though less than one second) to carry out, once it has been completed the robot must correct for the movement that has occurred since the stored laser scan was taken. This correction is carried out using odometry, which is sufficiently accurate to provide a good estimate of the *change* in the robot's pose during the search procedure (even though the *absolute* pose may be incorrect, which is the reason for running

As mentioned above, in the case considered here, the only long-range sensor is an LRF mounted on top of the robot. Thus, a method for detecting moving obstacles, i.e. any obstacle that is not part of the map, will have to rely on differences between the expected and actual LRF readings at the robot's current pose. Needless to say, this requires, in turn, that the robot's pose estimate, generated by the *Odometry* and *Odometric calibration* brain processes, is rather accurate. Assuming that this is the case, the FSM implementing the *Moving obstacle detection* (MOD) brain process works as follows: The FSM cycles between Steps 1 and 2, where Step 2 is a simple waiting state (the duration of which is typically set to around 0.02 to 0.10 s). In State 1, the robot compares the current LRF readings with the expected readings (given its pose in the map). Essentially, those readings for which the difference between the actual and expected readings exceed a certain threshold *T*mo are considered to represent moving obstacles. Thus, even in cases where the robot's pose is slightly incorrect (due to odometric drift), it will still be able reliably to detect moving obstacles, provided that *T*mo is set to a sufficiently large value. On the other hand, the threshold cannot be set *too* large, since the robot would not be able to detect, for example, moving obstacles just in front of a wall. In the investigation considered

Normally, when a moving obstacle is present within the range of the LRF, a set of consecutive LRF rays will be found to impinge on the obstacle. Given the first and last of those rays, estimates can be obtained for the position and radius of the obstacle. However, in order to minimize the risk of spurious detections, a list of moving obstacle *candidates* is maintained, such that only those candidates that fulfill a number of conditions are eventually placed in the actual list of moving obstacles. To be specific, detected obstacle candidates are removed if

in size by a given margin (typically 50%). On the other hand, for a detected obstacle candidate

Despite the precautions listed above, the robot will, from time to time, make spurious detections. Hence, a probability measure *pi* for each moving obstacle *i* is introduced as well. A newly detected moving obstacle candidate is given probability *pi* = *p*0. If the

such that the inferred radius (after adding the margin) exceeds a threshold *r*max

mo . An obstacle candidate that survives these two checks is artificially increased

mo .

mo or (ii) the inferred radius is smaller than a

mo (typically as

errors are simply ignored in the scan matching procedure; see also Subsect. 5.3 below.

the stored laser scan was taken.

**4.4 Moving obstacle detection**

odometric calibration in the first place!)

here, the value of *T*mo was equal to 0.30 m.

(i) they are located beyond a certain distance *d*max

a result of pose errors), the actual radius is set equal to *r*max

threshold *r*min

exceed the threshold *δ*, the robot returns to State 7. If instead the robot reaches within a distance *d*p (around 0.1 - 0.2 m) of the current waypoint, the robot jumps to state 9 where it checks whether it has reached the navigation goal (i.e. the final waypoint). If this is the case, the robot picks the next navigation goal from the list, and returns to State 1.

If not, the robot selects the next waypoint in the current path. Now, the simplest way to do so would be to just increase the index of the waypoint by one. Indeed, this is normally what happens. However, in certain situations, it may be favorable to skip one waypoint, namely in cases where the path changes direction abruptly. In such cases, passing all waypoints will lead to an unnecessarily jerky motion. Thus, in State 9, before selecting the next waypoint, the robot considers the angle between the vectors **v**<sup>1</sup> = **p***i*+<sup>1</sup> − **p***<sup>i</sup>* and **v**<sup>2</sup> = **p***i*+<sup>2</sup> − **p***i*<sup>+</sup>1, where **p***<sup>i</sup>* is the waypoint that the robot just has reached, and **p***i*+<sup>1</sup> and **p***i*+<sup>2</sup> are the two next waypoints (this procedure is of course skipped if **p***<sup>i</sup>* or **p***i*+<sup>1</sup> is the final waypoint along the path). If this angle exceeds a certain threshold *and* the path ahead of the robot is clear (measured as in State 3, but possibly with different thresholds defining the sector in front of the robot), the robot skips waypoint *i* + 1 and instead targets waypoint *i* + 2.

#### **4.2 Odometry**

The *Odometry* brain process is straightforward: Given the estimated wheel rotations since the last update (based on the pulse counts from the wheel encoders), the robot uses the simple kinematic model of a differentially steered two-wheeled robot (with two additional supports for balance) to obtain an estimate of its velocity (*vx*, *vy*). Using standard forward kinematics, the robot also obtains estimates of its position (*x*, *y*) and angle of heading *ϕ*.

#### **4.3 Odometric calibration**

The purpose of the *odometric calibration* brain process is to correct the inevitable errors in the pose obtained from the *odometry* brain process. Odometric calibration relies on scan matching and builds upon an earlier version (called *laser localization*) described by Sandberg et al. (2009). However, whereas the earlier version required the robot to stop before correcting the odometric errors, the new version operates continuously and in concert with other brain processes. Thus, using the brain process taxonomy described in Sect. 3, the new version is a cognitive process rather than a motor behavior.

The FSM of the odometric calibration brain process operates as follows: In State 1, the robot checks whether the (modulus of the) difference in motor signals (between the left and right motor) exceeds a given threshold *T*Δ*v*. If it does, the robot remains in State 1, the rationale being that scan matching is less likely to give a reliable result if the robot is currently executing a sharp turn rather than moving forward (or backward). This condition is less of a restriction than one might think: Since the grid navigation method attempts to find the shortest path from the starting point to the goal point, the resulting path typically consists of long straight lines, with occasional turns where necessary.

When the wheel speed difference drops below the threshold, the robot jumps to State 2 of the FSM, where it stores the most recent laser scan and then carries out a laser scan match. The procedure for the scan match has been described in full detail by Sandberg et al. (2009), and will not be given here. Suffice it to say that the scan matching procedure compares the stored laser scan to a virtual scan taken in the map, by directly computing (essentially) the root mean square error between the laser ray readings. Thus, unlike many other methods, this method does *not* rely on the (brittle and error-prone) identification of specific landmarks. Furthermore, unlike the version presented by Sandberg et al. (2009), the current version is able to filter out

14 Will-be-set-by-IN-TECH

exceed the threshold *δ*, the robot returns to State 7. If instead the robot reaches within a distance *d*p (around 0.1 - 0.2 m) of the current waypoint, the robot jumps to state 9 where it checks whether it has reached the navigation goal (i.e. the final waypoint). If this is the case,

If not, the robot selects the next waypoint in the current path. Now, the simplest way to do so would be to just increase the index of the waypoint by one. Indeed, this is normally what happens. However, in certain situations, it may be favorable to skip one waypoint, namely in cases where the path changes direction abruptly. In such cases, passing all waypoints will lead to an unnecessarily jerky motion. Thus, in State 9, before selecting the next waypoint, the robot considers the angle between the vectors **v**<sup>1</sup> = **p***i*+<sup>1</sup> − **p***<sup>i</sup>* and **v**<sup>2</sup> = **p***i*+<sup>2</sup> − **p***i*<sup>+</sup>1, where **p***<sup>i</sup>* is the waypoint that the robot just has reached, and **p***i*+<sup>1</sup> and **p***i*+<sup>2</sup> are the two next waypoints (this procedure is of course skipped if **p***<sup>i</sup>* or **p***i*+<sup>1</sup> is the final waypoint along the path). If this angle exceeds a certain threshold *and* the path ahead of the robot is clear (measured as in State 3, but possibly with different thresholds defining the sector in front of the robot), the robot

The *Odometry* brain process is straightforward: Given the estimated wheel rotations since the last update (based on the pulse counts from the wheel encoders), the robot uses the simple kinematic model of a differentially steered two-wheeled robot (with two additional supports for balance) to obtain an estimate of its velocity (*vx*, *vy*). Using standard forward kinematics,

The purpose of the *odometric calibration* brain process is to correct the inevitable errors in the pose obtained from the *odometry* brain process. Odometric calibration relies on scan matching and builds upon an earlier version (called *laser localization*) described by Sandberg et al. (2009). However, whereas the earlier version required the robot to stop before correcting the odometric errors, the new version operates continuously and in concert with other brain processes. Thus, using the brain process taxonomy described in Sect. 3, the new version is a

The FSM of the odometric calibration brain process operates as follows: In State 1, the robot checks whether the (modulus of the) difference in motor signals (between the left and right motor) exceeds a given threshold *T*Δ*v*. If it does, the robot remains in State 1, the rationale being that scan matching is less likely to give a reliable result if the robot is currently executing a sharp turn rather than moving forward (or backward). This condition is less of a restriction than one might think: Since the grid navigation method attempts to find the shortest path from the starting point to the goal point, the resulting path typically consists of long straight

When the wheel speed difference drops below the threshold, the robot jumps to State 2 of the FSM, where it stores the most recent laser scan and then carries out a laser scan match. The procedure for the scan match has been described in full detail by Sandberg et al. (2009), and will not be given here. Suffice it to say that the scan matching procedure compares the stored laser scan to a virtual scan taken in the map, by directly computing (essentially) the root mean square error between the laser ray readings. Thus, unlike many other methods, this method does *not* rely on the (brittle and error-prone) identification of specific landmarks. Furthermore, unlike the version presented by Sandberg et al. (2009), the current version is able to filter out

the robot picks the next navigation goal from the list, and returns to State 1.

the robot also obtains estimates of its position (*x*, *y*) and angle of heading *ϕ*.

skips waypoint *i* + 1 and instead targets waypoint *i* + 2.

cognitive process rather than a motor behavior.

lines, with occasional turns where necessary.

**4.2 Odometry**

**4.3 Odometric calibration**

moving obstacles. It does so by noting that there is an upper limit on the pose errors, since the odometric calibration is executed at least a few times per second. Thus, laser rays for which the discrepancy between the expected and actual reading is too large to be caused by pose errors are simply ignored in the scan matching procedure; see also Subsect. 5.3 below.

If the scan match error is below a threshold *T�*, the robot concludes that its pose is correct and jumps back to State 1. However, if the error exceeds the threshold, a search procedure is initiated in State 3 of the FSM. Here, the robot carries out a search in a region of size *Lx* × *Ly* × *L<sup>ϕ</sup>* in pose space, around the estimated pose (obtained from odometry) recorded when the stored laser scan was taken.

Since the search procedure takes some time (though less than one second) to carry out, once it has been completed the robot must correct for the movement that has occurred since the stored laser scan was taken. This correction is carried out using odometry, which is sufficiently accurate to provide a good estimate of the *change* in the robot's pose during the search procedure (even though the *absolute* pose may be incorrect, which is the reason for running odometric calibration in the first place!)

#### **4.4 Moving obstacle detection**

As mentioned above, in the case considered here, the only long-range sensor is an LRF mounted on top of the robot. Thus, a method for detecting moving obstacles, i.e. any obstacle that is not part of the map, will have to rely on differences between the expected and actual LRF readings at the robot's current pose. Needless to say, this requires, in turn, that the robot's pose estimate, generated by the *Odometry* and *Odometric calibration* brain processes, is rather accurate. Assuming that this is the case, the FSM implementing the *Moving obstacle detection* (MOD) brain process works as follows: The FSM cycles between Steps 1 and 2, where Step 2 is a simple waiting state (the duration of which is typically set to around 0.02 to 0.10 s). In State 1, the robot compares the current LRF readings with the expected readings (given its pose in the map). Essentially, those readings for which the difference between the actual and expected readings exceed a certain threshold *T*mo are considered to represent moving obstacles. Thus, even in cases where the robot's pose is slightly incorrect (due to odometric drift), it will still be able reliably to detect moving obstacles, provided that *T*mo is set to a sufficiently large value. On the other hand, the threshold cannot be set *too* large, since the robot would not be able to detect, for example, moving obstacles just in front of a wall. In the investigation considered here, the value of *T*mo was equal to 0.30 m.

Normally, when a moving obstacle is present within the range of the LRF, a set of consecutive LRF rays will be found to impinge on the obstacle. Given the first and last of those rays, estimates can be obtained for the position and radius of the obstacle. However, in order to minimize the risk of spurious detections, a list of moving obstacle *candidates* is maintained, such that only those candidates that fulfill a number of conditions are eventually placed in the actual list of moving obstacles. To be specific, detected obstacle candidates are removed if (i) they are located beyond a certain distance *d*max mo or (ii) the inferred radius is smaller than a threshold *r*min mo . An obstacle candidate that survives these two checks is artificially increased in size by a given margin (typically 50%). On the other hand, for a detected obstacle candidate such that the inferred radius (after adding the margin) exceeds a threshold *r*max mo (typically as a result of pose errors), the actual radius is set equal to *r*max mo .

Despite the precautions listed above, the robot will, from time to time, make spurious detections. Hence, a probability measure *pi* for each moving obstacle *i* is introduced as well. A newly detected moving obstacle candidate is given probability *pi* = *p*0. If the

in Indoor Environments 17

Reliable Long-Term Navigation in Indoor Environments 277

Fig. 9. Left panel: The zones used in the moving obstacle avoidance motor behavior. The robot is shown as a light gray disc. Middle and right panels: The two evasive maneuvers defined for cases in which the moving obstacle (dark gray disc) is located in Zone 1. If the robot is able to move backwards (based on the readings from the rear IR sensor) it does so (middle panel). On the other hand, if an obstacle behind the robot prevents it from moving backwards (right panel), it attempts instead to evade the obstacle by moving forward and to

is located. For example, if the moving obstacle is located in Zone 1, the robot checks (using the rear IR sensor) whether it can move backwards without collisions. If so, it sets the motor signals to equal, negative values, thus (asymptotically) moving straight backwards, until it has moved a certain, user-specified distance, at which point the robot stops as the MOA motor

If the robot is unable to move backwards (either initially or after some time), it instead attempts to circumnavigate the moving obstacle by turning right, setting the left motor signal to a large positive value, and the right motor signal to a small negative value. Once the robot has turned a certain angle (relative to the initial angle of heading), it instead sets the motor signals to equal positive values, thus (asymptotically) moving forward in a straight line, until it has moved a certain distance. At some point, the obstacle will disappear from the field of view of the LRF (either because it has passed the robot, or as a result of the robot's evasive action), at which point the obstacle danger level (*μ*) drops, and the MOA motor behavior is de-activated shortly thereafter, so that grid navigation can be resumed. A similar, but somewhat more elaborate, procedure is used if the obstacle is located in Zone 2. Zones 3 and 4 are handled as Zones 2 and 1, respectively, but with the motor signal signs etc. reversed. The utility function for the MOA motor behavior uses the obstacle danger level (see Subsect. 4.4) as a state variable. Hence, when the perceived obstacle danger level rises above a threshold, which is dependent on the parameter values used in Eq. (1), the MOA motor behavior is activated. At this point, the robot attempts to avoid the moving obstacle, as described above. In cases where it does so by turning, the perceived obstacle danger level typically drops. Thus, in order to avoid immediate de-activation, the MOA motor behavior uses a gamma parameter to raise its own utility once activated (in State 1); see also Sect. 3

The *Long-term memory* (LTM) brain process simply stores (and makes available to the other brain processes) the resources necessary for the robot to carry out its navigation task. In the case considered here, the LTM stores the map of the arena (needed during odometric

calibration) and the convex navigation grid (needed for navigation).

1 4

behavior is awaiting de-activation.

the right.

above.

**4.6 Long-term memory**

2 3

obstacle candidate is found again (in the next time step), its probability is modified as *pi* ← max(*αpi*, 1), where *α* > 1 is a constant. If, on the other hand, the previously detected obstacle candidate is not found, the probability is modified as *pi* ← *βpi*, where *β* < 1 is a constant. Whenever the probability *pi* drops below a threshold *p*min, the corresponding obstacle is removed from the list of candidates. Finally, the remaining candidates are added to the list of actual moving obstacles, when they have been detected (i.e. with probability above the minimum threshold) for at least Δ*T*mo s (a user-specified parameter, typically set to around 0.5 s).

In order to avoid multiple detections of the same moving obstacle (remembering that the MOD method runs many times per second), detected moving obstacle candidates that are within a distance *δ*max mo of a previously detected obstacle candidate are identified with that obstacle candidate. Obstacle candidates that do not fulfill this condition are added as separate obstacle candidates. In addition to generating a list of moving obstacles, the brain process also determines which of the moving obstacles is closest to the robot.

Furthermore, a scalar variable, referred to as the *moving obstacle danger level* (denoted *μ*) is computed (also in State 1 of the FSM). This variable is used as a state variable for the decision-making system, and influences the activation (or de-activation) of the moving obstacle avoidance motor behavior, which will be described below. Needless to say, a scalar variable for assessing the threat posed by a moving obstacle can be generated in a variety of ways, without any limit on the potential complexity of the computations involved. One may also, of course, consider using additional long-range sensors (e.g. cameras) in this context. Here, however, a rather simple definition has been used, in which the robot only considers moving obstacles in the frontal5 half-plane (relative direction from <sup>−</sup>*π*/2 to *<sup>π</sup>*/2). The robot first computes the change in distance to the nearest obstacle between two consecutive executions of State 1. If the distance is increasing, the current danger level *m* is set to zero. If not, the current danger level is determined (details will not be given here) based on the distance and angle to the moving obstacle. Next, the actual danger level *μ* is obtained as *μ* ← (1 − *α*)*μ* + *αm*, where *α* is a constant in the range ]0, 1].

#### **4.5 Moving obstacle avoidance**

The *Moving obstacle avoidance* (MOA) motor behavior is responsible for handling emergency collision avoidance6. Once the MOA motor behavior has been activated7, it devotes all its computational capacity to avoiding the nearest moving obstacle; it makes no attempt to assess the danger posed by the obstacle, a task that is instead handled by the decision-making system, using the moving obstacle danger level computed in the MOD brain process. Just as in the case of the MOD brain process, MOA can also be implemented in complex and elaborate ways.

Here, however, a rather simple MOA method has been used, in which the region in front of the robot is divided into four zones, as illustrated in Fig. 9. Should the MOA motor behavior be activated, the actions taken by the robot depend on the zone in which the moving obstacle

<sup>5</sup> The robot thus only handles frontal collisions; it is assumed that a moving obstacle, for example a person, will not actively try to collide with the robot from behind.

<sup>6</sup> Note that the grid navigation motor behavior also handles *some* collision avoidance, in the sense that it will not deliberately plan a path that intersects a nearby moving obstacle.

<sup>7</sup> Note that, whenever the MOA motor behavior is activated, the grid navigation motor behavior is turned off, and vice versa. This is so since, in the UF method, only one motor behavior is active at any given time; see Sect. 3.

16 Will-be-set-by-IN-TECH

obstacle candidate is found again (in the next time step), its probability is modified as *pi* ← max(*αpi*, 1), where *α* > 1 is a constant. If, on the other hand, the previously detected obstacle candidate is not found, the probability is modified as *pi* ← *βpi*, where *β* < 1 is a constant. Whenever the probability *pi* drops below a threshold *p*min, the corresponding obstacle is removed from the list of candidates. Finally, the remaining candidates are added to the list of actual moving obstacles, when they have been detected (i.e. with probability above the minimum threshold) for at least Δ*T*mo s (a user-specified parameter, typically set to around

In order to avoid multiple detections of the same moving obstacle (remembering that the MOD method runs many times per second), detected moving obstacle candidates that are

obstacle candidate. Obstacle candidates that do not fulfill this condition are added as separate obstacle candidates. In addition to generating a list of moving obstacles, the brain process also

Furthermore, a scalar variable, referred to as the *moving obstacle danger level* (denoted *μ*) is computed (also in State 1 of the FSM). This variable is used as a state variable for the decision-making system, and influences the activation (or de-activation) of the moving obstacle avoidance motor behavior, which will be described below. Needless to say, a scalar variable for assessing the threat posed by a moving obstacle can be generated in a variety of ways, without any limit on the potential complexity of the computations involved. One may also, of course, consider using additional long-range sensors (e.g. cameras) in this context. Here, however, a rather simple definition has been used, in which the robot only considers moving obstacles in the frontal5 half-plane (relative direction from <sup>−</sup>*π*/2 to *<sup>π</sup>*/2). The robot first computes the change in distance to the nearest obstacle between two consecutive executions of State 1. If the distance is increasing, the current danger level *m* is set to zero. If not, the current danger level is determined (details will not be given here) based on the distance and angle to the moving obstacle. Next, the actual danger level *μ* is obtained as

The *Moving obstacle avoidance* (MOA) motor behavior is responsible for handling emergency collision avoidance6. Once the MOA motor behavior has been activated7, it devotes all its computational capacity to avoiding the nearest moving obstacle; it makes no attempt to assess the danger posed by the obstacle, a task that is instead handled by the decision-making system, using the moving obstacle danger level computed in the MOD brain process. Just as in the case of the MOD brain process, MOA can also be implemented in complex and

Here, however, a rather simple MOA method has been used, in which the region in front of the robot is divided into four zones, as illustrated in Fig. 9. Should the MOA motor behavior be activated, the actions taken by the robot depend on the zone in which the moving obstacle

<sup>5</sup> The robot thus only handles frontal collisions; it is assumed that a moving obstacle, for example a

<sup>6</sup> Note that the grid navigation motor behavior also handles *some* collision avoidance, in the sense that it

<sup>7</sup> Note that, whenever the MOA motor behavior is activated, the grid navigation motor behavior is turned off, and vice versa. This is so since, in the UF method, only one motor behavior is active at

determines which of the moving obstacles is closest to the robot.

*μ* ← (1 − *α*)*μ* + *αm*, where *α* is a constant in the range ]0, 1].

person, will not actively try to collide with the robot from behind.

will not deliberately plan a path that intersects a nearby moving obstacle.

mo of a previously detected obstacle candidate are identified with that

0.5 s).

within a distance *δ*max

**4.5 Moving obstacle avoidance**

any given time; see Sect. 3.

elaborate ways.

Fig. 9. Left panel: The zones used in the moving obstacle avoidance motor behavior. The robot is shown as a light gray disc. Middle and right panels: The two evasive maneuvers defined for cases in which the moving obstacle (dark gray disc) is located in Zone 1. If the robot is able to move backwards (based on the readings from the rear IR sensor) it does so (middle panel). On the other hand, if an obstacle behind the robot prevents it from moving backwards (right panel), it attempts instead to evade the obstacle by moving forward and to the right.

is located. For example, if the moving obstacle is located in Zone 1, the robot checks (using the rear IR sensor) whether it can move backwards without collisions. If so, it sets the motor signals to equal, negative values, thus (asymptotically) moving straight backwards, until it has moved a certain, user-specified distance, at which point the robot stops as the MOA motor behavior is awaiting de-activation.

If the robot is unable to move backwards (either initially or after some time), it instead attempts to circumnavigate the moving obstacle by turning right, setting the left motor signal to a large positive value, and the right motor signal to a small negative value. Once the robot has turned a certain angle (relative to the initial angle of heading), it instead sets the motor signals to equal positive values, thus (asymptotically) moving forward in a straight line, until it has moved a certain distance. At some point, the obstacle will disappear from the field of view of the LRF (either because it has passed the robot, or as a result of the robot's evasive action), at which point the obstacle danger level (*μ*) drops, and the MOA motor behavior is de-activated shortly thereafter, so that grid navigation can be resumed. A similar, but somewhat more elaborate, procedure is used if the obstacle is located in Zone 2. Zones 3 and 4 are handled as Zones 2 and 1, respectively, but with the motor signal signs etc. reversed.

The utility function for the MOA motor behavior uses the obstacle danger level (see Subsect. 4.4) as a state variable. Hence, when the perceived obstacle danger level rises above a threshold, which is dependent on the parameter values used in Eq. (1), the MOA motor behavior is activated. At this point, the robot attempts to avoid the moving obstacle, as described above. In cases where it does so by turning, the perceived obstacle danger level typically drops. Thus, in order to avoid immediate de-activation, the MOA motor behavior uses a gamma parameter to raise its own utility once activated (in State 1); see also Sect. 3 above.

#### **4.6 Long-term memory**

The *Long-term memory* (LTM) brain process simply stores (and makes available to the other brain processes) the resources necessary for the robot to carry out its navigation task. In the case considered here, the LTM stores the map of the arena (needed during odometric calibration) and the convex navigation grid (needed for navigation).

in Indoor Environments 19

Reliable Long-Term Navigation in Indoor Environments 279

Fig. 10. Upper panels: Detection of a moving obstacle by the real robot. Left: The person approaching the robot. Right: a screenshot showing the LRF rays (for the real robot). The robot is shown as a white disc at its estimated pose, and the detected moving obstacle as a black disc. Lower panels: The same situation, shown for the simulator. Here, the laser rays (which obviously are not visible in the real robot) have been plotted in the screenshot rather

Nevertheless, before using any simulator for realistic robotics experiments, careful validation and system identification must be carried out, a process that will now be described briefly, in

than in the map view shown in the right panel.

the case of our simulator.

#### **5. Implementation and validation**

#### **5.1 The general-purpose robotics simulator**

A simulator referred to as the *General-purpose robotics simulator* (GPRSim) has been written with the main purpose of evaluating the UF decision-making method. The simulator allows a user to set up an arbitrary arena, with or without moving obstacles, and a robot, including both its physical properties (e.g. actuators, sensors, physical parameters such as mass, moment of inertia etc.) and its artificial brain (i.e. a set of brain process, as described above, and the decision-making system implementing the UF method, also described above). In order to make the simulations as realistic as possible, the simulator introduces noise at all relevant levels (e.g. in sensors and actuators). Moreover, simulated sensor readings are taken only with the frequency allowed by the physical counterpart.

#### **5.2 Simulations vs. reality**

Simulations are becoming increasingly important in many branches of science, and robotics is no exception. However, in the early days of behavior-based robotics many researchers expressed doubts as to the possibility of transferring results obtained in simulations onto real robots. In particular, Brooks took an especially dim view of the possibility of such transfer, as discussed in Brooks (1992). Similar critique of simulations has been voiced by, among others, Jakobi et al. (1995) and Miglino et al. (1996). However, their critique mainly targeted naive simulation approaches that use idealized sensors and actuators in block-world-like environments. In fact, both Jakobi et al. (1995) and Brooks (1992) also note that simulations are indeed useful, provided that great care is taken to simulate, for example, actuators and sensors in a realistic manner and to validate the simulation results using real robots. Furthermore, even though the discrepancy between simulations and reality is likely to increase as ever more complex robotic tasks are considered (as argued by Lipson (2001)), it is also likely that carrying out proper tests of such robots will, in fact, *require* simulations to be used, particularly if the simulations can be executed faster than real time. In addition, careful simulations allow researchers to test various hardware configurations before embarking on the often costly and time-consuming task of constructing the real counterpart.

In our view, simulations are an essential tool in mobile robot research, but should only be applied in cases where one can argue convincingly that (i) the various hardware components (e.g. sensors and actuators) can be simulated in a reliable way, and (ii) the actions taken do not require extreme accuracy regarding time or space. As an example regarding simulation of hardware components, consider the sensors used on our robot; in the application considered here almost all relevant sensing is carried out using the LRF, a sensor that is, in fact, very simple to simulate, in view of its high accuracy: Our LRF has a typical error of ± 1 mm out to distances of around 4 m. On the other hand, if a *camera* had been used on the real robot, it is unlikely that a reliable simulation could have been carried out. Even though GPRSim allows an almost photo-realistic representation of the environment, the readings from a simulated camera would be unlikely to capture all aspects (such as shadows, variations in light levels etc.) of vision. Regarding time and space, the implementation of the navigation method (described above) introduces, by construction, a margin between the robot and the arena objects. Thus, while the actions of the simulated robot may differ somewhat from those of a real robot (and, indeed, also differ between different runs in the simulator, depending on noise in actuators and sensors), there is little risk of *qualitative* differences in the results obtained.

18 Will-be-set-by-IN-TECH

A simulator referred to as the *General-purpose robotics simulator* (GPRSim) has been written with the main purpose of evaluating the UF decision-making method. The simulator allows a user to set up an arbitrary arena, with or without moving obstacles, and a robot, including both its physical properties (e.g. actuators, sensors, physical parameters such as mass, moment of inertia etc.) and its artificial brain (i.e. a set of brain process, as described above, and the decision-making system implementing the UF method, also described above). In order to make the simulations as realistic as possible, the simulator introduces noise at all relevant levels (e.g. in sensors and actuators). Moreover, simulated sensor readings are taken

Simulations are becoming increasingly important in many branches of science, and robotics is no exception. However, in the early days of behavior-based robotics many researchers expressed doubts as to the possibility of transferring results obtained in simulations onto real robots. In particular, Brooks took an especially dim view of the possibility of such transfer, as discussed in Brooks (1992). Similar critique of simulations has been voiced by, among others, Jakobi et al. (1995) and Miglino et al. (1996). However, their critique mainly targeted naive simulation approaches that use idealized sensors and actuators in block-world-like environments. In fact, both Jakobi et al. (1995) and Brooks (1992) also note that simulations are indeed useful, provided that great care is taken to simulate, for example, actuators and sensors in a realistic manner and to validate the simulation results using real robots. Furthermore, even though the discrepancy between simulations and reality is likely to increase as ever more complex robotic tasks are considered (as argued by Lipson (2001)), it is also likely that carrying out proper tests of such robots will, in fact, *require* simulations to be used, particularly if the simulations can be executed faster than real time. In addition, careful simulations allow researchers to test various hardware configurations before embarking on the often costly and

In our view, simulations are an essential tool in mobile robot research, but should only be applied in cases where one can argue convincingly that (i) the various hardware components (e.g. sensors and actuators) can be simulated in a reliable way, and (ii) the actions taken do not require extreme accuracy regarding time or space. As an example regarding simulation of hardware components, consider the sensors used on our robot; in the application considered here almost all relevant sensing is carried out using the LRF, a sensor that is, in fact, very simple to simulate, in view of its high accuracy: Our LRF has a typical error of ± 1 mm out to distances of around 4 m. On the other hand, if a *camera* had been used on the real robot, it is unlikely that a reliable simulation could have been carried out. Even though GPRSim allows an almost photo-realistic representation of the environment, the readings from a simulated camera would be unlikely to capture all aspects (such as shadows, variations in light levels etc.) of vision. Regarding time and space, the implementation of the navigation method (described above) introduces, by construction, a margin between the robot and the arena objects. Thus, while the actions of the simulated robot may differ somewhat from those of a real robot (and, indeed, also differ between different runs in the simulator, depending on noise in actuators and sensors), there is little risk of *qualitative* differences in the results obtained.

**5. Implementation and validation**

**5.2 Simulations vs. reality**

**5.1 The general-purpose robotics simulator**

only with the frequency allowed by the physical counterpart.

time-consuming task of constructing the real counterpart.

Fig. 10. Upper panels: Detection of a moving obstacle by the real robot. Left: The person approaching the robot. Right: a screenshot showing the LRF rays (for the real robot). The robot is shown as a white disc at its estimated pose, and the detected moving obstacle as a black disc. Lower panels: The same situation, shown for the simulator. Here, the laser rays (which obviously are not visible in the real robot) have been plotted in the screenshot rather than in the map view shown in the right panel.

Nevertheless, before using any simulator for realistic robotics experiments, careful validation and system identification must be carried out, a process that will now be described briefly, in the case of our simulator.

in Indoor Environments 21

Reliable Long-Term Navigation in Indoor Environments 281

odometric calibration. At this point, the alignment is almost perfect, despite the presence of the moving obstacle. Repeating the experiment 10 times, the estimated position (after odometric calibration) was found to be (*x*, *y*)=(2.40 ± 0.02, 4.22 ± 0.01) m and the estimated heading was 3.11 ± 0.02 radians, where the errors correspond to one standard deviation. The corresponding values found in the simulator were (*x*, *y*)=(2.38 ± 0.03, 4.23 ± 0.01) m, whereas the estimated heading was found to be 3.13 ± 0.02 radians.Several experiments of this kind were carried out. In all cases, both the simulated robot and the real robot were able to correct their estimated pose using odometric calibration, with essentially identical level of

The (simulated) arena used for studying long-term robustness is shown from above in the upper left panel of Fig. 12. The size of the arena, which represents a warehouse environment, is 10 × 10 m. The large object near the lower right corner represents an elevator shaft

Regarding the decision-making system, a single state variable was used, namely the obstacle danger level described in Subsect. 4.4 above. The four cognitive brain processes (*Odometry*, *Odometric calibration*, *Moving obstacle detection*, and *Long-term memory*) were allowed to run continuously. Hence, in their utility functions, the parameters *aij* were equal to zero, and *bi* was positive. The main task of the decision-making system was thus to select between the two motor behaviors, namely *Grid navigation* (*B*1) and *Moving obstacle avoidance* (*B*5), using the obstacle danger level as the state variable (*z*1). Since only the relative utility values matter (see Sect. 3), the parameters *a*1*<sup>j</sup>* were set to zero, *b*1*<sup>j</sup>* was set to a positive value (0.25), and the time constant *τ*<sup>1</sup> was set to 0.10 s, thus making *u*<sup>1</sup> rapidly approach a constant value of around 0.245. After extensive testing, suitable parameters for *B*<sup>5</sup> were found to be: *a*<sup>51</sup> = 1.00, *b*<sup>5</sup> = −0.45, and *τ*<sup>1</sup> = 0.10 s. The gamma parameter Γ<sup>5</sup> (used as described in Subsect. 4.5 above) was set to 3.0 in State 1 of *B*5. The corresponding time constant (for exponential decay of the gamma parameter) was set to 1.0 s. The sigmoid parameters *ci* were set to 1 for all brain

Several runs were carried out with GPRSim, using the robot configuration described above.

As a first step, a long run (*Run 1*) was carried out, without any moving obstacle present, with the intention of testing the interplay between navigation, odometry, and odometric calibration. The robot was released in the long corridor near the left edge of the arena, and was then required to reach a set of target points in a given order. A total of 11 different target points were used. When the final point was reached, the robot's next target was set as the first target point, thus repeating the cycle. The right panel of Fig. 12 shows the locations of the 11 target points. The robot's trajectory during the first 100 meters of movement (256 s) is shown in the lower left panel of the figure. Both the actual trajectory (thick green line) and the odometric trajectory (thin red line) are shown. As can be seen, the robot successfully planned and followed the paths between consecutive target points. Over the first 100 m of the robot's

precision.

**6. Results**

processes.

Here, the results of two runs will be presented.

**6.1 Run 1: Stationary arena**

(inaccessible to the robot).

#### **5.3 Validation of the simulator**

Using the real robot, several experiments have been carried out in order to validate the GPRSim simulator and to set appropriate parameter values for the simulator. In fact, careful validation of GPRSim (as well as system identification of sensors and actuators) has been carried out in previous (unpublished) work, in which navigation over distances of 10-20 m was considered. The odometric calibration method has been carefully tested both in those experiments and as a separate brain process; see Sandberg et al. (2009).

Fig. 11. An example of odometric calibration in the presence of a moving obstacle (shown as a black disc). The real robot, shown as a white disc, was used. The same laser readings (from a single laser scan) are shown in both panels. In the left panel, the origin and direction of the laser readings correspond to the robot's estimated (and incorrect) pose before odometric calibration. In the right panel, the estimated pose has been corrected.

However, in the previous work, moving obstacles were not included. Thus, here, a thorough effort has been made to ascertain the validity of the simulation results in situations where moving obstacles are present. Basically, there are two things that must be tested namely (i) moving obstacle detection and (ii) odometric calibration in the presence of moving obstacles. An example of the validation results obtained for moving obstacle detection is shown in Fig. 10. The upper panels show the results obtained in the real robot, and the lower panel show the results from GPRSim, for a given robot pose and obstacle configuration. As can be seen, the robot (shown as a white disc) detected the moving obstacle (shown as a black disc) in both cases.

Fig. 11 shows the results (obtained in the real robot) of a validation test regarding odometric calibration. In this particular test, the actual position of the robot was (*x*, *y*)=(2.45, 4.23) m. The heading angle was equal to *π* radians (so that the robot was directed towards the negative *x*-direction). For the purposes of testing the odometric calibration process, an error was introduced in the *estimated* pose; the estimated position was set to (*x*, *y*)=(2.25, 4.20) m, and the estimated heading to 3.0 radians. The left panel shows the robot and the moving obstacle (black disc), as well as the actual LRF readings, plotted with the origin and direction provided by the robot's *estimated* pose. As can be seen, the map is not very well aligned with the readings. The right panel shows the same laser readings, again with origin and direction given by the robot's estimated pose, but this time using the estimate obtained *after* odometric calibration. At this point, the alignment is almost perfect, despite the presence of the moving obstacle. Repeating the experiment 10 times, the estimated position (after odometric calibration) was found to be (*x*, *y*)=(2.40 ± 0.02, 4.22 ± 0.01) m and the estimated heading was 3.11 ± 0.02 radians, where the errors correspond to one standard deviation. The corresponding values found in the simulator were (*x*, *y*)=(2.38 ± 0.03, 4.23 ± 0.01) m, whereas the estimated heading was found to be 3.13 ± 0.02 radians.Several experiments of this kind were carried out. In all cases, both the simulated robot and the real robot were able to correct their estimated pose using odometric calibration, with essentially identical level of precision.

#### **6. Results**

20 Will-be-set-by-IN-TECH

Using the real robot, several experiments have been carried out in order to validate the GPRSim simulator and to set appropriate parameter values for the simulator. In fact, careful validation of GPRSim (as well as system identification of sensors and actuators) has been carried out in previous (unpublished) work, in which navigation over distances of 10-20 m was considered. The odometric calibration method has been carefully tested both in those

Fig. 11. An example of odometric calibration in the presence of a moving obstacle (shown as a black disc). The real robot, shown as a white disc, was used. The same laser readings (from a single laser scan) are shown in both panels. In the left panel, the origin and direction of the laser readings correspond to the robot's estimated (and incorrect) pose before odometric

However, in the previous work, moving obstacles were not included. Thus, here, a thorough effort has been made to ascertain the validity of the simulation results in situations where moving obstacles are present. Basically, there are two things that must be tested namely (i) moving obstacle detection and (ii) odometric calibration in the presence of moving obstacles. An example of the validation results obtained for moving obstacle detection is shown in Fig. 10. The upper panels show the results obtained in the real robot, and the lower panel show the results from GPRSim, for a given robot pose and obstacle configuration. As can be seen, the robot (shown as a white disc) detected the moving obstacle (shown as a black disc)

Fig. 11 shows the results (obtained in the real robot) of a validation test regarding odometric calibration. In this particular test, the actual position of the robot was (*x*, *y*)=(2.45, 4.23) m. The heading angle was equal to *π* radians (so that the robot was directed towards the negative *x*-direction). For the purposes of testing the odometric calibration process, an error was introduced in the *estimated* pose; the estimated position was set to (*x*, *y*)=(2.25, 4.20) m, and the estimated heading to 3.0 radians. The left panel shows the robot and the moving obstacle (black disc), as well as the actual LRF readings, plotted with the origin and direction provided by the robot's *estimated* pose. As can be seen, the map is not very well aligned with the readings. The right panel shows the same laser readings, again with origin and direction given by the robot's estimated pose, but this time using the estimate obtained *after*

experiments and as a separate brain process; see Sandberg et al. (2009).

calibration. In the right panel, the estimated pose has been corrected.

**5.3 Validation of the simulator**

in both cases.

The (simulated) arena used for studying long-term robustness is shown from above in the upper left panel of Fig. 12. The size of the arena, which represents a warehouse environment, is 10 × 10 m. The large object near the lower right corner represents an elevator shaft (inaccessible to the robot).

Regarding the decision-making system, a single state variable was used, namely the obstacle danger level described in Subsect. 4.4 above. The four cognitive brain processes (*Odometry*, *Odometric calibration*, *Moving obstacle detection*, and *Long-term memory*) were allowed to run continuously. Hence, in their utility functions, the parameters *aij* were equal to zero, and *bi* was positive. The main task of the decision-making system was thus to select between the two motor behaviors, namely *Grid navigation* (*B*1) and *Moving obstacle avoidance* (*B*5), using the obstacle danger level as the state variable (*z*1). Since only the relative utility values matter (see Sect. 3), the parameters *a*1*<sup>j</sup>* were set to zero, *b*1*<sup>j</sup>* was set to a positive value (0.25), and the time constant *τ*<sup>1</sup> was set to 0.10 s, thus making *u*<sup>1</sup> rapidly approach a constant value of around 0.245. After extensive testing, suitable parameters for *B*<sup>5</sup> were found to be: *a*<sup>51</sup> = 1.00, *b*<sup>5</sup> = −0.45, and *τ*<sup>1</sup> = 0.10 s. The gamma parameter Γ<sup>5</sup> (used as described in Subsect. 4.5 above) was set to 3.0 in State 1 of *B*5. The corresponding time constant (for exponential decay of the gamma parameter) was set to 1.0 s. The sigmoid parameters *ci* were set to 1 for all brain processes.

Several runs were carried out with GPRSim, using the robot configuration described above. Here, the results of two runs will be presented.

#### **6.1 Run 1: Stationary arena**

As a first step, a long run (*Run 1*) was carried out, without any moving obstacle present, with the intention of testing the interplay between navigation, odometry, and odometric calibration. The robot was released in the long corridor near the left edge of the arena, and was then required to reach a set of target points in a given order. A total of 11 different target points were used. When the final point was reached, the robot's next target was set as the first target point, thus repeating the cycle. The right panel of Fig. 12 shows the locations of the 11 target points. The robot's trajectory during the first 100 meters of movement (256 s) is shown in the lower left panel of the figure. Both the actual trajectory (thick green line) and the odometric trajectory (thin red line) are shown. As can be seen, the robot successfully planned and followed the paths between consecutive target points. Over the first 100 m of the robot's

in Indoor Environments 23

Reliable Long-Term Navigation in Indoor Environments 283

Fig. 13. Left panel: The trajectories of the moving obstacles in Run 2. Right panel: The robot's trajectory during the first 50 m of movement. The ellipses highlight the robot's three evasive

0.031 radians (1.8 degrees). Note, however, that most of the contribution to the error comes from a few, rather rare occasions, where the robot's pose accuracy was temporarily lost. In all cases, the robot quickly recovered (through continuous odometric calibration) to find a better pose estimate. An example of such an error correction is shown in the lower right panel of

In the second run (*Run 2*) presented here, the robot moved in the same arena as in Run 1, and with the same sequence of target points. However, in Run 2, there were three moving obstacles present in the arena. The approximate trajectories of the moving obstacles are illustrated in the left panel of Fig. 13. The right panel shows the first 50 m of the robot's movement. Here, the average position error was around 0.068 m and the average heading error around 0.025 radians (1.4 degrees). The ellipses highlight the three cases where the robot was forced to make an evasive maneuver (during the first 50 m of movement) in order to avoid a collision with a moving obstacle. Note that the odometric calibration process managed to maintain a low pose error even during the evasive maneuver. The run was extended beyond 100 m, with only a small increase in pose error; after 100 m, the average position error was 0.096 m and the average heading error 0.038 radians (2.2 degrees). In this run, the first 100 m took 352 s to traverse (compared to the 256 s in Run 1), due to the evasive maneuvers. Note that, after the third evasive maneuver, the robot modified the original path towards target point 3 (cf. the lower left panel of Fig. 12), having realized that a shorter path was available after evading the

Even though the results presented in Sect. 6 show that the method used here is capable of achieving robust navigation over long distances in the presence of moving obstacles, a

maneuvers.

Fig. 12.

moving obstacle.

**7. Discussion and conclusion**

**6.2 Run 2: Arena with moving obstacles**

Fig. 12. Upper left panel: The arena used for the simulations. Upper right panel: The 11 target points, with enumeration. Note that, after reaching the 11th target point, the robot's next target becomes the first target point etc. The robot is initially located near target point 10, as can be seen in the upper left panel. Lower left panel: The trajectory for the first 100 m of navigation. The actual trajectory is shown as a thick green line, and the odometric trajectory as a thin red line. Lower right panel: A later part of the trajectory showing the robot approaching 200 m of distance covered. As can be seen, in the long corridor, the robot suffered a rather large pose error at one point. The error was swiftly corrected, however, so that the robot could continue its task.

movement, the average position error<sup>8</sup> was around 0.063 m, and the average heading error

<sup>8</sup> The average position error is computed by averaging the *modulus* of the position error over time. Similarly the average heading error is obtained by averaging the *modulus* of the heading error (modulo 2*π*) over time.

22 Will-be-set-by-IN-TECH

Fig. 12. Upper left panel: The arena used for the simulations. Upper right panel: The 11 target points, with enumeration. Note that, after reaching the 11th target point, the robot's next target becomes the first target point etc. The robot is initially located near target point 10, as can be seen in the upper left panel. Lower left panel: The trajectory for the first 100 m of navigation. The actual trajectory is shown as a thick green line, and the odometric trajectory as a thin red line. Lower right panel: A later part of the trajectory showing the robot approaching 200 m of distance covered. As can be seen, in the long corridor, the robot suffered a rather large pose error at one point. The error was swiftly corrected, however, so

movement, the average position error<sup>8</sup> was around 0.063 m, and the average heading error

<sup>8</sup> The average position error is computed by averaging the *modulus* of the position error over time. Similarly the average heading error is obtained by averaging the *modulus* of the heading error (modulo

that the robot could continue its task.

2*π*) over time.

Fig. 13. Left panel: The trajectories of the moving obstacles in Run 2. Right panel: The robot's trajectory during the first 50 m of movement. The ellipses highlight the robot's three evasive maneuvers.

0.031 radians (1.8 degrees). Note, however, that most of the contribution to the error comes from a few, rather rare occasions, where the robot's pose accuracy was temporarily lost. In all cases, the robot quickly recovered (through continuous odometric calibration) to find a better pose estimate. An example of such an error correction is shown in the lower right panel of Fig. 12.

#### **6.2 Run 2: Arena with moving obstacles**

In the second run (*Run 2*) presented here, the robot moved in the same arena as in Run 1, and with the same sequence of target points. However, in Run 2, there were three moving obstacles present in the arena. The approximate trajectories of the moving obstacles are illustrated in the left panel of Fig. 13. The right panel shows the first 50 m of the robot's movement. Here, the average position error was around 0.068 m and the average heading error around 0.025 radians (1.4 degrees). The ellipses highlight the three cases where the robot was forced to make an evasive maneuver (during the first 50 m of movement) in order to avoid a collision with a moving obstacle. Note that the odometric calibration process managed to maintain a low pose error even during the evasive maneuver. The run was extended beyond 100 m, with only a small increase in pose error; after 100 m, the average position error was 0.096 m and the average heading error 0.038 radians (2.2 degrees). In this run, the first 100 m took 352 s to traverse (compared to the 256 s in Run 1), due to the evasive maneuvers. Note that, after the third evasive maneuver, the robot modified the original path towards target point 3 (cf. the lower left panel of Fig. 12), having realized that a shorter path was available after evading the moving obstacle.

#### **7. Discussion and conclusion**

Even though the results presented in Sect. 6 show that the method used here is capable of achieving robust navigation over long distances in the presence of moving obstacles, a

in Indoor Environments 25

Reliable Long-Term Navigation in Indoor Environments 285

Brooks, R. A. (1986). A robust layered control system for a mobile robot, *IEEE J. of Robotics and*

Brooks, R. A. (1992). Artificial life and real robots, *Proceedings of the First European Conference*

Bryson, J. J. (2007). Mechanisms of action selection: Introduction to the special issue, *Adaptive*

Dijkstra, E. W. (1959). A note on two problems in connexion with graphs, *Numerische*

Gat, E. (1991). *Reliable Goal-directed Reactive Control for Real-world Autonomous Mobile Robots*,

Jakobi, N., Husbands, P. & Harvey, I. (1995). Noise and the reality gap: The use of simulation

Kaneko, K., Kanehiro, F., Kajita, S. et al. (2002). Design of prototype humanoid robotics

Lipson, H. (2001). Uncontrolled engineering: A review of nolfi and floreano's evolutionary

McFarland, D. (1998). *Animal Behaviour: Psychobiology, Ethology and Evolution, 3rd Ed.*, Prentice

Miglino, O., Lund, H. H. & Nolfi, S. (1996). Evolving mobile robots in simulated and real

Okabe, A., Boots, B., Sugihara, K. & Chiu, S. N. (2000). *Spatial Tessellations: Concepts and*

Pirjanian, P. (1999). Behavior coordination mechanisms - state-of-the-art, *Technical report*, Institute for Robotics and Intelligent Systems, University of Southern California. Prescott, T. J., Bryson, J. J. & Seth, A. K. (2007). Introduction. modelling natual action selection,

Sakagami, Y., Watanabe, R., Aoyama, C., Matsunaga, S., Higaki, N. & Fujimura, K. (2002). The

Singh, J. S. & Wagh, M. D. (1987). Robot path planning using intersecting convex shapes: Analysis and simulation, *IEEE Journal of Robotics and Automation* RA-3: 101–108. Singh, S. & Agarwal, G. (2010). Complete graph technique based optimization in meadow

Thorpe, C. E. (1984). Path relaxation: Path planning for a mobile robot, *Proceedings of AAAI-84*,

von Neumann, J. & Morgenstern, O. (1944). *Theory of Games and Economic Behavior*, Princeton

Wahde, M. (2003). A method for behavioural organization for autonomous robots based

*International Conference on Intelligent Robots and Systems*, pp. 2478–2483. Sandberg, D., Wolff, K. & Wahde, M. (2009). A robot localization method based on laser scan

intelligent ASIMO: System overview and integration, *Proceedings of the 2002 IEEE/RSJ*

matching, *in* J.-H. Kim, S. S. Ge, P. Vadakkepat, N. Jesse et al. (eds), *Proocedings of*

method of robotic path planning, *International Journal of Engineering Science and*

on evolutionary optimization of utility functions, *Journal of Systems and Control*

*Applications of Voronoi Diagrams*, 2nd edn, John Wiley and Sons, Ltd.

in evolutionary robotics, *in* F. Morán, A. Moreno, J. J. Merelo & P. Chacón (eds), *Proceedings of the Third European Conference On Artifical Life, LNCS 929*, Springer,

platform for HRP, *Proceedings of the IEEE/RSJ International Conference on Intelligent*

PhD thesis, Virginia Polytechnic Institute and State University.

*Robots and Systems (IROS2002)*, pp. 2431–2436.

robotics (book review), *Artificial Life* 7: 419–424.

environments, *Artificial Life* 2: 417–434.

*Philos. Trans. R. Soc. Lond. B* 362: 1521–1529.

*FIRA2009, LNCS 5744*, Springer, pp. 179–186.

*Technology* 2: 4951–4958.

*Engineering (IMechI)* 217: 249–258.

pp. 318–321.

University Press.

*Automation* RA-2(1): 14–23.

*on Artificial Life*, pp. 3–10.

*Mathematik* 1: 269–271.

*Behavior* 15: 5–8.

pp. 704–720.

Hall.

possible criticism of our work is that there is no guarantee that the robot will not suddenly fail, for example by generating an incorrect heading estimate, such that its position error will grow quickly. Indeed, in very long runs, exactly such problems do occur, albeit rarely. Our approach to overcoming those problems would follow the philosophy of error correction rather than error avoidance. An example of this philosophy can be found in the topic of gait generation for humanoid robots: Rather than trying to generate a humanoid robot that will never fall, it is better to make sure that the robot can get up if it *does* fall. This approach has been applied in, for example, the HRP-2 robot, see Kaneko et al. (2002), and many other humanoid robots subsequently developed.

Similarly, rather than trying to further tune the brain process parameters or the parameters of the decision-making system, we would, for full long-term reliability, advocate an approach in which the robotic brain would be supplemented by (at least) two additional brain processes. These are: (i) An *Emergency stop* motor behavior which would cause the robot to stop if it finds itself in a situation where a collision with a stationary object is imminent (moving obstacles being handled by the *Moving obstacle avoidance* behavior). Such situations would occur, for example, if the odometric calibration suffers a catastrophic failure so that the error in the robot's estimated pose grows without bound. Once the robot has stopped it should turn towards the most favorable direction for calibration (which would hardly be the direction towards the stationary object and neither any direction in which *no* part of the map would be visible). Once a suitable direction has been found, the robot should activate the second added brain process, namely *Wake-up*. This cognitive process would attempt a global search (again matching the current laser scan to virtual scans taken in the map) for the robot's current pose. With these two brain processes, which are to be added in future work, the robot would be able to correct even catastrophic errors.

As for the currently available brain processes, the *Moving obstacle avoidance* brain process could be improved to handle also situations in which *several* moving obstacles are approaching the robot simultaneously. However, to some extent, the brain process already does this since, before letting the robot move, it generally checks whether or not there is a clear path in the intended direction of movement; see also the right panel of Fig. 9. In cases where multiple moving obstacles are approaching, typically the best course of action (at least for a rather slow robot) is to do nothing.

Another obvious part of our future work is to carry out long-term field tests with the real robot. Such field tests could, for example, involve a delivery task in an office environment, a hospital, or a factory.

To conclude, it has been demonstrated that the approach of using modularized brain processes in a given, unified format coupled with decision-making based on utility functions, is capable of robustly solving robot motor tasks such as navigation and delivery, allowing the robot to operate autonomously over great distances. Even so, the robot may occasionally suffer catastrophic errors which, on the other hand, we suggest can be dealt with by the inclusion of only a few additional brain processes.

#### **8. References**

Arkin, R. C. (1987). *Towards Cosmopolitan Robots: Intelligent Navigation in Extended Man-Made Environments*, PhD thesis, University of Massachusetts.

Arkin, R. C. (1998). *Behavior-Based Robotics*, MIT Press.

24 Will-be-set-by-IN-TECH

possible criticism of our work is that there is no guarantee that the robot will not suddenly fail, for example by generating an incorrect heading estimate, such that its position error will grow quickly. Indeed, in very long runs, exactly such problems do occur, albeit rarely. Our approach to overcoming those problems would follow the philosophy of error correction rather than error avoidance. An example of this philosophy can be found in the topic of gait generation for humanoid robots: Rather than trying to generate a humanoid robot that will never fall, it is better to make sure that the robot can get up if it *does* fall. This approach has been applied in, for example, the HRP-2 robot, see Kaneko et al. (2002), and many other

Similarly, rather than trying to further tune the brain process parameters or the parameters of the decision-making system, we would, for full long-term reliability, advocate an approach in which the robotic brain would be supplemented by (at least) two additional brain processes. These are: (i) An *Emergency stop* motor behavior which would cause the robot to stop if it finds itself in a situation where a collision with a stationary object is imminent (moving obstacles being handled by the *Moving obstacle avoidance* behavior). Such situations would occur, for example, if the odometric calibration suffers a catastrophic failure so that the error in the robot's estimated pose grows without bound. Once the robot has stopped it should turn towards the most favorable direction for calibration (which would hardly be the direction towards the stationary object and neither any direction in which *no* part of the map would be visible). Once a suitable direction has been found, the robot should activate the second added brain process, namely *Wake-up*. This cognitive process would attempt a global search (again matching the current laser scan to virtual scans taken in the map) for the robot's current pose. With these two brain processes, which are to be added in future work, the robot would be able

As for the currently available brain processes, the *Moving obstacle avoidance* brain process could be improved to handle also situations in which *several* moving obstacles are approaching the robot simultaneously. However, to some extent, the brain process already does this since, before letting the robot move, it generally checks whether or not there is a clear path in the intended direction of movement; see also the right panel of Fig. 9. In cases where multiple moving obstacles are approaching, typically the best course of action (at least for a rather

Another obvious part of our future work is to carry out long-term field tests with the real robot. Such field tests could, for example, involve a delivery task in an office environment, a

To conclude, it has been demonstrated that the approach of using modularized brain processes in a given, unified format coupled with decision-making based on utility functions, is capable of robustly solving robot motor tasks such as navigation and delivery, allowing the robot to operate autonomously over great distances. Even so, the robot may occasionally suffer catastrophic errors which, on the other hand, we suggest can be dealt with by the inclusion of

Arkin, R. C. (1987). *Towards Cosmopolitan Robots: Intelligent Navigation in Extended Man-Made*

*Environments*, PhD thesis, University of Massachusetts.

Arkin, R. C. (1998). *Behavior-Based Robotics*, MIT Press.

humanoid robots subsequently developed.

to correct even catastrophic errors.

slow robot) is to do nothing.

only a few additional brain processes.

hospital, or a factory.

**8. References**


**14** 

*Oman* 

*Sultan Qaboos University,* 

**Fuzzy Logic Based Navigation of Mobile Robots** 

Robots are no longer confined to engineered, well protected sealed corners, but they are currently "employed" in places closer and closer to "us". Robots are getting out of factories and are finding their way into our homes and to populated places such as, museum halls,

The gained benefit of the potential service and personal robots comes along with the necessity to design the robot in a way that makes it safe for it to interact with humans and in a way that makes it able to respond to a list of complex situations. This includes at least the possibility to have the robot situated in an unknown, unstructured and dynamic environment and to navigate its way in such an environment. One of the fundamental issues to be addressed in autonomous robotic system is the ability to move without collision. An "intelligent" robot should avoid undesirable and potentially dangerous impact with objects in its environment. This simple capability has been the subject of interest in

Behavior based navigation systems (Arkin, 1987, 1989; Arkin & Balch, 1997; AlYahmedi et al., 2009; Brooks, 1986, 1989; Fatmi et al. 2006 and Ching-Chih et al. 2010) have been developed as an alternative to the more traditional strategy of constructing representation of the world and then reasoning prior to acting. The main idea of behavior based navigation is to identify different responses (behaviors) to sensory inputs. For example, a behavior could be "avoiding obstacles" in which sonar information about a close obstacle should result in a movement away from the obstacle. A given set of behaviors is then blended in a certain way to produce either a trade off behavior or a more complex behavior. However, a number of issues with regard to behavior based navigation are still under investigation. These issues range from questions concerning the design of individual behaviors to behavior

An important problem in autonomous navigation is the need to deal with the large amount of uncertainties of the sensory information received by the robot which is incomplete and approximate as well as with the fact that the environment in which such robots operate

A fuzzy logic behavior based navigation approach is introduced in this chapter in order to deal with the uncertainty and ambiguity of the information the system receives. Issues of individual behavior design and action coordination of the behaviors will be addressed using

coordination issues, to intelligently improve "behaviors" through learning.

The approach described herein, consists of the following four tasks,

contains dynamics and variability elements.

office buildings, schools, airports, shopping malls and hospitals.

**1. Introduction** 

robotic research.

fuzzy logic.

Amur S. Al Yahmedi and Muhammed A. Fatmi

Wahde, M. (2009). A general-purpose method for decision-making in autonomous robots, *in* B.-C. Chien, T.-P. Hong, S.-M. Chen & M. Ali (eds), *Next-Generation Applied Intelligence, LNAI 5579*, Springer, pp. 1–10.

## **Fuzzy Logic Based Navigation of Mobile Robots**

Amur S. Al Yahmedi and Muhammed A. Fatmi

*Sultan Qaboos University, Oman* 

#### **1. Introduction**

26 Will-be-set-by-IN-TECH

286 Recent Advances in Mobile Robotics

Wahde, M. (2009). A general-purpose method for decision-making in autonomous robots,

*Intelligence, LNAI 5579*, Springer, pp. 1–10.

*in* B.-C. Chien, T.-P. Hong, S.-M. Chen & M. Ali (eds), *Next-Generation Applied*

Robots are no longer confined to engineered, well protected sealed corners, but they are currently "employed" in places closer and closer to "us". Robots are getting out of factories and are finding their way into our homes and to populated places such as, museum halls, office buildings, schools, airports, shopping malls and hospitals.

The gained benefit of the potential service and personal robots comes along with the necessity to design the robot in a way that makes it safe for it to interact with humans and in a way that makes it able to respond to a list of complex situations. This includes at least the possibility to have the robot situated in an unknown, unstructured and dynamic environment and to navigate its way in such an environment. One of the fundamental issues to be addressed in autonomous robotic system is the ability to move without collision. An "intelligent" robot should avoid undesirable and potentially dangerous impact with objects in its environment. This simple capability has been the subject of interest in robotic research.

Behavior based navigation systems (Arkin, 1987, 1989; Arkin & Balch, 1997; AlYahmedi et al., 2009; Brooks, 1986, 1989; Fatmi et al. 2006 and Ching-Chih et al. 2010) have been developed as an alternative to the more traditional strategy of constructing representation of the world and then reasoning prior to acting. The main idea of behavior based navigation is to identify different responses (behaviors) to sensory inputs. For example, a behavior could be "avoiding obstacles" in which sonar information about a close obstacle should result in a movement away from the obstacle. A given set of behaviors is then blended in a certain way to produce either a trade off behavior or a more complex behavior. However, a number of issues with regard to behavior based navigation are still under investigation. These issues range from questions concerning the design of individual behaviors to behavior coordination issues, to intelligently improve "behaviors" through learning.

An important problem in autonomous navigation is the need to deal with the large amount of uncertainties of the sensory information received by the robot which is incomplete and approximate as well as with the fact that the environment in which such robots operate contains dynamics and variability elements.

A fuzzy logic behavior based navigation approach is introduced in this chapter in order to deal with the uncertainty and ambiguity of the information the system receives. Issues of individual behavior design and action coordination of the behaviors will be addressed using fuzzy logic.

The approach described herein, consists of the following four tasks,

Fuzzy Logic Based Navigation of Mobile Robots 289

features that enable it to cope with uncertain, incomplete and approximate information. Thus, fuzzy logic stirs more and more interest amongst researchers in the field of robot navigation. Further, in the majority of fuzzy logic applications in navigation, a mathematical model of the dynamics of the robot nor the environment is needed in the design process of

The theory of fuzzy logic systems is inspired by the remarkable human capacity to reason with perception-based information. Rule based fuzzy logic provides a formal methodology for linguistic rules resulting from reasoning and decision making with uncertain and

In the fuzzy logic control inputs are processed in three steps (Fuzzification, Inference and

Fuzzification Inference Deffuzzification Inputs Outputs

In the fuzzification block one defines for example fuzzy set *A* in a universe of discourse X defined by its membership function *µA(x)* for each *x* representing the degree of membership of *x* in *X*. In fuzzy logic control, membership functions assigned with linguistic variables are used to fuzzify physical quantities. Next, in the inference block, fuzzified inputs are inferred to a fuzzy rules base. This rules base is used to characterize the relationship between fuzzy inputs and fuzzy outputs. For example, a simple fuzzy control rule relating input *v* to

*IF v is W then u is Y* (1)

output *u* might be expressed in the condition-action form as follows,

Where *W* and *Y* are fuzzy values defined on the universes of *v* and *u*, respectively.

The response of each fuzzy rule is weighted according to the degree of membership of its input conditions. The inference engine provides a set of control actions according to fuzzified inputs. Since the control actions are in fuzzy sense. Hence, a deffuzification method is required to transform fuzzy control actions into a crisp value of the fuzzy logic

In behavior based navigation the problem is decomposed into simpler tasks(independent behaviors). In fuzzy logic behavior based navigation systems each behavior is composed of a set of fuzzy logic rule statements aimed at achieving a well defined set of objectives, for

*If goal is near and to the left then turn left and move forward with a low speed*  In general the actions recommended by different behaviors are compiled to yield the most

The main problem in robot behavior based navigation is how to coordinate the activity of several behaviors, which may be active concurrently with the possibility of having behavior conflict. For example, one may have "goal reaching" behavior and " obstacle avoidance"

the motion controller.

imprecise information.

controller.

example a rule could be:

**2.3 Behavior coordination** 

appropriate action according to certain criteria.

behavior active at the same time as seen in Fig 2.

Deffuzification) as seen in Fig. 1.

Fig. 1. Fuzzy logic control steps.


#### **2. Behavior based navigation**

One of the long standing challenging aspect in mobile robotics is the ability to navigate autonomously, avoiding modeled and unmodeled obstacles especially in crowded and unpredictably changing environment. A successful way of structuring the navigation task in order to deal with the problem is within behavior based navigation approaches (Arkin, 1987, 1989; Arkin & Balch, 1997; AlYahmedi et al., 2009; Brooks, 1986, 1989; Fatmi et al. 2006; Ching-Chih et al. 2010; Maes, 1990; Mataric, 1997; Rosenblatt et al. 1989, 1994, 1995; Saffiotti, 1997 and Seraji & Howard, 2002).

#### **2.1 Introduction**

The basic idea in behavior based navigation is to subdivide the navigation task into small easy to manage, program and debug behaviors (simpler well defined actions) that focus on execution of specific subtasks. For example, basic behaviors could be "avoid obstacles" or "moving to a predefined position". This divide-and-conquer approach has turned out to be a successful approach, for it makes the system modular, which both simplifies the navigation solution as well as offers a possibility to add new behaviors to the system without causing any major increase in complexity. The suggested outputs from each concurrently active behaviors are then "blended" together according to some action coordination rule. The task then reduces to that of coupling actuators to sensory inputs, with desired robot behaviors. Each behavior can take inputs from the robot's sensors (e.g., camera, ultrasound, infrared, tactile) and/or from other behaviors in the system, and send outputs to the robot's actuators(effectors) (e.g., wheels, grippers, arm, and speech) and/or to other behaviors.

A variety of behavior-based control schemes have been inspired by the success of (Brooks, 1986, 1989), with his architecture which is known by the subsumption architecture. In this architecture behaviors are arranged in levels of priority where triggering a higher level behavior suppresses all lower level behaviors. (Arkin, 1987, 1989; Arkin & Balch, 1997), has described the use of reactive behaviors called motor schemas. In this method, potential field is used to define the output of each schema. Then, all the outputs are combined by weighted summation. Rosenblatt et al. (Rosenblatt et al. 1989, 1994, 1995), presented DAMN architecture in which a centralized arbitration of votes provided by independent behaviors combines into a "voted" output. Others (Saffiotti, 1997), (Seraji et al., 2001, 2002), (Yang et al. 2004, 2005; Selekwa et al., 2005 and Aguirre & Gonzales, 2006) used fuzzy logic system to represent and coordinate behaviors.

#### **2.2 Fuzzy behavior based navigation**

An important problem in autonomous navigation is the need to deal with the large amount of *uncertainties* that has to do with the sensory information received by the robot as well as with the fact that the environment in which such robots operate contains elements of *dynamics* and *variability* that limit the utility of prior knowledge. Fuzzy theory has the 288 Recent Advances in Mobile Robotics

• The use of fuzzy sets to represent the approximate positions and possibly shapes of

• The design of simple fuzzy behaviors (avoiding obstacles, goal reaching, wall

One of the long standing challenging aspect in mobile robotics is the ability to navigate autonomously, avoiding modeled and unmodeled obstacles especially in crowded and unpredictably changing environment. A successful way of structuring the navigation task in order to deal with the problem is within behavior based navigation approaches (Arkin, 1987, 1989; Arkin & Balch, 1997; AlYahmedi et al., 2009; Brooks, 1986, 1989; Fatmi et al. 2006; Ching-Chih et al. 2010; Maes, 1990; Mataric, 1997; Rosenblatt et al. 1989, 1994, 1995; Saffiotti,

The basic idea in behavior based navigation is to subdivide the navigation task into small easy to manage, program and debug behaviors (simpler well defined actions) that focus on execution of specific subtasks. For example, basic behaviors could be "avoid obstacles" or "moving to a predefined position". This divide-and-conquer approach has turned out to be a successful approach, for it makes the system modular, which both simplifies the navigation solution as well as offers a possibility to add new behaviors to the system without causing any major increase in complexity. The suggested outputs from each concurrently active behaviors are then "blended" together according to some action coordination rule. The task then reduces to that of coupling actuators to sensory inputs, with desired robot behaviors. Each behavior can take inputs from the robot's sensors (e.g., camera, ultrasound, infrared, tactile) and/or from other behaviors in the system, and send outputs to the robot's actuators(effectors) (e.g.,

A variety of behavior-based control schemes have been inspired by the success of (Brooks, 1986, 1989), with his architecture which is known by the subsumption architecture. In this architecture behaviors are arranged in levels of priority where triggering a higher level behavior suppresses all lower level behaviors. (Arkin, 1987, 1989; Arkin & Balch, 1997), has described the use of reactive behaviors called motor schemas. In this method, potential field is used to define the output of each schema. Then, all the outputs are combined by weighted summation. Rosenblatt et al. (Rosenblatt et al. 1989, 1994, 1995), presented DAMN architecture in which a centralized arbitration of votes provided by independent behaviors combines into a "voted" output. Others (Saffiotti, 1997), (Seraji et al., 2001, 2002), (Yang et al. 2004, 2005; Selekwa et al., 2005 and Aguirre & Gonzales, 2006) used fuzzy logic system to

An important problem in autonomous navigation is the need to deal with the large amount of *uncertainties* that has to do with the sensory information received by the robot as well as with the fact that the environment in which such robots operate contains elements of *dynamics* and *variability* that limit the utility of prior knowledge. Fuzzy theory has the

objects in the environment.

**2. Behavior based navigation** 

1997 and Seraji & Howard, 2002).

represent and coordinate behaviors.

**2.2 Fuzzy behavior based navigation** 

**2.1 Introduction** 

• The blending of the different fuzzy behaviors.

wheels, grippers, arm, and speech) and/or to other behaviors.

following…etc.).

features that enable it to cope with uncertain, incomplete and approximate information. Thus, fuzzy logic stirs more and more interest amongst researchers in the field of robot navigation. Further, in the majority of fuzzy logic applications in navigation, a mathematical model of the dynamics of the robot nor the environment is needed in the design process of the motion controller.

The theory of fuzzy logic systems is inspired by the remarkable human capacity to reason with perception-based information. Rule based fuzzy logic provides a formal methodology for linguistic rules resulting from reasoning and decision making with uncertain and imprecise information.

In the fuzzy logic control inputs are processed in three steps (Fuzzification, Inference and Deffuzification) as seen in Fig. 1.

Fig. 1. Fuzzy logic control steps.

In the fuzzification block one defines for example fuzzy set *A* in a universe of discourse X defined by its membership function *µA(x)* for each *x* representing the degree of membership of *x* in *X*. In fuzzy logic control, membership functions assigned with linguistic variables are used to fuzzify physical quantities. Next, in the inference block, fuzzified inputs are inferred to a fuzzy rules base. This rules base is used to characterize the relationship between fuzzy inputs and fuzzy outputs. For example, a simple fuzzy control rule relating input *v* to output *u* might be expressed in the condition-action form as follows,

$$IF \ v \text{ is } \mathcal{W} \text{ then } \mathfrak{u} \text{ is } Y \tag{1}$$

Where *W* and *Y* are fuzzy values defined on the universes of *v* and *u*, respectively.

The response of each fuzzy rule is weighted according to the degree of membership of its input conditions. The inference engine provides a set of control actions according to fuzzified inputs. Since the control actions are in fuzzy sense. Hence, a deffuzification method is required to transform fuzzy control actions into a crisp value of the fuzzy logic controller.

In behavior based navigation the problem is decomposed into simpler tasks(independent behaviors). In fuzzy logic behavior based navigation systems each behavior is composed of a set of fuzzy logic rule statements aimed at achieving a well defined set of objectives, for example a rule could be:

#### *If goal is near and to the left then turn left and move forward with a low speed*

In general the actions recommended by different behaviors are compiled to yield the most appropriate action according to certain criteria.

#### **2.3 Behavior coordination**

The main problem in robot behavior based navigation is how to coordinate the activity of several behaviors, which may be active concurrently with the possibility of having behavior conflict. For example, one may have "goal reaching" behavior and " obstacle avoidance" behavior active at the same time as seen in Fig 2.

Fuzzy Logic Based Navigation of Mobile Robots 291

well lead to poor performance in certain situations, for example if the robot is to encounter an obstacle right in front of it the "avoid obstacle" behavior may recommend the robot to turn left, while the "seek goal" behavior may request the robot to turn right since the goal is to the right of the robot, this may lead to trade off command that directs the robot forward

To deal with these limitations other schemes were recommended that achieve the coordination via considering the situation in which the robot is found, i.e each behavior is allowed to affect the robot motion based on the situational context. (Saffiott, 1997) uses the process of context-dependent-blending in which the current situation is used to decide the action taken using fuzzy logic. Independently (Tunstel et al., 1997) developed an approach similar to context-dependent-blending, in which adaptive hierarchy of multiple fuzzy behaviors are combined using the concept of degree of applicability. In this case certain behaviors are allowed to affect the overall behavior as required by the current situation and goal. The behavior fusion methodology in this chapter is motivated by the approaches used

The robot navigation tasks are divided into small independent behaviors that focus on execution of a specific subtask. For example, a behavior focuses on reaching the global goal, while another focuses on avoiding obstacles. Each behavior is composed of a set of fuzzy logic rules aimed at achieving a given desired objective. The navigation rules consist of a set of fuzzy logic rules for robot *velocity* (linear velocity m/s) and *steering* (angular velocity

Where the condition *C* is composed of fuzzy input variables and fuzzy connectives (And) and the action *A* is a fuzzy output variable. Equation (2) represents the typical form of natural *linguistic* rules .This rules reflect the human expert and reason to ensure logic, reliable and safe navigation. For example, obstacle avoidance behavior has inputs sensory data which can be represented by fuzzy sets with linguistic labels, such as {Near, Medium, Far}, corresponding to distance between robot and obstacle. Typical examples of fuzzy rules

If *Front left* is Near And *Front right* is Far, Then *Steering* is Right

If *Front left* is Far And *Front right* is Near, Then *Velocity* is Zero Where *Front left* and *Front right* are the distances acquired from sensors located in different

Many behaviors can be active simultaneously in a specific situation or *context*. Therefore, a coordination technique, solving the problem of activation of several behaviors is needed. We call the method *context dependent behavior coordination*. The coordination technique employed herein is motivated by the approaches used by Saffiotti (Saffiotti, 1997). The supervision layer based on the context makes a decision as to which behavior(s) to process (activate) rather than processing all behavior(s) and then blending the appropriate ones, as a result time and computational resources are saved. Fig.4 and Fig.5 represent the architecture of the

*IF C then A* (2)

resulting in a collision with the obstacle.

**2.4 Context-dependent behavior coordination** 

compared method and our approach, respectively.

by Saffiot and Tunstel et al.

rad/s) of the form

are as follow,

locations on the robot.

Fig. 2. Conflicts in bahviors.

The coordination task will be to reach a trade-off conclusion that provides the suitable command to the robot actuators which can result in choosing one behavior or a combination of all activated behaviors as shown in Fig.3. Behavior coordination is the point at which most strategies differ. Some of the earlier strategies are based on Brooks subsumption architecture (Brooks, 1986, 1989) uses a switching type of behavior coordination. In the Subsumption approach a prioritization scheme is used in which recommendation of only one behavior with the highest priority is selected, while recommendations of the remaining competing behaviors are ignored. This approach however, leads to inefficient results or poor performance in certain situations. For example if a robot is to encounter an obstacle right in front of it the action that will be selected is "avoid obstacle", the robot then decides to turn left to avoid the obstacle while the goal is to the right of the robot, so the "seek goal" behavior is affected in a negative way.

Fig. 3. Structure of behavior based navigation.

Other techniques combine the output of each behavior based on predetermined weighting factors, for example Arkin's motor schema approach(Arkin, 1987, 1989; Arkin & Balch, 1997), or Philipp A. and H.I. Christensen (ALthaus & Christensen, 2002) and the work of Rosenblatt(Rosenblatt et al. 1989, 1994, 1995), who developed the distributed architecture for mobile robot navigation, in which a centralized arbitration of votes provided by independent behaviors. In this method each behavior is allowed to vote for or against certain vehicle actions. The action that win the vote is carried out. These techniques may as 290 Recent Advances in Mobile Robotics

Robot

**STIMULIUS**

Fig. 2. Conflicts in bahviors.

behavior is affected in a negative way.

Fig. 3. Structure of behavior based navigation.

Obstacle

The coordination task will be to reach a trade-off conclusion that provides the suitable command to the robot actuators which can result in choosing one behavior or a combination of all activated behaviors as shown in Fig.3. Behavior coordination is the point at which most strategies differ. Some of the earlier strategies are based on Brooks subsumption architecture (Brooks, 1986, 1989) uses a switching type of behavior coordination. In the Subsumption approach a prioritization scheme is used in which recommendation of only one behavior with the highest priority is selected, while recommendations of the remaining competing behaviors are ignored. This approach however, leads to inefficient results or poor performance in certain situations. For example if a robot is to encounter an obstacle right in front of it the action that will be selected is "avoid obstacle", the robot then decides to turn left to avoid the obstacle while the goal is to the right of the robot, so the "seek goal"

Behavior 3

Behavior 2

Sensors Actuators

Behavior 1

Behavior n

Other techniques combine the output of each behavior based on predetermined weighting factors, for example Arkin's motor schema approach(Arkin, 1987, 1989; Arkin & Balch, 1997), or Philipp A. and H.I. Christensen (ALthaus & Christensen, 2002) and the work of Rosenblatt(Rosenblatt et al. 1989, 1994, 1995), who developed the distributed architecture for mobile robot navigation, in which a centralized arbitration of votes provided by independent behaviors. In this method each behavior is allowed to vote for or against certain vehicle actions. The action that win the vote is carried out. These techniques may as

Target

**COORDINATION**

well lead to poor performance in certain situations, for example if the robot is to encounter an obstacle right in front of it the "avoid obstacle" behavior may recommend the robot to turn left, while the "seek goal" behavior may request the robot to turn right since the goal is to the right of the robot, this may lead to trade off command that directs the robot forward resulting in a collision with the obstacle.

To deal with these limitations other schemes were recommended that achieve the coordination via considering the situation in which the robot is found, i.e each behavior is allowed to affect the robot motion based on the situational context. (Saffiott, 1997) uses the process of context-dependent-blending in which the current situation is used to decide the action taken using fuzzy logic. Independently (Tunstel et al., 1997) developed an approach similar to context-dependent-blending, in which adaptive hierarchy of multiple fuzzy behaviors are combined using the concept of degree of applicability. In this case certain behaviors are allowed to affect the overall behavior as required by the current situation and goal. The behavior fusion methodology in this chapter is motivated by the approaches used by Saffiot and Tunstel et al.

#### **2.4 Context-dependent behavior coordination**

The robot navigation tasks are divided into small independent behaviors that focus on execution of a specific subtask. For example, a behavior focuses on reaching the global goal, while another focuses on avoiding obstacles. Each behavior is composed of a set of fuzzy logic rules aimed at achieving a given desired objective. The navigation rules consist of a set of fuzzy logic rules for robot *velocity* (linear velocity m/s) and *steering* (angular velocity rad/s) of the form

$$\text{IF C then A}\tag{2}$$

Where the condition *C* is composed of fuzzy input variables and fuzzy connectives (And) and the action *A* is a fuzzy output variable. Equation (2) represents the typical form of natural *linguistic* rules .This rules reflect the human expert and reason to ensure logic, reliable and safe navigation. For example, obstacle avoidance behavior has inputs sensory data which can be represented by fuzzy sets with linguistic labels, such as {Near, Medium, Far}, corresponding to distance between robot and obstacle. Typical examples of fuzzy rules are as follow,

If *Front left* is Near And *Front right* is Far, Then *Steering* is Right

#### If *Front left* is Far And *Front right* is Near, Then *Velocity* is Zero

Where *Front left* and *Front right* are the distances acquired from sensors located in different locations on the robot.

Many behaviors can be active simultaneously in a specific situation or *context*. Therefore, a coordination technique, solving the problem of activation of several behaviors is needed. We call the method *context dependent behavior coordination*. The coordination technique employed herein is motivated by the approaches used by Saffiotti (Saffiotti, 1997). The supervision layer based on the context makes a decision as to which behavior(s) to process (activate) rather than processing all behavior(s) and then blending the appropriate ones, as a result time and computational resources are saved. Fig.4 and Fig.5 represent the architecture of the compared method and our approach, respectively.

Fuzzy Logic Based Navigation of Mobile Robots 293

The navigation task can be broken down to a set of simple behaviors. The behaviors can be represented using fuzzy *if-then* rules. A context dependent coordination method can be used

To validate the applicability of the method simulation and experimental studies were

To provide the robot with the ability to navigate autonomously avoiding modeled and unmodeled obstacles especially in crowded and unpredictably dynamic environment the following behaviors were designed: Goal reaching , Emergency situation, Obstacle avoidance, Wall following. Each behavior was represented using a fuzzy *if- then* rule base.

Where *l= 1…m* , and *m* is the number of rules in a given fuzzy rule base, *x1 … xn* are the

The goal reaching behavior tends to drive the robot from a given initial position to a stationary or moving target position. This behavior drives the robot to the left to the right or forward depending on *θerror*, the difference between the desired heading (the heading

Fig.6 gives a schematic block diagram of the goal reaching architecture. From this figure we can notice that the inputs of the goal reaching controller are the distance robot to goal (*Drg*)

Controller Robot Environment

( )( ) <sup>2</sup> <sup>2</sup> *Drg* <sup>=</sup> *<sup>X</sup> Goal* <sup>−</sup> *<sup>X</sup> <sup>M</sup>* <sup>+</sup> *YGoal* <sup>−</sup> *YM* (4)

*H*

tan (5)

⎟ ⎠ ⎞

*X X Y Y Goal M Goal M*

−

*error* −⎟

⎜ ⎜ ⎝ ⎛

<sup>−</sup> <sup>=</sup> <sup>−</sup><sup>1</sup>

1 1 : is and ... and is , is *<sup>l</sup> l ll R IF x A nn x A THEN y B* (3)

*1…Al*

*<sup>2</sup>* are the input fuzzy

**3. Simulation & experimental results** 

**3.2 Design of individual behaviors** 

**3.2.1 Goal reaching behavior** 

Calculate  *& Drg*

compute

( ) *XM* ,*YM* ,*<sup>H</sup>*

Fig. 6. Control architecture for goal reaching Behavior.

θ

θ*error*

*Drg ,*θ*error*

The fuzzy rule base comprises the following *if-then* rules:

sets, *Bl* is the output fuzzy set and *y* is the output variable.

required to reach the goal) and the actual current heading.

and *θerror* which are given by Equation (4) and Equation (5), respectively.

Fuzzy

Odometer

input variables which are the sensor data of the mobile robot, *Al*

( )

**3.1 Introduction** 

to blend behaviors.

performed.

Fig. 4. Architecture of compared method.

Fig. 5. Architecture of Context Dependent Behavior Coordination.

Our approach consists of the following characteristics.

	- 1. Goal reaching behavior
	- 2. Wall following behavior
	- 3. Emergency situation behavior
	- 4. Obstacle avoidance behavior.

#### **3. Simulation & experimental results**

#### **3.1 Introduction**

292 Recent Advances in Mobile Robotics

**behavior 1**

**behavior 2**

**behavior n**

Robot

**behavior 1**

**behavior 2**

**behavior n**

Robot

**Supervision**

**CONTROL**

**Planning**

Commands

Commands

Sensory data

Sensory data

Fig. 5. Architecture of Context Dependent Behavior Coordination.

• The robot navigation is comprised of four behaviors as follows,

blends behaviors depending on situation or context.

• Each behavior is composed of a set of fuzzy logic rules achieving a precise goal. • The output of each behavior represents the Steering angle and the Velocity.

• The supervision layer defines the priority of each behavior. It selects or activates and

Our approach consists of the following characteristics.

1. Goal reaching behavior 2. Wall following behavior 3. Emergency situation behavior 4. Obstacle avoidance behavior.

Fig. 4. Architecture of compared method.

**Modelling**

The navigation task can be broken down to a set of simple behaviors. The behaviors can be represented using fuzzy *if-then* rules. A context dependent coordination method can be used to blend behaviors.

To validate the applicability of the method simulation and experimental studies were performed.

#### **3.2 Design of individual behaviors**

To provide the robot with the ability to navigate autonomously avoiding modeled and unmodeled obstacles especially in crowded and unpredictably dynamic environment the following behaviors were designed: Goal reaching , Emergency situation, Obstacle avoidance, Wall following. Each behavior was represented using a fuzzy *if- then* rule base. The fuzzy rule base comprises the following *if-then* rules:

$$R^{\{l\}}: I \Gamma \ge\_1 \text{is } A\_1^l \text{ and } \dots \text{ and } \propto\_n \text{ is } A\_n^l \text{, THEN } y \text{ is } \mathbb{B}^l \tag{3}$$

Where *l= 1…m* , and *m* is the number of rules in a given fuzzy rule base, *x1 … xn* are the input variables which are the sensor data of the mobile robot, *Al 1…Al <sup>2</sup>* are the input fuzzy sets, *Bl* is the output fuzzy set and *y* is the output variable.

#### **3.2.1 Goal reaching behavior**

The goal reaching behavior tends to drive the robot from a given initial position to a stationary or moving target position. This behavior drives the robot to the left to the right or forward depending on *θerror*, the difference between the desired heading (the heading required to reach the goal) and the actual current heading.

Fig.6 gives a schematic block diagram of the goal reaching architecture. From this figure we can notice that the inputs of the goal reaching controller are the distance robot to goal (*Drg*) and *θerror* which are given by Equation (4) and Equation (5), respectively.

Fig. 6. Control architecture for goal reaching Behavior.

$$D\_{rg} = \sqrt{\left(X\_{Goal} - X\_M\right)^2 + \left(Y\_{Goal} - Y\_M\right)^2} \tag{4}$$

$$\theta\_{error} = \tan^{-1} \left( \frac{Y\_{Goal} - Y\_M}{X\_{Goal} - X\_M} \right) - H \tag{5}$$

Fuzzy Logic Based Navigation of Mobile Robots 295

the proposed scheme experimentally) is endowed with 14 infrared sensors (See Fig.8). These sensors are used to detect obstacles in short-range distances and in a cone of 10 degrees. These sensors are clustered into 6 groups in the simulated robot in such a way as to be

**Front left Front Right**

For each group, we refer to the minimum of distance measurement by group of sensors as: *Right Down= min (D9, D10) Right Up= min (D7, D8) Front Right= min (D4, D5, D6)* 

*Left Down=min (D11, D12) Left Up=min (D13, D14). Front Left=min (D1, D2, D3),* 

These distances represent the inputs of the fuzzy controller for behaviors like Emergency situation, Obstacle avoidance, Wall following behaviors. Each distance is fuzzified using the

0 20 40 60 80 100

Distance

The output variable are the Steering and the Velocity. Fig.10 illustrates the membership

Fig. 9. Input membership function for distance to obstacle, N:Near, M:Medium, F:Far

**Right Up**

**Right Down**

similar to Pekee(the simulated robot is very similar kinematically to Pekee).

**Left Up**

**Left Down**

*Di* is the distance acquired by the sensor S*i. i*=1….14.

N M F

following membership function described in Fig.9.

0

0.2

0.4

0.6

Degree of membership

functions for these outputs.

0.8

1

Fig. 8. Clustered sensors arrangement.

Where, *(XM, YM, H)* are the robot position and the heading measured by the robot odometer. *(XGaol, YGaol)* is the target position

Although, there is no restriction on the form of membership functions, the appropriate membership functions for *Drg* (in mm) and *θerror* (in degrees) shown in Fig.7 were chosen.

N: Negative, SN: Small Negative, Z: Zero, SP: Small Positive, P: Positive.

N for Near, S for Small, B for Big.

Fig. 7. Input membership functions for *Drg* and *θerror.*

The Goal reaching is expected to align the robot's heading with the direction of the goal so when *θerror* is positive, the Steering is left or when *θerror* is negative, the Steering is Right in a way that minimizes *θerror*. For The Velocity it is proportional to the distance to goal *Drg*. Example of goal reaching fuzzy rules.

If *θerror* is *Positive* And *Drg is Big then Velocity is Small Positive* 

If *θerror* is *Positive* And *Drg is Big then Steering is Left* 

If *θerror* is *Negative* And *Drg is Small then Velocity is Small Positive*

If *θerror* is *Negative* And *Drg is Small then Steering is Right*

If *θerror* is *Small Negative* And *Drg is Big then Velocity is Positive* 

If *θerror* is *Small Negative* And *Drg is Big then Steering is Right Front* 


Table 1. Fuzzy table rules for goal reaching behavior.

For other behaviors the robots needs to acquire information about the environment. The Pekee Robot (Pekee is the robotic platform that will be used to validate the functionality of 294 Recent Advances in Mobile Robotics

Where, *(XM, YM, H)* are the robot position and the heading measured by the robot odometer.

Although, there is no restriction on the form of membership functions, the appropriate membership functions for *Drg* (in mm) and *θerror* (in degrees) shown in Fig.7 were chosen.

0.2

0.4

0.6

Degree of membership

The Goal reaching is expected to align the robot's heading with the direction of the goal so when *θerror* is positive, the Steering is left or when *θerror* is negative, the Steering is Right in a way that minimizes *θerror*. For The Velocity it is proportional to the distance to goal *Drg*.

*Velocity Steering* 

 *Near Z Z Z Z Z Near F RF R LF L* 

*Small P P SP SP SP Small F RF R LF L* 

*Big P P SP P SP Big F RF R LF L* 

For other behaviors the robots needs to acquire information about the environment. The Pekee Robot (Pekee is the robotic platform that will be used to validate the functionality of

*Drg*

*θerror*

0.8

1

<sup>0</sup> <sup>20</sup> <sup>40</sup> <sup>60</sup> <sup>80</sup> <sup>100</sup> <sup>0</sup>

N S B

Drg

*Z SN N SP P* 

N for Near, S for Small, B for Big.

*(XGaol, YGaol)* is the target position

0.2

Positive, P: Positive.

*θerror*

*Drg*

0.4

0.6

Degree of membership

0.8

1


N SN Z SP P

<sup>θ</sup>error

Fig. 7. Input membership functions for *Drg* and *θerror.*

If *θerror* is *Positive* And *Drg is Big then Steering is Left* 

If *θerror* is *Negative* And *Drg is Small then Steering is Right* If *θerror* is *Small Negative* And *Drg is Big then Velocity is Positive*  If *θerror* is *Small Negative* And *Drg is Big then Steering is Right Front* 

*Z SN N SP P* 

Table 1. Fuzzy table rules for goal reaching behavior.

If *θerror* is *Positive* And *Drg is Big then Velocity is Small Positive* 

If *θerror* is *Negative* And *Drg is Small then Velocity is Small Positive*

N: Negative, SN: Small Negative, Z: Zero, SP: Small

Example of goal reaching fuzzy rules.

the proposed scheme experimentally) is endowed with 14 infrared sensors (See Fig.8). These sensors are used to detect obstacles in short-range distances and in a cone of 10 degrees. These sensors are clustered into 6 groups in the simulated robot in such a way as to be similar to Pekee(the simulated robot is very similar kinematically to Pekee).

Fig. 8. Clustered sensors arrangement.

For each group, we refer to the minimum of distance measurement by group of sensors as:

*Right Down= min (D9, D10) Right Up= min (D7, D8) Front Right= min (D4, D5, D6) Left Down=min (D11, D12) Left Up=min (D13, D14). Front Left=min (D1, D2, D3),* 

*Di* is the distance acquired by the sensor S*i. i*=1….14.

These distances represent the inputs of the fuzzy controller for behaviors like Emergency situation, Obstacle avoidance, Wall following behaviors. Each distance is fuzzified using the following membership function described in Fig.9.

Fig. 9. Input membership function for distance to obstacle, N:Near, M:Medium, F:Far

The output variable are the Steering and the Velocity. Fig.10 illustrates the membership functions for these outputs.

Fuzzy Logic Based Navigation of Mobile Robots 297

Front Left

Cases Right

8

9

10

Down

Example of obstacle avoidance rules

**3.2.2 Wall following behavior** 

Down

**3.2.3 Emergency situation behavior** 

Right Up

Table 3. Fuzzy table rules for wall following behavior.

surrounded by obstacles in away depicted in Table 4.

Cases Right

Right Up

Table 2. Fuzzy table rules for obstacle avoidance behavior.

*If Front left is Near And Front right is Far, Then Steering is Right If Front left is Near And Front right is Far, Then Velocity is Zero If Front left is Far And Front right is Near, Then Steering is left If Front left is Far And Front right is Near, Then Velocity is Zero* 

*If Right down is Medium and Front Right is Medium then Steering is Front If Right down is Medium and Front Right is Medium then Velocity is Positive* 

> Front Right

Front Right

Inputs Outputs

left

Down Steering Velocity

Left Up

 F N N R Z F M M R SP

 N N F L Z M M F L SP F F F P

 N F F N F SP N M N F SP N M N F SP

The objective of the control of the wall following behavior is to keep the robot at a safe close distance to the wall and to keep it in line with it. Example of wall following fuzzy rule:

Inputs Outputs

Front Left

The emergency situation behavior drives the robot to the left or to the right when it is

M M F F F P M M M F F P F F M M F P F M M M F P

Left Up

left

Down Steering Velocity

R: Right, RF: Right Front, FL: Front Left, F: Front, L: Left Z: Zero, SP: Small Positive, P: Positive

Fig. 10. Output membership functions for steering & velocity.

#### **3.2.1 Obstacle avoidance behavior**

The obstacle avoidance behavior tends to avoid collisions with obstacles that are in the vicinity of robot. We take into account different cases as shown in table2.


296 Recent Advances in Mobile Robotics

0.2 0.4 0.6 0.8 1

Degree of membership

The obstacle avoidance behavior tends to avoid collisions with obstacles that are in the

Front Left


Z SP P

Velocity(m/sec)

Down Steering Velocity

Z: Zero, SP: Small Positive, P: Positive

Inputs Outputs

left

Left Up

 F N R Z F M R SP

 N F L Z M F L SP

 N L Z M L SP

 N R Z M FR P

 N N F L Z M M F L SP

5 N R Z

6 N L Z


R RF F LF L

Steering rad/sec

Right Up

Fig. 10. Output membership functions for steering & velocity.

vicinity of robot. We take into account different cases as shown in table2.

Front Right

R: Right, RF: Right Front, FL: Front Left,

**3.2.1 Obstacle avoidance behavior** 

0.2 0.4 0.6 0.8 1

F: Front, L: Left

Cases Right

1

2

3

4

7

Down

Degree of membership


Table 2. Fuzzy table rules for obstacle avoidance behavior.

Example of obstacle avoidance rules

*If Front left is Near And Front right is Far, Then Steering is Right* 

*If Front left is Near And Front right is Far, Then Velocity is Zero* 

*If Front left is Far And Front right is Near, Then Steering is left* 

*If Front left is Far And Front right is Near, Then Velocity is Zero* 

#### **3.2.2 Wall following behavior**

The objective of the control of the wall following behavior is to keep the robot at a safe close distance to the wall and to keep it in line with it. Example of wall following fuzzy rule: *If Right down is Medium and Front Right is Medium then Steering is Front If Right down is Medium and Front Right is Medium then Velocity is Positive* 


Table 3. Fuzzy table rules for wall following behavior.

#### **3.2.3 Emergency situation behavior**

The emergency situation behavior drives the robot to the left or to the right when it is surrounded by obstacles in away depicted in Table 4.

Fuzzy Logic Based Navigation of Mobile Robots 299

The inputs of supervision layer are the degree of membership of each group of sensors in all membership function, *Drg* and *θerror*,in addition to the Steering and Velocity outputs of all

Cases RD RU FR FL LU LD *θerror Drg* Behavior

F F F SP

F F F Z

N N SN

N N N

M M SN

M M N

F F F F Goal

F F F SP Goal

F F F P Goal

M M F Wall

F M M Wall

*Sb* and *Vb* are the appropriate velocity and steering control commands sent to the motors in a given robot situation as a result of the decision to activate a certain behavior (avoid obstacle, wall following...). However, *Sb* and *Vb* can be the fusion result of many behaviors. As seen at Table 5 the direction of robot to avoid obstacles is taken as default to the left, but, the supervision layer takes into account the position of goal by *θerror* in such away as to minimize

The advantage of this method is the fact that the number of rules are reduced by reducing the number of input variables to the fuzzy rules of each behavior, for example in the

*IF context then behavior* (6)

Reaching F F F P

Reaching N

Goal

Fusion

Reaching

Reaching

Following

Following

*Si* and *Vi* are the output Steering and Velocity of each behavior, *i*=1...4

*Sb* and *Vb* are the output steering and velocity to motor.

The supervision layer program is based on fuzzy rules such as,

behaviors (*S1, V1, S2, V2, S3 , V3 , S4 and V4* ).

Table 5. Context priority.

the distance traveled towards the goal as shown in Fig.12.

obstacle avoidance behavior there was no need to add *θerror* as an input.


Table 4. Fuzzy table rules for "emergency" behavior.

#### **3.3 Blending of behaviors**

The question to answer once the behaviors are designed is how best decide what the actuators shall receive(in terms of steering and velocity)taking into account the context in which robot happens to be in and relative importance of each behavior. To achieve that the work herein proposes the following architecture with details.

Fig. 11. Supervision Architecture.

298 Recent Advances in Mobile Robotics

 Inputs Outputs Cases Right Up Front Right Front Left Left Up Steering Velocity

The question to answer once the behaviors are designed is how best decide what the actuators shall receive(in terms of steering and velocity)taking into account the context in which robot happens to be in and relative importance of each behavior. To achieve that the

Inference Defuzzification

**Emergency Situation**

Inference Defuzzification

**Obstacle Avoidance**

Inference Defuzzification

**Wall Fallowing**

Inference Defuzzification

**Goal Reaching**

Table 4. Fuzzy table rules for "emergency" behavior.

work herein proposes the following architecture with details.

**Behavior Activation**

Fig. 11. Supervision Architecture.

**3.3 Blending of behaviors** 

*Drg*

θ*error*

Right Down

Left Down

 Far M M M R SP M M M F L SP M F M L Z M F M L Z M M M M R Z

> *V1 S1*

> > *S2*

*V2*

*V3 S3*

V4 *S4*

> Vb

Sb

**Supervision**

*Si* and *Vi* are the output Steering and Velocity of each behavior, *i*=1...4

*Sb* and *Vb* are the output steering and velocity to motor.

The inputs of supervision layer are the degree of membership of each group of sensors in all membership function, *Drg* and *θerror*,in addition to the Steering and Velocity outputs of all behaviors (*S1, V1, S2, V2, S3 , V3 , S4 and V4* ).

The supervision layer program is based on fuzzy rules such as,




Table 5. Context priority.

*Sb* and *Vb* are the appropriate velocity and steering control commands sent to the motors in a given robot situation as a result of the decision to activate a certain behavior (avoid obstacle, wall following...). However, *Sb* and *Vb* can be the fusion result of many behaviors. As seen at Table 5 the direction of robot to avoid obstacles is taken as default to the left, but, the supervision layer takes into account the position of goal by *θerror* in such away as to minimize the distance traveled towards the goal as shown in Fig.12.

The advantage of this method is the fact that the number of rules are reduced by reducing the number of input variables to the fuzzy rules of each behavior, for example in the obstacle avoidance behavior there was no need to add *θerror* as an input.

Fuzzy Logic Based Navigation of Mobile Robots 301

Obstacle 3

E D

wall 2

A wall 1

E

Obstacle 2 <sup>F</sup> <sup>G</sup>

Obstacle 3

wall 1 Obstacle 1

B

wall 3

wall 2

<sup>C</sup> <sup>D</sup>

A

For the experiment (2) Fig.14 the robot had to reach goal 2. It uses the same path as experiment 1 up to the point C. Then, avoiding obstacle 2 and goal reaching behavior are activated up to point D. From this point the robot follows obstacle 3 as a wall up to point E. The presence of obstacle 4 changes the behavior of the robot to obstacle avoidance. The robot avoids obstacle 4 then it follows obstacle 5 as wall by keeping medium distance to it. After that, the robot is situated in a free space and goal reaching behavior is activated up to point G. The obstacle avoidance behavior is activated when the robot senses obstacle 7 the robot avoids obstacle 7 and the behavior is changing to wall following when a medium

Figs.15-16 illustrate the levels of activation of each behavior during experiment 1 and

Obstacle 4

F

Obstacle 5

G

Obstacle 6

Obstacle 2

C

B

Obstacle 1

wall 3

Goal 1

H

Start point

Obstacle 6

Obstacle 6

Obstacle 7

H

Goal 2

Obstacle 5

Obstacle 4

distance to obstacle is measured by the robot. Finally the robot reaches the goal.

Start Point

Fig. 13. Experiment 1.

Fig. 14. Experiment 2.

experiment 2, respectively.

#### **3.4 Simulation results**

To verify the validity of the proposed scheme, some typical cases are simulated in which a robot is to move from a given current position to a desired goal position in various unknown environment. In all cases the robot is able to navigate its way toward the goal while avoiding obstacles successfully.

Fig. 12. Fusion of obstacle avoidance and goal reaching behaviors.

In the experimental work, the functionality of the proposed scheme was tested in an environment mimicking a crowded dynamic environment. The environment was designed taking into account several situations such as: simple corridor, narrow space and an area with many different shapes as obstacles (round, rectangle, trapezoidal)in away that mimics an image of office or equipped room (indoor environment).

In experiment (1) Fig.13 the robot has to reach Goal 1 for the start point placed between two walls. The robot begins to execute the behavior according to the rule base of the supervision layer depending on the current context. First, the robot follows wall 1 with maximum velocity until it senses obstacle 1, then it changes its behavior to obstacle avoidance at point A up to point B during which the robot crosses a narrow space between obstacle 1 and wall 2. The goal reaching behavior and the obstacle avoidance behavior are activated separately or fused to leave the narrow space, until point C. The robot then was encountered by obstacle 2, wall 2 and wall 3, so the emergency situation behavior was active. Next, the presence of obstacle 2 in front of the robot makes the obstacle avoidance behavior active until point D. For the route between of point D to E the robot just follows wall 2. From point E, three behaviors are activated (wall following, obstacle avoidance and goal reaching). The wall follow behavior is activated when a medium distance between the robot and obstacle 4 and 5 is established(corresponding to the last two cases in Table 5). The goal reaching behavior is activated to guide the robot to the goal. Between point F and G the robot is situated far from obstacles which in turn makes the goal reaching behavior active up to the presence of obstacle 6 at which point both goal reaching and obstacle avoidance are active. Finally, the goal reaching behavior is activated to reach the goal when it is near to the robot.

Fig. 13. Experiment 1.

300 Recent Advances in Mobile Robotics

To verify the validity of the proposed scheme, some typical cases are simulated in which a robot is to move from a given current position to a desired goal position in various unknown environment. In all cases the robot is able to navigate its way toward the goal

 **Robot** The "optimized" track

In the experimental work, the functionality of the proposed scheme was tested in an environment mimicking a crowded dynamic environment. The environment was designed taking into account several situations such as: simple corridor, narrow space and an area with many different shapes as obstacles (round, rectangle, trapezoidal)in away that mimics

In experiment (1) Fig.13 the robot has to reach Goal 1 for the start point placed between two walls. The robot begins to execute the behavior according to the rule base of the supervision layer depending on the current context. First, the robot follows wall 1 with maximum velocity until it senses obstacle 1, then it changes its behavior to obstacle avoidance at point A up to point B during which the robot crosses a narrow space between obstacle 1 and wall 2. The goal reaching behavior and the obstacle avoidance behavior are activated separately or fused to leave the narrow space, until point C. The robot then was encountered by obstacle 2, wall 2 and wall 3, so the emergency situation behavior was active. Next, the presence of obstacle 2 in front of the robot makes the obstacle avoidance behavior active until point D. For the route between of point D to E the robot just follows wall 2. From point E, three behaviors are activated (wall following, obstacle avoidance and goal reaching). The wall follow behavior is activated when a medium distance between the robot and obstacle 4 and 5 is established(corresponding to the last two cases in Table 5). The goal reaching behavior is activated to guide the robot to the goal. Between point F and G the robot is situated far from obstacles which in turn makes the goal reaching behavior active up to the presence of obstacle 6 at which point both goal reaching and obstacle avoidance are active. Finally, the goal reaching behavior

Randoom track

Fig. 12. Fusion of obstacle avoidance and goal reaching behaviors.

an image of office or equipped room (indoor environment).

is activated to reach the goal when it is near to the robot.

**Obstacle**

**3.4 Simulation results** 

while avoiding obstacles successfully.

Goal position

Fig. 14. Experiment 2.

For the experiment (2) Fig.14 the robot had to reach goal 2. It uses the same path as experiment 1 up to the point C. Then, avoiding obstacle 2 and goal reaching behavior are activated up to point D. From this point the robot follows obstacle 3 as a wall up to point E. The presence of obstacle 4 changes the behavior of the robot to obstacle avoidance. The robot avoids obstacle 4 then it follows obstacle 5 as wall by keeping medium distance to it. After that, the robot is situated in a free space and goal reaching behavior is activated up to point G. The obstacle avoidance behavior is activated when the robot senses obstacle 7 the robot avoids obstacle 7 and the behavior is changing to wall following when a medium distance to obstacle is measured by the robot. Finally the robot reaches the goal.

Figs.15-16 illustrate the levels of activation of each behavior during experiment 1 and experiment 2, respectively.

Fuzzy Logic Based Navigation of Mobile Robots 303

The aim of experiment (3) (see Fig.17) is to show the ability of the robot to manage to escape from U shape obstacle and reaching a goal between two walls. From the start point the robot is situated in a U shape. In this context three behaviors are activated (the goal reaching behavior, obstacle avoidance and wall following). The avoid obstacle behavior always guided by the activation of goal reaching behavior especially at point A and B as shown in

A

Simulation Simple

Simulation Sample

Fig. 18. Levels of activation of each behavior during Experiment 3.

A B

B

Goal

Start point

Fig.18.

Fig. 17. Expriment 3.

Activation Level

Fig. 15. Levels of activation of each behavior during Experiment 1.

Fig. 16. Levels of activation of each behavior during Expriment 2.

302 Recent Advances in Mobile Robotics

Simulation Sample

Fig. 15. Levels of activation of each behavior during Experiment 1.

**Emergency Situation**

**Obstacle Avoidance**

**Goal Reaching**

Activation Level

**Wall Following**

A B C DE F G H

0 100 200 300 400 500 600

0 100 200 300 400 500 600

0 100 200 300 400 500 600

0 100 200 300 400 500 600

D E F G H

C

Fig. 16. Levels of activation of each behavior during Expriment 2.

Simulation Sample

Activation Level

The aim of experiment (3) (see Fig.17) is to show the ability of the robot to manage to escape from U shape obstacle and reaching a goal between two walls. From the start point the robot is situated in a U shape. In this context three behaviors are activated (the goal reaching behavior, obstacle avoidance and wall following). The avoid obstacle behavior always guided by the activation of goal reaching behavior especially at point A and B as shown in Fig.18.

Fig. 17. Expriment 3.

Fig. 18. Levels of activation of each behavior during Experiment 3.

Fuzzy Logic Based Navigation of Mobile Robots 305

Simulation Sample

• 16-Mhz Mitsubishi micro-controller (16-bit), with 256 KB Flash-ROM and 20 KB RAM. • 15 infrared sensors infrared telemeters (up to 1 measurement) arranged as shown in

**Front left Front Right**

**Right Up**

**Right Down**

A B

Fig. 20. Levels of activation of each behavior during expriment 4.

**Left Up**

**Left Down**

Fig. 21. IR sensors arrangement.

Activation Level

• Embedded Pc X86

• Camera

Fig21

Experiment (4) (Fig.19) shows the ability of the robot to escape from a trap situation and searching another path for reaching the goal.

Fig. 19. Expriment 4.

After crossing the corridor, no obstacles are in front of the robot. Thus, the robot goes to the direction of the goal. At point A the robot senses the existence of obstacles around it, in the front, left and right, and then it made a round to right to escape from this trap and continues its navigation looking for another path.

In point B, the activation of goal reaching behavior and obstacle avoidance behavior is observed to take place concurrently. Here, the supervision layer fused these 2 behaviors to get the appropriate action as shown in section 3.3 Fig.12. The orientation of the robot depends on the goal position.

The Fig.20 shows the level of activation of emergency situation in the point A and the activation of goal reaching behavior and obstacle avoidance at the point B.

#### **3.5 Experimental work**

The effectiveness of the suggested navigation approach was experimentally demonstrated on a robotic platform named Pekee (Pekee™ robot is an open robotic development toolkit of Wany Robotics).

#### **3.5.1 Pekee mobile robot**

Pekee is equipped with two driving wheels with an additional supporting wheel. Its length is 40 cm and width is 25.5 cm, max speed 1 meter/second rotation 360 degree in a circle of 70 cm. The velocities of driven wheels are independently controlled by a motor drive unit. In addition the robot is endowed by,


304 Recent Advances in Mobile Robotics

Experiment (4) (Fig.19) shows the ability of the robot to escape from a trap situation and

After crossing the corridor, no obstacles are in front of the robot. Thus, the robot goes to the direction of the goal. At point A the robot senses the existence of obstacles around it, in the front, left and right, and then it made a round to right to escape from this trap and continues

In point B, the activation of goal reaching behavior and obstacle avoidance behavior is observed to take place concurrently. Here, the supervision layer fused these 2 behaviors to get the appropriate action as shown in section 3.3 Fig.12. The orientation of the robot

The Fig.20 shows the level of activation of emergency situation in the point A and the

The effectiveness of the suggested navigation approach was experimentally demonstrated on a robotic platform named Pekee (Pekee™ robot is an open robotic development toolkit of

Pekee is equipped with two driving wheels with an additional supporting wheel. Its length is 40 cm and width is 25.5 cm, max speed 1 meter/second rotation 360 degree in a circle of 70 cm. The velocities of driven wheels are independently controlled by a motor drive unit.

activation of goal reaching behavior and obstacle avoidance at the point B.

• Infrared link for communication between robots and peripherals.

• Serial infrared link for data transfer between Pekee and docking station or PC.

**Trap**

B

A

searching another path for reaching the goal.

Fig. 19. Expriment 4.

its navigation looking for another path.

depends on the goal position.

**3.5 Experimental work** 

**3.5.1 Pekee mobile robot** 

In addition the robot is endowed by,

• 2 gyro meters (pan and tilt) • 2 temperature sensors. • 1 variable frequency buzzer.

• 2 odometers (180 impulses/wheel-turn).

Wany Robotics).

Goal

Fig. 20. Levels of activation of each behavior during expriment 4.


Fig. 21. IR sensors arrangement.

Fuzzy Logic Based Navigation of Mobile Robots 307

Fig. 23. Pekee Navigates in a crowded environment.

Fig. 24. Pekee Navigates in a crowded environment.

#### **3.5.2 Pekee to PC communication**

In the experimental work, the robot and the PC are connected via TCP/IP protocol using 2 network wireless cards one on the robot and the other on the computer or using a RJ45 cable.

The PC is equipped by a 54G network card compiles with the IEEE 802.11b standard in order to communicate with other 802.11b compliant wireless devices at 11 Mbps (the robot wireless network card). The card runs at speed of up to 54Mbps and operates on the same 2.4 GHz frequency band as 802.11b WI-FI products. This frequency band is suitable in industrial, science, and medical band operation. The work space of this card is as maximum 300 m.

The PC compiles, links the source code and executes the program. Then, it transmits the frames to the robot embedded PC via TCP\IP. These frames will be transmitted to the micro controller via the OPP bus. The micro controller is responsible to execute frames and transmits order to actuators (motors, buzzer…) also it sends data about the robot sensors status, robot position, measured distances ….

#### **3.5.3 Experimental results**

In order to validate the results of simulated experiments, a navigation task has been tested in a real world as an environment similar to a small equipped room. Fig. 22-26 shows that Pekee was able to navigate from a given starting point to a target point while avoiding obstacles.

Fig. 22. Pekee Navigates in a crowded environment.

306 Recent Advances in Mobile Robotics

In the experimental work, the robot and the PC are connected via TCP/IP protocol using 2 network wireless cards one on the robot and the other on the computer or using a RJ45

The PC is equipped by a 54G network card compiles with the IEEE 802.11b standard in order to communicate with other 802.11b compliant wireless devices at 11 Mbps (the robot wireless network card). The card runs at speed of up to 54Mbps and operates on the same 2.4 GHz frequency band as 802.11b WI-FI products. This frequency band is suitable in industrial, science, and medical band operation. The work space of this card is as maximum

The PC compiles, links the source code and executes the program. Then, it transmits the frames to the robot embedded PC via TCP\IP. These frames will be transmitted to the micro controller via the OPP bus. The micro controller is responsible to execute frames and transmits order to actuators (motors, buzzer…) also it sends data about the robot sensors

In order to validate the results of simulated experiments, a navigation task has been tested in a real world as an environment similar to a small equipped room. Fig. 22-26 shows that Pekee was able to navigate from a given starting point to a target point while avoiding

**3.5.2 Pekee to PC communication** 

status, robot position, measured distances ….

Fig. 22. Pekee Navigates in a crowded environment.

**3.5.3 Experimental results** 

cable.

300 m.

obstacles.

Fig. 23. Pekee Navigates in a crowded environment.

Fig. 24. Pekee Navigates in a crowded environment.

Fuzzy Logic Based Navigation of Mobile Robots 309

process (activate) rather than processing all behavior(s) and then blending the appropriate ones, as a result time and computational resources are saved. Simulation and experimental

As an improvement to our implemented system we are planning to incorporate learning to

As a further work the algorithm is to be tested on a "robotic cane", which is a device to help the blind or visually impaired users navigate safely and quickly among obstacles and other hazards. During operation, the user will be able to push the lightweight "Robotic Cane" forward. When the "Robotic Cane's ultrasonic sensors detect an obstacle, the behavior based navigation that will be developed determines a suitable direction of motion that steers the user around it. The steering action results in a very noticeable force felt in the handle, which easily guides the user without any conscious effort on his/her part. The navigation methodology that will be followed in the robotic cane will be based on the behavior based navigation developed herein, in particular the work will use fuzzy logic based navigation scheme to steer the "Robotic Cane". Further the user is continually interacting with the robotic cane, this calls for the need to address the issue of cooperation and/or conflict

Aguirre E. & Gonzales A. (2000). Fuzzy behaviors for mobile robot navigation:design, coordination and fusion, *Int. J. of Approximate Reasoning*, Vol. 25, pp. 255-289. Althaus P. & Christensen H. I.(2002). Behavior coordination for navigation in office

AlYahmedi, A. S., El-Tahir, E., Pervez, T. (2009). Behavior based control of a robotic based

Arkin, R. C. (1987). Towards Cosmopolitan Robots: Intelligent Navigation in Extended Man-

Arkin, R. C. (1989). Motor schema-based mobile robot navigation, *Int. J. of Robotic Research*,

Arkin, R. C. & Balch, T.(1997) AuRA: Principles and Practice in Review, *Journal of* 

Brooks R. A.(1986). A Robust Layered Control System for a Mobile Robot, *IEEE Journal of* 

Brooks R. A.(1989). A Robot that Walks; Emergent Behavior from a Carefully Evolved

Ching-Chih, T., Chin-Cheng, C., Cheng-Kain, C, Yi Yu, L. (2010). Behavior-based navigation

*International Journal of Fuzzy Systems, Vol. 12, No. 1, March, 2010* , pp. 25-32. Fatmi, A., ALYahmedi, A. S., Khriji, L., Masmoudi, N(2006). A fuzzy logic based navigation

*Robotics and Automation*, Vol. 2, No. 1, (March 1986), pp. 14–23.

environment, *Proceedings of 2002 IEEE/RSJ Int. Conference on Intelligent Robots and* 

navigation aid for the blind, *Control & Applications Conference, CA2009*, July 13-July

made Environments, *PhD Thesis*, University of Massachusetts, Department of

*Experimental and Theoretical Artificial Intelligence(JETAI)*, Vol. 9, No. 2/3, pp. 175-188.

Network, *IEEE International Conference on Robotics and Automation*, Scottsdale, AZ,

using heuristic fuzzy kohonen clustering network for mobile service robots,

of a mobile robot, *World academy of science, Engineering and Technology*, issue 22,

studies were done to validate the applicability of the proposed strategy.

improve the effectiveness of the navigation approach.

resolution between the user and the robotic cane.

15, 2009, Cambridge, UK.

Vol 8, pp. 92-112.

pp. 292–296, May 1989

2006, pp. 169-174.

*Systems*, pp. 2298-2304, Switzerland, 2002

Computer and Information Science.

**4. References** 

Fig. 25. Pekee Navigates in a crowded environment.

Fig. 26. Pekee Navigates in a crowded environment.

#### **3.6 Conclusions**

A successful way of structuring the navigation task in order to deal with the problem of mobile robot navigation is demonstrated. Issues of individual behavior design and action coordination of the behaviors were addressed using fuzzy logic. The coordination technique employed in this work consists of two layers. A Layer of primitive basic behaviors, and the supervision layer which based on the context makes a decision as to which behavior(s) to process (activate) rather than processing all behavior(s) and then blending the appropriate ones, as a result time and computational resources are saved. Simulation and experimental studies were done to validate the applicability of the proposed strategy.

As an improvement to our implemented system we are planning to incorporate learning to improve the effectiveness of the navigation approach.

As a further work the algorithm is to be tested on a "robotic cane", which is a device to help the blind or visually impaired users navigate safely and quickly among obstacles and other hazards. During operation, the user will be able to push the lightweight "Robotic Cane" forward. When the "Robotic Cane's ultrasonic sensors detect an obstacle, the behavior based navigation that will be developed determines a suitable direction of motion that steers the user around it. The steering action results in a very noticeable force felt in the handle, which easily guides the user without any conscious effort on his/her part. The navigation methodology that will be followed in the robotic cane will be based on the behavior based navigation developed herein, in particular the work will use fuzzy logic based navigation scheme to steer the "Robotic Cane". Further the user is continually interacting with the robotic cane, this calls for the need to address the issue of cooperation and/or conflict resolution between the user and the robotic cane.

#### **4. References**

308 Recent Advances in Mobile Robotics

Fig. 25. Pekee Navigates in a crowded environment.

Fig. 26. Pekee Navigates in a crowded environment.

A successful way of structuring the navigation task in order to deal with the problem of mobile robot navigation is demonstrated. Issues of individual behavior design and action coordination of the behaviors were addressed using fuzzy logic. The coordination technique employed in this work consists of two layers. A Layer of primitive basic behaviors, and the supervision layer which based on the context makes a decision as to which behavior(s) to

**3.6 Conclusions** 


**15** 

*Israel* 

**Mobile Robots** 

*Tel-Aviv University* 

Eugene Kagan and Irad Ben-Gal

**Navigation of Quantum-Controlled** 

The actions of autonomous mobile robots in stochastic medium imply certain intellectual behavior, which allows fulfilling the mission in spite of the environmental uncertainty and the robot's influence on the characteristics of the medium. To provide such a behavior, the controllers of the robots are considered as probabilistic automata with decision-making and, in some cases, learning abilities. General studies in this direction began in the 1960s (Fu & Li, 1969; Tsetlin, 1973) and resulted in practical methods of on-line decision-making and

Along with the indicated studies, in recent years, the methods of mobile robot's navigation and control are considered in the framework of quantum computation (Nielsen & Chuang, 2000), which gave rise to the concept of quantum mobile robot (Benioff, 1998; Dong, et al., 2006). Such approach allowed including both an environmental influence on the robot's actions and the changes of the environment by the robot by the use of the same model, and the ability to apply the methods of quantum communication and decision-making (Levitin,

Following Benioff, *quantum robots* are "mobile systems that have a quantum computer and any other needed ancillary systems on board… Quantum robots move in and interact (locally) with environments of quantum systems" (Benioff, 1998). If, in contrast, the quantum robots interact with a non-quantum environment, then they are considered as *quantum-controlled mobile robots*. According to Perkowski, these robots are such that "their controls are quantum but sensors and effectors are classical" (Raghuvanshi, et al., 2007). In the other words, in the quantum-controlled mobile robot, the input data obtained by classical (non-quantum) sensors are processed by the use of quantum-mechanical methods,

In this chapter, we present a brief practical introduction into quantum computation and information theory and consider the methods of path planning and navigation of quantum-

We begin with a brief introduction into quantum information theory and stress the facts, which are required for the tasks of mobile robots' navigation. An application of quantummechanical methods for the mobile robot's control is based on the statistical interpretation of

**2. Quantum information theory and models of quantum computations** 

1969; Helstrom, 1976; Davies, 1978; Holevo, 2001) to the mobile robot's control.

navigation of mobile robots (Unsal, 1998; Kagan & Ben-Gal, 2008).

and the results are output to classical (non-quantum) effectors.

controlled mobile robots based on quantum decision-making.

**1. Introduction**


## **Navigation of Quantum-Controlled Mobile Robots**

Eugene Kagan and Irad Ben-Gal *Tel-Aviv University Israel* 

#### **1. Introduction**

310 Recent Advances in Mobile Robotics

Huq, R., Mann, G. K. I., Gosine, R. G.(2008). Mobile robot navigation using motor schema

Langer D., Rosenblatt J.K. & Hebert M. (1994). A Behavior-Based System For Off-Road Navigation, *IEEE Journal of Robotics and Automation* , Vol. 10, No. 6, pp. 776-782. Maes P. (1990). How to do the Right Thing, *Connection Science Journal, Special Issue on Hybrid* 

Mataric´ M. J.(1997). Behavior-Based Control: Examples from Navigation, Learning, and

Saffiotti A.(1997). The uses of fuzzy logic for autonomous robot navigation: a catalogue

Selekwa M. F., Damion D., & Collins, Jr. E. G. (2005). Implementation of Multi-valued Fuzzy

Seraji H. & Howard A.(2002). Behavior-based robot navigation on challenging terrain: A fuzzy logic approach, *IEEE Trans. Rob. Autom.* Vol. 18, No. 3, pp. 308-321 Seraji H., Howard A. & Tunstell E.(2001). Terrain-Based Navigation of Planetary Rovers: A

*Intelligence and Robotics & Automation in Space*, Canada, June 18-22, 2001. Tunstel E., Lippincott T. & Jamshidi M.(1997). Behavior Hierarchy for Autonomous Mobile

Yang S. X., Li H., Meng M. Q.-H , & Liu P. X. (2004). An Embedded Fuzzy Controller for a

Yang S. X., Moallem M., & Patel R. V. (2005). A Layered Goal-Oriented Fuzzy Motion

*and cybernetics—part b: cybernetics*, Vol. 35, no. 6, 1214-1224.

raisonn'e, *Soft Computing Research journal,* Vol. 1, No. 4, pp. 180-197.

pp. 422-436.

*Systems,* Vol. 1.

*TR-97-01*, Pittsburgh, PA, 1995.

pp., 3699-3706, April 2005

*NASA ACE Center*, Vol. 3,No. 1,pp. 37--49.

*Fuzzy Systems*, Vol. 12, No. 4, pp.436-446.

and fuzzy context dependent behavior modulation, *Applied soft computing,* 8, 2008,

Group Behavior, *Journal of Experimental and Theoretical Artificial Intelligence,* special issue on Software Architectures for Physical Agents, Vol. 9, No.2/3, pp. 323-336 Rosenblatt J. & Payton D. W.(1989). A Fine-Grained Alternative to the Subsumption

Architecture for Mobile Robot Control, *Proceedings of the IEEE/INNS International Joint Conference on Neural Networks*, Washington DC, June 1989, vol. 2, pp. 317-324. Rosenblatt J.(1995). DAMN: A Distributed Architecture for Mobile Navigation, *Ph.D.* 

*dissertation, Carnegie Mellon University Robotics Institute Technical Report CMU-RI-*

Behavior Control for Robot Navigation in Cluttered Environments, *Proceedings of the 2005 IEEE International Conference on Robotics and Automation*, Barcelona, Spain,

Fuzzy Logic Approach, *Proceeding of the 6th International Symposium on Artificial* 

Robots: Fuzzy-behavior modulation and evolution, *International Journal of Intelligent Automation and Soft Computing, Special Issue: Autonomous Control Engineering at* 

Behavior-Based Mobile Robot with Guaranteed Performance, *IEEE Transactions on* 

Planning Strategy for Mobile Robot Navigation, *IEEE transactions on systems, man,* 

The actions of autonomous mobile robots in stochastic medium imply certain intellectual behavior, which allows fulfilling the mission in spite of the environmental uncertainty and the robot's influence on the characteristics of the medium. To provide such a behavior, the controllers of the robots are considered as probabilistic automata with decision-making and, in some cases, learning abilities. General studies in this direction began in the 1960s (Fu & Li, 1969; Tsetlin, 1973) and resulted in practical methods of on-line decision-making and navigation of mobile robots (Unsal, 1998; Kagan & Ben-Gal, 2008).

Along with the indicated studies, in recent years, the methods of mobile robot's navigation and control are considered in the framework of quantum computation (Nielsen & Chuang, 2000), which gave rise to the concept of quantum mobile robot (Benioff, 1998; Dong, et al., 2006). Such approach allowed including both an environmental influence on the robot's actions and the changes of the environment by the robot by the use of the same model, and the ability to apply the methods of quantum communication and decision-making (Levitin, 1969; Helstrom, 1976; Davies, 1978; Holevo, 2001) to the mobile robot's control.

Following Benioff, *quantum robots* are "mobile systems that have a quantum computer and any other needed ancillary systems on board… Quantum robots move in and interact (locally) with environments of quantum systems" (Benioff, 1998). If, in contrast, the quantum robots interact with a non-quantum environment, then they are considered as *quantum-controlled mobile robots*. According to Perkowski, these robots are such that "their controls are quantum but sensors and effectors are classical" (Raghuvanshi, et al., 2007). In the other words, in the quantum-controlled mobile robot, the input data obtained by classical (non-quantum) sensors are processed by the use of quantum-mechanical methods, and the results are output to classical (non-quantum) effectors.

In this chapter, we present a brief practical introduction into quantum computation and information theory and consider the methods of path planning and navigation of quantumcontrolled mobile robots based on quantum decision-making.

#### **2. Quantum information theory and models of quantum computations**

We begin with a brief introduction into quantum information theory and stress the facts, which are required for the tasks of mobile robots' navigation. An application of quantummechanical methods for the mobile robot's control is based on the statistical interpretation of

Navigation of Quantum-Controlled Mobile Robots 313

An actual evolution of the quantum-mechanical system is governed by the evolution

The elementary state, which is considered in quantum information theory (Nielsen & Chuang, 2000), is called *qubit* (quantum bit) and is represented by a two-element complex vector

and 1 0, 1 ( )*<sup>T</sup>* = , which correspond to the bit values "0" and "1" known in classical information theory (Cover & Thomas, 1991). In general, vectors 0 and 1 determine the

*s ss s s* = = + . In particular, if there are defined two states

right" and "spin left", then " " 12 1 12 0 →= ⋅ + ⋅ ( ) ( ) and " " 12 1 12 0 ← = ⋅− ⋅ ( ) ( ) .

Moreover, since it holds true that 0 12" " 12" " = ⋅→ + ⋅← ( ) ( ) and

1 12" " 12" " = ⋅→ − ⋅← ( ) ( ) , the pairs of the vectors 0 and 1 , and " " → and

*i* ⎛ ⎞ <sup>−</sup> <sup>=</sup> ⎜ ⎟ ⎝ ⎠

0 *i*

, and

1000

0010

⎝ ⎠

Z

, Phase shift operator: 8

0100 I 0

⎛ ⎞ ⎜ ⎟ ⎛ ⎞ ⎜ ⎟ <sup>=</sup> <sup>=</sup> ⎜ ⎟ ⎜ ⎟ ⎝ ⎠ ⎜ ⎟

0001 0X

In general, the evolution of the qubits is governed by the use of the following operators:

, <sup>0</sup> Y

CNOT

operations, let us present the corresponding quantum gates and their pseuodocode.

The Pauli operators are the most known qubits operators that are in use in general quantum mechanics, while the other three operators are more specific for quantum information theory. According to the Kitaev-Solovey theorem (Nielsen & Chuang, 2000), an algebra ∪ = {{ 0 , 1 , CNOT, H, S } } , which consists of the qubits 0 and 1 , and CNOT, Hadamard and phase shift operators, forms a universal algebra that models any operation of the Boolean algebra % = {{0,1}, ¬, &, ∨}. Notice that the qubit operators are reversible. In fact, direct calculations show that NOT NOT X X ( (*s ss* )) =⋅⋅ = <sup>K</sup> , H H⋅⋅ = *s s* and so on. To illustrate the actions of the simplest qubit operators and their relation with classical logic

*T*

←= − , which represent the electron states "spin

1 0

0 1 ⎛ ⎞ <sup>=</sup> ⎜ ⎟ <sup>−</sup> ⎝ ⎠

S

;

1 0

⎛ ⎞ = ⎜ ⎟ ⎝ ⎠

.

;

0 *<sup>i</sup> e* π

σ

1 2 0 1 <sup>≤</sup> *s s* + ≤ . Among such vectors, two vectors are specified 0 1, 0 ( )*<sup>T</sup>* <sup>=</sup>

or state vector *s*

<sup>K</sup> . Below, we consider the

*s ss* = is represented as

operators, which are applied to the state matrix

( ) 1 2 , *<sup>T</sup>*

*s ss* = , 2 2

( ) 12 1 2 , 01 *<sup>T</sup>*

*T* → = and " " 1 2, 1 2 ( )

" " ← can be used interchangeably.



1 0 0 1 ⎛ ⎞ <sup>=</sup> ⎜ ⎟ ⎝ ⎠

H

, 0 1 X

1 0 ⎛ ⎞ <sup>=</sup> ⎜ ⎟ ⎝ ⎠

2 1 1 ⎛ ⎞ <sup>=</sup> ⎜ ⎟ <sup>−</sup> ⎝ ⎠

" " 1 2,1 2 ( )

Pauli operators: I

**2.2 Concepts of the quantum information theory** 

states and operators, which are used in quantum information theory.

states "spin up" and "spin down" of electron, i.e. 0 "" ≡ ↑ and 1 "" ≡ ↓ . Given vectors 0 1, 0 ( )*<sup>T</sup>* <sup>=</sup> and 1 0, 1 ( )*<sup>T</sup>* <sup>=</sup> , any qubit ( ) 1 2 , *<sup>T</sup>*

quantum mechanics (Ballentine, 2006); for a complete review of quantum computation and information theory, see, e.g., (Nielsen & Chuang, 2000).

#### **2.1 Basic notation and properties of quantum-mechanical systems**

In general, in the considerations of the finite quantum-mechanical systems, it is postulated (Ballentine, 2006) that the state of the quantum-mechanical system is represented by a Hermitian complex matrix σ = σ *jk* , \* σ *jk k* = σ *<sup>j</sup>* , where ( )\* <sup>⋅</sup> stands for complex conjugate, with the unit sum of the diagonal elements, tr 1 (σ ) = ; and that the observed value of the quantum-mechanical system is specified by the eigenvalues of the matrix σ . Since matrix σ is Hermitial, its eigenvalues are real numbers. If matrix σ is diagonal, then such a representation of the state is equivalent to the representation of the state of stochastic classical system, in which diagonal elements σ*jj* form a probability vector (Holevo, 2001).

Let ( ) 1 2 *s ss* <sup>=</sup> , , � … be a vector of complex variables *jj j s a ib* <sup>=</sup> <sup>+</sup> , *<sup>j</sup>* <sup>=</sup> 1, 2,… , and let \* *jj j s a ib* = − be its complex conjugate. According to the Dirac's "bra[c]ket" notation, rowvector ( ) 1 2 *s ss* = , ,… is called *bra-vector* and a column-vector ( ) \* \* 1 2 , , *<sup>T</sup> s ss* = … , where ( )*<sup>T</sup>* ⋅ stands for the transposition of the vector, is called *ket-vector*. Vector *s* � or, equivalently, a Hermitian matrix ρ = *s s* , are interpreted as a state of quantum object; vector *s* � is called *state vector* and matrix ρis called *state matrix* or *density matrix*.

Let *s ss* 1 11 12 <sup>=</sup> ( ) , , � … and *s ss* 2 21 22 <sup>=</sup> ( , , ) � … be two state vectors. The inner product ( ) ( ) \* \* \* \* 2 1 21 22 11 12 21 11 22 12 ,, ,, *<sup>T</sup> ss s s s s s s s s* = ⋅ =⋅+⋅+ … … … is called *amplitude* of the event that the object moves from the state 1*s* � to the state 2*<sup>s</sup>* � . The *probability* of this event is defined by a square of the absolute value of the amplitude 2 1 *s s* , i.e. ( ) <sup>2</sup> *Ps s s s* 2 1 21 <sup>|</sup> <sup>=</sup> , where 2 2 2 1 *ss i* =+ = + α β αβ . By definition, regarding the probability of any state *s* � , it is assumed that 0 |1 ≤ *Pss* ( ) ≤ . For example, let *s*<sup>1</sup> = (1, 0) and *s*<sup>2</sup> = (1 2,1 2 ) , where both *Ps s* ( 1 1 | 1 ) = and *Ps s* ( 2 2 | 1 ) = . However, the probability of transition from the state 1*s* � to the state 2*s* � is ( ) <sup>2</sup> *Ps s s s* 2 1 21 <sup>|</sup> = = 1 2 .

The *measurement* of the state of the object is described by an *observation operator* O , which in the finite case is determined by a squire matrix O o = *jk* . The result of the observation of the state *s* � is defined by a multiplication O o ( ) ( ) \* \* 1 2 1 2 ,, ,, *<sup>T</sup> jk s s ss ss* = ⋅⋅ … … , and the state, which is obtained after an observation is defined as *s s* ' = O . For example, assume that the measurement is conducted by the use of the operator O 1 0 0 1 ⎛ ⎞ <sup>=</sup> ⎜ ⎟ <sup>−</sup> ⎝ ⎠ . Then, the observation of

the states *s*<sup>1</sup> = (1, 0) and *s*<sup>2</sup> = (1 2,1 2 ) results in *s s* 1 1 O = 1 and *s s* 2 2 O = 0 , i.e. operator O unambiguously detects the states 1*s* � and 2*<sup>s</sup>* � . Moreover, an observation of the state 1*s* � does not change this state, i.e. <sup>O</sup> <sup>1</sup> ( ) 1, 0 *<sup>T</sup> s* = , while the observation of the state 2*s* � results in the new state O <sup>2</sup> (1 2, 1 2 ) *T s* = − . From such a property of observation, it follows that in contrast to the classical systems, the actual state of the quantum-mechanical system obtains a value, which was measured by the observer, and further evolution of the system starts from this value. In the other words, the evolution the quantum-mechanical system depends on the fact of its observation.

312 Recent Advances in Mobile Robotics

quantum mechanics (Ballentine, 2006); for a complete review of quantum computation and

In general, in the considerations of the finite quantum-mechanical systems, it is postulated (Ballentine, 2006) that the state of the quantum-mechanical system is represented by a

representation of the state is equivalent to the representation of the state of stochastic

σ

Let ( ) 1 2 *s ss* <sup>=</sup> , , � … be a vector of complex variables *jj j s a ib* <sup>=</sup> <sup>+</sup> , *<sup>j</sup>* <sup>=</sup> 1, 2,… , and let

*jj j s a ib* = − be its complex conjugate. According to the Dirac's "bra[c]ket" notation, row-

 is called *state matrix* or *density matrix*. Let *s ss* 1 11 12 <sup>=</sup> ( ) , , � … and *s ss* 2 21 22 <sup>=</sup> ( , , ) � … be two state vectors. The inner product

*ss s s s s s s s s* = ⋅ =⋅+⋅+ … … … is called *amplitude* of the event that

square of the absolute value of the amplitude 2 1 *s s* , i.e. ( ) <sup>2</sup> *Ps s s s* 2 1 21 <sup>|</sup> <sup>=</sup> , where

assumed that 0 |1 ≤ *Pss* ( ) ≤ . For example, let *s*<sup>1</sup> = (1, 0) and *s*<sup>2</sup> = (1 2,1 2 ) , where both *Ps s* ( 1 1 | 1 ) = and *Ps s* ( 2 2 | 1 ) = . However, the probability of transition from the state 1*s*

The *measurement* of the state of the object is described by an *observation operator* O , which in the finite case is determined by a squire matrix O o = *jk* . The result of the observation of the

which is obtained after an observation is defined as *s s* ' = O . For example, assume that the

the states *s*<sup>1</sup> = (1, 0) and *s*<sup>2</sup> = (1 2,1 2 ) results in *s s* 1 1 O = 1 and *s s* 2 2 O = 0 , i.e.

*T*

follows that in contrast to the classical systems, the actual state of the quantum-mechanical system obtains a value, which was measured by the observer, and further evolution of the system starts from this value. In the other words, the evolution the quantum-mechanical

� and 2*<sup>s</sup>*

= *s s* , are interpreted as a state of quantum object; vector *s*

. By definition, regarding the probability of any state *s*

1 2 1 2 ,, ,, *<sup>T</sup> jk s s ss ss* = ⋅⋅ … … , and the state,

*s* = , while the observation of the state 2*s*

1 0 0 1 ⎛ ⎞ <sup>=</sup> ⎜ ⎟ <sup>−</sup> ⎝ ⎠

*s* = − . From such a property of observation, it

σ

*<sup>j</sup>* , where ( )\* <sup>⋅</sup> stands for complex conjugate,

σ

*jj* form a probability vector (Holevo, 2001).

� . The *probability* of this event is defined by a

) = ; and that the observed value of the

σ

1 2 , , *<sup>T</sup> s ss* = … , where ( )*<sup>T</sup>* ⋅

is diagonal, then such a

� or, equivalently, a

. Then, the observation of

� . Moreover, an observation of the

� is called

� , it is

�

�

. Since matrix

information theory, see, e.g., (Nielsen & Chuang, 2000).

σ = σ

is Hermitial, its eigenvalues are real numbers. If matrix

with the unit sum of the diagonal elements, tr 1 (

classical system, in which diagonal elements

ρ

the object moves from the state 1*s*

2 1 *ss i* =+ = + α

to the state 2*s*

state *s*

state 1*s*

β

ρ

2 2

� is ( ) <sup>2</sup> *Ps s s s* 2 1 21 <sup>|</sup> = = 1 2 .

measurement is conducted by the use of the operator O

� does not change this state, i.e. <sup>O</sup> <sup>1</sup> ( ) 1, 0 *<sup>T</sup>*

operator O unambiguously detects the states 1*s*

results in the new state O <sup>2</sup> (1 2, 1 2 )

system depends on the fact of its observation.

 αβ

( ) ( ) \* \* \* \* 2 1 21 22 11 12 21 11 22 12 ,, ,, *<sup>T</sup>*

Hermitian complex matrix

σ

\*

Hermitian matrix

*state vector* and matrix

**2.1 Basic notation and properties of quantum-mechanical systems** 

 *jk* , \* σ *jk k* = σ

quantum-mechanical system is specified by the eigenvalues of the matrix

vector ( ) 1 2 *s ss* = , ,… is called *bra-vector* and a column-vector ( ) \* \*

� to the state 2*<sup>s</sup>*

� is defined by a multiplication O o ( ) ( ) \* \*

stands for the transposition of the vector, is called *ket-vector*. Vector *s*

An actual evolution of the quantum-mechanical system is governed by the evolution operators, which are applied to the state matrix σ or state vector *s* <sup>K</sup> . Below, we consider the states and operators, which are used in quantum information theory.

#### **2.2 Concepts of the quantum information theory**

The elementary state, which is considered in quantum information theory (Nielsen & Chuang, 2000), is called *qubit* (quantum bit) and is represented by a two-element complex vector ( ) 1 2 , *<sup>T</sup> s ss* = , 2 2 1 2 0 1 <sup>≤</sup> *s s* + ≤ . Among such vectors, two vectors are specified 0 1, 0 ( )*<sup>T</sup>* <sup>=</sup> and 1 0, 1 ( )*<sup>T</sup>* = , which correspond to the bit values "0" and "1" known in classical information theory (Cover & Thomas, 1991). In general, vectors 0 and 1 determine the states "spin up" and "spin down" of electron, i.e. 0 "" ≡ ↑ and 1 "" ≡ ↓ . Given vectors 0 1, 0 ( )*<sup>T</sup>* <sup>=</sup> and 1 0, 1 ( )*<sup>T</sup>* <sup>=</sup> , any qubit ( ) 1 2 , *<sup>T</sup> s ss* = is represented as ( ) 12 1 2 , 01 *<sup>T</sup> s ss s s* = = + . In particular, if there are defined two states " " 1 2,1 2 ( ) *T* → = and " " 1 2, 1 2 ( ) *T* ←= − , which represent the electron states "spin right" and "spin left", then " " 12 1 12 0 →= ⋅ + ⋅ ( ) ( ) and " " 12 1 12 0 ← = ⋅− ⋅ ( ) ( ) . Moreover, since it holds true that 0 12" " 12" " = ⋅→ + ⋅← ( ) ( ) and 1 12" " 12" " = ⋅→ − ⋅← ( ) ( ) , the pairs of the vectors 0 and 1 , and " " → and " " ← can be used interchangeably.

In general, the evolution of the qubits is governed by the use of the following operators:

Pauli operators: I 1 0 0 1 ⎛ ⎞ <sup>=</sup> ⎜ ⎟ ⎝ ⎠ , 0 1 X 1 0 ⎛ ⎞ <sup>=</sup> ⎜ ⎟ ⎝ ⎠ , <sup>0</sup> Y 0 *i i* ⎛ ⎞ <sup>−</sup> <sup>=</sup> ⎜ ⎟ ⎝ ⎠ , and 1 0 Z 0 1 ⎛ ⎞ <sup>=</sup> ⎜ ⎟ <sup>−</sup> ⎝ ⎠ ; - Hadamard operator: <sup>1</sup> 1 1 H 2 1 1 ⎛ ⎞ <sup>=</sup> ⎜ ⎟ <sup>−</sup> ⎝ ⎠ , Phase shift operator: 8 1 0 S 0 *<sup>i</sup> e* π ⎛ ⎞ = ⎜ ⎟ ⎝ ⎠


The Pauli operators are the most known qubits operators that are in use in general quantum mechanics, while the other three operators are more specific for quantum information theory. According to the Kitaev-Solovey theorem (Nielsen & Chuang, 2000), an algebra ∪ = {{ 0 , 1 , CNOT, H, S } } , which consists of the qubits 0 and 1 , and CNOT, Hadamard and phase shift operators, forms a universal algebra that models any operation of the Boolean algebra % = {{0,1}, ¬, &, ∨}. Notice that the qubit operators are reversible. In fact, direct calculations show that NOT NOT X X ( (*s ss* )) =⋅⋅ = <sup>K</sup> , H H⋅⋅ = *s s* and so on. To illustrate the actions of the simplest qubit operators and their relation with classical logic operations, let us present the corresponding quantum gates and their pseuodocode.

;

Navigation of Quantum-Controlled Mobile Robots 315

Assume that the states of the robot are described by four qubits: " " 1, 0 <sup>1</sup> ( )*<sup>T</sup>* ↑= = *<sup>s</sup>* ,

*T*

actions "*turn right*" and "*turn left*" are, respectively, conducted by the use of the Hadamard

Hadamard operator it holds true that 1 3 Η = *s s* , 3 1 Η = *s s* , 2 4 Η = *s s* , and 4 2 Η = *s s* ; and similarly, for the reverse Hadamard it holds true that 1 4

Let us consider the states and the actions of the robot in particular. If there is no influence on the environment, then the relations between the states can be represented as follows. Assume that the robot is in the state 1*s* . Then according to the above-indicated equality *sss* <sup>134</sup> = ⋅+ ⋅ (12 12 ) ( ) , and the probability that at the next time moment the robot

probability that it will be at the state 4*s* is also

*ss s s s s* <sup>+</sup> = = = = = . The similar equalities hold true for the

other remaining states 2*s* , 3*s* and 4*s* . In general, for complex amplitudes *jk s* , it follows that 1 13 3 14 4 *s ss ss* = + , 2 23 3 24 4 *s ss ss* = − , 3 31 1 32 2 *s ss ss* = + , and

*k j k j jk*

σ*s s*

4 44 \* <sup>2</sup>

after normalization results in the states (1 2 1 2 ,1 2 ( ) ( ) )

ε

non-diagonal elements of the matrices can obtain arbitrary values, let us use these elements for

= = ⎜ ⎟ ⎜ ⎟ ⎝ ⎠

*s s*

 *ss s s s s s* <sup>+</sup> = = = = . Now let us take into account an influence of the environment. Recall that the quantum state of the qubit is equivalently defined both by the state-vector *is* and by the density matrix. Let

and reverse Hadamard <sup>1</sup> 1 1

will be in the state 3*s* is ( ) { } <sup>2</sup> <sup>1</sup>

ρ

4 41 1 42 2 *s ss ss* = − ; thus ( ) { } <sup>2</sup> <sup>1</sup> | Pr | *t t*

*<sup>s</sup>* <sup>=</sup> , and let 3 31 32 ( ) , *<sup>T</sup>*

σ

*T*

. If, e.g.,

ε

ρ

the states are defined by the density matrices 1 11

specifying the relation with the environmental variable

, and

2 \* 31 31 32

31 32 32

0 *i*

( ( ) 1 2 ,1 2 1 2 ( ) )

ε

*ss s*

*s ss*

⎛ ⎞

*<sup>R</sup>* Η = *s s* and 3 2

( ) 1 , ( )*<sup>T</sup>* − =− *s ab* conventionally are not distinguished.

( ) { } <sup>2</sup> <sup>1</sup> 4 1 | Pr | 1 2 1 2 4 1

*t t*

→= = *s* and " " 1 2, 1 2 <sup>4</sup> ( )

2 1 1 *<sup>R</sup>* ⎛ ⎞ <sup>−</sup> Η = ⎜ ⎟ ⎝ ⎠

3 1 | Pr | 1 2 1 2 3 1

*ss s s s s* <sup>+</sup> = = = = = and the

*t t*

*s ss* <sup>=</sup> , 4 41 42 ( ) , *<sup>T</sup>*

⎛ ⎞ = = ⎜ ⎟ ⎝ ⎠

ε

*s ii* ′ = −

. Then, the application of the Hadamard operators Η and *<sup>R</sup>* Η to the

2 \* 41 41 42

41 42 42

*ss s*

*s ss*

⎛ ⎞

*<sup>R</sup>* Η = *s s* , where the states *sa b* = + 0 1 and

*T*

*<sup>R</sup>* Η = *s s* ,

operators. Then for the

←= = − *s* , and the

*s ss* = . Then, by definition,

, 2 22

. In particular, assume that

ε

= 1 3 , then ( ) 0.72 0.24 , 0.24 *<sup>T</sup> s ii* ′ = − and

σ*s s*

, while tr 1 (σ

,

⎛ ⎞ = = ⎜ ⎟ ⎝ ⎠

*<sup>i</sup>*) = . Since the

*T*

and

ε

" " 0, 1 <sup>2</sup> ( )*<sup>T</sup>* ↓= = *<sup>s</sup>* , " " 1 2,1 2 <sup>3</sup> ( )

1 1 1 2 1 1 ⎛ ⎞ Η = ⎜ ⎟ <sup>−</sup> ⎝ ⎠

4 1

ρ

<sup>1</sup> ( ) 1, 0 *<sup>T</sup>*

*s s*

( ) 3 3

 σ

σε

state σ <sup>3</sup> ( ) ε

σ

*<sup>s</sup>* <sup>=</sup> and 2 ( ) 0, 1 *<sup>T</sup>*

3 33 \* <sup>2</sup>

= = ⎜ ⎟ ⎜ ⎟ ⎝ ⎠

0

*i*

*si i* ′′ = − + ε

⎛ ⎞ <sup>⋅</sup> = + ⎜ ⎟ − ⋅ ⎝ ⎠

ε

*<sup>R</sup>* Η = *s s* , 2 3

The other types of the qubit gates, e.g. phase shift operator S and its derivatives, cannot be represented by the classical operators and require quantum computing devices. In such computing, it is assumed that each matrix operation is conducted in one computation step providing a power of quantum computing. The indicated dependence of quantum states on the observation process allows an implementation of such operations by the use of adaptive computation schemes. Below, we demonstrate a relation between quantum operations and evolving algebras and consider appropriate probabilistic decision-making.

#### **3. Probabilistic automata and mobile robots control**

In this section we describe a simulation example, which illustrates an implementation of qubit operators for the mobile robot's navigation. Then, based on this example, we consider the model of probabilistic control and algorithmic learning methods based on evolving algebras.

#### **3.1 Control of the mobile robot by qubit operators**

Let us consider a mobile robot, which moves on the plain, and assume that the inner states of the robots correspond to its direction on a plane. Let *S* =↑ ↓ → ← {" ", " ", " ", " "} be a set of pure or errorless states such that each state corresponds to a certain direction of the robot. The set *V step forward step backward turn left turn ri* = {" ", " ", " ", " ", " " *ght stay still* } includes the actions that are available to the robot, where the steps "*forward*" and "*backward*" are restricted by a certain fixed distance. Being at time moment *t* in the state *<sup>t</sup> s S* ∈ and choosing the action *<sup>t</sup> v V*∈ , the robot receives to its input a finite scalar value ( ) , *t tt* ε ε = *s v* , which depends on the robot's position in the environment. Notice that the elements of the set *V* form a group with multiplication acting on the set of states *S* . The steps "*forward*", "*backward*" and "*stay still*" action do not change the state of the robot but change its position relatively to the environment that changes the obtained value ε.

314 Recent Advances in Mobile Robotics

CNOT: *x x* ′ =

**else** *y*′ = *y*

FAN OUT:

C-EXCHANGE (FREDKIN GATE):

*y* : 0 =

The other types of the qubit gates, e.g. phase shift operator S and its derivatives, cannot be represented by the classical operators and require quantum computing devices. In such computing, it is assumed that each matrix operation is conducted in one computation step providing a power of quantum computing. The indicated dependence of quantum states on the observation process allows an implementation of such operations by the use of adaptive computation schemes. Below, we demonstrate a relation between quantum operations and

In this section we describe a simulation example, which illustrates an implementation of qubit operators for the mobile robot's navigation. Then, based on this example, we consider the model of probabilistic control and algorithmic learning methods based on evolving

Let us consider a mobile robot, which moves on the plain, and assume that the inner states of the robots correspond to its direction on a plane. Let *S* =↑ ↓ → ← {" ", " ", " ", " "} be a set of pure or errorless states such that each state corresponds to a certain direction of the robot. The set *V step forward step backward turn left turn ri* = {" ", " ", " ", " ", " " *ght stay still* } includes the actions that are available to the robot, where the steps "*forward*" and "*backward*" are

choosing the action *<sup>t</sup> v V*∈ , the robot receives to its input a finite scalar value ( ) , *t tt*

which depends on the robot's position in the environment. Notice that the elements of the set *V* form a group with multiplication acting on the set of states *S* . The steps "*forward*", "*backward*" and "*stay still*" action do not change the state of the robot but change its position

> ε.

restricted by a certain fixed distance. Being at time moment *t* in the state *<sup>t</sup>*

**if** 1 *x* = **then** *y*′ = *y*

CNOT with constant input

 *x x*′

 *y y*′

 *x x*

0 *x*

 *x x*′

 *y y*′

 *z z*′

*s S* ∈ and

ε ε= *s v* ,

*x x*′

 *x x*′

 *y y*′

 *z z*′

*x y*

*y x*

evolving algebras and consider appropriate probabilistic decision-making.

**3. Probabilistic automata and mobile robots control** 

relatively to the environment that changes the obtained value

**3.1 Control of the mobile robot by qubit operators** 

NOT ( *x x* ′ := ):

**else** 1 *x*′ =

CCNOT

**then** *z z* ′ = **else** *z z* ′ =

EXCHANGE:

*x y* ′ := , *y x* ′ :=

algebras.

**if** 1 *x* = **then** 0 *x*′ =

(TOFFOLI GATE): *x x* ′ = , *y y* ′ = **if** 1 *x* = **and** *y* = 1

Assume that the states of the robot are described by four qubits: " " 1, 0 <sup>1</sup> ( )*<sup>T</sup>* ↑= = *<sup>s</sup>* , " " 0, 1 <sup>2</sup> ( )*<sup>T</sup>* ↓= = *<sup>s</sup>* , " " 1 2,1 2 <sup>3</sup> ( ) *T* →= = *s* and " " 1 2, 1 2 <sup>4</sup> ( ) *T* ←= = − *s* , and the actions "*turn right*" and "*turn left*" are, respectively, conducted by the use of the Hadamard 1 1 1 2 1 1 ⎛ ⎞ Η = ⎜ ⎟ <sup>−</sup> ⎝ ⎠ and reverse Hadamard <sup>1</sup> 1 1 2 1 1 *<sup>R</sup>* ⎛ ⎞ <sup>−</sup> Η = ⎜ ⎟ ⎝ ⎠ operators. Then for the Hadamard operator it holds true that 1 3 Η = *s s* , 3 1 Η = *s s* , 2 4 Η = *s s* , and 4 2 Η = *s s* ; and similarly, for the reverse Hadamard it holds true that 1 4 *<sup>R</sup>* Η = *s s* , 4 1 *<sup>R</sup>* Η = *s s* , 2 3 *<sup>R</sup>* Η = *s s* and 3 2 *<sup>R</sup>* Η = *s s* , where the states *sa b* = + 0 1 and ( ) 1 , ( )*<sup>T</sup>* − =− *s ab* conventionally are not distinguished.

Let us consider the states and the actions of the robot in particular. If there is no influence on the environment, then the relations between the states can be represented as follows. Assume that the robot is in the state 1*s* . Then according to the above-indicated equality *sss* <sup>134</sup> = ⋅+ ⋅ (12 12 ) ( ) , and the probability that at the next time moment the robot will be in the state 3*s* is ( ) { } <sup>2</sup> <sup>1</sup> 3 1 | Pr | 1 2 1 2 3 1 *t t* ρ *ss s s s s* <sup>+</sup> = = = = = and the probability that it will be at the state 4*s* is also ( ) { } <sup>2</sup> <sup>1</sup> 4 1 | Pr | 1 2 1 2 4 1 *t t* ρ *ss s s s s* <sup>+</sup> = = = = = . The similar equalities hold true for the other remaining states 2*s* , 3*s* and 4*s* . In general, for complex amplitudes *jk s* , it follows that 1 13 3 14 4 *s ss ss* = + , 2 23 3 24 4 *s ss ss* = − , 3 31 1 32 2 *s ss ss* = + , and 4 41 1 42 2 *s ss ss* = − ; thus ( ) { } <sup>2</sup> <sup>1</sup> | Pr | *t t k j k j jk* ρ*ss s s s s s* <sup>+</sup> = = = = .

Now let us take into account an influence of the environment. Recall that the quantum state of the qubit is equivalently defined both by the state-vector *is* and by the density matrix. Let <sup>1</sup> ( ) 1, 0 *<sup>T</sup> <sup>s</sup>* <sup>=</sup> and 2 ( ) 0, 1 *<sup>T</sup> <sup>s</sup>* <sup>=</sup> , and let 3 31 32 ( ) , *<sup>T</sup> s ss* <sup>=</sup> , 4 41 42 ( ) , *<sup>T</sup> s ss* = . Then, by definition,

the states are defined by the density matrices 1 11 1 0 0 0 σ *s s* ⎛ ⎞ = = ⎜ ⎟ ⎝ ⎠ , 2 22 0 0 0 1 σ *s s* ⎛ ⎞ = = ⎜ ⎟ ⎝ ⎠ ,

2 \* 31 31 32 3 33 \* <sup>2</sup> 31 32 32 *s ss s s ss s* σ ⎛ ⎞ = = ⎜ ⎟ ⎜ ⎟ ⎝ ⎠ , and 2 \* 41 41 42 4 44 \* <sup>2</sup> 41 42 42 *s ss s s ss s* σ ⎛ ⎞ = = ⎜ ⎟ ⎜ ⎟ ⎝ ⎠ , while tr 1 (σ*<sup>i</sup>*) = . Since the

non-diagonal elements of the matrices can obtain arbitrary values, let us use these elements for specifying the relation with the environmental variable ε . In particular, assume that ( ) 3 3 0 0 *i i* ε σε σ ε ⎛ ⎞ <sup>⋅</sup> = + ⎜ ⎟ − ⋅ ⎝ ⎠ . Then, the application of the Hadamard operators Η and *<sup>R</sup>* Η to the *T*

$$\text{state } \sigma\_3(\varepsilon) \text{ after normalization results in the states } \left| s' \right\rangle = \left( 1/\sqrt{2} - i \left( 1/\sqrt{2} \right) \varepsilon, i \left( 1/\sqrt{2} \right) \varepsilon \right) \quad \text{and} \quad \left| s'' \right\rangle = \left( 1 - i \left( 1/\sqrt{2} \right) \varepsilon \right)^T.$$

$$\text{If } \left| s'' \right\rangle = \left( -i \left( 1/\sqrt{2} \right) \varepsilon, 1/\sqrt{2} + i \left( 1/\sqrt{2} \right) \varepsilon \right)^T. \text{ If, e.g., } \left| \varepsilon = 1/3 \text{, then } \left| s' \right\rangle = \left( 0.72 - 0.24i, 0.24i \right)^T \text{ and } \varepsilon = 1/3 \text{, then } \left| s'' \right\rangle = \left( 0.72 - 0.24i, 0.24i \right)^T. \text{ Then } \left| s'' \right\rangle = \left( 0.72 - 0.24i, 0.24i \right)^T. \text{ Hence } \left| s'' \right\rangle = \left( 0.72 - 0.24i, 0.24i \right)^T. \text{ Hence } \left| s'' \right\rangle = \left( 0.72 - 0.24i, 0.24i \right)^T. \text{ Hence } \left| s'' \right\rangle = \left( 0.72 - 0.24i, 0.24i \right)^T. \text{ If } s'' \text{ is a.s.}$$

Navigation of Quantum-Controlled Mobile Robots 317

( *aa a a* 0 1 ,,, … *<sup>r</sup>* ) ← ,

to change their domains according to the data flow, as it is required for the learning property. Notice that the implementation of the updates of the algebra \$ by the use of quantum gates results in the universal algebra < , which defines quantum computations. Nevertheless, similar to the algebra < , the direct implementation of the evolving algebra

Let us consider an implementation of the evolving algebra for probabilistic control of the mobile robots control (Kagan & Ben-Gal, 2008). Since during its mission the robot acts in a stochastic environment, the input variable *x X*∈ is random; thus, given characteristic *f* and *g* , both inner states *s S* ∈ and outputs *y* ∈*Y* are also random variables. Assume that the namespace *G* , in addition to elements 1 , 0 and ◊ , includes all possible realizations of the inputs, inner states and outputs, i.e. *G XYS* = {1, 0, ◊∪ ∪ ∪ } . For the realizations of

 (*s s* ′) ← ′′ , *ss G* ′, ′′∈ . Now we can formulate transition function *g* in the terms of evolving algebra. We say that the pairs (*xs x s X S* ′ ′ ′′ ′′ ,, , ) ( )∈ × are equal (in the sense of the function *g* ) if *g* : , (*xs s* ′ ′) � and *g* : , (*xs s* ′′ ′′) � . Then, since there are at most *m* distinguished realizations of the inner states and there may exist the pairs from *X S*× , such that the map *g* is not defined, the space *X S*× is partitioned into at most 1 *m* + equivalence classes. Denote such equivalence classes by *<sup>g</sup> A* . Then, the transition function *g* is defined as a guarded update if ( ) , *<sup>g</sup>*

*s s* ← , which is checked for each pair (*xs X S* , )∈ × , and if the appropriate class *<sup>g</sup> A*

The presented algorithmic model allows simulations of quantum-control of the mobile robot and its navigation on the basis of the qubits model of states. Below, we consider an example

Let us start with a simulation example. The simulation follows an idea of the experiment of checking a spin of elementary particle by three Stern-Gerlach apparatus, which are defined

As above, let the inner states of the robot be specified by four qubits " " 1, 0 <sup>1</sup> ( )*<sup>T</sup>* ↑= = *<sup>s</sup>* ,

addition, assume that there are two types of detectors defined by the above-defined Pauli operators X and Z , so that *s s* 3 3 X 1 = + , *s s* 4 4 X 1 = − , *s s* 1 1 Z 1 = + and *s s* 2 2 Z 1 = − .

probabilities ( ) <sup>1</sup> *Ps s* 3| and ( ) <sup>1</sup> *Ps s* 4| , the robot chooses the left or right trajectory and arrives

*s* and the robot obtains a new state <sup>1</sup>

*T*

→= = *s* and " " 1 2, 1 2 <sup>4</sup> ( )

**4. Navigation of quantum-controlled mobile robot along predefined path** 

( *aa a a* 0 1 ,,, … *<sup>r</sup>* ) ← , which specifies the value to the basic function,

is a term. Thus, evolving algebra \$ permits its functions

<sup>1</sup> (*x*)∈{1, 0, ◊} , and for the inner states we define the terms

*<sup>j</sup> xs A* ∈

*T*

←= = − *s* . In

*s* . According to the maximum of

*s* and arrives to the first detector Z . Detector Z



where 0 1 ,,,, *<sup>r</sup> aa aaG* … ∈ and

inputs, we define the terms

<sup>2</sup> (*s*)∈ ◊ {1, 0, } and the local updates

φ

ψ

then ( ) *<sup>j</sup>* φ

is not found, then

checks the state <sup>0</sup>

ϕ

ψ

= 1 then

ψ

\$ is possible only in particular cases of computable operators.

ψ

(*s*) ← ◊ is specified.

of such simulations with probabilistic decision-making.

by a sequence of certain quantum operators (Albert, 1994).

" " 0, 1 <sup>2</sup> ( )*<sup>T</sup>* ↓= = *<sup>s</sup>* , " " 1 2,1 2 <sup>3</sup> ( )

The robot starts with a random initial state <sup>0</sup>

ϕ

ϕ

( ) 0.24 , 0.72 0.24 *<sup>T</sup> si i* ′′ =− + instead of the states 1 ( ) 1, 0 *<sup>T</sup> <sup>s</sup>* <sup>=</sup> and 2 ( ) 0, 1 *<sup>T</sup> s* = , which are obtained in the errorless case. The transition probabilities are ρ ρ (*ss ss* 1 2 | | 0.76 ′ ′′ ) = = ( ) and ρ ρ (*ss ss* 2 1 | | 0.24 ′ ′′ ) = = ( ) . The presented method of control is illustrated by the example, shown in Fig. 1. In the simulation (Rybalov et al., 2010), the robot was programmed to follow the trajectory by the use of a compass sensor, and its turns were controlled by Hadamard and reverse Hadamard operators.

Fig. 1. From left to right: the planned trajectory with corresponding Hadamard and reverse Hadamard operator; the resulting trajectory with the directions of turns; and the mobile robot following the trajectory by the use of the compass sensor.

The presented approach to control the robot is a particular case of the dynamics of open quantum dynamical system (Holevo, 2001). In general, the dynamics of such systems is determined by transitions *s s* → U , where U is an appropriate Hermitian operator.

#### **3.2 Probabilistic model and algorithmic learning**

Let us take into account the observation process and present a brief description of the algorithmic model of the robot's control system with variable evolution structure. As it is usual for automata models, let *X* be a set of input values, *Y* be a set of output values, and *S* be a set of inner states. The automaton is defined by two characteristic functions *fXS Y* : × → and *gX S S* : × → , such that discrete time dynamics is defined as ( , ) *t tt y fx s* = and ( ) <sup>1</sup> , *t tt s gx s* <sup>+</sup> = , where *<sup>t</sup> x X*∈ , *<sup>t</sup> y* ∈*Y* , <sup>1</sup> , *t t ss S* <sup>+</sup> ∈ , *t* = 0, 1, 2,… In the case of *learning automata*, the probability distributions are defined over the sets *X* , *Y* and *S* , and functions *f* and *g* act on such distributions (Fu & Li, 1969; Tsetlin, 1973), while the metrical or topological structures of the sets are constant. By the other approaches, the *program structure learning* is specified by a convergence to the appropriate transition function *g* , or by the choice of a metric or topology over the set *S* , called *data structure learning*.

Algorithmically, the variable data structure is defined as follows (Gurevich, 1991). Let *G* be a global namespace with three distinguished elements 1 " = *true"*, 0 " = *false"* and ◊ = "*undef"*. A map : *<sup>r</sup>* ϕ *G G* → is called basic function of arity *r* , while a basic function of arity 0 *r* = is considered as a distinguished element, and basic functions : 1, 0, { } *<sup>r</sup>* ψ *G* → ◊ are considered as terms. The evolving algebra \$ = {*G*,ϕ <sup>0</sup> ,ϕ<sup>1</sup> ,…} is defined by the following updates:

316 Recent Advances in Mobile Robotics

 (*ss ss* 2 1 | | 0.24 ′ ′′ ) = = ( ) . The presented method of control is illustrated by the example, shown in Fig. 1. In the simulation (Rybalov et al., 2010), the robot was programmed to follow the trajectory by the use of a compass sensor, and its turns were controlled by

Fig. 1. From left to right: the planned trajectory with corresponding Hadamard and reverse Hadamard operator; the resulting trajectory with the directions of turns; and the mobile

The presented approach to control the robot is a particular case of the dynamics of open quantum dynamical system (Holevo, 2001). In general, the dynamics of such systems is determined by transitions *s s* → U , where U is an appropriate Hermitian operator.

Let us take into account the observation process and present a brief description of the algorithmic model of the robot's control system with variable evolution structure. As it is usual for automata models, let *X* be a set of input values, *Y* be a set of output values, and *S* be a set of inner states. The automaton is defined by two characteristic functions *fXS Y* : × → and *gX S S* : × → , such that discrete time dynamics is defined as

case of *learning automata*, the probability distributions are defined over the sets *X* , *Y* and *S* , and functions *f* and *g* act on such distributions (Fu & Li, 1969; Tsetlin, 1973), while the metrical or topological structures of the sets are constant. By the other approaches, the *program structure learning* is specified by a convergence to the appropriate transition function *g* , or by the choice of a metric or topology over the set *S* , called *data structure learning*. Algorithmically, the variable data structure is defined as follows (Gurevich, 1991). Let *G* be a global namespace with three distinguished elements 1 " = *true"*, 0 " = *false"* and

arity 0 *r* = is considered as a distinguished element, and basic functions : 1, 0, { } *<sup>r</sup>*

*s gx s* <sup>+</sup> = , where *<sup>t</sup> x X*∈ , *<sup>t</sup> y* ∈*Y* , <sup>1</sup> ,

*<sup>s</sup>* <sup>=</sup> and 2 ( ) 0, 1 *<sup>T</sup>*

 ρ

ρ

*t t*

*G G* → is called basic function of arity *r* , while a basic function of

ϕ <sup>0</sup> ,ϕ *ss S* <sup>+</sup> ∈ , *t* = 0, 1, 2,… In the

ψ

<sup>1</sup> ,…} is defined by the following

*G* → ◊

*s* = , which are

(*ss ss* 1 2 | | 0.76 ′ ′′ ) = = ( ) and

( ) 0.24 , 0.72 0.24 *<sup>T</sup> si i* ′′ =− + instead of the states 1 ( ) 1, 0 *<sup>T</sup>*

obtained in the errorless case. The transition probabilities are

Hadamard and reverse Hadamard operators.

*<sup>R</sup>* H

H

**3.2 Probabilistic model and algorithmic learning** 

robot following the trajectory by the use of the compass sensor.

H

*<sup>R</sup>* H

*<sup>R</sup>* H H

H *<sup>R</sup>* H

( , ) *t tt y fx s* = and ( ) <sup>1</sup> , *t tt*

◊ = "*undef"*. A map : *<sup>r</sup>*

updates:

ϕ

are considered as terms. The evolving algebra \$ = {*G*,

ρ

 

<sup>H</sup> <sup>H</sup>

*<sup>R</sup>* H

*<sup>R</sup>* H

 ρ


where 0 1 ,,,, *<sup>r</sup> aa aaG* … ∈ and ψ is a term. Thus, evolving algebra \$ permits its functions to change their domains according to the data flow, as it is required for the learning property. Notice that the implementation of the updates of the algebra \$ by the use of quantum gates results in the universal algebra < , which defines quantum computations. Nevertheless, similar to the algebra < , the direct implementation of the evolving algebra \$ is possible only in particular cases of computable operators.

Let us consider an implementation of the evolving algebra for probabilistic control of the mobile robots control (Kagan & Ben-Gal, 2008). Since during its mission the robot acts in a stochastic environment, the input variable *x X*∈ is random; thus, given characteristic *f* and *g* , both inner states *s S* ∈ and outputs *y* ∈*Y* are also random variables. Assume that the namespace *G* , in addition to elements 1 , 0 and ◊ , includes all possible realizations of the inputs, inner states and outputs, i.e. *G XYS* = {1, 0, ◊∪ ∪ ∪ } . For the realizations of inputs, we define the terms ψ <sup>1</sup> (*x*)∈{1, 0, ◊} , and for the inner states we define the terms ψ <sup>2</sup> (*s*)∈ ◊ {1, 0, } and the local updates ϕ(*s s* ′) ← ′′ , *ss G* ′, ′′∈ .

Now we can formulate transition function *g* in the terms of evolving algebra. We say that the pairs (*xs x s X S* ′ ′ ′′ ′′ ,, , ) ( )∈ × are equal (in the sense of the function *g* ) if *g* : , (*xs s* ′ ′) � and *g* : , (*xs s* ′′ ′′) � . Then, since there are at most *m* distinguished realizations of the inner states and there may exist the pairs from *X S*× , such that the map *g* is not defined, the space *X S*× is partitioned into at most 1 *m* + equivalence classes. Denote such equivalence classes by *<sup>g</sup> A* . Then, the transition function *g* is defined as a guarded update if ( ) , *<sup>g</sup> <sup>j</sup> xs A* ∈ then ( ) *<sup>j</sup>* φ *s s* ← , which is checked for each pair (*xs X S* , )∈ × , and if the appropriate class *<sup>g</sup> A* is not found, then φ(*s*) ← ◊ is specified.

The presented algorithmic model allows simulations of quantum-control of the mobile robot and its navigation on the basis of the qubits model of states. Below, we consider an example of such simulations with probabilistic decision-making.

#### **4. Navigation of quantum-controlled mobile robot along predefined path**

Let us start with a simulation example. The simulation follows an idea of the experiment of checking a spin of elementary particle by three Stern-Gerlach apparatus, which are defined by a sequence of certain quantum operators (Albert, 1994).

As above, let the inner states of the robot be specified by four qubits " " 1, 0 <sup>1</sup> ( )*<sup>T</sup>* ↑= = *<sup>s</sup>* , " " 0, 1 <sup>2</sup> ( )*<sup>T</sup>* ↓= = *<sup>s</sup>* , " " 1 2,1 2 <sup>3</sup> ( ) *T* →= = *s* and " " 1 2, 1 2 <sup>4</sup> ( ) *T* ←= = − *s* . In addition, assume that there are two types of detectors defined by the above-defined Pauli operators X and Z , so that *s s* 3 3 X 1 = + , *s s* 4 4 X 1 = − , *s s* 1 1 Z 1 = + and *s s* 2 2 Z 1 = − . The robot starts with a random initial state <sup>0</sup> *s* and arrives to the first detector Z . Detector Z checks the state <sup>0</sup> *s* and the robot obtains a new state <sup>1</sup> *s* . According to the maximum of probabilities ( ) <sup>1</sup> *Ps s* 3| and ( ) <sup>1</sup> *Ps s* 4| , the robot chooses the left or right trajectory and arrives

Navigation of Quantum-Controlled Mobile Robots 319

( ) ( ) ( ) () , *<sup>f</sup>* , *xs A y p y <sup>p</sup> x s* <sup>∈</sup> <sup>=</sup> ∑ of the probabilities of the pairs (*xs X S* , )∈ <sup>×</sup> . Similarly, the

probability *p*( )*s* of the inner state *s S* ∈ of the system is defined by the use of equivalence

Recall that, by definition, the equivalence classes ( ) *<sup>f</sup> A y* , *y* ∈*Y* , and *<sup>g</sup> A* , ( ) *<sup>g</sup> A s* , form

Ornstein distance between the partitions (Ornstein, 1974). Then, for the finite time case,

α α

defined by these partitions. Thus the goal of the robots navigation is to find a process for

minimum over the considered time interval. Below we will consider general informational

The section presents information theoretic methods for quantum inspired decision-making and general path-planning algorithms. We start with a motivating example of informational decision-making, then we consider the logic of quantum mechanics and informational distance between the partitions of the events space. Finally, we present the navigation algorithms, which are based on the representation of the states' evolution by the use of

An application of informational criteria for decision-making and path planning of the quantum-controlled mobile robots is motivated by the criteria of classical information theory (Cover & Thomas, 1991). Recall that in the classical case, an informational distance between the probability vectors *pp p* <sup>=</sup> ( <sup>1</sup> , , *<sup>n</sup>* ) � … and *qq q* <sup>=</sup> ( <sup>1</sup> , , *<sup>n</sup>* ) � … , 1 <sup>1</sup> *<sup>n</sup>*

*<sup>j</sup> <sup>j</sup> <sup>q</sup>* <sup>=</sup> ∑ <sup>=</sup> , 0 *<sup>j</sup> <sup>p</sup>* <sup>≥</sup> , 0 *<sup>j</sup> <sup>q</sup>* <sup>&</sup>gt; , *j n* <sup>=</sup> 1, 2, , … , is specified by the *relative Shannon entropy* or

where by convention it is assumed that 0log 0 0 <sup>=</sup> . The distance *KL*(*p q* || ) � � satisfies both *KL p q* ( || 0 ) <sup>≥</sup> � � and *KL p q* ( || 0 ) <sup>=</sup> � � if and only if *j j p q* <sup>=</sup> for all *j n* <sup>=</sup> 1, 2, , … , and, in general

*j jj j j j j j jj KL p q p pq p p p q* <sup>=</sup> <sup>=</sup> <sup>=</sup> <sup>=</sup> ∑ ∑∑ <sup>=</sup> <sup>−</sup> � � ,

*Kullback-Leibler distance* ( ) ( ) 1 11 || log log log *n nn*

**5.1 Decision-making by the use of quantum-mechanical information measure** 

⎛ ⎞ ⎜ ⎟ = − ⎝ ⎠ <sup>∑</sup> . Since the structure of the partitions

*<sup>t</sup>* is constant, the distance represents the relation between probability measures

of the space *X S*× . Then, the relation between the dynamics of inner

α α α

, then empty sets are added to *<sup>g</sup>*

α

*Orn t <sup>t</sup> d* α

 and *<sup>g</sup>* α

 , and if *g f* α > α

> *t* and *<sup>g</sup>* α

0 0 , *T T g f*

 α− − ⎛ ⎞ ⎜ ⎟ ⎝ ⎠ reaches its

. For example, let

α, being

*<sup>t</sup>* , is defined as

*<sup>j</sup> <sup>j</sup> <sup>p</sup>* <sup>=</sup> ∑ <sup>=</sup> ,

, then *<sup>f</sup>*

α

class ( ) ( ) *<sup>g</sup>* <sup>1</sup> *As g s* <sup>−</sup> <sup>=</sup> as a sum ( ) ( ) ( ) , ( ) *<sup>g</sup>* , *xs A s <sup>p</sup> <sup>s</sup> <sup>p</sup> x s* <sup>∈</sup> <sup>=</sup> ∑ .

states is determined by the relation between the partitions *<sup>f</sup>*

*t T* = 0, 1, 2, , 1 … − , the Ornstein distance between partitions *<sup>f</sup>*

{} {} ( ) ( ) ( ) 1 1 <sup>1</sup> 0 0 0

*T d*

, 11 , *T T g f <sup>T</sup> g f Orn t <sup>t</sup> Orn t t <sup>t</sup> d*

− − <sup>−</sup>

<sup>=</sup> <sup>=</sup> ∑ <sup>−</sup> , where max , { } *g f <sup>k</sup>* <sup>=</sup>

α < α

=

governing the inner states *s S* <sup>∈</sup> such that the distance {} {} 1 1

algorithms of local search, which can be implemented for such a task.

**5. Information theoretic decision-making and path planning** 

partitions *<sup>f</sup>*

α α

> α

*t* and *<sup>g</sup>* α

partitions.

<sup>1</sup> <sup>1</sup> *<sup>n</sup>*

*f* α

α

 and *<sup>g</sup>* α

α

( ) ( ) ( ) <sup>1</sup> <sup>0</sup> , *g f <sup>k</sup> g f Orn i i <sup>i</sup> d*

*pA pA* <sup>−</sup>

is completed by empty sets, while if *g f*

to the second detector X . Then, after the similar actions and obtaining the state <sup>2</sup> *s* , the robot continues to the third detector Z , which checks the robot's state and results in the state <sup>3</sup> *s* . The fragment of the experiment with the robot following its path is shown in Fig. 2.

Fig. 2. The mobile robot follows a path (from right to left) with three simulated detectors using touch sensor (" Z detector") and light sensors (" X detector").

Let us consider the actions of the robot in particular. As indicated above, the robot starts with an initial state ( ) 0 00 1 2 , *<sup>T</sup> s ss* = , where [ ] 0 0 1 2 *s s*, 0, 1 ∈ are random values such that () () 2 2 0 0 1 2 0 1 <+≤ *s s* . Then the first detector Z results in the state 1 0 *s s* = Z and the decision-making is conducted regarding the further left or right trajectory, which is based on the probabilities ( ) <sup>2</sup> 1 1 *Ps s s s* 3 3 <sup>|</sup> <sup>=</sup> and ( ) <sup>2</sup> 1 1 *Ps s s s* 4 4 <sup>|</sup> <sup>=</sup> . If ( ) ( ) 1 1 *Ps s Ps s* 3 4 | | <sup>&</sup>gt; , then the robot turns left, and if ( ) ( ) 1 1 *Ps s Ps s* 3 4 | | < , then the robot turns right (the ties are broken randomly). Following the chosen trajectory, the robot arrives to the second detector X . The check with this detector results in the state 2 1 *s s* = X , and the decision regarding the further trajectory is obtained on the basis of the probabilities ( ) <sup>2</sup> 2 2 *Ps s s s* 1 1 <sup>|</sup> <sup>=</sup> and ( ) <sup>2</sup> 2 2 *Ps s s s* 2 2 <sup>|</sup> <sup>=</sup> . Similar to the above, if ( ) ( ) 2 2 *Ps s Ps s* 1 2 | | <sup>&</sup>gt; , then the robot turns left, and if ( ) ( ) 2 2 *Ps s Ps s* 1 2 | | < , then the robot turns right (the ties are broken randomly). The third check is again conducted by the Z detector, which results in the state 3 2 *s s* = Z , and the decision-making is conducted by the same manner as for the state <sup>1</sup> *s* .

Now let us present a general description of the process (Kagan & Ben-Gal, 2008), that implements the above-indicated equivalence classes *<sup>f</sup> A* and *<sup>g</sup> A* . As indicated above, the evolution of the quantum-mechanical system with observations does not depend on the previous states and starts from the value of the state, which is obtained by the measurement. Thus, the outputs *y* ∈*Y* of the system are specified by a Markov process, which is controlled by input states *x X*∈ and inner states *s S* ∈ . Then the probability () () ( ) *<sup>f</sup> p y pA y* = of the equivalence class ( ) ( ) *<sup>f</sup>* <sup>1</sup> *A yfy* <sup>−</sup> = is defined as a sum 318 Recent Advances in Mobile Robotics

continues to the third detector Z , which checks the robot's state and results in the state <sup>3</sup>

Fig. 2. The mobile robot follows a path (from right to left) with three simulated detectors

*s ss* = , where [ ] 0 0

1 2 0 1 <+≤ *s s* . Then the first detector Z results in the state 1 0

Let us consider the actions of the robot in particular. As indicated above, the robot starts

decision-making is conducted regarding the further left or right trajectory, which is based on the probabilities ( ) <sup>2</sup> 1 1 *Ps s s s* 3 3 <sup>|</sup> <sup>=</sup> and ( ) <sup>2</sup> 1 1 *Ps s s s* 4 4 <sup>|</sup> <sup>=</sup> . If ( ) ( ) 1 1 *Ps s Ps s* 3 4 | | <sup>&</sup>gt; ,

then the robot turns left, and if ( ) ( ) 1 1 *Ps s Ps s* 3 4 | | < , then the robot turns right (the ties are broken randomly). Following the chosen trajectory, the robot arrives to the second detector

the further trajectory is obtained on the basis of the probabilities ( ) <sup>2</sup> 2 2 *Ps s s s* 1 1 <sup>|</sup> <sup>=</sup> and

( ) <sup>2</sup> 2 2 *Ps s s s* 2 2 <sup>|</sup> <sup>=</sup> . Similar to the above, if ( ) ( ) 2 2 *Ps s Ps s* 1 2 | | <sup>&</sup>gt; , then the robot turns left,

and if ( ) ( ) 2 2 *Ps s Ps s* 1 2 | | < , then the robot turns right (the ties are broken randomly). The

Now let us present a general description of the process (Kagan & Ben-Gal, 2008), that implements the above-indicated equivalence classes *<sup>f</sup> A* and *<sup>g</sup> A* . As indicated above, the evolution of the quantum-mechanical system with observations does not depend on the previous states and starts from the value of the state, which is obtained by the measurement. Thus, the outputs *y* ∈*Y* of the system are specified by a Markov process, which is controlled by input states *x X*∈ and inner states *s S* ∈ . Then the probability () () ( ) *<sup>f</sup> p y pA y* = of the equivalence class ( ) ( ) *<sup>f</sup>* <sup>1</sup> *A yfy* <sup>−</sup> = is defined as a sum

third check is again conducted by the Z detector, which results in the state 3 2

the decision-making is conducted by the same manner as for the state <sup>1</sup>

1 2 *s s*, 0, 1 ∈ are random values such that

*s s* = X , and the decision regarding

*s* .

*s s* = Z and the

*s s* = Z , and

*s* , the robot

*s* .

to the second detector X . Then, after the similar actions and obtaining the state <sup>2</sup>

The fragment of the experiment with the robot following its path is shown in Fig. 2.

with an initial state ( ) 0 00

() () 2 2 0 0

using touch sensor (" Z detector") and light sensors (" X detector").

1 2 , *<sup>T</sup>*

X . The check with this detector results in the state 2 1

( ) ( ) ( ) () , *<sup>f</sup>* , *xs A y p y <sup>p</sup> x s* <sup>∈</sup> <sup>=</sup> ∑ of the probabilities of the pairs (*xs X S* , )∈ <sup>×</sup> . Similarly, the probability *p*( )*s* of the inner state *s S* ∈ of the system is defined by the use of equivalence class ( ) ( ) *<sup>g</sup>* <sup>1</sup> *As g s* <sup>−</sup> <sup>=</sup> as a sum ( ) ( ) ( ) , ( ) *<sup>g</sup>* , *xs A s <sup>p</sup> <sup>s</sup> <sup>p</sup> x s* <sup>∈</sup> <sup>=</sup> ∑ .

Recall that, by definition, the equivalence classes ( ) *<sup>f</sup> A y* , *y* ∈*Y* , and *<sup>g</sup> A* , ( ) *<sup>g</sup> A s* , form partitions *<sup>f</sup>* α and *<sup>g</sup>* α of the space *X S*× . Then, the relation between the dynamics of inner states is determined by the relation between the partitions *<sup>f</sup>* α and *<sup>g</sup>* α . For example, let ( ) ( ) ( ) <sup>1</sup> <sup>0</sup> , *g f <sup>k</sup> g f Orn i i <sup>i</sup> d* α α *pA pA* <sup>−</sup> <sup>=</sup> <sup>=</sup> ∑ <sup>−</sup> , where max , { } *g f <sup>k</sup>* <sup>=</sup> α α , and if *g f* α > α , then *<sup>f</sup>* α is completed by empty sets, while if *g f* α < α , then empty sets are added to *<sup>g</sup>* α , being Ornstein distance between the partitions (Ornstein, 1974). Then, for the finite time case, *t T* = 0, 1, 2, , 1 … − , the Ornstein distance between partitions *<sup>f</sup>* α*t* and *<sup>g</sup>* α*<sup>t</sup>* , is defined as {} {} ( ) ( ) ( ) 1 1 <sup>1</sup> 0 0 0 , 11 , *T T g f <sup>T</sup> g f Orn t <sup>t</sup> Orn t t <sup>t</sup> d* α α *T d* α α − − <sup>−</sup> = ⎛ ⎞ ⎜ ⎟ = − ⎝ ⎠ <sup>∑</sup> . Since the structure of the partitions *f* α*t* and *<sup>g</sup>* α*<sup>t</sup>* is constant, the distance represents the relation between probability measures defined by these partitions. Thus the goal of the robots navigation is to find a process for governing the inner states *s S* <sup>∈</sup> such that the distance {} {} 1 1 0 0 , *T T g f Orn t <sup>t</sup> d* α α − − ⎛ ⎞ ⎜ ⎟ ⎝ ⎠ reaches its minimum over the considered time interval. Below we will consider general informational algorithms of local search, which can be implemented for such a task.

#### **5. Information theoretic decision-making and path planning**

The section presents information theoretic methods for quantum inspired decision-making and general path-planning algorithms. We start with a motivating example of informational decision-making, then we consider the logic of quantum mechanics and informational distance between the partitions of the events space. Finally, we present the navigation algorithms, which are based on the representation of the states' evolution by the use of partitions.

#### **5.1 Decision-making by the use of quantum-mechanical information measure**

An application of informational criteria for decision-making and path planning of the quantum-controlled mobile robots is motivated by the criteria of classical information theory (Cover & Thomas, 1991). Recall that in the classical case, an informational distance between the probability vectors *pp p* <sup>=</sup> ( <sup>1</sup> , , *<sup>n</sup>* ) � … and *qq q* <sup>=</sup> ( <sup>1</sup> , , *<sup>n</sup>* ) � … , 1 <sup>1</sup> *<sup>n</sup> <sup>j</sup> <sup>j</sup> <sup>p</sup>* <sup>=</sup> ∑ <sup>=</sup> , <sup>1</sup> <sup>1</sup> *<sup>n</sup> <sup>j</sup> <sup>j</sup> <sup>q</sup>* <sup>=</sup> ∑ <sup>=</sup> , 0 *<sup>j</sup> <sup>p</sup>* <sup>≥</sup> , 0 *<sup>j</sup> <sup>q</sup>* <sup>&</sup>gt; , *j n* <sup>=</sup> 1, 2, , … , is specified by the *relative Shannon entropy* or *Kullback-Leibler distance* ( ) ( ) 1 11 || log log log *n nn j jj j j j j j jj KL p q p pq p p p q* <sup>=</sup> <sup>=</sup> <sup>=</sup> <sup>=</sup> ∑ ∑∑ <sup>=</sup> <sup>−</sup> � � , where by convention it is assumed that 0log 0 0 <sup>=</sup> . The distance *KL*(*p q* || ) � � satisfies both *KL p q* ( || 0 ) <sup>≥</sup> � � and *KL p q* ( || 0 ) <sup>=</sup> � � if and only if *j j p q* <sup>=</sup> for all *j n* <sup>=</sup> 1, 2, , … , and, in general

Navigation of Quantum-Controlled Mobile Robots 321

Fig. 3. The mobile robot with ultra-sonic sensor acting in the stochastic environment: after

Now let us consider the logic of quantum mechanics and the structure of quantum events over which the informational algorithms act. Such a formal scheme of quantum events, which is called *quantum logic* (Cohen, 1989), was introduced by Birkhoff and von Neumann (Birkhoff & Neumann, 1936) as an attempt of to find an axiomatic description of quantum

A quantum logic is a lattice Λ of events *A* , which contains the smallest element ∅ , the greatest element *I* , relation ⊂ , unary operation ′, and binary operations ∪ and ∩ . It is assumed that for the events *A*∈ Λ the following usual set properties hold true: a) For any element *A*∈ Λ there exist an event *A*′∈ Λ such that ( ) *A A* ′ ′ = ; b) *A A* ∩ ′ = ∅ and *AAI* ∪ =′ ; c) For any pair of events *A B*, ∈ Λ , *A B* ⊂ implies *B A* ′ ⊂ ′ , and *A B* ⊂ implies *BA BA* =∪ ∩ ( ′) ; d) For any countable sequence 1 2 *A A*, ,… of events from Λ their union is in Λ , i.e., ( ) ∪ ∈Λ *j j A* . Notice that is not required that the events *A*∈ Λ are subsets of the same set. That leads to the fact that under the requirements of quantum logic the distributive rule may not hold and, in general, *B AA BA BA* ∩ ∪ ≥∩ ∪∩ ( ′) ( ) ( ′) . In contrast, in the probabilistic scheme, it is postulated that the events are the subsets of the set of

In quantum logic Λ , events *A*∈ Λ and *B*∈ Λ are called *orthogonal*, and denoted by *A B* ⊥ ,

μ

(*I*) = 1 and for any ∨-orthogonal system

= {*A A* <sup>1</sup> , , … *<sup>n</sup>*} , of events *Aj* ∈Λ , is called ∨-*orthogonal system* if

μ

α and β

μ

α

over the quantum logic Λ is defined as a

α

 if ( <sup>1</sup> ) 1 *<sup>n</sup>* μ

: 0, 1 Λ →[ ] , an ∨-orthogonal

∪ = *j j* <sup>=</sup> *A* , *Aj* ∈

relatively to partition

α

are partitions of quantum

= {*A A* <sup>1</sup> , , … *<sup>n</sup>*} it

is defined as

α

collision, the obstacle randomly changes its location in the environment.

**5.2 Logic of quantum mechanics and informational distance** 

elementary outcomes, so the distribution rule is satisfied.

α

*n n j j j j*

μ

 μ<sup>=</sup> *A A* <sup>=</sup> ∪ = ∑ . Given a state

is a *partition* of the logic Λ with respect to the state

Following classical ergodic theory (Rokhlin, 1967), *entropy* of the partition

<sup>∈</sup> = −∑ , and *conditional entropy* of partition

∈ ∈ = −∑ ∑ , where

 μ

logic Λ (Yuan, 2005; Zhao & Ma, 2007). In addition, similarly to the ergodic theory (Rokhlin,

( 1 1 ) *<sup>k</sup>* ∪ ⊥ *j j* <sup>=</sup> *A Ak*<sup>+</sup> , *k n* = 1, 2, , 1 … − . The state

holds true that ( <sup>1</sup> ) ( ) <sup>1</sup>

μμ

is ( | ) ( , lo ) g ( | ) *B A H*

 *AB AB* β

 α μ

: 0, 1 Λ →[ ] such that

( ) ( )log ( ) *<sup>A</sup> H AA*

α μ

mechanics.

if *A B* ⊂ ′ , and a finite set

map μ

system

μ

β

α

(Yuan, 2005).

μ

α β

α

*KL* (*p q* || || ) <sup>≠</sup> *KL*(*q p*) �� �� . Vectors *<sup>p</sup>* � and *<sup>q</sup>* � represent the states of the stochastic system, and distance *KL*( ) *p q* || � � characterizes the information-theoretic difference between these states.

In contrast, in quantum-mechanical systems, the state is represented by the above-presented Hermitian density matrix σ with tr 1 (σ ) = . The informational measures for such states are defined on the basis of the von Neumann entropy (Nielsen & Chuang, 2000) ( ) ( ) <sup>1</sup> tr log log *<sup>n</sup> j j <sup>j</sup> VN* σ σσ λ λ <sup>=</sup> =− =−∑ , where λ *<sup>j</sup>* are the eigenvalues of the matrix σ . Then the *relative von Neumann entropy* of the state σ′ relative to the state σ′′ is defined as ( ) ( )( ) 1 1 || tr log tr log log log *n n jj jj j j VN* σ σ σσ σσ λλ λλ <sup>=</sup> <sup>=</sup> ′ ′′ ′ ′ ′ ′′ ′ ′ ′ ′′ =− = − − ∑ ∑ , where λ′ and λ′′ are eigenvalues of the matrices σ′ and σ′′ , correspondingly.

Let *<sup>t</sup>* σ be a state of the system at time moment *t* , and consider its representation 1 *t t n j j j* σ λ *<sup>E</sup>* <sup>=</sup> <sup>=</sup> ∑ , where 1 2 *t t t* λ < λ λ < < … *<sup>n</sup>* a r e e i ge n v a l ue s o f t h e m a t r i x *<sup>t</sup>* σ a nd *EE E* = { 1, , … *<sup>n</sup>*} is a set of matrices such that *EE E <sup>j</sup> <sup>k</sup> jk k* = δ and 1 *n <sup>j</sup> <sup>j</sup> E I* <sup>=</sup> ∑ <sup>=</sup> , where 1 *jk* δ = if *j k* = , and 0 *jk* δ = otherwise. According to the dynamics of the quantum system, if the system is in the state *<sup>t</sup>* σ , then its next state *<sup>t</sup>* <sup>1</sup> σ <sup>+</sup> is specified by the use of the selected operator *Ej* according to the projection postulate ( ) <sup>1</sup> tr *ttt* σσσ *EE E jj j* <sup>+</sup> = . This postulate represents the above indicated influence of the measurement, i.e. of the application of the operator *Ej* , to the state of the system, and the generalized the Bayesian rule to the evolution of quantum-mechanical systems. The decision-making problem required a definition of such projection that given a state *<sup>t</sup>* σ , the next state *<sup>t</sup>* <sup>1</sup> σ <sup>+</sup> is optimal in a certain sense. Since for a state *<sup>t</sup>* σ , there exist several sets *E* of matrices with the above indicated properties, the decision-making includes two stages (Davies, 1978; Holevo, 2001) and requires finding the set *EE E* = { 1, , … *<sup>n</sup>*} , and then selecting an operator *Ej* from the set *E* according to optimality criteria.

One of the methods, which is widely used in classical information theory, implies a choice of such probability vector *<sup>t</sup>* <sup>1</sup> *p* � <sup>+</sup> , that given a vector *<sup>t</sup> <sup>p</sup>* � , the Kullback-Leibler distance ( ) <sup>1</sup> || *t t KL p p* � � <sup>+</sup> reaches its maximum. Likewise, in the simulations, we implemented the choice of the set *E* and operator *E E <sup>j</sup>* ∈ such that it maximizes the relative von Neumann entropy ( ) <sup>1</sup> || *t t VN* σ σ <sup>+</sup> . In the simulations (Kagan et al., 2008), the mission of the robot was to navigate in the environment and to find the objects, which randomly change their location. The amplitudes, which defined the states, were derived from the distances between the objects and as they were measured by the ultra-sonic sensor. The scheme of the simulation and the fragment of the robot's movement are shown in Fig. 3.

The robot scans the environment, chooses such an object that maximizes the von Neumann relative entropy, and moves to this object. After the collision, the object was moved to a new random location. The comparison between quantum and classical decision-making demonstrated the difference in nearly 50% of the decisions, and in the search for variable number of objects, quantum decision-making demonstrated nearly 10% fewer decision errors than the classical one. Such results motivated an application of information theoretic methods for navigation of quantum-controlled mobile robots. In the next section we consider the algorithms which follow this direction.

320 Recent Advances in Mobile Robotics

In contrast, in quantum-mechanical systems, the state is represented by the above-presented

defined on the basis of the von Neumann entropy (Nielsen & Chuang, 2000)

<sup>=</sup> <sup>=</sup> ′ ′′ ′ ′ ′ ′′ ′ ′ ′ ′′ =− = − − ∑ ∑ , where

λ

σ

 λλ

′′ , correspondingly.

δ

= otherwise. According to the dynamics of the quantum system, if the

σ

represents the above indicated influence of the measurement, i.e. of the application of the operator *Ej* , to the state of the system, and the generalized the Bayesian rule to the evolution of quantum-mechanical systems. The decision-making problem required a

σ

properties, the decision-making includes two stages (Davies, 1978; Holevo, 2001) and requires finding the set *EE E* = { 1, , … *<sup>n</sup>*} , and then selecting an operator *Ej* from the set *E*

One of the methods, which is widely used in classical information theory, implies a choice of

*t t KL p p* � � <sup>+</sup> reaches its maximum. Likewise, in the simulations, we implemented the choice of the set *E* and operator *E E <sup>j</sup>* ∈ such that it maximizes the relative von Neumann

to navigate in the environment and to find the objects, which randomly change their location. The amplitudes, which defined the states, were derived from the distances between the objects and as they were measured by the ultra-sonic sensor. The scheme of the

The robot scans the environment, chooses such an object that maximizes the von Neumann relative entropy, and moves to this object. After the collision, the object was moved to a new random location. The comparison between quantum and classical decision-making demonstrated the difference in nearly 50% of the decisions, and in the search for variable number of objects, quantum decision-making demonstrated nearly 10% fewer decision errors than the classical one. Such results motivated an application of information theoretic methods for navigation of quantum-controlled mobile robots. In the next section we

� <sup>+</sup> , that given a vector *<sup>t</sup> <sup>p</sup>*

simulation and the fragment of the robot's movement are shown in Fig. 3.

be a state of the system at time moment *t* , and consider its representation

� � characterizes the information-theoretic difference between these states.

� represent the states of the stochastic system, and

) = . The informational measures for such states are

*<sup>j</sup>* are the eigenvalues of the matrix

 λλ

 and 1 *n*

σσσ*EE E jj j*

σ

, the next state *<sup>t</sup>* <sup>1</sup>

, there exist several sets *E* of matrices with the above indicated

<sup>+</sup> . In the simulations (Kagan et al., 2008), the mission of the robot was

<sup>+</sup> is specified by the use of the selected

<sup>+</sup> = . This postulate

� , the Kullback-Leibler distance

′ relative to the state

< < … *<sup>n</sup>* a r e e i ge n v a l ue s o f t h e m a t r i x *<sup>t</sup>*

σ. Then

′′ is defined as

σ

δ= if

a nd

λ′ and λ′′

σ

*<sup>j</sup> <sup>j</sup> E I* <sup>=</sup> ∑ <sup>=</sup> , where 1 *jk*

<sup>+</sup> is optimal in a certain

� and *<sup>q</sup>*

 with tr 1 (σ

> λ

( ) ( )( ) 1 1 || tr log tr log log log *n n jj jj j j VN*

σ

<sup>=</sup> =− =−∑ , where

λ < λ

σ

definition of such projection that given a state *<sup>t</sup>*

σ

consider the algorithms which follow this direction.

*EE E* = { 1, , … *<sup>n</sup>*} is a set of matrices such that *EE E <sup>j</sup> <sup>k</sup> jk k* =

the *relative von Neumann entropy* of the state

 λ

> σσ

σ′ and σ

*t t t*

 λ

, then its next state *<sup>t</sup>* <sup>1</sup>

operator *Ej* according to the projection postulate ( ) <sup>1</sup> tr *ttt*

*KL* (*p q* || || ) <sup>≠</sup> *KL*(*q p*) �� �� . Vectors *<sup>p</sup>*

( ) ( ) <sup>1</sup> tr log log *<sup>n</sup> j j <sup>j</sup> VN*

σσ

 σσ

*<sup>E</sup>* <sup>=</sup> <sup>=</sup> ∑ , where 1 2

are eigenvalues of the matrices

distance *KL*( ) *p q* ||

σ

σ

Let *<sup>t</sup>* σ

σ

σ

1 *t t n j j j*

*j k* = , and 0 *jk* δ

system is in the state *<sup>t</sup>*

sense. Since for a state *<sup>t</sup>*

according to optimality criteria.

such probability vector *<sup>t</sup>* <sup>1</sup> *p*

( ) <sup>1</sup> ||

entropy ( ) <sup>1</sup> || *t t VN* σ σ

 λ

Hermitian density matrix

Fig. 3. The mobile robot with ultra-sonic sensor acting in the stochastic environment: after collision, the obstacle randomly changes its location in the environment.

#### **5.2 Logic of quantum mechanics and informational distance**

Now let us consider the logic of quantum mechanics and the structure of quantum events over which the informational algorithms act. Such a formal scheme of quantum events, which is called *quantum logic* (Cohen, 1989), was introduced by Birkhoff and von Neumann (Birkhoff & Neumann, 1936) as an attempt of to find an axiomatic description of quantum mechanics.

A quantum logic is a lattice Λ of events *A* , which contains the smallest element ∅ , the greatest element *I* , relation ⊂ , unary operation ′, and binary operations ∪ and ∩ . It is assumed that for the events *A*∈ Λ the following usual set properties hold true: a) For any element *A*∈ Λ there exist an event *A*′∈ Λ such that ( ) *A A* ′ ′ = ; b) *A A* ∩ ′ = ∅ and *AAI* ∪ =′ ; c) For any pair of events *A B*, ∈ Λ , *A B* ⊂ implies *B A* ′ ⊂ ′ , and *A B* ⊂ implies *BA BA* =∪ ∩ ( ′) ; d) For any countable sequence 1 2 *A A*, ,… of events from Λ their union is in Λ , i.e., ( ) ∪ ∈Λ *j j A* . Notice that is not required that the events *A*∈ Λ are subsets of the same set. That leads to the fact that under the requirements of quantum logic the distributive rule may not hold and, in general, *B AA BA BA* ∩ ∪ ≥∩ ∪∩ ( ′) ( ) ( ′) . In contrast, in the probabilistic scheme, it is postulated that the events are the subsets of the set of elementary outcomes, so the distribution rule is satisfied.

In quantum logic Λ , events *A*∈ Λ and *B*∈ Λ are called *orthogonal*, and denoted by *A B* ⊥ , if *A B* ⊂ ′ , and a finite set α = {*A A* <sup>1</sup> , , … *<sup>n</sup>*} , of events *Aj* ∈Λ , is called ∨-*orthogonal system* if ( 1 1 ) *<sup>k</sup>* ∪ ⊥ *j j* <sup>=</sup> *A Ak*<sup>+</sup> , *k n* = 1, 2, , 1 … − . The state μ over the quantum logic Λ is defined as a map μ : 0, 1 Λ →[ ] such that μ (*I*) = 1 and for any ∨-orthogonal system α = {*A A* <sup>1</sup> , , … *<sup>n</sup>*} it holds true that ( <sup>1</sup> ) ( ) <sup>1</sup> *n n j j j j* μ μ <sup>=</sup> *A A* <sup>=</sup> ∪ = ∑ . Given a state μ : 0, 1 Λ →[ ] , an ∨-orthogonal system α is a *partition* of the logic Λ with respect to the state μ if ( <sup>1</sup> ) 1 *<sup>n</sup>* μ ∪ = *j j* <sup>=</sup> *A* , *Aj* ∈α (Yuan, 2005).

Following classical ergodic theory (Rokhlin, 1967), *entropy* of the partition α is defined as ( ) ( )log ( ) *<sup>A</sup> H AA* μ α α μμ <sup>∈</sup> = −∑ , and *conditional entropy* of partition α relatively to partition β is ( | ) ( , lo ) g ( | ) *B A H*μ *AB AB* β α α β μ μ ∈ ∈ = −∑ ∑ , whereα and β are partitions of quantum logic Λ (Yuan, 2005; Zhao & Ma, 2007). In addition, similarly to the ergodic theory (Rokhlin,

Navigation of Quantum-Controlled Mobile Robots 323

actions such that the inner states of the robot correspond to the environmental states, which,

Let us return to the representation of the robot's states by qubits 0 1 1, 0 0, 1 () () *T T sa b a b* =+= + , where both amplitudes *a* and *b* are real numbers. In such

*<sup>b</sup>* : 0, 1 *X* → [ ] for some universal set *X* , which are defined as (Hannachi, et al., 2007)

 π

> μπ

> > μ μ

. Over the pairs of membership functions

μ μ

> 

 = *sign a a sign b b* − + and ( ) ( () () ) 2 2 2 arcsin 1 2 *<sup>b</sup>* μ

*<sup>b</sup>* , fuzzy analogs of quantum mechanical operators are defined (Hannachi, et al., 2007). Let

Fuzzy analog Η of the Hadamard operator Η is the following (Hannachi, et al., 2007):

 *a b* + − )) . Similarly, reverse fuzzy Hadamard operator is defined by the following transformation (Rybalov et al., 2010):

Straightforward calculations show that the direct and reverse fuzzy Hadamard operators Η and *<sup>R</sup>* Η are reversible and preserve the properties of corresponding quantum Hadamard operators. Trajectories of the robot that acts according to the fuzzy Hadamard operators are illusrated by Fig. 4; the simulattions have been conducted likewise in the example shown in

Fig. 4. Trajectories and turns of the mobile robot according to quantum and fuzzy control. According to simulations, the turns of quantum and fuzzy controlled robot are different; however, the states of the robot and the results of its actions are statistically equivalent. Such preliminary results show that in case of real amplitudes, fuzzy logic models of quantum

*a b* ) ( ( )

*a b* − +

with backward transformations ( ) 2 2 1 sin 2 sin 2 1 *a b a b a sign* = μ μ

μ

= *sign a a sign b b* + + ,

+ − + − and

 μπ *<sup>a</sup>* : 0, 1 *X* → [ ]

μ*<sup>a</sup>* and

**6. Notes on fuzzy logic representation of quantum control** 

 μπ

us consider the Hadamard operators, which represent the turns of the robot.

 *a b* , max 0, min 1, 1 2 , min 1, max 0, 1 2 ( ( ( ) − + μ μ

( ) , min 1, max 0, 3 2 , max 0, min 1, 1 2 ( ( ( )) ( ( ))) *<sup>R</sup>* Η =

*a b* .

μ μ

Quantum control Fuzzy control

a case, the state-vector *s* can be represented by two membership functions

in their turn, are changed by the robot.

( ) ( () () ) 2 2 2 arcsin 1 2 *<sup>a</sup>*

( ) 2 2 sin 2 sin 2 *ba b a b sign* =− −

 μπ

*a b* − −

 

control may be applied.

and μ

> π

> > μ μ

μ

μ

Η = ( ) μ μ

> μ μ

Fig. 1.

1967), a Rokhlin distance is defined as *dHH* μμμ (α, || β αβ βα ) = + ( ) ( ) between the partitions α and β of the quantum logic Λ , which preserves the metric properties (Khare & Roy, 2008), as it holds for the probabilistic scheme.

#### **5.3 Actions of the robot over the quantum events**

The indicated properties of the partitions and states of quantum logic allow an application of informational A\*-type search algorithms acting over partitions space (Kagan & Ben-Gal, 2006) for navigation of quantum-controlled mobile robots. Let χ be a set of all possible partitions of the logic Λ given a state μ . By *d*μ (α, β ) , , α β χ ∈ , we denote a Rokhlin distance between the partitions α and β , and by *d*μ (α, β ) � an estimated distance such that *d d* μ μ (α, , β αβ ) ≤ ( ) � ; as a distance *d*μ � the above-defined Ornstein distance *dOrn* ( ) α, β can be applied. Let *r R*<sup>+</sup> ∈ be a constant value. The non-empty neighborhood *N r* ( ) α, ⊂ χ of partition α ∈ χ is a set of partitions such that α α ∉*N r* ( , ) and for every partition β α ∈*N r* ( , ) it holds true that *d rd* μ μ (α, , β αβ ) ≤ ≤ ( ) � . In the algorithm, α stands for a partition specified by the robot, and τ stands for a partition specified by the environment. The robot is located in the environment, which is specified by a partition *cur* τ =ϑ and starts with the initial partition α*cur* = θ . Then, the actions of the robot are defined by the following sequence:

The robots actions given the environment *cur* τ:


$$\text{-}\qquad \text{Update estimation } \tilde{d}\_{\mu} \left( a\_{\text{cur}}, \tau\_{\text{cur}} \right) \leftarrow \max \left| \tilde{d}\_{\mu} \left( a\_{\text{cur}}, \tau\_{\text{cur}} \right), r + \min\_{a \in \mathbb{N}(a\_{\text{cur}}, r)} \tilde{d}\_{\mu} \left( a, \tau\_{\text{cur}} \right) \right|;$$


The robots actions while the environment changes ( *cur* τ ←τ):


Let us clarify the application of the algorithms to the quantum logic Λ by half steps, *t* = 0, 1 2 , 1, 3 2 ,… , which correspond to the robot's and the environment's actions. Let the initial map by [ ] <sup>0</sup> μ : 0, 1 Λ → . Regarding the robot's states, it means a choice of the basis for the states; in the above-given examples, such a choice corresponds to the definition of the qubits for the states 1 " " ↑ = *s* and 2 " " ↓ = *s* , or that is equivalent to the states 3 " " → = *s* and 4 " " ← = *s* . By the use of the map <sup>0</sup> μ the robot chooses the state <sup>0</sup> *s* and a certain partition which specifies the next map 1 2 μ . According to the obtained map 1 2 μ , the environment changes and the map <sup>1</sup> μ is specified. Now the robot chooses the <sup>1</sup> *s* and according to the chosen partition specifies the map 3 2 μ . The environment, in its turn, changes according to the map 3 2 μ, and so on.

The presented actions over the partitions of quantum logic provide a representation, which differs from the one use in the quantum learning algorithms (Chen & Dong, 2008; Dong, et al., 2010) and implements information theoretic decision-making using the Rokhlin distance. However, the meaning of the actions is similar, and the goal of the robot is to determine the 322 Recent Advances in Mobile Robotics

The indicated properties of the partitions and states of quantum logic allow an application of informational A\*-type search algorithms acting over partitions space (Kagan & Ben-Gal,

, and by *d*

 μ

 αβ) ≤ ≤ ( ) � . In the algorithm,

, ,

μ. By *d*

be applied. Let *r R*<sup>+</sup> ∈ be a constant value. The non-empty neighborhood *N r* ( )

(

τ:

> α α

 μ

 ατ

the states; in the above-given examples, such a choice corresponds to the definition of the qubits for the states 1 " " ↑ = *s* and 2 " " ↓ = *s* , or that is equivalent to the states 3 " " → = *s*

The presented actions over the partitions of quantum logic provide a representation, which differs from the one use in the quantum learning algorithms (Chen & Dong, 2008; Dong, et al., 2010) and implements information theoretic decision-making using the Rokhlin distance. However, the meaning of the actions is similar, and the goal of the robot is to determine the

μ

μ

μ

, and so on.

 *cur cur* , max , , min ← + { α τ*cur cur*

β

α

β

μ (α, β ) , , α β χ

μ (α, β

α

μμμ(

of the quantum logic Λ , which preserves the metric properties (Khare

 αβ

, ||

χ

stands for a partition specified by the environment.

α α∈*N r cur* ,

� the above-defined Ornstein distance *dOrn* ( )

. Then, the actions of the robot are defined by the following

μ α τ

 ατ

the robot chooses the state <sup>0</sup>

μ

. According to the obtained map 1 2

is specified. Now the robot chooses the <sup>1</sup>

, *cur* } �� � ;

<sup>←</sup> <sup>∈</sup> � ;

τ ←τ):

: 0, 1 Λ → . Regarding the robot's states, it means a choice of the basis for

μμ( ) ( )( )

 *cur* , max , , , <sup>←</sup> { *cur cur cur* <sup>−</sup> } � �� . Let us clarify the application of the algorithms to the quantum logic Λ by half steps, *t* = 0, 1 2 , 1, 3 2 ,… , which correspond to the robot's and the environment's actions. Let the

 α

 βα) = + ( ) ( ) between the

) � an estimated distance such that

∉*N r* ( , ) and for every partition

τ =ϑ

α

μ ( ) α τ

*s* and a certain

μ, the

. The environment, in its turn,

*s* and

be a set of all possible

α, βcan

stands for a

and starts

α, ⊂ χof

∈ , we denote a Rokhlin

1967), a Rokhlin distance is defined as *dHH*

2006) for navigation of quantum-controlled mobile robots. Let

α and β

is a set of partitions such that

μ



*cur next* ←α.

ατ

according to the chosen partition specifies the map 3 2

μ

μ

α

τ

The robot is located in the environment, which is specified by a partition *cur*


& Roy, 2008), as it holds for the probabilistic scheme.

**5.3 Actions of the robot over the quantum events** 

∈*N r* ( , ) it holds true that *d rd*

α*cur* = θ

μ

α

μ

and 4 " " ← = *s* . By the use of the map <sup>0</sup>

environment changes and the map <sup>1</sup>

changes according to the map 3 2

partition which specifies the next map 1 2

The robots actions while the environment changes ( *cur*

α τ

The robots actions given the environment *cur*

partitions of the logic Λ given a state

partition specified by the robot, and

distance between the partitions

 αβ) ≤ ( ) � ; as a distance *d*

 μ

, , β

> α ∈ χ

with the initial partition


initial map by [ ] <sup>0</sup> μ

partitions

*d d* μ

partition

sequence:

β

 (α

> α

α and β actions such that the inner states of the robot correspond to the environmental states, which, in their turn, are changed by the robot.

#### **6. Notes on fuzzy logic representation of quantum control**

Let us return to the representation of the robot's states by qubits 0 1 1, 0 0, 1 () () *T T sa b a b* =+= + , where both amplitudes *a* and *b* are real numbers. In such a case, the state-vector *s* can be represented by two membership functions μ*<sup>a</sup>* : 0, 1 *X* → [ ] and μ*<sup>b</sup>* : 0, 1 *X* → [ ] for some universal set *X* , which are defined as (Hannachi, et al., 2007) ( ) ( () () ) 2 2 2 arcsin 1 2 *<sup>a</sup>* μ π = *sign a a sign b b* − + and ( ) ( () () ) 2 2 2 arcsin 1 2 *<sup>b</sup>* μ π = *sign a a sign b b* + + , with backward transformations ( ) 2 2 1 sin 2 sin 2 1 *a b a b a sign* = μ μ μπ μπ + − + − and ( ) 2 2 sin 2 sin 2 *ba b a b sign* =− − μ μ μπ μπ . Over the pairs of membership functions μ*<sup>a</sup>* and μ*<sup>b</sup>* , fuzzy analogs of quantum mechanical operators are defined (Hannachi, et al., 2007). Let us consider the Hadamard operators, which represent the turns of the robot. Fuzzy analog Η of the Hadamard operator Η is the following (Hannachi, et al., 2007): Η = ( ) μ μ *a b* , max 0, min 1, 1 2 , min 1, max 0, 1 2 ( ( ( ) − + μ μ *a b* ) ( ( ) μ μ *a b* + − )) . Similarly, reverse fuzzy Hadamard operator is defined by the following transformation (Rybalov et al., 2010): ( ) , min 1, max 0, 3 2 , max 0, min 1, 1 2 ( ( ( )) ( ( ))) *<sup>R</sup>* Η = μ μ *a b* − − μ μ *a b* − + μ μ*a b* .

Straightforward calculations show that the direct and reverse fuzzy Hadamard operators Η and *<sup>R</sup>* Η are reversible and preserve the properties of corresponding quantum Hadamard operators. Trajectories of the robot that acts according to the fuzzy Hadamard operators are illusrated by Fig. 4; the simulattions have been conducted likewise in the example shown in Fig. 1.

Fig. 4. Trajectories and turns of the mobile robot according to quantum and fuzzy control.

According to simulations, the turns of quantum and fuzzy controlled robot are different; however, the states of the robot and the results of its actions are statistically equivalent. Such preliminary results show that in case of real amplitudes, fuzzy logic models of quantum control may be applied.

Navigation of Quantum-Controlled Mobile Robots 325

Cover, T. M. & Thomas, J. A. (1991). *Elements of Information Theory*. John Wiley & Sons, New

Davies, E. B. (1978). Information and Quantum Measurement. *IEEE Trans. Inform. Theory*,

Dong, D.-Y.; Chen, C.-L.; Zhang, C.-B. & Chen, Z.-H. (2006). Quantum Robot: Structure,

Dong, D.; Chen, C.; Chu, J. & Tarn, T.-J. (2010). Robust Quantum-Inspired Reinforcement Learning for Robot Navigation. *IEEE/ASME Trans. Mechatronics*, To appear. Fu, K. S. & Li, T. J. (1969). Formulation of Learning Automata and Automata Games.

Gurevich, Y. (1991). Evolving Algebras: An Attempt to Discover Semantics. *Bull. European* 

Hannachi, M. S.; Hatakeyama, Y. & Hirota, K. (2007). Emulating Qubits with Fuzzy Logic. *J.* 

Helstrom, C. W. (1976). *Quantum Detection and Estimation Theory*. Academic Press, New

Kagan, E. & Ben-Gal, I. (2008). Application of Probabilistic Self-Stabilization Algorithms to

Kagan, E. ; Salmona, E. & Ben-Gal, I. (2008). Probabilistic Mobile Robot with Quantum

Kagan, E. & Ben-Gal, I. (2006). An Informational Search for a Moving Target. *Proc. IEEE 24-*

Khare, M. & Roy, S. (2008). Conditional Entropy and the Rokhlin Metric on an

Levitin, L. B. (1969). On a Quantum Measure of the Quantity of Information. *Proc. Fourth All-*

Malley, J. D. & Hornstein, J. (1993). Quantum Statistical Inference. *Statistical Sciences*, Vol. 8,

Nielsen, M. A. & Chuang, I. L. (2000). *Quantum Computation and Quantum Information*.

Ornstein, D. S. (1974). *Ergodic Theory, Randomness, and Dynamical Systems*. Yale University

Raghuvanshi, A.; Fan, Y.; Woyke, M. & Perkowski, M. (2007). Quantum Robots for

Rokhlin, V. A. (1967). Lectures on the Entropy Theory of Measure-Preserving

Rybalov, A.; Kagan, E.; Manor, Y. & Ben-Gal, I. (2010). Fuzzy Model of Control for Quantum-Controlled Mobile Robots. *Proc. IEEE 26-th Conv. EEEI*. Eilat, Israel. Tsetlin, M. L. (1973). *Automaton Theory and Modeling of Biological Systems*. Academic Press,

Orthomodular Lattice with Bayesian State. *Int. J. Theor. Phys.*, Vol. 47, pp. 1386-

*Union Conf. Problems of Information Transmission and Coding*. IPPI AN USSR, Moscow, pp. 111-115 (In Russian). *English translation*: A. Blaquieve, et al., eds. (1987). *Information Complexity and Control in Quantum Physics*. Springer-Verlag, New

Holevo, A. S. (2001). *Statistical Structure of Quantum Theory*. Springer-Verlag, Berlin.

Robot's Control. *Proc. 15-th Israeli Conf. IE&M'08*, Tel-Aviv, Israel.

Decision-Making. *Proc. IEEE 25-th Conv. EEEI*. Eilat, Israel.

Cambridge University Press, Cambridge, England.

Teenagers, *Proc. IEEE Conf. ISMV'07*, Helsinki, Finland.

Transformations. *Rus. Math. Surveys*, Vol. 22, pp. 1-52.

*th Convention of EEEI*. Eilat, Israel.

*Advanced Computational Intelligence and Intelligent Informatics*, Vol. 11, No. 2, pp. 242-

Algorithms and Applications. *Robotica*, Vol. 24, No. 4, pp. 513-521.

York.

249.

York.

1396.

York, pp. 15-47.

New York.

No. 4, pp. 433-457.

Press, New Haven, 1974.

Vol. 24, No. 5, pp. 596-599.

*Information Science*, Vol. 1, No. 3, pp. 237-256.

*Assoc. Theor. Comp. Science*, Vol. 43, pp. 264-284.

#### **7. Conclusion**

In the chapter we presented a brief introduction into the methods of navigation of quantumcontrolled mobile robots and considered the ideas of its implementation by the use of probabilistic and information theoretic techniques. The described methods represent such a property of the quantum-controlled mobile robots that the state of quantum-mechanical system includes the state of the environment as a part of its inner state.

In particular, the state of the mobile robot in the environment was defined by the use of the density matrix, which, in addition to the inner state of the robot, included the state of the environment. Such a specification of the state allowed calculations of both the robot's influence on the environment and the environmental influence on the robot by the use of the unified techniques.

The decision-making methods, which define the robot's behavior, implemented the indicated representation of the state and were based on the probabilistic and informational schemes. These schemes generalize the known maximum probability and maximum information criteria while taking into account additional information regarding the robot's influence on the environment, and correspond to the statistical considerations of quantummechanical methods (Malley & Hornstein, 1993; Barndorff-Nielsen & Gill, 2003).

In general, the actions of quantum-controlled mobile robot were specified by the choices of certain partitions of quantum logic. The choices were based on informational distances following the line of informational search algorithms (Kagan & Ben-Gal, 2006). As indicated above, such a method gives an alternative view to quantum learning and path-planning algorithms (Chen & Dong, 2008; Dong, et al., 2010).

The presented methods were simulated by the use of small mobile robots, while the complete realization of quantum control requires quantum-mechanical on-board computers. However, as it follows from preliminary considerations (Rybalov, et al., 2010), fuzzy control of the mobile robot demonstrates similar results as probabilistic and informational schemes of quantum control; thus in some cases fuzzy logic models of quantum control may be applied.

#### **8. References**

Albert, D. Z. (1994). *Quantum Mechanics and Experience*. Harvard University Press, Cambridge, Massachusetts and London, England.


324 Recent Advances in Mobile Robotics

In the chapter we presented a brief introduction into the methods of navigation of quantumcontrolled mobile robots and considered the ideas of its implementation by the use of probabilistic and information theoretic techniques. The described methods represent such a property of the quantum-controlled mobile robots that the state of quantum-mechanical

In particular, the state of the mobile robot in the environment was defined by the use of the density matrix, which, in addition to the inner state of the robot, included the state of the environment. Such a specification of the state allowed calculations of both the robot's influence on the environment and the environmental influence on the robot by the use of the

The decision-making methods, which define the robot's behavior, implemented the indicated representation of the state and were based on the probabilistic and informational schemes. These schemes generalize the known maximum probability and maximum information criteria while taking into account additional information regarding the robot's influence on the environment, and correspond to the statistical considerations of quantum-

In general, the actions of quantum-controlled mobile robot were specified by the choices of certain partitions of quantum logic. The choices were based on informational distances following the line of informational search algorithms (Kagan & Ben-Gal, 2006). As indicated above, such a method gives an alternative view to quantum learning and path-planning

The presented methods were simulated by the use of small mobile robots, while the complete realization of quantum control requires quantum-mechanical on-board computers. However, as it follows from preliminary considerations (Rybalov, et al., 2010), fuzzy control of the mobile robot demonstrates similar results as probabilistic and informational schemes of quantum control; thus in some cases fuzzy logic models of quantum control may be

Albert, D. Z. (1994). *Quantum Mechanics and Experience*. Harvard University Press,

Ballentine, L. E (2006). *Quantum Mechanics. A Modern Development*. Word Scientific,

Barndorff-Nielsen, O. E. & Gill, R. D. (2003). On Quantum Statistical Inference. *J. Royal* 

Chen, C.-L. & Dong, D.-Y. (2008). Superposition-Inspired Reinforcement Learning and

Cohen, D. W. (1989). *An Introduction to Hilbert Space and Quantum Logic*. Springer-Verlag,

Quantum Reinforcement Learning. In *Reinforcement Learning: Theory and Applications*, C. Weber, M. Elshaw, N. M. Mayer, (Eds.), InTech Education and

Benioff, P. (1998). Quantum Robots and Environments. *Phys. Rev*. *A*, Vol. 58, pp. 893-904. Birkhoff, G. & Neumann, J. von. (1936). The Logic of Quantum Mechanics. *Annals Math.*,

Cambridge, Massachusetts and London, England.

*Statistical Society B*, Vol. 65, No. 4, pp. 775-816.

Publishing, Vienna, Austria, pp. 59-84.

mechanical methods (Malley & Hornstein, 1993; Barndorff-Nielsen & Gill, 2003).

algorithms (Chen & Dong, 2008; Dong, et al., 2010).

system includes the state of the environment as a part of its inner state.

**7. Conclusion** 

unified techniques.

applied.

**8. References** 

Singapore.

New York.

Vol. 37, No. 4, pp. 823-843.


**16** 

*Korea* 

Seung-Hun Kim

*Korea Electronics Technology Institute,* 

**Tracking Control for Reliable Outdoor** 

In the past several years there has been increasing interest in applications of a mobile robot. Personal service robots perform the missions of guiding tourists in museum, cleaning room and nursing the elderly [1]. Mobile robots have been used for the purpose of patrol,

The indoor environment has a variety of features such as walls, doors and furniture that can be used for mapping and navigation of a mobile robot. In contrast to the indoor cases, it is hard to find any specific features in outdoor environment without some artificial landmarks [3]. Fortunately, the existence of curbs on roadways is very useful to build a map and localization in outdoor environment. The detected curb information could be used for not only map building and localization but also navigating safely [4]. And also the mobile robot

The present paper deals with the development of a robot which can patrol areas such as industrial complexes and research centers. We have already reported about outdoor navigation of a mobile robot [5]. The extended Kalman filter is applied for the fusion between odometry and Differential Global Positioning System(DGPS) measurement data. It is insufficient for reliable navigation since the error of DGPS measurement data increased near high buildings and trees [6]. Hence, it is necessary to correct the pose of a mobile robot when the position data from the odometry and DGPS are inaccurate. This chapter proposes the curb detection algorithm and calculate the pose error from the curb edge data and then

The outdoor mobile robot system on an ATRV-mini from RWI Inc. is constructed as shown in Fig. 1. It is a four-wheeled robot designed for outdoor use and equipped with odometry. This robot is chosen because it can climb up and down over speed bumps and slopes. It is added a laser range finder to detect obstacles and build a map of the environment. For global localization, a DGPS receiver is added. A camera that can zoom-in and zoom-out,

Fig. 2 shows the software architecture of the mobile robot system. The system consists of three parts which are the mobile server, the navigation server and the remote server. The mobile sever controls the motor in the ATRV according to the velocity and angular velocity

decides to go or stop using the width of the road calculated from the detected curb.

reconnaissance, surveillance and exploring planets, etc [2].

use it to correct the pose of the mobile robot.

was installed to send the image data to the control station.

**2. Outdoor mobile robot system** 

**1. Introduction**

**Navigation Using Curb Detection** 


## **Tracking Control for Reliable Outdoor Navigation Using Curb Detection**

Seung-Hun Kim *Korea Electronics Technology Institute, Korea* 

#### **1. Introduction**

326 Recent Advances in Mobile Robotics

Unsal, C. (1998). *Intelligent Navigation of Autonomous Vehicles in an Automated Highway* 

Yuan, H.-J. (2005). Entropy of Partitions on Quantum Logic. *Commun. Theor. Phys.*, Vol. 43,

Zhao, Y.-X. & Ma, Z.-H. (2007). Conditional Entropy of Partitions on Quantum Logic.

Polytechnic Institute and State University, VI, USA.

*Commun. Theor. Phys.*, Vol. 48, No. 1, pp. 11-13.

No. 3, pp. 437-439.

*System: Learning Methods and Interacting Vehicles Approach*. PhD Thesis, Virginia

In the past several years there has been increasing interest in applications of a mobile robot. Personal service robots perform the missions of guiding tourists in museum, cleaning room and nursing the elderly [1]. Mobile robots have been used for the purpose of patrol, reconnaissance, surveillance and exploring planets, etc [2].

The indoor environment has a variety of features such as walls, doors and furniture that can be used for mapping and navigation of a mobile robot. In contrast to the indoor cases, it is hard to find any specific features in outdoor environment without some artificial landmarks [3]. Fortunately, the existence of curbs on roadways is very useful to build a map and localization in outdoor environment. The detected curb information could be used for not only map building and localization but also navigating safely [4]. And also the mobile robot decides to go or stop using the width of the road calculated from the detected curb.

The present paper deals with the development of a robot which can patrol areas such as industrial complexes and research centers. We have already reported about outdoor navigation of a mobile robot [5]. The extended Kalman filter is applied for the fusion between odometry and Differential Global Positioning System(DGPS) measurement data. It is insufficient for reliable navigation since the error of DGPS measurement data increased near high buildings and trees [6]. Hence, it is necessary to correct the pose of a mobile robot when the position data from the odometry and DGPS are inaccurate. This chapter proposes the curb detection algorithm and calculate the pose error from the curb edge data and then use it to correct the pose of the mobile robot.

#### **2. Outdoor mobile robot system**

The outdoor mobile robot system on an ATRV-mini from RWI Inc. is constructed as shown in Fig. 1. It is a four-wheeled robot designed for outdoor use and equipped with odometry. This robot is chosen because it can climb up and down over speed bumps and slopes. It is added a laser range finder to detect obstacles and build a map of the environment. For global localization, a DGPS receiver is added. A camera that can zoom-in and zoom-out, was installed to send the image data to the control station.

Fig. 2 shows the software architecture of the mobile robot system. The system consists of three parts which are the mobile server, the navigation server and the remote server. The mobile sever controls the motor in the ATRV according to the velocity and angular velocity

Tracking Control for Reliable Outdoor Navigation Using Curb Detection 329

The outdoor mobile robot requires ability to navigate in a dynamic and potentially dangerous environment due to curbs and obstacles. Using the laser range finder, it can measure the distance from the obstacles and calculate the distance between obstacles. These data obtained from the laser range finder can be used in environment mapping and obstacle

An important feature of road environment is the existence of curbs as shown in Fig. 3. The

The proposed algorithm is to detect curbs through the laser range finder, which was positioned on the mobile robot and looking down the road with a small tilt angle as shown in Fig. 4. The tilted laser range finder projects three-dimensional shape of curbs on two-

The mobile robot builds the map of the curbs of roads and the map is used for tracking and localization. (, , ) *L L n x y* is obtained by the laser range finder, where *n* represents n-th data and , *<sup>L</sup> <sup>L</sup> x y* are the distance data in the laser range finder's coordinate as shown in Fig. 5. The

curb edges on the both sides of the mobile robot is used for mapping and tracking.

**3. Curb detection** 

curbs could be used in mobile robot navigation.

Fig. 3. Road and curb on the road.

dimension space. And then it extracts curb edge points.

Fig. 4. Tilted laser range finder on the mobile robot.

avoidance.

Fig. 1. Mobile robot system configuration.

(a) DGPS antenna (b) PC (c) Image camera (d) LMS200 laser range finder

(e) Attitude/heading sensor (f) Wireless LAN bridges (g) Joypad (h) Control station

from the navigation server and transfers the odometry to the navigation server. The navigation server makes the mobile robot localize using latitude and longitude data from the DGPS and attitude from 3-axis angle sensor. It detects curbs using distance from the tilted laser range finder and sends the distance, image and navigation data to the remote server through the wireless LAN. The remote server operates in two modes, autonomous mode or teleoperated mode, display the image and the navigation information.

Fig. 2. Software diagram of the mobile robot system.

#### **3. Curb detection**

328 Recent Advances in Mobile Robotics

Fig. 1. Mobile robot system configuration.

Fig. 2. Software diagram of the mobile robot system.

(a) DGPS antenna (b) PC (c) Image camera (d) LMS200 laser range finder

(e) Attitude/heading sensor (f) Wireless LAN bridges (g) Joypad (h) Control station

mode or teleoperated mode, display the image and the navigation information.

from the navigation server and transfers the odometry to the navigation server. The navigation server makes the mobile robot localize using latitude and longitude data from the DGPS and attitude from 3-axis angle sensor. It detects curbs using distance from the tilted laser range finder and sends the distance, image and navigation data to the remote server through the wireless LAN. The remote server operates in two modes, autonomous The outdoor mobile robot requires ability to navigate in a dynamic and potentially dangerous environment due to curbs and obstacles. Using the laser range finder, it can measure the distance from the obstacles and calculate the distance between obstacles. These data obtained from the laser range finder can be used in environment mapping and obstacle avoidance.

An important feature of road environment is the existence of curbs as shown in Fig. 3. The curbs could be used in mobile robot navigation.

Fig. 3. Road and curb on the road.

The proposed algorithm is to detect curbs through the laser range finder, which was positioned on the mobile robot and looking down the road with a small tilt angle as shown in Fig. 4. The tilted laser range finder projects three-dimensional shape of curbs on twodimension space. And then it extracts curb edge points.

Fig. 4. Tilted laser range finder on the mobile robot.

The mobile robot builds the map of the curbs of roads and the map is used for tracking and localization. (, , ) *L L n x y* is obtained by the laser range finder, where *n* represents n-th data and , *<sup>L</sup> <sup>L</sup> x y* are the distance data in the laser range finder's coordinate as shown in Fig. 5. The curb edges on the both sides of the mobile robot is used for mapping and tracking.

Tracking Control for Reliable Outdoor Navigation Using Curb Detection 331

The laser range finder senses the surface of roads due to the small angle between the laser range finder and the mobile robot. Therefore, the longest straight line of the raw data from laser range finder is the surface of roads and the curbs locates the edges of the road. The longest straight line is extracted using Hough Transform [7], [8], [9]. In Hough transform,

length and angle from the origin of a normal to the line. Using this parameterization, an

 = *x y n n* cos sin θ

found by Hough Transform.

The straight line in Fig. 9. shows the longest straight line represented by *<sup>d</sup>*

 θ

 and *<sup>d</sup>* θ

ρ

ρ and θ

+ (1)

as shown in Fig. 8.

ρ

 and *<sup>d</sup>* θ.

θ ∈[0,2 ) π

, which represent the

. The dominant

Fig. 7. Projection of collinear points onto a line.

each line is represented by two parameters, commonly called

ρ

(,) *n n x y* is the n-th measurement data, where n is 1 to 361 and

are obtained by Hough Transform, called *<sup>d</sup>*

**3.1 Extracting the longest straight line** 

equation of the line can be written as:

ρ and θ

Fig. 8. Dominant *<sup>d</sup>*

ρ and *<sup>d</sup>* θ

Fig. 5. Curb detection on the road.

Even though the angle between the robot and the curb changes, the feature points of the curb do not change as shown in Fig. 6. The points around 1600mm of the axis Y represent the road surface and the curved points between 0mm and 2000mm of the axis X are the curb of the road.

Fig. 6. Angles (0°, 30°, -30°) between the mobile robot and the curb and raw data.

Fig. 7. Projection of collinear points onto a line.

#### **3.1 Extracting the longest straight line**

330 Recent Advances in Mobile Robotics

Even though the angle between the robot and the curb changes, the feature points of the curb do not change as shown in Fig. 6. The points around 1600mm of the axis Y represent the road surface and the curved points between 0mm and 2000mm of the axis X are the curb

Fig. 6. Angles (0°, 30°, -30°) between the mobile robot and the curb and raw data.

Fig. 5. Curb detection on the road.

of the road.

The laser range finder senses the surface of roads due to the small angle between the laser range finder and the mobile robot. Therefore, the longest straight line of the raw data from laser range finder is the surface of roads and the curbs locates the edges of the road. The longest straight line is extracted using Hough Transform [7], [8], [9]. In Hough transform, each line is represented by two parameters, commonly called ρ and θ , which represent the length and angle from the origin of a normal to the line. Using this parameterization, an equation of the line can be written as:

$$
\rho = x\_n \cos \theta + y\_n \sin \theta \tag{1}
$$

(,) *n n x y* is the n-th measurement data, where n is 1 to 361 and θ ∈[0,2 ) π . The dominant ρ and θ are obtained by Hough Transform, called *<sup>d</sup>* ρ and *<sup>d</sup>* θas shown in Fig. 8.

Fig. 8. Dominant *<sup>d</sup>* ρ and *<sup>d</sup>* θfound by Hough Transform.

The straight line in Fig. 9. shows the longest straight line represented by *<sup>d</sup>* ρ and *<sup>d</sup>* θ.

Tracking Control for Reliable Outdoor Navigation Using Curb Detection 333

The scanned data of a flat road surface has two specific characteristics. One is that the scanned data of road surface is on the straight line and the interval between adjacent two points is small. The other is that the slope of the straight line is parallel to the direction *XL* of

When the distance between adjacent two points is continuously within *d* and the slope

(3)). The end points (stars in Fig. 12) of the straight line are the road edges and also the start points of the curbs. Therefore the curb edge points could be obtained by finding the end

1 1 ( )( ) *ii ii x x y y <sup>d</sup>* <sup>−</sup> + + +− ≤ and <sup>1</sup> <sup>1</sup>

, the two points are considered as being on the straight line (Eq.

tan *i i*

1

σ

(3)

*i i y y x x*

 <sup>−</sup> <sup>+</sup> + ⎛ ⎞ <sup>−</sup> ⎜ ⎟ <sup>≥</sup> <sup>−</sup> ⎝ ⎠

Fig. 11. Grouped points and found road surface.

σ

2 2

**3.4 Extracting the curb edge points** 

a laser range finder.

between them is less than

points of the straight line.

Fig. 12. Extracted curb edge points.

Fig. 9. Raw data and the longest straight line.

#### **3.2 Fitting the points around the longest straight line**

It is needed to fit the raw data points corresponding to the straight line for identifying the road surface and the curbs. The road surface and curbs exist on the boundary of the straight line from Fig. 10. To obtain the boundary points of the line, ρ is first calculated by Eq. (1) for each raw scan data with *<sup>d</sup>* θ . And then, choose the points satisfying the following Eq. (2). The points of Fig. 6 show the fitting points with Δρof 100mm.

$$
\rho\_d - \Delta \rho \le \rho \le \rho\_d + \Delta \rho \tag{2}
$$

Fig. 10. Points around the longest straight line.

#### **3.3 Grouping the points and finding the road surface**

The points around the longest straight line include the road surface and obstacles. To extract the road surface, we classify into six groups (ellipses in Fig. 11). Points of each group are continuous. The biggest group among the six groups indicates the sensed road surface.

Fig. 11. Grouped points and found road surface.

#### **3.4 Extracting the curb edge points**

332 Recent Advances in Mobile Robotics

It is needed to fit the raw data points corresponding to the straight line for identifying the road surface and the curbs. The road surface and curbs exist on the boundary of the straight

> *d d* − Δ ≤ ≤ +Δ ρρρ

The points around the longest straight line include the road surface and obstacles. To extract the road surface, we classify into six groups (ellipses in Fig. 11). Points of each group are continuous. The biggest group among the six groups indicates the sensed road surface.

ρ

ρ

. And then, choose the points satisfying the following Eq. (2).

of 100mm.

 ρ is first calculated by Eq. (1)

(2)

Fig. 9. Raw data and the longest straight line.

for each raw scan data with *<sup>d</sup>*

**3.2 Fitting the points around the longest straight line** 

line from Fig. 10. To obtain the boundary points of the line,

ρ

θ

The points of Fig. 6 show the fitting points with Δ

Fig. 10. Points around the longest straight line.

**3.3 Grouping the points and finding the road surface** 

The scanned data of a flat road surface has two specific characteristics. One is that the scanned data of road surface is on the straight line and the interval between adjacent two points is small. The other is that the slope of the straight line is parallel to the direction *XL* of a laser range finder.

When the distance between adjacent two points is continuously within *d* and the slope between them is less than σ , the two points are considered as being on the straight line (Eq. (3)). The end points (stars in Fig. 12) of the straight line are the road edges and also the start points of the curbs. Therefore the curb edge points could be obtained by finding the end points of the straight line.

$$\sqrt{\left(\mathbf{x}\_{i} - \mathbf{x}\_{i+1}\right)^{2} + \left(y\_{i} - y\_{i+1}\right)^{2}} \le d \quad \text{and} \quad \left|\tan^{-1}\left(\frac{y\_{i} - y\_{i+1}}{\mathbf{x}\_{i} - \mathbf{x}\_{i+1}}\right)\right| \ge \sigma \tag{3}$$

Fig. 12. Extracted curb edge points.

Tracking Control for Reliable Outdoor Navigation Using Curb Detection 335

If we assume that the curb is straight, we can express the curb edges to a line equation such

From the subsequent two curb points ( ), ( ) ( ) *Ck Ck x y* and ( 1), ( 1) ( ) *Ck Ck x y* − − , we can find the line

( ) ( 1) ( ) ( ) ( ) ( 1) ( ) ( 1) ( 1) ( ) ( ) ( 1) ( 1) ( )

− −

( )( ) ( ) 0

− +− + − =

− − − −

*Ck Ck C k C k Ck Ck Ck Ck Ck Ck Ck Ck Ck Ck*

*y y y y x x x x y y xx x y xy x y*

<sup>−</sup> −= − <sup>−</sup>

( )

( ) ( 1) ( 1) ( )

− −

We can also obtain the distance *<sup>k</sup> d* between the robot and the curb from the line equation

*<sup>k</sup>* between robot and curb by geometric relation as shown in Fig. 11.

*ax by c <sup>d</sup> a b* <sup>+</sup> <sup>+</sup> <sup>=</sup> +

2 2 *k k*

() () 2 2

atan2( , ) acos( )

The proposed curb detection algorithm is experimented to identify the performance of it. The mobile robot was driven along a road with the distance (1m) between the mobile robot

π

server sends the data of curb edge points (position data of stars in Fig. 14) to the remote server. The remote server receives the image and curbs edges data from the navigation server and displays the map of curbs and navigation information as shown in Fig. 16. The rectangular and the dots in the map represent the mobile robot and curb edge points,

() ()

*Lk Lk*

*<sup>L</sup>* in Fig. 4) between the mobile robot and the laser range finder

/ 4 are selected in Eq. (4.3). The navigation

*k*

+

*d*

*x y*

*Ck Ck Ck Ck*

−

( ) ( 1) ( 1) ( )

*cx y x y*

*Ck Ck Ck Ck*

*k*

() ()

atan2( , )

*y x d x y*

*k Lk Lk*

*k Lk Lk*

2 2 () ()

*k*

+

*Lk Lk*

acos( )

*y x*

= −

as 50mm and

−

*ay y bx x*

= − = − = −

*ax by c* + + = 0 (5)

(6)

(7)

(8)

**3.6 Curb line equation** 

equation parameters (,,) *abc* as shown in Eq. (6).

**3.7 Distance and angle between robot and curb** 

*<sup>k</sup>* can be obtained such as Eq. (8).

*k*

=

φ

σ

β

φ

φ

α

*kkk*

α β

= − =

as Eq. (5).

and angle

The angle

φ

φ

and curbs. The tilted angle (

is 25°. The values *d* and

respectively.

Eq. (7) shows the distance *<sup>k</sup> d* .

#### **3.5 Parameter estimation of curb**

The position of the mobile robot and the extracted curb point can be simultaneously represented in global coordinates. If we can calculate the relative pose of the mobile robot to the curb position, we can correct the mobile robot's pose against its colliding with the curb. Fig. 13 represents the both position in global coordinates.

k ( ) ( ) : position of the robot in global coordinate : y position of the robot in global coordinate : orientation of the robot in global coordinate : position of the curb in robot coordinate : y po *k k L k L k x x y x x y* θ( ) ( ) sition of the curb in robot coordinate : position of the curb in global coordinate : y position of the curb in global coordinate *C k C k x x y*

We have obtained ( ), ( ) (, ) *nx y k Lk Lk* from curb detection method as explained in Section III. Curb position ( ), ( ) ( ) *Ck Ck x y* in global coordinate can be obtained from the curb position ( ), ( ) ( ) *Lk Lk x y* as shown in Eq. (4).

$$
\begin{pmatrix} \mathbf{x}\_{\mathcal{C}(k)} \\ \mathbf{y}\_{\mathcal{C}(k)} \\ \mathbf{1} \end{pmatrix} = \begin{pmatrix} -\sin\theta\_k & -\cos\theta\_k & \mathbf{x}\_k \\ \cos\theta\_k & -\sin\theta\_k & \mathbf{y}\_k \\ \mathbf{0} & \mathbf{0} & \mathbf{1} \end{pmatrix} \begin{pmatrix} \mathbf{x}\_{L(k)} \\ \mathbf{y}\_{L(k)} \\ \mathbf{1} \end{pmatrix} \tag{4}$$

#### **3.6 Curb line equation**

334 Recent Advances in Mobile Robotics

The position of the mobile robot and the extracted curb point can be simultaneously represented in global coordinates. If we can calculate the relative pose of the mobile robot to the curb position, we can correct the mobile robot's pose against its colliding with the curb.

XR(k)

αk

k d

βk

XR(k-1)

φk-1 αk-1

sition of the curb in robot coordinate : position of the curb in global coordinate : y position of the curb in global coordinate

: position of the robot in global coordinate : y position of the robot in global coordinate : orientation of the robot in global coordinate : position of the curb in robot coordinate

We have obtained ( ), ( ) (, ) *nx y k Lk Lk* from curb detection method as explained in Section III. Curb position ( ), ( ) ( ) *Ck Ck x y* in global coordinate can be obtained from the curb position

> ( ) ( ) ( ) ( )

 θ

 θ

*C k L k k kk C k k k k Lk*

⎛ ⎞ ⎛ ⎞ − − ⎛ ⎞ ⎜ ⎟ ⎜ ⎟⎜ ⎟ ⎜ ⎟ = − ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ ⎝ ⎠ ⎝ ⎠⎝ ⎠

sin cos cos sin 1 1 0 01

*x x x y y y* θ

θ

βk-1

k-1 d

() () () () (, , ) ( , ) *k Lk Lk Ck Ck nx y x y*

> -1 ( -1) ( -1) ( -1) ( -1)

> > (4)

( , , ( , ) *k Lk Lk Ck Ck nx y x y*

**3.5 Parameter estimation of curb** 

Fig. 13 represents the both position in global coordinates.

YG

k ( ) ( )

θ

*y*

*y*

( ), ( ) ( ) *Lk Lk x y* as shown in Eq. (4).

*L k L k*

*x x*

( ) ( )

*x x*

*C k C k*

*k k*

*x x y*

: y po

Fig. 13. Positions of the mobile robot and curbs in global coordinate.

YR(k)

YR(k-1)

k

φ

XG

If we assume that the curb is straight, we can express the curb edges to a line equation such as Eq. (5).

$$a\mathbf{x} + by + c = \mathbf{0} \tag{5}$$

From the subsequent two curb points ( ), ( ) ( ) *Ck Ck x y* and ( 1), ( 1) ( ) *Ck Ck x y* − − , we can find the line equation parameters (,,) *abc* as shown in Eq. (6).

$$\begin{aligned} y - y\_{\mathcal{C}(k)} &= \frac{y\_{\mathcal{C}(k)} - y\_{\mathcal{C}(k-1)}}{x\_{\mathcal{C}(k)} - x\_{\mathcal{C}(k-1)}} (\mathbf{x} - \mathbf{x}\_{\mathcal{C}(k)}) \\ \mathbf{x} \left( y\_{\mathcal{C}(k)} - y\_{\mathcal{C}(k-1)} \right) \mathbf{x} + (\mathbf{x}\_{\mathcal{C}(k-1)} - \mathbf{x}\_{\mathcal{C}(k)}) y \\ &+ (\mathbf{x}\_{\mathcal{C}(k)} y\_{\mathcal{C}(k-1)} - \mathbf{x}\_{\mathcal{C}(k-1)} y\_{\mathcal{C}(k)}) = \mathbf{0} \\ a &= y\_{\mathcal{C}(k)} - y\_{\mathcal{C}(k-1)} \\ b &= \mathbf{x}\_{\mathcal{C}(k-1)} - \mathbf{x}\_{\mathcal{C}(k)} \\ c &= \mathbf{x}\_{\mathcal{C}(k)} y\_{\mathcal{C}(k-1)} - \mathbf{x}\_{\mathcal{C}(k-1)} y\_{\mathcal{C}(k)} \end{aligned} \tag{6}$$

#### **3.7 Distance and angle between robot and curb**

We can also obtain the distance *<sup>k</sup> d* between the robot and the curb from the line equation and angle φ*<sup>k</sup>* between robot and curb by geometric relation as shown in Fig. 11. Eq. (7) shows the distance *<sup>k</sup> d* .

$$d\_k = \frac{|a\mathbf{x}\_k + b\mathbf{y}\_k + c|}{\sqrt{a^2 + b^2}} \tag{7}$$

The angle φ*<sup>k</sup>* can be obtained such as Eq. (8).

$$\begin{aligned} \phi\_k &= \alpha\_k - \beta\_k\\ \alpha\_k &= \operatorname{atan2}(y\_{L(k)}, x\_{L(k)})\\ \beta\_k &= \operatorname{acos}(\frac{d\_k}{\sqrt{\mathbf{x}^2\_{L(k)} + \mathbf{y}^2\_{L(k)}}})\\ \phi\_k &= \operatorname{atan2}(y\_{L(k)}, x\_{L(k)}) - \operatorname{acos}(\frac{d\_k}{\sqrt{\mathbf{x}^2\_{L(k)} + \mathbf{y}^2\_{L(k)}}}) \end{aligned} \tag{8}$$

The proposed curb detection algorithm is experimented to identify the performance of it. The mobile robot was driven along a road with the distance (1m) between the mobile robot and curbs. The tilted angle (φ *<sup>L</sup>* in Fig. 4) between the mobile robot and the laser range finder is 25°. The values *d* and σ as 50mm and π / 4 are selected in Eq. (4.3). The navigation server sends the data of curb edge points (position data of stars in Fig. 14) to the remote server. The remote server receives the image and curbs edges data from the navigation server and displays the map of curbs and navigation information as shown in Fig. 16. The rectangular and the dots in the map represent the mobile robot and curb edge points, respectively.

Tracking Control for Reliable Outdoor Navigation Using Curb Detection 337

The left and right-side stars and the black triangles in Fig. 15(b), 17 show the detected curbs and the robot path, respectively. The left-side stars in Figures are irregular because the rightside curb has regular shape (height: 13cm), but the left-side of the road is not the curb. It is just the structure with low height (2cm). Even though the mobile robot moves zigzag, the

detected right-side curb path is smooth. The average error of the curb points is 2cm.

Fig. 16. Screen of the remote server.

Fig. 17. Robot path and extracted curb positions.

Fig. 14. Extracted curb edge points on the road.

Fig. 16. Screen of the remote server.

336 Recent Advances in Mobile Robotics

(a) (b)

(a) The mobile robot tracking the curbs (b) Robot path and detected curbs.

Fig. 14. Extracted curb edge points on the road.

Fig. 15. Curb detection experiment.

The left and right-side stars and the black triangles in Fig. 15(b), 17 show the detected curbs and the robot path, respectively. The left-side stars in Figures are irregular because the rightside curb has regular shape (height: 13cm), but the left-side of the road is not the curb. It is just the structure with low height (2cm). Even though the mobile robot moves zigzag, the detected right-side curb path is smooth. The average error of the curb points is 2cm.

Fig. 17. Robot path and extracted curb positions.

Tracking Control for Reliable Outdoor Navigation Using Curb Detection 339

(c)

An ATRV-mini is a nonholonomic mobile robot, equivalent to a four-wheeled differential drive. Kanayama proposed a kinematic control law in Cartesian coordinates for solving the trajectory tracking problems [10]. We add a term related to the distance between the robot

Yr

*r*

between robot and curb.

Xr

XG

φ

Fig. 18. Results for curb detection. (c) Angle

**4. Tracking control using curb position** 

and the right-side curb of roads to Kanayama controller.

YG

*r y*

*c y*

Fig. 19. Mobile robot and reference robot.

θ*c*

θ*r*

*y e x e*

*c*

<sup>c</sup> *x <sup>r</sup>* <sup>O</sup> *x*

Xc Yc

*e*θ

Fig. 18 shows the distance between robot and curb, the curb edge angle α and angle φ between robot and curb, respectively. We can know that the proposed curb detection algorithm produces proper results and the distance and angle information calculated from the curb position could be used for reliable and safe navigation.

Fig 18. Results for curb detection. (a) Distance *d* between robot and curb (b) Curb edge angle α

338 Recent Advances in Mobile Robotics

between robot and curb, respectively. We can know that the proposed curb detection algorithm produces proper results and the distance and angle information calculated from

(a)

(b)

Fig 18. Results for curb detection. (a) Distance *d* between robot and curb (b) Curb edge

angle α α

and angle

φ

Fig. 18 shows the distance between robot and curb, the curb edge angle

the curb position could be used for reliable and safe navigation.

Fig. 18. Results for curb detection. (c) Angle φbetween robot and curb.

#### **4. Tracking control using curb position**

An ATRV-mini is a nonholonomic mobile robot, equivalent to a four-wheeled differential drive. Kanayama proposed a kinematic control law in Cartesian coordinates for solving the trajectory tracking problems [10]. We add a term related to the distance between the robot and the right-side curb of roads to Kanayama controller.

Fig. 19. Mobile robot and reference robot.

Tracking Control for Reliable Outdoor Navigation Using Curb Detection 341

We have performed a straight path tracking experiment, which target point is (78m, -22m) and linear velocity is 0.5m/sec. The gains of controller were determined by some simulations and experiments. Fig. 20 shows the reference path and the error of path, position and angle of the robot. Fig. 20(a) shows the reference path and tracking path and the position error of 20cm at the final point, respectively. Even though DGPS position error of 1m has occurred at the point of 12m from X-axis, the mobile robot returned to its

Reference and Tracking path

reference path tracking path


(a)

Commanded linear velocity

<sup>0</sup> <sup>20</sup> <sup>40</sup> <sup>60</sup> <sup>80</sup> <sup>100</sup> <sup>120</sup> <sup>140</sup> <sup>160</sup> -0.1

Time [sec]

(b)

**4.1 Straight path tracking** 

reference path by the stable tracking controller.

5

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7

linear velocity [m/sec]

Fig. 20. (Continued)

10

15

20

25

30

Considering the Fig. 19, in which the mobile robot *c* is represented by the coordinates [ ]*<sup>T</sup> c c* **p** = *x y* θ of the center of mass, moving over plane with linear velocity υ and angular velocity ω . The matrix equation that describes the kinematic model of a mobile robot is given by

$$
\begin{bmatrix}
\dot{\mathbf{x}}\_c \\
\dot{y}\_c \\
\dot{\theta}
\end{bmatrix} = \begin{bmatrix}
\cos\theta & -\sin\theta \\
\sin\theta & \cos\theta \\
0 & 1
\end{bmatrix} \begin{bmatrix}
\nu \\
\nu \\
\theta
\end{bmatrix} \tag{9}
$$

The posture tracking error vector *pe* is calculated in the reference robot frame , *X Yr r* .

$$\begin{aligned} e\_p &= R(\theta)(p\_r - p) \\ e\_p &= \begin{bmatrix} e\_x \\ e\_y \\ e\_\theta \end{bmatrix} = \begin{bmatrix} \cos\theta & \sin\theta & 0 \\ -\sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x\_r - x\_c \\ y\_r - y\_c \\ \theta\_r - \theta\_c \end{bmatrix} \end{aligned} \tag{10}$$

where *R*( ) θ is the rotation matrix. Differentiating the Eq. (10), yields

$$\begin{bmatrix} \dot{e}\_x\\ \dot{e}\_y\\ \dot{e}\_\theta \end{bmatrix} = \begin{bmatrix} \alpha e\_y - \nu + \nu\_r \cos(e\_\theta) \\ -\alpha e\_x + \nu\_r \sin(e\_\theta) \\ \alpha e\_r - \alpha \end{bmatrix} \tag{11}$$

The asymptotic convergence of posture error can be obtained

$$V\_c = \begin{bmatrix} \nu\_c \\ \alpha\_c \end{bmatrix} = \begin{bmatrix} k\_x \ e\_x + \nu\_r \cos(e\_\theta) \\ \alpha\_r + k\_y \ \nu\_r \ e\_y + k\_\theta \ \nu\_r \sin(e\_\theta) \end{bmatrix}' \tag{12}$$

where , *<sup>x</sup> <sup>y</sup> k k* and *k*θare positive constants, defined as controller gains.

For reliable and safe navigation, we design a new controller *Vd* by adding the extracted curb distance and angle information to the above Kanayama controller.

$$\begin{aligned} e\_d &= d\_{ref} - d\\ V\_d &= \begin{bmatrix} \nu\_d\\ \alpha\_d \end{bmatrix} = \begin{bmatrix} k\_x \ e\_x + \nu\_r \cos(\varepsilon\_\theta) \\\ \alpha\_r + \nu\_r (k\_y \ e\_y + k\_\theta \sin(\varepsilon\_\theta) + k\_d \ e\_d + k\_\phi \sin\phi) \end{bmatrix}' \end{aligned} \tag{13}$$

where *ref d* is the reference distance from the curb for safety and , *<sup>d</sup> k k*φ are positive constants as controller gains.

If the robot is too close to the curb, the added terms of the proposed controller try to prohibit the robot colliding with curbs, and vice versa.

We have performed two kinds of experiments to evaluate the performance of the proposed path tracking controller in outdoor environment. One is that the mobile robot has to follow a straight path. The other is that the mobile robot has to follow a curved path.

#### **4.1 Straight path tracking**

340 Recent Advances in Mobile Robotics

Considering the Fig. 19, in which the mobile robot *c* is represented by the coordinates

cos sin sin cos 0 1

θ

θ

The posture tracking error vector *pe* is calculated in the reference robot frame , *X Yr r* .

θ

θ

*y r x y xr*

ω

*c xx r*

ω

*r*

*c r yry r*

 are positive constants, defined as controller gains. For reliable and safe navigation, we design a new controller *Vd* by adding the extracted

⎡ ⎤ <sup>⎡</sup> <sup>+</sup> <sup>⎤</sup> = = ⎢ ⎥ <sup>⎢</sup> <sup>⎥</sup> + + ⎣ ⎦ ⎢⎣ ⎥⎦

cos( )

⎡ ⎤ <sup>⎡</sup> <sup>+</sup> <sup>⎤</sup> = = ⎢ ⎥ <sup>⎢</sup> <sup>⎥</sup> + + ++ ⎣ ⎦ ⎢⎣ ⎥⎦

If the robot is too close to the curb, the added terms of the proposed controller try to prohibit

We have performed two kinds of experiments to evaluate the performance of the proposed path tracking controller in outdoor environment. One is that the mobile robot has to follow a

θ

θθ

*d r r yy d d*

*ke e <sup>V</sup> ke k e ke k*

υ

*<sup>V</sup> k ek e*

ωυ

=− + ⎢ ⎥ <sup>⎢</sup> <sup>⎥</sup> ⎢ ⎥ <sup>⎢</sup> <sup>⎥</sup> <sup>−</sup> ⎣ ⎦ <sup>⎣</sup> <sup>⎦</sup>

ω ω

*e e e ee e*

 υυ

⎡ ⎤ <sup>⎡</sup> − + <sup>⎤</sup> ⎢ ⎥ <sup>⎢</sup> <sup>⎥</sup>

 υ

⎡ ⎤ ⎡ ⎤ <sup>−</sup> ⎢ ⎥ ⎢ ⎥⎡ <sup>⎤</sup> ⎢ ⎥ <sup>=</sup> ⎢ ⎥⎢ <sup>⎥</sup> <sup>⎣</sup> <sup>⎦</sup> ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦

cos sin 0 sin cos 0 001

*x r c p y r c*

 θ

 θ

*e x x e e y y*

⎡ ⎤ <sup>⎡</sup> ⎤⎡ ⎤ <sup>−</sup> ⎢ ⎥ <sup>⎢</sup> ⎥⎢ ⎥ <sup>=</sup> ⎢ ⎥ <sup>=</sup> − − <sup>⎢</sup> ⎥⎢ ⎥ ⎢ ⎥ <sup>⎢</sup> ⎥⎢ ⎥ <sup>−</sup> ⎣ ⎦ <sup>⎣</sup> ⎦⎣ ⎦

. The matrix equation that describes the kinematic model of a mobile robot is

 θ

 θ υ

ω

*r c*

θ

θ

cos( ) sin( )

θ

cos( )

θ

 υ

( sin( ) sin )

θ

*ke e*

υ

sin( )

 θ

> φ

 φ

θ

υ

, (10)

, (12)

, (13)

are positive

φ

and angular

(9)

(11)

of the center of mass, moving over plane with linear velocity

*c c x y*

 

θ

( )( )

*p r*

*e*θ

*e*

υ

ω

curb distance and angle information to the above Kanayama controller.

*d xx r*

where *ref d* is the reference distance from the curb for safety and , *<sup>d</sup> k k*

straight path. The other is that the mobile robot has to follow a curved path.

ω υ

The asymptotic convergence of posture error can be obtained

*c*

 

θ

is the rotation matrix.

Differentiating the Eq. (10), yields

θ

*d ref*

*ed d*

= −

υ

ω

*d*

the robot colliding with curbs, and vice versa.

constants as controller gains.

*e R pp*

= −

θ

[ ]*<sup>T</sup> c c* **p** = *x y*

velocity

given by

where *R*( ) θ

where , *<sup>x</sup> <sup>y</sup> k k* and *k*

θ

ω

We have performed a straight path tracking experiment, which target point is (78m, -22m) and linear velocity is 0.5m/sec. The gains of controller were determined by some simulations and experiments. Fig. 20 shows the reference path and the error of path, position and angle of the robot. Fig. 20(a) shows the reference path and tracking path and the position error of 20cm at the final point, respectively. Even though DGPS position error of 1m has occurred at the point of 12m from X-axis, the mobile robot returned to its reference path by the stable tracking controller.

Fig. 20. (Continued)

Tracking Control for Reliable Outdoor Navigation Using Curb Detection 343

angle error

<sup>0</sup> <sup>20</sup> <sup>40</sup> <sup>60</sup> <sup>80</sup> <sup>100</sup> <sup>120</sup> <sup>140</sup> <sup>160</sup> -14

Time [sec]

(f) Fig. 20. Results for straight path tracking. (a) Reference path and tracking path (b) Controller linear velocity (c) Controller angular velocity (d) X-axis position error for robot coordinate

We have performed a curved path tracking experiment, which linear velocity is 0.3m/sec and angular velocity is 0.06rad/s. Fig. 21 shows the reference path and the error of path, position and angle of the robot. Fig. 21(a) shows the reference path and tracking path. The X-axis position error and the angular error are 40cm and 7° at the final point, respectively. The reason that the errors are bigger than the straight path tracking experiment is related to the characteristic of skid steering system of our mobile robot platform. It is necessary to

Reference and Traking path

reference path tracking path


X [m]

(a)

(e) Y-axis position error for robot coordinate (f) Angle error for robot coordinate.


optimize the gains of controller to reduce the errors.


Fig. 21. (Continued)

0

2

4

Y [m]

6

8

10

**4.2 Curved path tracking** 

[deg]

Fig. 20. (Continued)

Fig. 20. Results for straight path tracking. (a) Reference path and tracking path (b) Controller linear velocity (c) Controller angular velocity (d) X-axis position error for robot coordinate (e) Y-axis position error for robot coordinate (f) Angle error for robot coordinate.

#### **4.2 Curved path tracking**

342 Recent Advances in Mobile Robotics

Commanded angular velocity

<sup>0</sup> <sup>20</sup> <sup>40</sup> <sup>60</sup> <sup>80</sup> <sup>100</sup> <sup>120</sup> <sup>140</sup> <sup>160</sup> -10

Time [sec]

(c)

X-axis position error

<sup>0</sup> <sup>20</sup> <sup>40</sup> <sup>60</sup> <sup>80</sup> <sup>100</sup> <sup>120</sup> <sup>140</sup> <sup>160</sup> -1.2

(d)

Y-axis position error

<sup>0</sup> <sup>20</sup> <sup>40</sup> <sup>60</sup> <sup>80</sup> <sup>100</sup> <sup>120</sup> <sup>140</sup> <sup>160</sup> -0.4

(e)

Time [sec]

Time [sec]


0 0.2



Fig. 20. (Continued)

[m]


[m]

0

5

angular velocity [deg/sec]

10

15

20

We have performed a curved path tracking experiment, which linear velocity is 0.3m/sec and angular velocity is 0.06rad/s. Fig. 21 shows the reference path and the error of path, position and angle of the robot. Fig. 21(a) shows the reference path and tracking path. The X-axis position error and the angular error are 40cm and 7° at the final point, respectively. The reason that the errors are bigger than the straight path tracking experiment is related to the characteristic of skid steering system of our mobile robot platform. It is necessary to optimize the gains of controller to reduce the errors.

Fig. 21. (Continued)

Tracking Control for Reliable Outdoor Navigation Using Curb Detection 345

Y-axis position error

<sup>0</sup> <sup>5</sup> <sup>10</sup> <sup>15</sup> <sup>20</sup> <sup>25</sup> <sup>30</sup> <sup>35</sup> <sup>40</sup> <sup>45</sup> -0.14

Time [sec]

(e)

angle error

<sup>0</sup> <sup>5</sup> <sup>10</sup> <sup>15</sup> <sup>20</sup> <sup>25</sup> <sup>30</sup> <sup>35</sup> <sup>40</sup> <sup>45</sup> -6

Time [sec]

(f)

In this chapter, an algorithm is described to detect road curb position through a laser range finder. The curbs map was built and the width of the road was calculated by the detected curbs. The proposed algorithm is to calculate the distance and the angle between the mobile robot and the curb. A new controller is designed by adding the extracted curb distance and angle information to the tracking controller. It helps the mobile robot to navigate safely and reliably. To identify the availability of the proposed algorithm, navigation experiments was

performed in outdoor environment and proper results was obtained.

(a) Reference path and tracking path (b) Controller linear velocity (c) Controller angular velocity (d) X-axis position error for robot coordinate (e) Y-axis position error for robot


> -4 -2 0 2 4

Fig. 21. Results for curved path tracking.

**5. Conclusion** 

coordinate (f) Angle error for robot coordinate

[deg]

6 8

[m]

Fig. 21. (Continued)

Fig. 21. Results for curved path tracking.

(a) Reference path and tracking path (b) Controller linear velocity (c) Controller angular velocity (d) X-axis position error for robot coordinate (e) Y-axis position error for robot coordinate (f) Angle error for robot coordinate

#### **5. Conclusion**

344 Recent Advances in Mobile Robotics

Commanded linear velocity

<sup>0</sup> <sup>5</sup> <sup>10</sup> <sup>15</sup> <sup>20</sup> <sup>25</sup> <sup>30</sup> <sup>35</sup> <sup>40</sup> <sup>45</sup> <sup>0</sup>

Time [sec]

(b)

Commanded angular velocity

<sup>0</sup> <sup>5</sup> <sup>10</sup> <sup>15</sup> <sup>20</sup> <sup>25</sup> <sup>30</sup> <sup>35</sup> <sup>40</sup> <sup>45</sup> -4

Time [sec]

(c)

X-axis position error

<sup>0</sup> <sup>5</sup> <sup>10</sup> <sup>15</sup> <sup>20</sup> <sup>25</sup> <sup>30</sup> <sup>35</sup> <sup>40</sup> <sup>45</sup> -0.5

Time [sec]

(d)

0.05 0.1 0.15


Fig. 21. (Continued)

[m]

angular velocity [deg/sec]

0.2 0.25 0.3 0.35

linear velocity [m/sec]

In this chapter, an algorithm is described to detect road curb position through a laser range finder. The curbs map was built and the width of the road was calculated by the detected curbs. The proposed algorithm is to calculate the distance and the angle between the mobile robot and the curb. A new controller is designed by adding the extracted curb distance and angle information to the tracking controller. It helps the mobile robot to navigate safely and reliably. To identify the availability of the proposed algorithm, navigation experiments was performed in outdoor environment and proper results was obtained.

**Part 4** 

**Methods for Control** 

#### **6. References**


## **Part 4**

**Methods for Control** 

346 Recent Advances in Mobile Robotics

[1] S. Thrun at el. (2000). Probabilistic Algorithms and the Interactive Museum Tour-Guide Robot Minerva, *International Journal of Robotics Research*, Vol.19, No.11, pp. 972-999 [2] Richard Thrapp at el. (2001). Robust localization algorithms for an autonomous campus tour guide, *Proc. of IEEE Int'l Conf. on Robotics and Automation*, pp. 2065-2071 [3] C.C. Wang and C. Thorpe, "Simultaneous Localization and Mapping with Detection and

[4] W. S. Wijesoma, K. R. S. Kodagoda, and Arjuna P. Balasuriya, "Road-Boundary

[5] Seung-Hun Kim at el. (2006). A hybrid autonomous/teleoperated strategy for reliable mobile robot outdoor navigation, *SICE-ICASE Int. Joint Conf.*, pp. 3120-3125 [6] K. Ohno at el. (2003). Outdoor Navigation of a Mobile Robot between Buildings based on

[7] J. Forsberg at el. (March 1995). Mobile robot navigation using the rangeweighted Hough

[8] R. O. Duda. & P. E. Hart. (1972). Use of the Hough transformation to detect lines and

[9] P. V. C. Hough. (December 1962). Methods and means for recognizing complex patterns,

[10] Y. Kanayama at el. (1990). A stable tracking control method for an autonomous mobile

transform, *IEEE Robotics & Automation Magazine*, vol. 2, pp. 18-26

curves in pictures, *Communications of the ACM*, vol. 15, pp. 11-15

robot, *IEEE Int. Conf. on Robotics and Automation*, vol. 1, pp. 384-389

Tracking of Moving Objects", Proc. of IEEE Int'l Conf. on Robotics and

Detection and Tracking Using Ladar Sensing", IEEE Trans. On Robotics and

DGPS and Odometry Data Fusion, *Proc. of IEEE Int'l Conf. on Robotics and* 

**6. References** 

Automation, 2002, pp. 2918-2924

Automation, Vol.20, No.3, 2004

*Automation*, pp. 1978-1984

in U.S. Patent 3 069 654

**0**

**17**

*Poland*

Krzysztof Okarma and Piotr Lech

*West Pomeranian University of Technology, Szczecin*

**Statistical Video Based Control of Mobile Robots**

A typical approach to the control of mobile robots is based on the analysis of signals from various kinds of sensors (e.g. infra-red or ultrasound). Another type of input data used for motion control of robots can be video signals acquired by the cameras. In such case many algorithms can be utilised e.g. SLAM (Simultaneous Localization and Mapping) and its modifications, being still an active field of research (Kalyan et al., 2010), which require usually the full analysis of the images (or video frames) being the input data (Se et al., 2001; 2002), in some applications using also additional operations such as Principal Component Analysis (Tamimi & Zell, 2004). Similar problems may also occur in the robot localisation and

An interesting alternative can be the application of the fast image analysis techniques based on the statistical experiments. Presented statistical approach is related to the first step of such control systems, which can be considered as the pre-processing technique, reducing the amount of data for further analysis with similar control accuracy. Discussed methods can be utilised for many control algorithms based on the image and video data e.g. the proportional steering of line following robots (Cowan et al., 2003) considered as a representative example in further discussions. Nevertheless, some other applications of the presented approach are also possible e.g. fast object classification based on the shape analysis (Okarma & Lech, 2009) or motion detection, as well as fast reduced-reference image quality estimation (Okarma &

Considering the autonomous mobile robots, one of their most important parameters, corresponding directly to their working properties, is the power consumption related to the maximum possible working time. Limited energy resources are often the main element reducing the practical applicability of many such constructions regardless of many modern energy sources which can be used such as e.g. solar based solutions. The limited capacity of the installed batteries reduces the range and capabilities of these devices and each of the possible attempts to optimise energy consumption usually affects the mobility and efficiency

It is possible to reduce the consumed energy by finding the shortest path to the target (robot's drive) or conducting the optimisation of the amount of processed data by the control algorithm. Since the computational cost of mapping algorithms, searching for the optimal path, finding robot's own location, visual SLAM algorithms etc., based on the image analysis is strongly dependent on the resolution and representation of the analysed image, a significant "smart" decrease of the amount of processed data seems to be an interesting direction of

**1. Introduction**

Lech, 2008b).

of the robots.

map building processes (Hähnel et al., 2003).

**2. Reduction of power consumption by mobile robots**

### **Statistical Video Based Control of Mobile Robots**

Krzysztof Okarma and Piotr Lech

*West Pomeranian University of Technology, Szczecin Poland*

#### **1. Introduction**

A typical approach to the control of mobile robots is based on the analysis of signals from various kinds of sensors (e.g. infra-red or ultrasound). Another type of input data used for motion control of robots can be video signals acquired by the cameras. In such case many algorithms can be utilised e.g. SLAM (Simultaneous Localization and Mapping) and its modifications, being still an active field of research (Kalyan et al., 2010), which require usually the full analysis of the images (or video frames) being the input data (Se et al., 2001; 2002), in some applications using also additional operations such as Principal Component Analysis (Tamimi & Zell, 2004). Similar problems may also occur in the robot localisation and map building processes (Hähnel et al., 2003).

An interesting alternative can be the application of the fast image analysis techniques based on the statistical experiments. Presented statistical approach is related to the first step of such control systems, which can be considered as the pre-processing technique, reducing the amount of data for further analysis with similar control accuracy. Discussed methods can be utilised for many control algorithms based on the image and video data e.g. the proportional steering of line following robots (Cowan et al., 2003) considered as a representative example in further discussions. Nevertheless, some other applications of the presented approach are also possible e.g. fast object classification based on the shape analysis (Okarma & Lech, 2009) or motion detection, as well as fast reduced-reference image quality estimation (Okarma & Lech, 2008b).

#### **2. Reduction of power consumption by mobile robots**

Considering the autonomous mobile robots, one of their most important parameters, corresponding directly to their working properties, is the power consumption related to the maximum possible working time. Limited energy resources are often the main element reducing the practical applicability of many such constructions regardless of many modern energy sources which can be used such as e.g. solar based solutions. The limited capacity of the installed batteries reduces the range and capabilities of these devices and each of the possible attempts to optimise energy consumption usually affects the mobility and efficiency of the robots.

It is possible to reduce the consumed energy by finding the shortest path to the target (robot's drive) or conducting the optimisation of the amount of processed data by the control algorithm. Since the computational cost of mapping algorithms, searching for the optimal path, finding robot's own location, visual SLAM algorithms etc., based on the image analysis is strongly dependent on the resolution and representation of the analysed image, a significant "smart" decrease of the amount of processed data seems to be an interesting direction of


 -

 

 

-
- -

 ---

!" #

&"# ' ((


> )-

dependent on the frame rate of the analysed video sequence

dependent on the image resolution

\$- 

-

- -

Fig. 2. Illustration of the general vision based control algorithm of the mobile robot

 × 480 2.15 5.022 × 288 2.39 4.519 × 240 2.69 4.268 × 144 2.80 4.168 × 120 2.95 4.369 Table 1. Path length and power consumption of the mobile robot used in the experiments

Resolution in pixels Length of the path [m] Total energy consumption [kJ]

Frame rate [fps] Length of the path [m] Total energy consumption [kJ] 2.39 4.519 2.44 4.432 2.66 4.332 3.23 4.668 3.33 4.798 Table 2. Path length and power consumption of the mobile robot used in the experiments

Statistical Video Based Control of Mobile Robots 351

 


 - 

research in this area. Reducing the computational load of the processor the overall demand for the electrical energy decreases, so it can be assumed that limiting the amount of processed data in the system, energy savings can be obtained, despite using a non-optimal (longer) motion path. For the video based control algorithms such restriction can be achieved simply by reducing the image resolution or reducing the frame rate of the acquired image sequence used for he robot's motion control. Nevertheless, such decrease of the amount of analysed data may lead to some steering errors, as illustrated in Fig. 1, where the optimal shortest path cannot be used due to the presence of some obstacles. Each of the indicated alternative paths has a different length related to the different energy necessary for the robot to reach the target point.

Fig. 1. Illustration of the problem of the optimal path searching in the presence of obstacles

The optimal robot's path in the known environment can be chosen using the machine vision approach with motion detection and robot's motion parameters estimation, using the general algorithm presented in Fig. 2 such that the total energy consumption, related both to the robot's drive and the control part dependent mainly on the processing of the vision data, is minimum. Conducting the experimental verification of the idea for the mobile robot based on a simple netbook platform using the joystick port for controlling the drives, for the testing area of 6 square meters illustrated in Fig. 1, the maximum power consumption is about 5.2 A (42.5% by the drive part and 57.5% by the control part of the system). The length of the optimum straight path, assuming no obstacles on the scene, is equal to 2 meters so all the alternative trajectories are longer.

The experiments conducted in the environment described above has been related to the decrease of the resolution of the image (obtained results are presented in Table 1) and the frame rate for the fixed resolution of 320 × 240 pixels (results shown in Table 2). For the reduction to 8 frames per second the vision based control algorithm could not find any appropriate path.

2 Will-be-set-by-IN-TECH

research in this area. Reducing the computational load of the processor the overall demand for the electrical energy decreases, so it can be assumed that limiting the amount of processed data in the system, energy savings can be obtained, despite using a non-optimal (longer) motion path. For the video based control algorithms such restriction can be achieved simply by reducing the image resolution or reducing the frame rate of the acquired image sequence used for he robot's motion control. Nevertheless, such decrease of the amount of analysed data may lead to some steering errors, as illustrated in Fig. 1, where the optimal shortest path cannot be used due to the presence of some obstacles. Each of the indicated alternative paths has a different length related to the different energy necessary for the robot to reach the target

> --

Fig. 1. Illustration of the problem of the optimal path searching in the presence of obstacles The optimal robot's path in the known environment can be chosen using the machine vision approach with motion detection and robot's motion parameters estimation, using the general algorithm presented in Fig. 2 such that the total energy consumption, related both to the robot's drive and the control part dependent mainly on the processing of the vision data, is minimum. Conducting the experimental verification of the idea for the mobile robot based on a simple netbook platform using the joystick port for controlling the drives, for the testing area of 6 square meters illustrated in Fig. 1, the maximum power consumption is about 5.2 A (42.5% by the drive part and 57.5% by the control part of the system). The length of the optimum straight path, assuming no obstacles on the scene, is equal to 2 meters so all the

The experiments conducted in the environment described above has been related to the decrease of the resolution of the image (obtained results are presented in Table 1) and the frame rate for the fixed resolution of 320 × 240 pixels (results shown in Table 2). For the reduction to 8 frames per second the vision based control algorithm could not find any

point.


alternative trajectories are longer.

appropriate path.

Fig. 2. Illustration of the general vision based control algorithm of the mobile robot


Table 1. Path length and power consumption of the mobile robot used in the experiments dependent on the image resolution


Table 2. Path length and power consumption of the mobile robot used in the experiments dependent on the frame rate of the analysed video sequence

For the infinite number of samples the reduction of the sampling distance takes place and the probability of choosing the point representing the object on the image can be expressed as:

Statistical Video Based Control of Mobile Robots 353

*KN <sup>N</sup>* <sup>=</sup> *AOb ASc*

*AOb* <sup>≈</sup> *KN* · *ASc*

Since the total area of the scene is equal to *ASc* = *N* · *kx* · *ky*, the estimated object's area is equal to *AOb* ≈ *KN* · *kx* · *ky*, where *kx* and *ky* are the scale factors for horizontal and vertical

Considering the above analysis, the probability of choosing the point belonging to the object can be used in the proposed algorithm with a reduced number of analysed samples instead of the full image analysis using a statistical experiment based on the Monte Carlo method. This method originates directly from the law of large numbers, because the sequence of successive approximations of the estimated value is convergent to the sought solution and the distance of the actual value after performing the specified number of statistical tests to the solution can

After the binarization the luminance (or colour) samples represented as "ones" or "zeros" corresponding to the allowed values of the specified logical condition, which are stored in one-dimensional vector are chosen randomly. For a single draw a random variable *Xi* of the

1 for black samples

An important element of the method is the proper choice of the logical condition for the binarization allowing a proper separation of the object from the background. Such choice depends on the specific application, for example for the light based vehicles' tracking purposes the chroma keying based on the CIELAB colour model can be appropriate (Mazurek

> *n* ∑ *i*=1

According to the Lindberg-Levy's theorem, the distribution of *Yn* tends to the normal distribution *N*(*my*, *σy*) if *n* → ∞. Since the expected value and variance of *Yn* are equal

*Yn* <sup>=</sup> <sup>1</sup> *n* · (2)

*<sup>N</sup>* (3)

0 for white samples, (4)

*Xi* (6)

*my* = *p* (7)

*P*(*Xi* = 1) = *p P*(*Xi* = 0) = *q* (5)

*<sup>n</sup>* respectively, the distrubution of the random value *Yn* is normal

*p* = lim *N*→∞

coordinates respectively, equivalent to the number of samples per unit.

*Xi* =

so the estimated area of the object is:

be determined using the central limit theorem.

leading to the following probability expressions:

For *n* independent draws the variable *Yn* is obtained:

where *p* + *q* = 1, *E*(*Xi*) = *p*, *V*(*Xi*) = *p* · *q*.

two-way distribution is obtained:

& Okarma, 2006).

to *<sup>E</sup>*(*Yn*) = *<sup>p</sup>* and *<sup>V</sup>*(*Yn*) = *<sup>p</sup>*·*<sup>q</sup>*

with the following parameters:

Analysing the results presented in Tables 1 and 2 a significant influence of the amount of the analysed video data on the obtained results and the power consumption can be noticed. The decrease of the resolution as well as the reduction of the video frame rate causes the increase of the path's length but due to the reduction of the processor's computational load the total energy consumption may decrease. Nevertheless, the decrease of the amount of the processed data should be balanced with the control correctness in order to prevent an excessive extension of the robot's path.

Since the simple decrease of the resolution may cause significant control errors as discussed above, a "smart" decrease of the number of analysed pixels, which does not cause large control errors should be used for such purpose much more efficiently. In the proposed statistical approach the main decrease of the amount of data is related to the random draw of a specified number of pixels for further analysis using the modified Monte Carlo method. In a typical version of this method (as often used for the fast object's area estimation) the pixels are chosen randomly by an independent drawing of two coordinates (horizontal and vertical), what complicates the error estimation. For the simplification, the additional mapping of pixels to the one-dimensional vector can be used, what is equivalent to the data transmission e.g. as an uncompressed stream. The additional reduction takes place during the binarization performed for the drawn pixels.

#### **3. The Monte Carlo method for the fast estimation of the objects' geometrical properties**

#### **3.1 The area estimation**

Conducting a statistical experiment based on the Monte Carlo method, an efficient method of estimation of the number of pixels in the image fulfilling a specified condition can be proposed, which can be used for the motion detection purposes as well as for the area estimation of objects together with their basic geometrical features. Instead of counting all the pixels from a high resolution image, the reduction of the number of pixels used for calculations can be achieved by using a pseudo-random generator of uniform distribution. Due to that the performance of the algorithm can be significantly increased. The logical condition which has to be fulfilled can be defined for a greyscale image as well as for the colour one, similarly to the chroma keying commonly used in television. leading to the binary image, which can be further processed. In order to prevent using two independent pseudo-random generators it is assumed that the numbered samples taken from the binary image are stored in the one-dimensional vector, where "1" denotes the black pixels and "0" stands for the white ones representing the background (or reversely). Then a single element from the vector is drawn, which is returned, so each random choice is conducted independently. For simplifying the further theoretical analysis the presence of a single moving dark object (Ob) in the scene (Sc) with a light background can be assumed.

In order to determine the object's geometrical features as well as some motion parameters, such as direction and velocity, the random choice of pixels cannot be performed using the whole image due to the integrating properties of the Monte Carlo method. For preventing possible errors caused by such integration the scene can be divided into smaller and smaller squares, so the *KN* squares from *N* elements of the scene would represent the object. In such case the probability of choosing the point on the object's surface (assuming the generator's uniform distribution) is equal to:

$$p = \frac{K\_N}{N} \tag{1}$$

4 Will-be-set-by-IN-TECH

Analysing the results presented in Tables 1 and 2 a significant influence of the amount of the analysed video data on the obtained results and the power consumption can be noticed. The decrease of the resolution as well as the reduction of the video frame rate causes the increase of the path's length but due to the reduction of the processor's computational load the total energy consumption may decrease. Nevertheless, the decrease of the amount of the processed data should be balanced with the control correctness in order to prevent an excessive extension

Since the simple decrease of the resolution may cause significant control errors as discussed above, a "smart" decrease of the number of analysed pixels, which does not cause large control errors should be used for such purpose much more efficiently. In the proposed statistical approach the main decrease of the amount of data is related to the random draw of a specified number of pixels for further analysis using the modified Monte Carlo method. In a typical version of this method (as often used for the fast object's area estimation) the pixels are chosen randomly by an independent drawing of two coordinates (horizontal and vertical), what complicates the error estimation. For the simplification, the additional mapping of pixels to the one-dimensional vector can be used, what is equivalent to the data transmission e.g. as an uncompressed stream. The additional reduction takes place during the binarization

**3. The Monte Carlo method for the fast estimation of the objects' geometrical**

Conducting a statistical experiment based on the Monte Carlo method, an efficient method of estimation of the number of pixels in the image fulfilling a specified condition can be proposed, which can be used for the motion detection purposes as well as for the area estimation of objects together with their basic geometrical features. Instead of counting all the pixels from a high resolution image, the reduction of the number of pixels used for calculations can be achieved by using a pseudo-random generator of uniform distribution. Due to that the performance of the algorithm can be significantly increased. The logical condition which has to be fulfilled can be defined for a greyscale image as well as for the colour one, similarly to the chroma keying commonly used in television. leading to the binary image, which can be further processed. In order to prevent using two independent pseudo-random generators it is assumed that the numbered samples taken from the binary image are stored in the one-dimensional vector, where "1" denotes the black pixels and "0" stands for the white ones representing the background (or reversely). Then a single element from the vector is drawn, which is returned, so each random choice is conducted independently. For simplifying the further theoretical analysis the presence of a single moving dark object (Ob) in the scene (Sc)

In order to determine the object's geometrical features as well as some motion parameters, such as direction and velocity, the random choice of pixels cannot be performed using the whole image due to the integrating properties of the Monte Carlo method. For preventing possible errors caused by such integration the scene can be divided into smaller and smaller squares, so the *KN* squares from *N* elements of the scene would represent the object. In such case the probability of choosing the point on the object's surface (assuming the generator's

*<sup>p</sup>* <sup>=</sup> *KN*

*<sup>N</sup>* (1)

of the robot's path.

**properties**

**3.1 The area estimation**

performed for the drawn pixels.

with a light background can be assumed.

uniform distribution) is equal to:

For the infinite number of samples the reduction of the sampling distance takes place and the probability of choosing the point representing the object on the image can be expressed as:

$$p = \lim\_{N \to \infty} \frac{K\_N}{N} = \frac{A\_{Ob}}{A\_{Sc}} \tag{2}$$

so the estimated area of the object is:

$$A\_{Ob} \approx K\_N \cdot \frac{A\_{Sc}}{N} \tag{3}$$

Since the total area of the scene is equal to *ASc* = *N* · *kx* · *ky*, the estimated object's area is equal to *AOb* ≈ *KN* · *kx* · *ky*, where *kx* and *ky* are the scale factors for horizontal and vertical coordinates respectively, equivalent to the number of samples per unit.

Considering the above analysis, the probability of choosing the point belonging to the object can be used in the proposed algorithm with a reduced number of analysed samples instead of the full image analysis using a statistical experiment based on the Monte Carlo method. This method originates directly from the law of large numbers, because the sequence of successive approximations of the estimated value is convergent to the sought solution and the distance of the actual value after performing the specified number of statistical tests to the solution can be determined using the central limit theorem.

After the binarization the luminance (or colour) samples represented as "ones" or "zeros" corresponding to the allowed values of the specified logical condition, which are stored in one-dimensional vector are chosen randomly. For a single draw a random variable *Xi* of the two-way distribution is obtained:

$$X\_{i} = \begin{cases} 1 & \text{for black samples} \\ 0 & \text{for white samples} \end{cases} \tag{4}$$

leading to the following probability expressions:

$$P(X\_i = 1) = p \qquad P(X\_i = 0) = q \tag{5}$$

where *p* + *q* = 1, *E*(*Xi*) = *p*, *V*(*Xi*) = *p* · *q*.

An important element of the method is the proper choice of the logical condition for the binarization allowing a proper separation of the object from the background. Such choice depends on the specific application, for example for the light based vehicles' tracking purposes the chroma keying based on the CIELAB colour model can be appropriate (Mazurek & Okarma, 2006).

For *n* independent draws the variable *Yn* is obtained:

$$Y\_n = \frac{1}{n} \cdot \sum\_{i=1}^n X\_i \tag{6}$$

According to the Lindberg-Levy's theorem, the distribution of *Yn* tends to the normal distribution *N*(*my*, *σy*) if *n* → ∞. Since the expected value and variance of *Yn* are equal to *<sup>E</sup>*(*Yn*) = *<sup>p</sup>* and *<sup>V</sup>*(*Yn*) = *<sup>p</sup>*·*<sup>q</sup> <sup>n</sup>* respectively, the distrubution of the random value *Yn* is normal with the following parameters:

$$m\_{\mathcal{Y}} = p \tag{7}$$

Such prepared array, considered as a reduced resolution "greyscale" image, can be directly utilised in some typical control algorithms. Further decrease of the computational cost can be obtained by the additional binarization of this image leading to the ultra-low resolution binary image considered in further discussions, where each of the blocks can be classified as a representative of an object or the background. For the line following robot control purposes the objects is equivalent to the path. The control accuracy of the mobile robots corresponds to the quality loss of the data present in this image. Such quality can be treated as the quality of the binary image assuming the knowledge of the reference image (without any distortions). Unfortunately, most of the image quality assessment methods, even the most recent ones, can be successfully applied only for greyscale or colour images and the specific character of the binary images is not respected by them. For this reason only the methods designed exclusively for the binary images can be used. Such binary image quality assessment methods can be used as the optimisation criteria for the proposed algorithm. In the result the proper choice of the block size, binarization threshold and the relative number of randomly drawn pixels can be

Statistical Video Based Control of Mobile Robots 355

Depending on the logical condition used for the statistical analysis (the construction of the 1-D binary vector), the algorithm can be implemented using various colour spaces (or only some chosen channels), with the possibility of utilising independent logical conditions for

The real-time navigation of an autonomous mobile robot based on the machine vision algorithms requires a fast processing of images acquired by the camera (or cameras) mounted on the robot. In some more sophisticated systems the data fusion based on the additional informations acquired by some other sensors, such as PIR sensors, ultrasound detectors, a radar technique equipment or optical barriers, is also possible. Most of such sensors are responsible for the motion detection, especially if the presence of some moving obstacles in the robot's surrounding is assumed. The vision based sensors can be divided into three main groups: analogue (i.e. comparing the luminance with a given threshold value), digital (obtained by the sampling and quantization of acquired analogue signal) and the digital ones acquired directly from digital video cameras. There are also some restrictions of typical alternative solutions, e.g. passive infrared detectors are sensitive to temperature changes and they are useless for the motion detection of objects with the same temperature as the background. Some more robust solutions, such as ultrasound and radar detectors, are usually active (energy emitting) and require an additional power supply. The application of a vision based motion detection procedure utilising the Monte Carlo method for the mobile robots control purposes is caused mainly by the fact that no additional hardware is necessary, since the images used by the motion detector are acquired by the same camera as in the classification

Assuming the scene with the constant light conditions without any moving objects, it can be characterised by a constant value defined as the number of black (or white) pixels of the respective binary image, which usually represent the object visible on the image obtained from the camera. The algorithm of the motion detection utilises the rapid changes of the number of such pixels (treated as "ones") when an object moves respectively to the light sources. Such change is caused by the dependence of the image pixel's luminance (and colour) on the angle between normal vector to its surface and the direction of the light ray passing the pixel, as well as the influence of the surface's reflection coefficient, shape, roughness etc. Real objects usually have a heterogeneous structure of their surface so the number of analysed points changes dynamically (assuming constant threshold or chroma keying range). For slow

chosen.

procedure.

each channel (chroma keying).

**3.2 The Monte Carlo based motion detection**

$$
\sigma\_{\mathcal{Y}} = \sqrt{\frac{p \cdot q}{n}} \tag{8}
$$

Considering the asymptotic normal distribution *N*(*p*, *<sup>p</sup>* · *<sup>q</sup>*/*n*) it can be stated that the central limit theorem is fulfilled for the variable *Yn*. Substituting:

$$\mathcal{U}\_n = \frac{Y\_n - m\_y}{\sigma\_y} \tag{9}$$

obtained normal distribution can be standardized towards the standard normal distribution *N*(0, 1).

In the interval estimation method the following formula is used:

$$p(|\mathcal{U}\_{\mathfrak{n}}| \le \mathfrak{a}) = 1 - \mathfrak{a} \tag{10}$$

Assuming the interval:

$$|\mathcal{U}\_{\mathbb{N}}| \le \mu\_{\mathbb{N}} \tag{11}$$

considering also the formulas (3), (7), (8) and (9), the following expression can be achieved:

$$\left| Y\_{\hbar} - \frac{\mathcal{K}\_{\mathcal{N}}}{N} \right| \le \varepsilon\_{\mathcal{N}} \tag{12}$$

where

$$
\varepsilon\_{\mathfrak{A}} = \frac{\mathfrak{u}\_{\mathfrak{A}}}{\sqrt{n}} \cdot \sqrt{\frac{\mathcal{K}\_N}{N} \cdot \left(1 - \frac{\mathcal{K}\_N}{N}\right)}\tag{13}
$$

The probability estimator *p* (eq. 1), for *k* elements from *n* draws, fulfilling the specified logical condition used for the definition of *Xi* (eq. 4) and representing the number of the drawn pixels representing the object, can be expressed as:

$$
\hat{p} = \frac{k}{n} = \frac{1}{n} \cdot \sum\_{i=1}^{n} X\_i = Y\_n \tag{14}
$$

and the object's area estimator as:

$$
\hat{A}\_{Ob} = \mathfrak{P} \cdot A\_{\mathrm{Sc}} = \frac{\mathrm{k}}{\mathrm{n}} \cdot A\_{\mathrm{Sc}} \tag{15}
$$

Using the equations (12) and (15) the obtained formula describing the interval estimation for the object's area is:

$$\left| \frac{\hat{A}\_{Ob}}{A\_{Sc}} - \frac{\mathcal{K}\_N}{N} \right| \le \varepsilon\_d \tag{16}$$

where *εα* is specified by the equation (13).

It is worth to notice that all the above considerations are correct only for a random number generator with the uniform distribution, which should have as good statistical properties as possible. The discussed algorithm are identical to the method of area estimation of the 2-D object's (expressed in pixels). Nevertheless, the applicability of such approach is limited by the integrating character of the method so an additional modification based on the block approach is necessary, as mentioned earlier.

6 Will-be-set-by-IN-TECH

*Un* <sup>=</sup> *Yn* <sup>−</sup> *my σy*

obtained normal distribution can be standardized towards the standard normal distribution

considering also the formulas (3), (7), (8) and (9), the following expression can be achieved:

*Yn* <sup>−</sup> *KN N* 

> *KN N* · <sup>1</sup> <sup>−</sup> *KN N*

The probability estimator *p* (eq. 1), for *k* elements from *n* draws, fulfilling the specified logical condition used for the definition of *Xi* (eq. 4) and representing the number of the drawn pixels

> *n* ∑ *i*=1

 

*εα* <sup>=</sup> *<sup>u</sup><sup>α</sup>* <sup>√</sup>*<sup>n</sup>* ·

*<sup>p</sup>*<sup>ˆ</sup> <sup>=</sup> *<sup>k</sup>*

*A*ˆ

 

*A*ˆ *Ob ASc*

*<sup>n</sup>* <sup>=</sup> <sup>1</sup> *n* ·

*Ob* <sup>=</sup> *<sup>p</sup>*<sup>ˆ</sup> · *ASc* <sup>=</sup> *<sup>k</sup>*

Using the equations (12) and (15) the obtained formula describing the interval estimation for

<sup>−</sup> *KN N*

It is worth to notice that all the above considerations are correct only for a random number generator with the uniform distribution, which should have as good statistical properties as possible. The discussed algorithm are identical to the method of area estimation of the 2-D object's (expressed in pixels). Nevertheless, the applicability of such approach is limited by the integrating character of the method so an additional modification based on the block approach

 

*<sup>p</sup>* · *<sup>q</sup>*

*<sup>n</sup>* (8)

*p*(|*Un*| ≤ *α*) = 1 − *α* (10)


≤ *εα* (12)

*Xi* = *Yn* (14)

*<sup>n</sup>* · *ASc* (15)

≤ *εα* (16)

*<sup>p</sup>* · *<sup>q</sup>*/*n*) it can be stated that the

(9)

(13)

*σ<sup>y</sup>* =

Considering the asymptotic normal distribution *N*(*p*,

In the interval estimation method the following formula is used:

central limit theorem is fulfilled for the variable *Yn*.

representing the object, can be expressed as:

and the object's area estimator as:

where *εα* is specified by the equation (13).

is necessary, as mentioned earlier.

the object's area is:

Substituting:

Assuming the interval:

*N*(0, 1).

where

Such prepared array, considered as a reduced resolution "greyscale" image, can be directly utilised in some typical control algorithms. Further decrease of the computational cost can be obtained by the additional binarization of this image leading to the ultra-low resolution binary image considered in further discussions, where each of the blocks can be classified as a representative of an object or the background. For the line following robot control purposes the objects is equivalent to the path. The control accuracy of the mobile robots corresponds to the quality loss of the data present in this image. Such quality can be treated as the quality of the binary image assuming the knowledge of the reference image (without any distortions). Unfortunately, most of the image quality assessment methods, even the most recent ones, can be successfully applied only for greyscale or colour images and the specific character of the binary images is not respected by them. For this reason only the methods designed exclusively for the binary images can be used. Such binary image quality assessment methods can be used as the optimisation criteria for the proposed algorithm. In the result the proper choice of the block size, binarization threshold and the relative number of randomly drawn pixels can be chosen.

Depending on the logical condition used for the statistical analysis (the construction of the 1-D binary vector), the algorithm can be implemented using various colour spaces (or only some chosen channels), with the possibility of utilising independent logical conditions for each channel (chroma keying).

#### **3.2 The Monte Carlo based motion detection**

The real-time navigation of an autonomous mobile robot based on the machine vision algorithms requires a fast processing of images acquired by the camera (or cameras) mounted on the robot. In some more sophisticated systems the data fusion based on the additional informations acquired by some other sensors, such as PIR sensors, ultrasound detectors, a radar technique equipment or optical barriers, is also possible. Most of such sensors are responsible for the motion detection, especially if the presence of some moving obstacles in the robot's surrounding is assumed. The vision based sensors can be divided into three main groups: analogue (i.e. comparing the luminance with a given threshold value), digital (obtained by the sampling and quantization of acquired analogue signal) and the digital ones acquired directly from digital video cameras. There are also some restrictions of typical alternative solutions, e.g. passive infrared detectors are sensitive to temperature changes and they are useless for the motion detection of objects with the same temperature as the background. Some more robust solutions, such as ultrasound and radar detectors, are usually active (energy emitting) and require an additional power supply. The application of a vision based motion detection procedure utilising the Monte Carlo method for the mobile robots control purposes is caused mainly by the fact that no additional hardware is necessary, since the images used by the motion detector are acquired by the same camera as in the classification procedure.

Assuming the scene with the constant light conditions without any moving objects, it can be characterised by a constant value defined as the number of black (or white) pixels of the respective binary image, which usually represent the object visible on the image obtained from the camera. The algorithm of the motion detection utilises the rapid changes of the number of such pixels (treated as "ones") when an object moves respectively to the light sources. Such change is caused by the dependence of the image pixel's luminance (and colour) on the angle between normal vector to its surface and the direction of the light ray passing the pixel, as well as the influence of the surface's reflection coefficient, shape, roughness etc. Real objects usually have a heterogeneous structure of their surface so the number of analysed points changes dynamically (assuming constant threshold or chroma keying range). For slow

Object

Detected edges

Elements with the estimated area equal to the size of the elementary block

1 2 3 4 5

Statistical Video Based Control of Mobile Robots 357

191817161514

The simplest approach to the edge detection, which is necessary for further processing, is based on the array *P*. For this purpose the second array *K* with the same size is created and

• zero if the corresponding element's value in the array *P* is equal to zero (background), • zero if the corresponding element's value in the array *P* is equal to the size of the elementary block (all pixels represent the inner part of the object) and none of its

neighbouring blocks (using the 8-directional neighbourhood) has the zero value,

The obtained array *K* is the projection of edges detected from the source image, so the number of its non-zero elements represents the estimated value of object's perimeter expressed in squares of *r* × *r* pixels. For a better estimation the number of square elements can be increased (smaller values of the parameter *r*) and then the previously discussed operation should be repeated. In such case the analysis of the blocks in the array *P* with the zero values (representing the inner parts of the object) is not necessary, so the further analysis can be conducted only for the significantly reduced number of indexed elements classified as the edge in the previous step. Such alternative edge detection method based on the two-step algorithm with the same initial step of the algorithm and another pass of the previous algorithm conducted as the second step with the edge detection calculated using the array *K* (the edge obtained in the first step is treated as the new object without fill) allows more accurate estimation of the edge with slight increase of the computational complexity. All the pixels with the zero values should be replaced from the array *K* and using such corrected image the two curves are obtained as the result (assuming the scale factor of the block greater than *t* = 2 - at least 3 × 3 elements). As the estimate of the edge the middle line between the two obtained curves can be used, providing slightly lower accuracy but also a reduced computational cost. The estimate of the total object's area can also be determined with similarly increased accuracy as the sum of the values in the array *P*. The limit accuracy of the algorithm is determined by the size of the elementary block equal to 1 pixel what is equivalent

Geometrical parameters of the objects, which can be estimated using the modified Monte Carlo method, can be divided into the two major groups: local (such as mean diameter or the

2120

Fig. 3. The idea of the block based edge estimation

its elements have the following values:

• one for the others (representing the edge).

The idea of the edge detection is illustrated in Fig. 3

to the well-known convolution edge detection filters.

2728

changes of the light conditions, especially for outside environment, the binarization threshold can be updated. It it worth to notice that in the proposed approach the necessity of the storage of the single value only (the number of pixels corresponding to the "ones") allows the reduction of the required system's operating memory size.

A typical well known video based algorithm of motion detection is based on the comparison of two neighbouring frames of the video signal but it requires the storage of the images used during the comparison so relatively large amount of memory is needed. Some more robust algorithms of background estimation, directly related to motion detection, usually based on moving average or median filter, are also more computationally demanding and require the analysis of several video frames (Cucchiara et al., 2003; Piccardi, 2004). Even a comparison of all the pixels from two video frames can be treated as a time consuming operation for an autonomous mobile robot with limited computational performance and relatively small amount of operating memory, so in the real-time applications some high performance processing units would be required. Instead of them the statistical Monte Carlo approach described above can be used for the reduction of the computational complexity, where only some randomly chosen pixels are taken into account. Therefore the comparison of full frames is reduced to the comparison of only two values representing the number of "ones" in each frame. Any rapid change of that value is interpreted as the presence of a moving object. A slightly modified method based on the additional division of the image into smaller blocks can be applied for a more robust detection. In the presence of moving objects the changes can be detected only in some of obtained blocks so the integrating character of the Monte Carlo method is eliminated.

The fast motion detection based on the comparison of two values representing the estimated areas of the object in the two consecutive binary frames is based on the following formula:

$$
\hat{A}\_{Ob}(\mathbf{i} + \delta) - \hat{A}\_{Ob}(\mathbf{i}) > \text{threshold} \tag{17}
$$

where *threshold* >> *εα* and *δ* ≥ 1 denotes the shift between both analysed frames.

In the proposed block based approach the application of the formula (17) for each of the *r* × *r* pixels blocks allows proper motion detection, using the array of the Monte Carlo motion detectors, also in the situation when the motion of the mobile robot causes the relative motion of the objects observed by the integrated camera. For the perspectively looking camera, typical for such applications, even if the objects do not change their size on the image, the changes can be observed among the blocks. The objects moving towards the camera always cause the increase of their estimated area in the image used by the motion detector. The estimation of the geometrical parameters of the objects, often necessary for the proper navigation of the controlled robot, can be performed for the frame with the maximum value of the estimated area occupied by the object on the image with eliminated background, assuming that robot is not moving directly towards the object. If the size of the object increases in consecutive frames, such estimation should be performed immediately preventing possible collision with the object.

#### **3.3 The estimation of geometrical parameters**

The extraction of some geometrical parameters, such as perimeters or diameters, using the discussed approach is also possible. For this purpose the analysed binary image has to be divided into *T* × *S* blocks of *r* × *r* pixels each using a square grid. Then the area of each object's fragment in the elementary square elements (blocks) is calculated and the results of such estimation can be stored in the array *P* containing *T* × *S* elements.

8 Will-be-set-by-IN-TECH

changes of the light conditions, especially for outside environment, the binarization threshold can be updated. It it worth to notice that in the proposed approach the necessity of the storage of the single value only (the number of pixels corresponding to the "ones") allows

A typical well known video based algorithm of motion detection is based on the comparison of two neighbouring frames of the video signal but it requires the storage of the images used during the comparison so relatively large amount of memory is needed. Some more robust algorithms of background estimation, directly related to motion detection, usually based on moving average or median filter, are also more computationally demanding and require the analysis of several video frames (Cucchiara et al., 2003; Piccardi, 2004). Even a comparison of all the pixels from two video frames can be treated as a time consuming operation for an autonomous mobile robot with limited computational performance and relatively small amount of operating memory, so in the real-time applications some high performance processing units would be required. Instead of them the statistical Monte Carlo approach described above can be used for the reduction of the computational complexity, where only some randomly chosen pixels are taken into account. Therefore the comparison of full frames is reduced to the comparison of only two values representing the number of "ones" in each frame. Any rapid change of that value is interpreted as the presence of a moving object. A slightly modified method based on the additional division of the image into smaller blocks can be applied for a more robust detection. In the presence of moving objects the changes can be detected only in some of obtained blocks so the integrating character of the Monte Carlo

The fast motion detection based on the comparison of two values representing the estimated areas of the object in the two consecutive binary frames is based on the following formula:

In the proposed block based approach the application of the formula (17) for each of the *r* × *r* pixels blocks allows proper motion detection, using the array of the Monte Carlo motion detectors, also in the situation when the motion of the mobile robot causes the relative motion of the objects observed by the integrated camera. For the perspectively looking camera, typical for such applications, even if the objects do not change their size on the image, the changes can be observed among the blocks. The objects moving towards the camera always cause the increase of their estimated area in the image used by the motion detector. The estimation of the geometrical parameters of the objects, often necessary for the proper navigation of the controlled robot, can be performed for the frame with the maximum value of the estimated area occupied by the object on the image with eliminated background, assuming that robot is not moving directly towards the object. If the size of the object increases in consecutive frames, such estimation should be performed immediately preventing possible collision with

The extraction of some geometrical parameters, such as perimeters or diameters, using the discussed approach is also possible. For this purpose the analysed binary image has to be divided into *T* × *S* blocks of *r* × *r* pixels each using a square grid. Then the area of each object's fragment in the elementary square elements (blocks) is calculated and the results of

such estimation can be stored in the array *P* containing *T* × *S* elements.

*Ob*(*i*) > *threshold* (17)

the reduction of the required system's operating memory size.

*A*ˆ

**3.3 The estimation of geometrical parameters**

*Ob*(*<sup>i</sup>* <sup>+</sup> *<sup>δ</sup>*) <sup>−</sup> *<sup>A</sup>*<sup>ˆ</sup>

where *threshold* >> *εα* and *δ* ≥ 1 denotes the shift between both analysed frames.

method is eliminated.

the object.

Fig. 3. The idea of the block based edge estimation

The simplest approach to the edge detection, which is necessary for further processing, is based on the array *P*. For this purpose the second array *K* with the same size is created and its elements have the following values:


The idea of the edge detection is illustrated in Fig. 3

The obtained array *K* is the projection of edges detected from the source image, so the number of its non-zero elements represents the estimated value of object's perimeter expressed in squares of *r* × *r* pixels. For a better estimation the number of square elements can be increased (smaller values of the parameter *r*) and then the previously discussed operation should be repeated. In such case the analysis of the blocks in the array *P* with the zero values (representing the inner parts of the object) is not necessary, so the further analysis can be conducted only for the significantly reduced number of indexed elements classified as the edge in the previous step. Such alternative edge detection method based on the two-step algorithm with the same initial step of the algorithm and another pass of the previous algorithm conducted as the second step with the edge detection calculated using the array *K* (the edge obtained in the first step is treated as the new object without fill) allows more accurate estimation of the edge with slight increase of the computational complexity. All the pixels with the zero values should be replaced from the array *K* and using such corrected image the two curves are obtained as the result (assuming the scale factor of the block greater than *t* = 2 - at least 3 × 3 elements). As the estimate of the edge the middle line between the two obtained curves can be used, providing slightly lower accuracy but also a reduced computational cost. The estimate of the total object's area can also be determined with similarly increased accuracy as the sum of the values in the array *P*. The limit accuracy of the algorithm is determined by the size of the elementary block equal to 1 pixel what is equivalent to the well-known convolution edge detection filters.

Geometrical parameters of the objects, which can be estimated using the modified Monte Carlo method, can be divided into the two major groups: local (such as mean diameter or the

Fig. 4. The idea of the "cut-off" operation

the image representing the tracked line.

• ignoring the empty lines (without any "ones").

control possibilities of the controlled robot.

**–** the detection of the crossing lines

the control flag is set to 0).

line-crossing or using the speed control

• binarization of the current frame,

• orphan blocks removal,

• control operation:

block

**4.2 Extension of the robot control by vision based prediction**

• filling the gaps in the detected line using the approximation methods,

neighbouring "ones"),

infra-red sensors.

row of the obtained simplified binary image which is equivalent to the typical line of the

Statistical Video Based Control of Mobile Robots 359

Nevertheless, the machine vision algorithm can be disturbed by some artifacts caused by the errors during the preprocessing step (e.g. insufficient filtration of some undesired elements on the image). An optimal situation would take place for a single "one" in the lowest row of

In the simplified version of the algorithm no line crossings can be assumed. The reduction of

• limitation of the line thickness by the proper choice of the binarization threshold (lines containing more than a single "one" can be ignored assuming no line crossings),

• orphan blocks removal (applicable for all blocks with the value "one" without any

The control algorithm of the robot should point towards such a position that only a single block of the bottom image row will be filled (representing the line). In the case when two neighbouring blocks represent the line the choice of the block should be based on the simple analysis of the current trend, assuming that the turn angles of the line match the motion and

The control algorithm for a single frame can be described as follows (Okarma & Lech, 2010):

\* if not turning: moving forward with the maximum speed for the symmetrical

\* else: turning and using the speed control if the maximum value is not in the middle

**–** speed control: velocity should be proportional to the sum of the values in the middle blocks of each horizontal line (if zero, the minimum speed is set before the turning and

the possible disturbances can be achieved using the following additional operations:

average area) and global (e.g. the number of objects in the specified area). The estimation of the relative global parameters is not necessary if only a single object is present on the analysed fragment of the image. The most useful parameters used for image analysis, which can be also used for robot's control purposes, are insensitive to image deformations which can occur during the acquisition, as well as to some typical geometrical transformations. In such sense the usefulness of the simplest parameters such as area and perimeter is limited. Nevertheless, the majority of more advanced parameters (such as moments) can be usually determined using the previously described ones, similarly as some motion parameters e.g. direction or velocity (Okarma & Lech, 2008a).

#### **4. Application for the line following robots**

One of the most important tasks of the control systems designed for mobile robots is line tracking. Its typical optical implementation is based on a line of sensors receiving information about the position of the traced line, usually located underneath the robot. Such line sensor is built from a specified number of cells limiting the resolution of the optical system. Another important parameter of such systems is the distance from the sensors to the centre of steering, responsible for the maximum possible speed of the properly controlled robot. The smoothness of the robot's motion is also dependent on the spacing between the cells forming the line sensor.

A significant disadvantage of such optical systems is relatively low resolution of the tracking system, which can be increases using some other vision based systems with wide possibilities of analysing data acquired from the front Dupuis & Parizeau (2006); Rahman et al. (2005). Nevertheless the analysis of the full acquired image even with low resolution is usually computationally demanding and time consuming, especially in the presence of some obstacles, some line intersections etc., assuming also varying lighting conditions. Another serious drawback of using line of sensors is the short time for the reaction limited by the distance between the sensors and the steering centre (or wheels). The proposed approach, based on the fast image analysis using the Monte Carlo method, preserves the main advantages of the vision systems allowing the reduction of the amount of processed data. Considering its application for the control of the mobile robots, the camera moves relatively to the static scene, differently than in the primary version of the algorithm, but the working properties of the method are similar.

In order to filter the undesired data, usually related to some contaminations which should not be used for the robot control purposes, the binarization threshold should be properly set. In the conducted experiments the additional "cut-off" value has been set as minimum 20% black pixels possibly representing the object within the block in order to avoid the influence of the small artifacts, especially close to the region of interest. Finally, the simplified binary representation of the image is obtained where such artifacts (the elements which in fact do not represent the followed line) have been removed during the "cut-off" operation illustrated in Fig. 4 where the original binary image and the intermediate result obtained by the Monte Carlo method are also presented.

#### **4.1 A simple proportional control algorithm for controlling a line following robot**

The mobile robot control process can be based on the popular proportional control approach. It is typically used for controlling the robots with sensors based on the infra-red receivers grouped into the line containing a specified number of the infra-red cells. In the proposed method the differential steering signals for the motors are obtained using the first (the lowest) 10 Will-be-set-by-IN-TECH

average area) and global (e.g. the number of objects in the specified area). The estimation of the relative global parameters is not necessary if only a single object is present on the analysed fragment of the image. The most useful parameters used for image analysis, which can be also used for robot's control purposes, are insensitive to image deformations which can occur during the acquisition, as well as to some typical geometrical transformations. In such sense the usefulness of the simplest parameters such as area and perimeter is limited. Nevertheless, the majority of more advanced parameters (such as moments) can be usually determined using the previously described ones, similarly as some motion parameters e.g.

One of the most important tasks of the control systems designed for mobile robots is line tracking. Its typical optical implementation is based on a line of sensors receiving information about the position of the traced line, usually located underneath the robot. Such line sensor is built from a specified number of cells limiting the resolution of the optical system. Another important parameter of such systems is the distance from the sensors to the centre of steering, responsible for the maximum possible speed of the properly controlled robot. The smoothness of the robot's motion is also dependent on the spacing between the cells forming the line

A significant disadvantage of such optical systems is relatively low resolution of the tracking system, which can be increases using some other vision based systems with wide possibilities of analysing data acquired from the front Dupuis & Parizeau (2006); Rahman et al. (2005). Nevertheless the analysis of the full acquired image even with low resolution is usually computationally demanding and time consuming, especially in the presence of some obstacles, some line intersections etc., assuming also varying lighting conditions. Another serious drawback of using line of sensors is the short time for the reaction limited by the distance between the sensors and the steering centre (or wheels). The proposed approach, based on the fast image analysis using the Monte Carlo method, preserves the main advantages of the vision systems allowing the reduction of the amount of processed data. Considering its application for the control of the mobile robots, the camera moves relatively to the static scene, differently than in the primary version of the algorithm, but the working

In order to filter the undesired data, usually related to some contaminations which should not be used for the robot control purposes, the binarization threshold should be properly set. In the conducted experiments the additional "cut-off" value has been set as minimum 20% black pixels possibly representing the object within the block in order to avoid the influence of the small artifacts, especially close to the region of interest. Finally, the simplified binary representation of the image is obtained where such artifacts (the elements which in fact do not represent the followed line) have been removed during the "cut-off" operation illustrated in Fig. 4 where the original binary image and the intermediate result obtained by the Monte

The mobile robot control process can be based on the popular proportional control approach. It is typically used for controlling the robots with sensors based on the infra-red receivers grouped into the line containing a specified number of the infra-red cells. In the proposed method the differential steering signals for the motors are obtained using the first (the lowest)

**4.1 A simple proportional control algorithm for controlling a line following robot**

direction or velocity (Okarma & Lech, 2008a).

**4. Application for the line following robots**

properties of the method are similar.

Carlo method are also presented.

sensor.

Fig. 4. The idea of the "cut-off" operation

row of the obtained simplified binary image which is equivalent to the typical line of the infra-red sensors.

Nevertheless, the machine vision algorithm can be disturbed by some artifacts caused by the errors during the preprocessing step (e.g. insufficient filtration of some undesired elements on the image). An optimal situation would take place for a single "one" in the lowest row of the image representing the tracked line.

In the simplified version of the algorithm no line crossings can be assumed. The reduction of the possible disturbances can be achieved using the following additional operations:


#### **4.2 Extension of the robot control by vision based prediction**

The control algorithm of the robot should point towards such a position that only a single block of the bottom image row will be filled (representing the line). In the case when two neighbouring blocks represent the line the choice of the block should be based on the simple analysis of the current trend, assuming that the turn angles of the line match the motion and control possibilities of the controlled robot.

The control algorithm for a single frame can be described as follows (Okarma & Lech, 2010):

	- **–** the detection of the crossing lines
		- \* if not turning: moving forward with the maximum speed for the symmetrical line-crossing or using the speed control
		- \* else: turning and using the speed control if the maximum value is not in the middle block
	- **–** speed control: velocity should be proportional to the sum of the values in the middle blocks of each horizontal line (if zero, the minimum speed is set before the turning and the control flag is set to 0).

**4.3 Application of the Centre of Gravity**

of Mass). It can be determined for a grayscale image as:

previous filtration of any artifacts from the image.

If the robot is located directly over the straight line its central point should be exactly between the two edges obtained using the two-step algorithm of edge detection discussed above. In such situation the sum of the pixel values on its left side should be identical as on its right side. Any significant disturbance can be interpreted as the beginning of a line curve with the assumption that no additional elements, except two line edges, are visible on the image, so the mobile robot should begin turning. An efficient and simple method for controlling the robot which can be used for such purpose is based on the image's Centre of Gravity (Centre

Statistical Video Based Control of Mobile Robots 361

*xc* =

*yc* =

where *Xi*,*<sup>j</sup>* represents the luminance of the pixel located at position (*i*, *j*).

computation time as has been verified in the conducted experiments.

Monte Carlo method can be additionally verified by the CoG calculations.

Gravity used for determining the current trend (direction of the line).

∑ *i*,*j*

> ∑ *i*,*j Xi*,*<sup>j</sup>*

∑ *i*,*j*

> ∑ *i*,*j Xi*,*<sup>j</sup>*

Calculating the coordinates of the Centre of Gravity (CoG), they can be used by the control algorithm as the current target point for the mobile robot. The horizontal coordinate *xc* can be used for controlling the turning angle and the vertical *yc* for the speed control, assuming the

The approach discussed above can also be implemented using the block based Monte Carlo method using the "cut-off" operation for decrease of the influence of noise, leading to the hybrid control method. For each block the local area estimator *A*ˆ is used as the input for the "cut-off" operation leading to the simplified binary representation utilised for steering using the method discussed above. In comparison to the classical Center of Gravity method its implementation using the Monte Carlo approach causes about 8 times reduction of the

One of the main modifications proposed in the hybrid method is the switch between the CoG and Monte Carlo based control in order to increase the robustness of the control. For this purpose the sum of each column of the simplified binary image is calculated and stored in an additional vector. If the vector containing the sums of the values, obtained by counted blocks with "ones", has more than one local minimum (e.g. 2 4 0 0 1 6 0 0 1), some additional objects are present on the image. In such situation the calculation of the Centre of Gravity coordinates would be disturbed by a neighbouring object located close to the line, so the motion control should be based on the previous value. Otherwise, the control values determined by the

A more complicated situation may take place for the presence of artifacts together with some discontinuities of the followed line. In this situation the vector containing the sum of the column in the simplified block representation of the image has more than one local maximum. In such case the additional elimination of the blocks which are not directly connected to the followed line (detected as represented by the maximum value in the vector) should be conducted e.g. using the morphological erosion, before the calculations of the Centre of

*i* · *Xi*,*<sup>j</sup>*

*j* · *Xi*,*<sup>j</sup>*

(18)

(19)

Fig. 5. The illustration of the various configurations of the followed line

The main element of the modified control system is the variable (flag), determined using the previous frames, informing the control system about the current state (e.g. 0 for moving forward or turning, 1 for turning with detected crossing lines).

In the case of detected crossing line the control depends on the symmetry. For the symmetrical line-crossing the robot should move forward with the maximum speed or the additional speed control. If the maximum value is not in the middle block the line-crossing is not symmetrical and the robot should turn left or right and decrease it speed. The speed of the robot should be proportional to the sum of the values in the middle blocks of each horizontal line. If that sum is equal to zero, the minimum speed should be set before the turning. In that case the mentioned above flag should be set to 1. The illustration of the various scenarios is presented in Fig. 5.

One of the typical features of the proportionally controlled systems is the oscillating character of the path for the robot reaching the specified line, causing also the characteristic oscillations of the images acquired from the camera. Nevertheless, the conducted experiments have verified the usefulness of the fast image analysis based on the Monte Carlo method for the efficient controlling of the line following robot in a simplified but very efficient way, especially useful for the real-time applications. The application of the proposed solution for the control of the mobile robot, despite of the simplicity of its control algorithm, is comparable with some typical ones based on the classical approach (proportional steering based on the infra-red sensors).

The main advantage of the proposed method is the possibility of using some prediction algorithms allowing the increase of the robot's dynamic properties and fluent changes of its speed depending on the shape of the line in front of the robot. Another relevant feature is its low computational complexity causing the relatively high processing speed, especially important in the real-time embedded systems.

#### **4.3 Application of the Centre of Gravity**

12 Will-be-set-by-IN-TECH

 

Fig. 5. The illustration of the various configurations of the followed line

forward or turning, 1 for turning with detected crossing lines).

important in the real-time embedded systems.

The main element of the modified control system is the variable (flag), determined using the previous frames, informing the control system about the current state (e.g. 0 for moving

In the case of detected crossing line the control depends on the symmetry. For the symmetrical line-crossing the robot should move forward with the maximum speed or the additional speed control. If the maximum value is not in the middle block the line-crossing is not symmetrical and the robot should turn left or right and decrease it speed. The speed of the robot should be proportional to the sum of the values in the middle blocks of each horizontal line. If that sum is equal to zero, the minimum speed should be set before the turning. In that case the mentioned above flag should be set to 1. The illustration of the various scenarios is presented

One of the typical features of the proportionally controlled systems is the oscillating character of the path for the robot reaching the specified line, causing also the characteristic oscillations of the images acquired from the camera. Nevertheless, the conducted experiments have verified the usefulness of the fast image analysis based on the Monte Carlo method for the efficient controlling of the line following robot in a simplified but very efficient way, especially useful for the real-time applications. The application of the proposed solution for the control of the mobile robot, despite of the simplicity of its control algorithm, is comparable with some typical ones based on the classical approach (proportional steering based on the infra-red

The main advantage of the proposed method is the possibility of using some prediction algorithms allowing the increase of the robot's dynamic properties and fluent changes of its speed depending on the shape of the line in front of the robot. Another relevant feature is its low computational complexity causing the relatively high processing speed, especially

 


in Fig. 5.

sensors).

If the robot is located directly over the straight line its central point should be exactly between the two edges obtained using the two-step algorithm of edge detection discussed above. In such situation the sum of the pixel values on its left side should be identical as on its right side. Any significant disturbance can be interpreted as the beginning of a line curve with the assumption that no additional elements, except two line edges, are visible on the image, so the mobile robot should begin turning. An efficient and simple method for controlling the robot which can be used for such purpose is based on the image's Centre of Gravity (Centre of Mass). It can be determined for a grayscale image as:

$$\mathbf{x}\_{\mathcal{L}} = \frac{\sum\_{i,j} i \cdot \mathbf{X}\_{i,j}}{\sum\_{i,j} \mathbf{X}\_{i,j}} \tag{18}$$

$$y\_c = \frac{\sum\_{i,j} j \cdot X\_{i,j}}{\sum\_{i,j} X\_{i,j}} \tag{19}$$

where *Xi*,*<sup>j</sup>* represents the luminance of the pixel located at position (*i*, *j*).

Calculating the coordinates of the Centre of Gravity (CoG), they can be used by the control algorithm as the current target point for the mobile robot. The horizontal coordinate *xc* can be used for controlling the turning angle and the vertical *yc* for the speed control, assuming the previous filtration of any artifacts from the image.

The approach discussed above can also be implemented using the block based Monte Carlo method using the "cut-off" operation for decrease of the influence of noise, leading to the hybrid control method. For each block the local area estimator *A*ˆ is used as the input for the "cut-off" operation leading to the simplified binary representation utilised for steering using the method discussed above. In comparison to the classical Center of Gravity method its implementation using the Monte Carlo approach causes about 8 times reduction of the computation time as has been verified in the conducted experiments.

One of the main modifications proposed in the hybrid method is the switch between the CoG and Monte Carlo based control in order to increase the robustness of the control. For this purpose the sum of each column of the simplified binary image is calculated and stored in an additional vector. If the vector containing the sums of the values, obtained by counted blocks with "ones", has more than one local minimum (e.g. 2 4 0 0 1 6 0 0 1), some additional objects are present on the image. In such situation the calculation of the Centre of Gravity coordinates would be disturbed by a neighbouring object located close to the line, so the motion control should be based on the previous value. Otherwise, the control values determined by the Monte Carlo method can be additionally verified by the CoG calculations.

A more complicated situation may take place for the presence of artifacts together with some discontinuities of the followed line. In this situation the vector containing the sum of the column in the simplified block representation of the image has more than one local maximum. In such case the additional elimination of the blocks which are not directly connected to the followed line (detected as represented by the maximum value in the vector) should be conducted e.g. using the morphological erosion, before the calculations of the Centre of Gravity used for determining the current trend (direction of the line).

line reconstruction can be treated as one of the most relevant advantages of the image based motion control. Such reconstruction can also be performed using the ultra-low resolution binary image as the input data. Another possibility, being analysed in our experiments, is the adaptation of the Centre of Gravity method for the real time steering based on the binary

Statistical Video Based Control of Mobile Robots 363

The additional robustness of the method can be achieved by the adaptive change of the block size used in the modified Monte Carlo method according to the quality assessment of the

Positive results of the simulations performed in Java 3D based Simbad environment allow the proper implementation of the proposed soft computing approach in the experimental video controlled line following robot. Good simulation results, as weel as the low energy consumption by the experimental netbook-based robot, have been also obtained considering the synthetic reduced resolution "grayscale" image constructed using the Monte Carlo method, where the relative areas of the objects in each block have been used instead of the

The planned future work is related to the physical application of the proposed approach for some other types of robot control algorithms based on video analysis, e.g. SLAM. Another direction of the future research should be the development of a dedicated binary image quality assessment method, which could be used for the optimisation of the proposed algorithm. Another interesting direction of the future work is the extension of the proposed method towards direct operations using the colour images. For this purpose a verification of the usefulness of the typical colour models is necessary by means of the chroma keying used

Cowan, N., Shakernia, O., Vidal, R. & Sastry, S. (2003). Vision-based follow-the-leader,

Cucchiara, R., Grana, C., Piccardi, M. & Prati, A. (2003). Detecting moving objects, ghosts,

Dupuis, J.-F. & Parizeau, M. (2006). Evolving a vision-based line-following robot controller,

Geiger, C., Reimann, C., Stocklein, J. & Paelke, V. (2002). JARToolkit - a Java binding

Hähnel, D., Triebel, R., Burgard, W. & Thrun, S. (2003). Map building with mobile robots

Hugues, L. & Bredeche, N. (2006). Simbad: An autonomous robot simulation package for

Kalyan, B., Lee, K. & Wijesoma, W. (2010). FISST-SLAM: Finite set statistical approach

*Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems*, Vol. 2,

and shadows in video streams, *IEEE Transactions on Pattern Analysis and Machine*

*Proceedings of the 3rd Canadian Conference on Computer and Robot Vision*, Quebec City,

for ARToolkit, *Proceedings of the The First IEEE International Workshop on Augmented*

in dynamic environments, *International Conference on Robotics and Automation*, Vol. 2,

education and research, *in* S. Nolfi, G. Baldassarre, R. Calabretta, J. C. T. Hallam, D. Marocco, J.-A. Meyer, O. Miglino & D. Parisi (eds), *SAB*, Vol. 4095 of *Lecture Notes*

to simultaneous localization and mapping, *International Journal of Robotics Research*

binary image and the additional analysis of the line continuity.

luminance level (after normalisation).

for the thresholding in the Monte Carlo method.

Las Vegas, Nevada, pp. 1796–1801.

*Reality Toolkit*, Darmstadt, Germany, p. 5.

*in Computer Science*, Springer, pp. 831–842.

*Intelligence* 25(10): 1337–1342.

Taipei, Taiwan, pp. 1557–1563.

Canada, p. 75.

29(10): 1251–1262.

image analysis.

**7. References**

#### **5. Simulations in the Simbad environment**

In order to verify the working properties of the proposed methods, regardless of some tests conducted using a netbook based prototype, a test application has been created using Java programming language in NetBeans environment allowing the integration of many libraries developed for Java e.g JARToolKit (Geiger et al., 2002) allowing to access the Augmented Reality Toolkit (ARToolKit) functionality via Java and the use of different rendering libraries allows high and low level access. The environment of the mobile robot as well as the robot itself have been modelled using the Simbad simulator (Hugues & Bredeche, 2006), which is available for this platform.

A significant advantage of the simulator is its 3D environment, due to the utilisation of Sun Java3D technology. Nevertheless, an important restrictions of Simbad, in opposite to some other similar products, are the limited working properties for the controlling the real robots die to its internal structure related to the modelling of physical phenomena. However, this results in a simplified and transparent programming source code, so the implementation of the presented algorithms does not cause any considerable problems. The only restriction in the initialisation part of the robot controller is the directing the camera towards the "ceiling", caused by the inability to put the followed line on the ground. Instead of it the line has been located over the robot and the lines in the front of the robot are shown in the top part of the image. The only negative consequence is the reverse transformation of the left and right directions between the robot and the camera. The proportional controlled differential drive robots used in the experiments are typically implemented in Simbad and conducted simulations have confirmed the working properties of the presented steering methods for all control modes with different simulation speed values. The test application used for the simulations is illustrated in Fig . 6.

Fig. 6. Illustration of the test application

#### **6. Conclusions and future work**

Obtained results allowed the full control of the line following robot (speed, direction and "virtual" line reconstruction). Due to the ability of path prediction, in comparison to the typical line following robots based on the line of sensors mounted under the robot, the "virtual" 14 Will-be-set-by-IN-TECH

In order to verify the working properties of the proposed methods, regardless of some tests conducted using a netbook based prototype, a test application has been created using Java programming language in NetBeans environment allowing the integration of many libraries developed for Java e.g JARToolKit (Geiger et al., 2002) allowing to access the Augmented Reality Toolkit (ARToolKit) functionality via Java and the use of different rendering libraries allows high and low level access. The environment of the mobile robot as well as the robot itself have been modelled using the Simbad simulator (Hugues & Bredeche, 2006), which is

A significant advantage of the simulator is its 3D environment, due to the utilisation of Sun Java3D technology. Nevertheless, an important restrictions of Simbad, in opposite to some other similar products, are the limited working properties for the controlling the real robots die to its internal structure related to the modelling of physical phenomena. However, this results in a simplified and transparent programming source code, so the implementation of the presented algorithms does not cause any considerable problems. The only restriction in the initialisation part of the robot controller is the directing the camera towards the "ceiling", caused by the inability to put the followed line on the ground. Instead of it the line has been located over the robot and the lines in the front of the robot are shown in the top part of the image. The only negative consequence is the reverse transformation of the left and right directions between the robot and the camera. The proportional controlled differential drive robots used in the experiments are typically implemented in Simbad and conducted simulations have confirmed the working properties of the presented steering methods for all control modes with different simulation speed values. The test application used for the

Obtained results allowed the full control of the line following robot (speed, direction and "virtual" line reconstruction). Due to the ability of path prediction, in comparison to the typical line following robots based on the line of sensors mounted under the robot, the "virtual"

**5. Simulations in the Simbad environment**

available for this platform.

simulations is illustrated in Fig . 6.

Fig. 6. Illustration of the test application

**6. Conclusions and future work**

line reconstruction can be treated as one of the most relevant advantages of the image based motion control. Such reconstruction can also be performed using the ultra-low resolution binary image as the input data. Another possibility, being analysed in our experiments, is the adaptation of the Centre of Gravity method for the real time steering based on the binary image analysis.

The additional robustness of the method can be achieved by the adaptive change of the block size used in the modified Monte Carlo method according to the quality assessment of the binary image and the additional analysis of the line continuity.

Positive results of the simulations performed in Java 3D based Simbad environment allow the proper implementation of the proposed soft computing approach in the experimental video controlled line following robot. Good simulation results, as weel as the low energy consumption by the experimental netbook-based robot, have been also obtained considering the synthetic reduced resolution "grayscale" image constructed using the Monte Carlo method, where the relative areas of the objects in each block have been used instead of the luminance level (after normalisation).

The planned future work is related to the physical application of the proposed approach for some other types of robot control algorithms based on video analysis, e.g. SLAM. Another direction of the future research should be the development of a dedicated binary image quality assessment method, which could be used for the optimisation of the proposed algorithm.

Another interesting direction of the future work is the extension of the proposed method towards direct operations using the colour images. For this purpose a verification of the usefulness of the typical colour models is necessary by means of the chroma keying used for the thresholding in the Monte Carlo method.

#### **7. References**


**18** 

*1Chile 2Colombia* 

**An Embedded Type-2 Fuzzy Controller** 

*Department of Electrical Engineering, University of Chile, Santiago* 

*2Laboratory for Automation, Microelectronics and Computational Intelligence (LAMIC), Faculty of Engineering, Universidad Distrital Francisco José de Caldas, Bogotá D.C.* 

Fuzzy logic systems (FLS) have been used in different applications with satisfactory performance (Wang, 1997). The human perception cannot be modelled by traditional mathematical techniques, thus, the introduction of fuzzy set (FS) theory in this modelling has been suitable (John & Coupland, 2007). When real-world applications are treated, many sources of uncertainty often appear. Several natures of uncertainties would influence the performance of a system. It is independent from what kind of methodology is used to

Type-1 Fuzzy logic systems (T1-FLS) have limited capabilities to directly handle data uncertainties (Mendel, 2007). Once a type-1 membership function (MF) has been defined, uncertainty disappears because a T1-MF is totally precise (Hagras, 2007). Type-2 fuzzy logic systems (T2-FLS) make possible to model and handle uncertainties. These are rule based systems in which linguistic variables are described by means of Type-2 fuzzy sets (T2-FSs) that include a footprint of uncertainty (FOU) (Mendel, 2001). It provides a measure of dispersion to capture more about uncertainties (Mendel, 2007). While T2-FSs have non-crisp

A representation of the inference model for T2-FLS is depicted in Figure 1 (Mendel, 2007). It begins with fuzzification, which maps crisp points into T2-FSs. Next, inference engine computes the rule base by making logical combinations of antecedent T2-FS, whose results are implicated with consequent T2-FS to form an aggregated output type-2 fuzzy set. Afterwards, Type-Reduction (TR) takes all output sets and performs a centroid calculation of this combined type-2 fuzzy set, which leads to a type-1 fuzzy set called *type-reduced set*. That reduced set is finally defuzzyfied in order to obtain a crisp output (Mendel, 2001; Karnik & Mendel 2001). The computational complexity of this model is reduced if interval type-2 fuzzy sets are used (Mendel, 2001), it is convenient in the context of hardware implementation in order to make softer the computational effort and sped up the inference

Type-2 fuzzy hardware is a topic of special interest, since the application of T2-FLS to particular fields that demand mobile electronic solutions would be necessary. Some recent

MFs, T1-FSs have crisp membership grades (MGs) (John & Coupland, 2007).

**1. Introduction** 

handle it (Mendel, 2001).

time.

**for a Mobile Robot Application** 

Leonardo Leottau1 and Miguel Melgarejo2 *1Advanced Mining Technology Center (AMTC),* 


## **An Embedded Type-2 Fuzzy Controller for a Mobile Robot Application**

Leonardo Leottau1 and Miguel Melgarejo2

*1Advanced Mining Technology Center (AMTC), Department of Electrical Engineering, University of Chile, Santiago 2Laboratory for Automation, Microelectronics and Computational Intelligence (LAMIC), Faculty of Engineering, Universidad Distrital Francisco José de Caldas, Bogotá D.C. 1Chile* 

*2Colombia* 

#### **1. Introduction**

16 Will-be-set-by-IN-TECH

364 Recent Advances in Mobile Robotics

Mazurek, P. & Okarma, K. (2006). Car-by-light tracking approach based on log-likelihood

Okarma, K. & Lech, P. (2008a). Monte Carlo based algorithm for fast preliminary video

Okarma, K. & Lech, P. (2009). Application of Monte Carlo preliminary image analysis and

Okarma, K. & Lech, P. (2010). A fast image analysis technique for the line tracking robots, *in*

Rahman, M., Rahman, M., Haque, A. & Islam, M. (2005). Architecture of the vision system of

Se, S., Lowe, D. & Little, J. (2002). Mobile robot localization and mapping with uncertainty

Tamimi, H. & Zell, A. (2004). Vision based localization of mobile robots using kernel

*International Multitopic Conference, IEEE INMIC 2005*, Karachi, pp. 1–8. Se, S., Lowe, D. & Little, J. (2001). Local and global localization for mobile robots using visual

*(2)*, Vol. 6114 of *Lecture Notes in Computer Science*, Springer, pp. 329–336. Piccardi, M. (2004). Background subtraction techniques: a review, *Proceedings of the*

*(1)*, Vol. 5101 of *Lecture Notes in Computer Science*, Springer, pp. 790–799. Okarma, K. & Lech, P. (2008b). A statistical reduced-reference approach to digital image

Vol. 5337 of *Lecture Notes in Computer Science*, Springer, pp. 43–54.

*and Vision: International Journal* 18(4): 439–452.

*Systems*, Vol. 1, Maui, Hawaii, pp. 414–420.

*Robots and Systems*, Sendai, Japan, pp. 1896–1901.

Zakopane, Poland, pp. 15–20.

Netherlands, pp. 3099–3104.

21: 735–758.

track-before-detect algorithm, *Proceedings of the 10th International Conference "Computer Systems Aided Science Industry and Transport" TRANSCOMP*, Vol. 2,

analysis, *in* M. Bubak, G. D. van Albada, J. Dongarra & P. M. A. Sloot (eds), *ICCS*

quality assessment, *in* L. Bolc, J. L. Kulikowski & K. W. Wojciechowski (eds), *ICCVG*,

classification method for automatic reservation of parking space, *Machine Graphics*

L. Rutkowski, R. Scherer, R. Tadeusiewicz, L. A. Zadeh & J. M. Zurada (eds), *ICAISC*

*IEEE International Conference on Systems, Man & Cybernetics*, Vol. 4, The Hague,

a line following mobile robot operating in static environment, *Proceedings of the 9th*

landmarks, *Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and*

using scale-invariant visual landmarks, *International Journal of Robotics Research*

approaches, *Proceedings of the 2004 IEEE/RSJ International Conference on Intelligent*

Fuzzy logic systems (FLS) have been used in different applications with satisfactory performance (Wang, 1997). The human perception cannot be modelled by traditional mathematical techniques, thus, the introduction of fuzzy set (FS) theory in this modelling has been suitable (John & Coupland, 2007). When real-world applications are treated, many sources of uncertainty often appear. Several natures of uncertainties would influence the performance of a system. It is independent from what kind of methodology is used to handle it (Mendel, 2001).

Type-1 Fuzzy logic systems (T1-FLS) have limited capabilities to directly handle data uncertainties (Mendel, 2007). Once a type-1 membership function (MF) has been defined, uncertainty disappears because a T1-MF is totally precise (Hagras, 2007). Type-2 fuzzy logic systems (T2-FLS) make possible to model and handle uncertainties. These are rule based systems in which linguistic variables are described by means of Type-2 fuzzy sets (T2-FSs) that include a footprint of uncertainty (FOU) (Mendel, 2001). It provides a measure of dispersion to capture more about uncertainties (Mendel, 2007). While T2-FSs have non-crisp MFs, T1-FSs have crisp membership grades (MGs) (John & Coupland, 2007).

A representation of the inference model for T2-FLS is depicted in Figure 1 (Mendel, 2007). It begins with fuzzification, which maps crisp points into T2-FSs. Next, inference engine computes the rule base by making logical combinations of antecedent T2-FS, whose results are implicated with consequent T2-FS to form an aggregated output type-2 fuzzy set. Afterwards, Type-Reduction (TR) takes all output sets and performs a centroid calculation of this combined type-2 fuzzy set, which leads to a type-1 fuzzy set called *type-reduced set*. That reduced set is finally defuzzyfied in order to obtain a crisp output (Mendel, 2001; Karnik & Mendel 2001). The computational complexity of this model is reduced if interval type-2 fuzzy sets are used (Mendel, 2001), it is convenient in the context of hardware implementation in order to make softer the computational effort and sped up the inference time.

Type-2 fuzzy hardware is a topic of special interest, since the application of T2-FLS to particular fields that demand mobile electronic solutions would be necessary. Some recent

An Embedded Type-2 Fuzzy Controller for a Mobile Robot Application 367

A FLC is a control strategy whose decisions are made by using a fuzzy inference system. Particularly, an IT2-FLC uses IT2-FSs to represent the inputs and/or outputs of the T2-FLS and these sets are completely characterized by their Footprints of Uncertainty (Mendel, 2001). So, this kind of FLC can model the uncertainty by means of the FOUs. In this way, uncertainties in sensors, actuators, operational changes, environmental changes, noise can be considered (Hagras, 2007; Mendel 2001). A FOU is described by its upper and lower MFs. Each MF is composed by as many membership grades (MGs) as discretization levels are considered in antecedents and consequents universes of discourse (Baturone et al., 2000). This work considers two alternatives for computing the MGs of IT2-FSs: memory based approach and function computing method, which are involved in fuzzyfier and inference engine stages. These two alternatives are the most commonly used in fuzzy hardware implementations (Baturone et al., 2000). Moreover, centroids and center of sets methods are considered for TR stage. As TR algorithm, the Enhanced Karnik-Mendel algorithm is used (Wu & Mendel, 2009). For the sake of clarity, figure 1 depicts the stage structure of an IT2-

**2. Hardware Implementation of an IT2-FLC for robot mobile applications** 

FLC. These mentioned methods and alternatives are briefly introduced below:

several machine cycles depending on the complexity of MF (Baturone et al., 2000).

This method carries out a direct computation of MF by using numeric algorithms that avoid constructing and storing look up tables (Leottau & Melgarejo, 2010a). It reduces memory usage and facilitates the implementation of MF. However, its execution could require

This approach stores the MGs of every input value into a memory. This strategy is executed considerably fast, because it uses the input value as the pointer to the memory and to

The centroid TR combines all the rule-output T2-FSs using union and then finds the centroid of this T2-FS (Karnik & Mendel, 2001). Therefore, this method presents high accuracy but low computational performance (Mendel, 2001). Cent TR is chosen in order to obtain the best accurate output while the hardware platform is forced to the highest computational effort.

In center-of-sets TR, each type-2 consequent set is replaced by its centroid. Then, the weighted average of these centroids is found, being the weight of each centroid the degree of firing of it corresponding rule (Mendel, 2001). CoS TR is chosen because its convenient trade-off between accuracy and computational cost. It is more accurate than Heights method

The EKM algorithm is an iterative procedure to obtain the generalized centroid of an T2-FS. It uses statistically defined values as initialization points to reduce the amount of iterations that are necessary, in this way it converges monotonically and super-exponentially fast (Wu & Mendel, 2009). In addition, as it has been mentioned in (Leottau & Melgarejo, 2010a), EKM is the fastest algorithm reported up to that date for finding the generalized centroid of

*Function Computing Approach (FCA):* 

*Memory Based Approach (MBA):* 

*Centroid Type-Reduction (Cent):* 

*Center-of-sets type-reduction (CoS):* 

*Enhanced Karnik Mendel Algorithm (EKM):* 

a inferred IT2-FS over DSC technology.

directly retrieve the MG (Leottau & Melgarejo, 2010a).

(Mendel, 2001) and computationally less expensive than Cent TR.

Fig. 1. Type-2 fuzzy system.

applications of T2-FLS have been developed in fields like robotics, communication and control systems among others (Castro et al., 2007; Hagras, 2007; Melgarejo & Peña, 2004, 2007; Torres & Saez, 2008). It is worth to think about the possibility of embedding T2-FLS handling these applications in order to achieve better communication speeds in smaller areas (Leottau & Melgarejo, 2010a).

Control and robotics are one of the most widely used application fields of fuzzy logic. The advantages of type-2 fuzzy logic controllers (T2-FLCs) over type-1 fuzzy logic controllers (T1-FLC) have also been demonstrated and documented in (Castro et al., 2007; Figueroa et al., 2005; Hagras 2004, 2007; Martinez et al., 2008; Torres & Saez, 2008). Although this kind of works presents improvements in the T2-FLCs performance, it is necessary to propose methodologies where these advances can be reflected in design processes as (Hagras 2008, Leottau & Melgarejo, 2010b; Melin & Castillo, 2003; Wu & Tan, 2004) and hardware implementation approaches over embedded devices as (Hagras, 2008; Leottau & Melgarejo, 2010a; Melgarejo & Peña, 2004, 2007). In this way T2-FLS would become widely used in different applicative contexts.

This work presents the implementation of an Interval Type-2 Fuzzy Logic Controller (IT2- FLC) for tracking the trajectory of a mobile robot application. The FLC is design according to the approach proposed in (Leottau & Melgarejo, 2010b), which involves some of the T1-FLC and T2-FLC properties. A hardware implementation of the designed T2-FLC is carried out over a digital signal controller (DSC) embedded platform. Different tests are performed for evaluating the performance of the IT2-FLC with respect to a T1-FLC. Simulation and emulation (i.e. with the embedded controller) results evidence that the IT2-FLC is robust to type reducer changes and exhibits better performance than its T1-FLC counterpart when noise is added to inputs and outputs. The IT2-FLC outperforms the T1-FLC in all tested cases, taking the interval time square error (ITSE) as performance index.

The chapter is organized as follows: Section 2 presents an overview of hardware implementation of IT2-FLCs. Section 3 describes the design approach followed for developing the IT2-FLC and section 4 describes its hardware implementation. Section 5 presents test, obtained results and discussion. Finally, conclusions and future work are presented in Section 6.

### **2. Hardware Implementation of an IT2-FLC for robot mobile applications**

A FLC is a control strategy whose decisions are made by using a fuzzy inference system. Particularly, an IT2-FLC uses IT2-FSs to represent the inputs and/or outputs of the T2-FLS and these sets are completely characterized by their Footprints of Uncertainty (Mendel, 2001). So, this kind of FLC can model the uncertainty by means of the FOUs. In this way, uncertainties in sensors, actuators, operational changes, environmental changes, noise can be considered (Hagras, 2007; Mendel 2001). A FOU is described by its upper and lower MFs. Each MF is composed by as many membership grades (MGs) as discretization levels are considered in antecedents and consequents universes of discourse (Baturone et al., 2000). This work considers two alternatives for computing the MGs of IT2-FSs: memory based approach and function computing method, which are involved in fuzzyfier and inference engine stages. These two alternatives are the most commonly used in fuzzy hardware implementations (Baturone et al., 2000). Moreover, centroids and center of sets methods are considered for TR stage. As TR algorithm, the Enhanced Karnik-Mendel algorithm is used (Wu & Mendel, 2009). For the sake of clarity, figure 1 depicts the stage structure of an IT2- FLC. These mentioned methods and alternatives are briefly introduced below:

#### *Function Computing Approach (FCA):*

366 Recent Advances in Mobile Robotics

applications of T2-FLS have been developed in fields like robotics, communication and control systems among others (Castro et al., 2007; Hagras, 2007; Melgarejo & Peña, 2004, 2007; Torres & Saez, 2008). It is worth to think about the possibility of embedding T2-FLS handling these applications in order to achieve better communication speeds in smaller

Control and robotics are one of the most widely used application fields of fuzzy logic. The advantages of type-2 fuzzy logic controllers (T2-FLCs) over type-1 fuzzy logic controllers (T1-FLC) have also been demonstrated and documented in (Castro et al., 2007; Figueroa et al., 2005; Hagras 2004, 2007; Martinez et al., 2008; Torres & Saez, 2008). Although this kind of works presents improvements in the T2-FLCs performance, it is necessary to propose methodologies where these advances can be reflected in design processes as (Hagras 2008, Leottau & Melgarejo, 2010b; Melin & Castillo, 2003; Wu & Tan, 2004) and hardware implementation approaches over embedded devices as (Hagras, 2008; Leottau & Melgarejo, 2010a; Melgarejo & Peña, 2004, 2007). In this way T2-FLS would become widely used in

This work presents the implementation of an Interval Type-2 Fuzzy Logic Controller (IT2- FLC) for tracking the trajectory of a mobile robot application. The FLC is design according to the approach proposed in (Leottau & Melgarejo, 2010b), which involves some of the T1-FLC and T2-FLC properties. A hardware implementation of the designed T2-FLC is carried out over a digital signal controller (DSC) embedded platform. Different tests are performed for evaluating the performance of the IT2-FLC with respect to a T1-FLC. Simulation and emulation (i.e. with the embedded controller) results evidence that the IT2-FLC is robust to type reducer changes and exhibits better performance than its T1-FLC counterpart when noise is added to inputs and outputs. The IT2-FLC outperforms the T1-FLC in all tested

The chapter is organized as follows: Section 2 presents an overview of hardware implementation of IT2-FLCs. Section 3 describes the design approach followed for developing the IT2-FLC and section 4 describes its hardware implementation. Section 5 presents test, obtained results and discussion. Finally, conclusions and future work are

cases, taking the interval time square error (ITSE) as performance index.

Fig. 1. Type-2 fuzzy system.

areas (Leottau & Melgarejo, 2010a).

different applicative contexts.

presented in Section 6.

This method carries out a direct computation of MF by using numeric algorithms that avoid constructing and storing look up tables (Leottau & Melgarejo, 2010a). It reduces memory usage and facilitates the implementation of MF. However, its execution could require several machine cycles depending on the complexity of MF (Baturone et al., 2000).

#### *Memory Based Approach (MBA):*

This approach stores the MGs of every input value into a memory. This strategy is executed considerably fast, because it uses the input value as the pointer to the memory and to directly retrieve the MG (Leottau & Melgarejo, 2010a).

#### *Centroid Type-Reduction (Cent):*

The centroid TR combines all the rule-output T2-FSs using union and then finds the centroid of this T2-FS (Karnik & Mendel, 2001). Therefore, this method presents high accuracy but low computational performance (Mendel, 2001). Cent TR is chosen in order to obtain the best accurate output while the hardware platform is forced to the highest computational effort.

#### *Center-of-sets type-reduction (CoS):*

In center-of-sets TR, each type-2 consequent set is replaced by its centroid. Then, the weighted average of these centroids is found, being the weight of each centroid the degree of firing of it corresponding rule (Mendel, 2001). CoS TR is chosen because its convenient trade-off between accuracy and computational cost. It is more accurate than Heights method (Mendel, 2001) and computationally less expensive than Cent TR.

#### *Enhanced Karnik Mendel Algorithm (EKM):*

The EKM algorithm is an iterative procedure to obtain the generalized centroid of an T2-FS. It uses statistically defined values as initialization points to reduce the amount of iterations that are necessary, in this way it converges monotonically and super-exponentially fast (Wu & Mendel, 2009). In addition, as it has been mentioned in (Leottau & Melgarejo, 2010a), EKM is the fastest algorithm reported up to that date for finding the generalized centroid of a inferred IT2-FS over DSC technology.

$$\mathbf{x}(t+1) = \mathbf{x}(t) + \cos[\varphi(t) + \theta(t)] + \sin[\theta(t)]\sin[\varphi(t)] \tag{1}$$

$$y(t+1) = y(t) + \sin\{\varphi(t) + \theta(t)\} - \sin[\theta(t)]\cos[\varphi(t)] \tag{2}$$

$$
\varphi(t+1) = \varphi(t) - \sin^{-1}[2\sin(\theta(t))/b] \tag{3}
$$

$$\begin{aligned} &\text{Rule } 0 \colon \text{if } Ex^- \text{ and } E\mathfrak{g}^- \text{ and } \Delta\mathfrak{g}^- \text{ then } \mathfrak{g}^+\\ &\text{Rule } 1 \colon \text{if } Ex^- \text{ and } E\mathfrak{g}^- \text{ and } \Delta\mathfrak{g}^+ \text{ then } \mathfrak{g}^-\\ &\text{Rule } 2 \colon \text{if } Ex^- \text{ and } E\mathfrak{g}^+ \text{ and } \Delta\mathfrak{g}^- \text{ then } \mathfrak{g}^-\\ &\text{Rule } 3 \colon \text{if } Ex^- \text{ and } E\mathfrak{g}^+ \text{ and } \Delta\mathfrak{g}^+ \text{ then } \mathfrak{g}^-\\ &\text{Rule } 4 \colon \text{if } Ex^+ \text{ and } E\mathfrak{g}^- \text{ and } \Delta\mathfrak{g}^- \text{ then } \mathfrak{g}^{++}\\ &\text{Rule } 5 \colon \text{if } Ex^+ \text{ and } E\mathfrak{g}^- \text{ and } \Delta\mathfrak{g}^+ \text{ then } \mathfrak{g}^+\\ &\text{Rule } 6 \colon \text{if } Ex^+ \text{ and } E\mathfrak{g}^+ \text{ and } \Delta\mathfrak{g}^- \text{ then } \mathfrak{g}^+\\ &\text{Rule } 7 \colon \text{if } Ex^+ \text{ and } E\mathfrak{g}^+ \text{ and } \Delta\mathfrak{g}^+ \text{ then } \mathfrak{g}^- \end{aligned} \tag{4}$$

An Embedded Type-2 Fuzzy Controller for a Mobile Robot Application 371

is carried out using Microsoft Excel® solver tool that uses the Generalized Reduced Gradient Method as non linear optimization algorithm (Frontline Systems, 2010). A screen

of used Excel® spreadsheet is shown in Figure 5.

Fig. 4. Reducing the dimensionality of an IT2-FS.

Fig. 5. A screen of used Excel® spreadsheet for MF parameters' tuning.

Once the T1-FLC parameters are tuned, returning to T2-FLS scheme using the same uncertainties defined in step 3.3 is proposed in (Leottau & Melgarejo, 2010b), but in this case, locating the FOUs over their embedded T1-FSs obtained in previously tuning. So, By

**3.5 Returning to Type-2** 


The sources of uncertainty mentioned above are included in the final design. But, as it is mentioned in step 3.2 of this procedure, FOUs in this part of design, makes reference to linguistics uncertainties. This is the main advantage of use IT2-FS here, because the initial tuning would be based in some knowledge of the control process.

As it is proposed in (Leottau & Melgarejo, 2010b), it is used the middle of universe as center of antecedent sets location and the 10% of universe size as a measure for uncertainty. As universe size of ܧ߮οܧ isʹߨ, the FOU wide of this IT2-FSs is initially defined as 2ߨ ή ͳͲΨ ൌ ͲǤʹߨ and center of sets are located at zero. Please, see Figure 7.a for more clarity.

Fig. 3. Simulink model for the truck system.

The whole system is simulated using Matlab®, fuzzy Toolbox and Simulink. In addition, the IT2-FLC is designed and simulated with the IT2 Fuzzy Toolbox by the Tijuana Institute of Technology and Baja California Autonomous University (Castro et al., 2007). The Simulink model for the truck system is depicted in Figure 3.

As a figure of merit, the integral time square error (ITSE) is used (Dorf & Bishop, 1998). The objective of this example application is not to achieve an optimal performance in the controller. Thus, tuning parameters is carried out using the prove-error method, looking that the mobile robot follows a defined trajectory (see Figure 6).

#### **3.4 The reduction of dimensionality**

The reduction of dimensionality consists of converting the IT2-FLC into a T1-FLC. It is possible compressing the FOU in the IT2-FSs up to transform it in a T1-FS as is shown in Figure 4. The main objective of this procedure is making easier the tuning of fuzzy sets parameters (Leottau & Melgarejo, 2010b). The amount of parameters used for modelling a T2-FS is greater than for a T1-FS. In this way, tuning these parameters in a T1-FLC is easier and faster (Mendel, 2001).

In this application, the embedded T1-FS that are located in the middle of each FOU is used as a tuning start point. Once the IT2-FSs are converted to T1-FSs, the MF parameters' tuning 370 Recent Advances in Mobile Robotics

• Uncertainties in control outputs. Actuators characteristics could change with time. The mobile robots mechanics often are susceptible to wear or tear (Hagras, 2007). • Uncertainties associated with changing operational conditions (Mendel, 2007).

The sources of uncertainty mentioned above are included in the final design. But, as it is mentioned in step 3.2 of this procedure, FOUs in this part of design, makes reference to linguistics uncertainties. This is the main advantage of use IT2-FS here, because the initial

As it is proposed in (Leottau & Melgarejo, 2010b), it is used the middle of universe as center of antecedent sets location and the 10% of universe size as a measure for uncertainty. As universe size of ܧ߮οܧ isʹߨ, the FOU wide of this IT2-FSs is initially defined as 2ߨ ή ͳͲΨ ൌ ͲǤʹߨ and center of sets are located at zero. Please, see Figure 7.a for more clarity.

The whole system is simulated using Matlab®, fuzzy Toolbox and Simulink. In addition, the IT2-FLC is designed and simulated with the IT2 Fuzzy Toolbox by the Tijuana Institute of Technology and Baja California Autonomous University (Castro et al., 2007). The Simulink

As a figure of merit, the integral time square error (ITSE) is used (Dorf & Bishop, 1998). The objective of this example application is not to achieve an optimal performance in the controller. Thus, tuning parameters is carried out using the prove-error method, looking

The reduction of dimensionality consists of converting the IT2-FLC into a T1-FLC. It is possible compressing the FOU in the IT2-FSs up to transform it in a T1-FS as is shown in Figure 4. The main objective of this procedure is making easier the tuning of fuzzy sets parameters (Leottau & Melgarejo, 2010b). The amount of parameters used for modelling a T2-FS is greater than for a T1-FS. In this way, tuning these parameters in a T1-FLC is easier

In this application, the embedded T1-FS that are located in the middle of each FOU is used as a tuning start point. Once the IT2-FSs are converted to T1-FSs, the MF parameters' tuning

tuning would be based in some knowledge of the control process.

Fig. 3. Simulink model for the truck system.

**3.4 The reduction of dimensionality** 

and faster (Mendel, 2001).

model for the truck system is depicted in Figure 3.

that the mobile robot follows a defined trajectory (see Figure 6).

is carried out using Microsoft Excel® solver tool that uses the Generalized Reduced Gradient Method as non linear optimization algorithm (Frontline Systems, 2010). A screen of used Excel® spreadsheet is shown in Figure 5.

Fig. 4. Reducing the dimensionality of an IT2-FS.

Fig. 5. A screen of used Excel® spreadsheet for MF parameters' tuning.

#### **3.5 Returning to Type-2**

Once the T1-FLC parameters are tuned, returning to T2-FLS scheme using the same uncertainties defined in step 3.3 is proposed in (Leottau & Melgarejo, 2010b), but in this case, locating the FOUs over their embedded T1-FSs obtained in previously tuning. So, By

An Embedded Type-2 Fuzzy Controller for a Mobile Robot Application 373

**Fuzzyfier** FCA MBA **UA =DA** 250 250 **Inference Engine** FCA -

**UC** 1000 1000 **DC** 100 4 **RAM Consumption** 456 Bytes (22.8%) 44 Bytes (0.28%) **DATA Mem. Consumption** 4600 Bytes (57.5%) 136 Bytes (0.017%)

models have been characterized and tested over an IT2-FLS with the following

), nine IT2-FSs in the consequent (

of that IT2-FLS are similar and even more complex than the IT2-FLC designed for this mobile robot application, following considerations are based in that methodological

According inference times reported in (Leottau & Melgarejo, 2010a) for a DSC-56F8013, a complete inference is carried out between 500uS and 60mS for the fastest (MBA, Dc=10) and the slowest (FCA, Dc=1000) computational model respectively. That times can be used as references for determine the IT2-FLC sample time (*Ts*) for the truck system. Among this time, the embedded FLC samples the inputs variables for computing a new crisp output.

output is limited to ±40º, it is necessary to define a sampling time such that

rotate at least 80º. If it is used a Hitec servo motor (Hitec Servo Motors, 2010) that handling

Computational model chosen for the IT2-FLC with centroid TR is implemented with FCA for fuzzyfier and inference engine. Although (Leottau & Melgarejo, 2010a) evidences that processors implemented with FCA are slower than those implemented with MBA, this is not a problem taking into account that selected sampling time is enough. On the other hand, available DSC56F8013 offers limited memory resources, with that respect (Leottau & Melgarejo, 2010a) evidence that FCA is the computational model with less data memory consumption. In this way, we consider that cost-benefit trade-off of this computational

Computational models with CoS TR offer small data memory consumption but lower accuracy regarding methods as Centroid (Mendel, 2001). On the other hand, computational models based on MBA offer the highest memory consumptions and the highest accuracy (Leottau & Melgarejo, 2010a). Thus, by combining CoS TR with a fuzzyfier based on MBA can be a convenient trade-off between accuracy and data memory usage. Thus, computational

model chosen for the IT2-FLS with CoS TR is implemented with MBA for the fuzzyfier.

), three IT2-FSs by input (

Table 2. Computational models features.

N=2

operating speed from 0.05 sec/60º, defining a sampling time of

**4.2.2 Computational model with centroid type-reduction** 

**4.2.3 Computational model with CoS type-reduction** 

R=9

characteristics: two inputs (

**4.2.1 Processing time** 

=6), nine rules (

maintaining a convenient extra range.

model is convenient for this application.

(MA=M·N

proposal.

Since θ **IT2-FLC with centroid TR IT2-FLC with CoS TR** 

M=3

MC=9

), six antecedent sets

). Since characteristics

Ts=100mS

θ can

is enough,

using the tuned T1-FSs obtained in step 3.4 as the central embedded T1-FS as in Figure 4, the same FOUs wide resulting by step 3.3 are included, so the FLC returns to its initial interval type-2 nature. Then a fine tuning is carried out again by using the prove-error method taking into account the system response obtained by simulation.

### **4. Hardware implementation of the IT2-FLC for the truck**

Designed IT2-FLC showing in section 3 has the following characteristics: three inputs (N=3), two interval type-2 fuzzy sets (IT2-FSs) by input (*M=2*), six antecedent sets (*MA=M·N=6*), eight rules (*R=8*), four IT2-FSs in the consequent (*MC=4*). It must be taken into account that negative values in antecedent sets are handled as the complement of their respective positive values, so in practical terms it is defined one IT2-FSs by input (�� = 1 and �� � = 3). It is applicable for example for calculating the data memory usages. Universe of discourse' sizes have been defined in section 3.2. Its discretization levels in antecedents (*DA*) and consequent (*DC*) must be defined according computational and hardware resources available on the embedded platform to this implementation.

#### **4.1 Available embedded platform for implementing the IT2-FLC**

The available embedded platform for implementing the IT2-FLC is the Digital Signal Controller DSC-56F8013 evaluation board (Freescale Semiconductor, 2010). Next, some hardware resources of DSC-56F8013 are listed in Table 1.


Table 1. DSP-56F8013 basic features.

#### **4.2 Determining computational model for implementing the IT2-FLC for the truck system**

A summary of computational models for implementing the embedded IT2-FLC is shown in Table 2. The rest of this section explains why these computational models are chosen and how can be determined some of its features.

A methodological proposal for implementing interval type-2 fuzzy processors (IT2-FP) over DSC technology is reported in (Leottau & Melgarejo, 2010a). There, several computational


Table 2. Computational models features.

models have been characterized and tested over an IT2-FLS with the following characteristics: two inputs (N=2), three IT2-FSs by input (M=3), six antecedent sets (MA=M·N=6), nine rules (R=9), nine IT2-FSs in the consequent (MC=9). Since characteristics of that IT2-FLS are similar and even more complex than the IT2-FLC designed for this mobile robot application, following considerations are based in that methodological proposal.

#### **4.2.1 Processing time**

372 Recent Advances in Mobile Robotics

using the tuned T1-FSs obtained in step 3.4 as the central embedded T1-FS as in Figure 4, the same FOUs wide resulting by step 3.3 are included, so the FLC returns to its initial interval type-2 nature. Then a fine tuning is carried out again by using the prove-error method

Designed IT2-FLC showing in section 3 has the following characteristics: three inputs (

positive values, so in practical terms it is defined one IT2-FSs by input (�� = 1 and ��

two interval type-2 fuzzy sets (IT2-FSs) by input (*M=2*), six antecedent sets (*MA=M·N=6*), eight rules (*R=8*), four IT2-FSs in the consequent (*MC=4*). It must be taken into account that negative values in antecedent sets are handled as the complement of their respective

It is applicable for example for calculating the data memory usages. Universe of discourse' sizes have been defined in section 3.2. Its discretization levels in antecedents (*DA*) and consequent (*DC*) must be defined according computational and hardware resources

The available embedded platform for implementing the IT2-FLC is the Digital Signal Controller DSC-56F8013 evaluation board (Freescale Semiconductor, 2010). Next, some

N=3),

� = 3).

taking into account the system response obtained by simulation.

**4. Hardware implementation of the IT2-FLC for the truck** 

available on the embedded platform to this implementation.

hardware resources of DSC-56F8013 are listed in Table 1.

**Communication Interfaces** 

Table 1. DSP-56F8013 basic features.

how can be determined some of its features.

**system** 

**4.1 Available embedded platform for implementing the IT2-FLC** 

**Word length** 16-bit

**Program Flash Memory** 8KB (4KWord)

**Core frequency** Up to 32 MIPS at 32MHz

**RAM** 2KB (1KWord) **PWM module** One 6-channel

**ADC** Two 3-channel 12-bit

**Quad Timer** One 16-bit Quad Timer.

**On-Chip Emulation** JTAG/Enhanced On-Chip Emulation (OnCE™) for

**4.2 Determining computational model for implementing the IT2-FLC for the truck** 

A summary of computational models for implementing the embedded IT2-FLC is shown in Table 2. The rest of this section explains why these computational models are chosen and

A methodological proposal for implementing interval type-2 fuzzy processors (IT2-FP) over DSC technology is reported in (Leottau & Melgarejo, 2010a). There, several computational

One Serial Communication Interface (SCI) with LIN slave functionality. One Serial Peripheral Interface (SPI). One Inter-Integrated Circuit (I2C) Port.

unobtrusive, real-time debugging.

According inference times reported in (Leottau & Melgarejo, 2010a) for a DSC-56F8013, a complete inference is carried out between 500uS and 60mS for the fastest (MBA, Dc=10) and the slowest (FCA, Dc=1000) computational model respectively. That times can be used as references for determine the IT2-FLC sample time (*Ts*) for the truck system. Among this time, the embedded FLC samples the inputs variables for computing a new crisp output.

Since θ output is limited to ±40º, it is necessary to define a sampling time such that θ can rotate at least 80º. If it is used a Hitec servo motor (Hitec Servo Motors, 2010) that handling operating speed from 0.05 sec/60º, defining a sampling time of Ts=100mS is enough, maintaining a convenient extra range.

#### **4.2.2 Computational model with centroid type-reduction**

Computational model chosen for the IT2-FLC with centroid TR is implemented with FCA for fuzzyfier and inference engine. Although (Leottau & Melgarejo, 2010a) evidences that processors implemented with FCA are slower than those implemented with MBA, this is not a problem taking into account that selected sampling time is enough. On the other hand, available DSC56F8013 offers limited memory resources, with that respect (Leottau & Melgarejo, 2010a) evidence that FCA is the computational model with less data memory consumption. In this way, we consider that cost-benefit trade-off of this computational model is convenient for this application.

#### **4.2.3 Computational model with CoS type-reduction**

Computational models with CoS TR offer small data memory consumption but lower accuracy regarding methods as Centroid (Mendel, 2001). On the other hand, computational models based on MBA offer the highest memory consumptions and the highest accuracy (Leottau & Melgarejo, 2010a). Thus, by combining CoS TR with a fuzzyfier based on MBA can be a convenient trade-off between accuracy and data memory usage. Thus, computational model chosen for the IT2-FLS with CoS TR is implemented with MBA for the fuzzyfier.

An Embedded Type-2 Fuzzy Controller for a Mobile Robot Application 375

On the other hand, the DATA memory usage for the inference engine with centroid TR and

memory, the maximum discretization levels in the consequent regarding DATA memory

Thus, with centroid TR and MBA for fuzzyfier and inference engine, *Dc* must be less than

Equation 7 evidences that *Dc* must be than three hundred twelve points. So, *Dc* is defined as one hundred taking into account the study carried out in (Leottau & Melgarejo, 2010a). This

The obtained IT2-FLC for tracking the trajectory of the truck Backer-Upper is presented in this sub-section. The IT2-FLC has been designed following the procedure described throughout section 3. Preliminary response of *x* and � obtained in step 3.3 is presented in Figure 6. This response has been obtained with the IT2-FSs resulting in the initialization step. These IT2-FSs and resulting T1-FSs tuned in step 3.4 are shown in Figure 7.a. The final

Fig. 6. IT2-FLC design procedure. Preliminary response of *x* and obtained in step 3.3.

Figure 8 resumes the approach outlined throughout this chapter applied for designing and implementing an IT2-FLC for the truck system. After modelling and defining parameters steps in 3.1 and 3.2, initialization and the first design in step 3.3 is carried out by simulation with Matlab® over Simulink. Subsequent to reduction of dimensionality in step 3.4, the tuning of the T1-FLC is carried out by using Excel®. Then, tuned T1-FLC is tested over simulation and emulation. Later than returning to Type-2 and the final fine tuning in step

<sup>0</sup> <sup>100</sup> <sup>200</sup> <sup>300</sup> <sup>400</sup> <sup>500</sup> <sup>600</sup> <sup>700</sup> <sup>800</sup> <sup>900</sup> <sup>1000</sup> <sup>0</sup>

time [x0.1 s]

MBA is

**5.2 Tests** 

x(t) [inch]

2·DC·MC

resources can be calculated as:

study evidence that by using

and *2·MA*

memory consumption and processing time.

**5.1 Obtained IT2-FLC for the truck backer-upper** 

IT2-FSs obtained after step 3.5 are shown in Figure 7.b.

**5. Tests, results and discussion** 

·DA

������) = ��������������� �2·��

312. On the other hand, RAM usage for inference engine with CoS TR is

Dc=100

������) = ������������� �2·�� � 2 · �) 2⁄ = 486 (6)

for the fuzzyfier*.* Since DSC56F8013 offers 4KWord in Flash

� · ��) �2 · � � = �12 �) (7)

*,* results maintaining a trade-off between accuracy,

2·R=16

Xref(t) x(t)

 *Words.* 

#### **4.2.4 Interconnecting the emulated mobile robot plant with the embedded IT2-FLC**

The truck system is emulated by using Matlab® over Simulink, thus it is necessary interconnect the DSC56F8013 with the computer in order to communicate the embedded IT2-FLC with the truck system. Since 56F8013 evaluation board include a RS-232 interface, for easy connection to a host processor the serial port is used. Since emulation is carried out over a general propose processor which serial port baud-rate is limited to 115200, it is selected a sped of 57600 bauds in order to not to force the platform.

As it is mentioned in section 3, in a context of simulation, a block of Matlab® fuzzy toolbox or it2fuzzy toolbox is included in the Simulink model for simulating the T1-FLC and the IT2- FLC respectively (Figure 3). In order to interconnect the truck system plant with the IT2-FLC embedded in the DSC56F8013, these blocks must be replaced by a Matlab function that carries out the RS-232 serial port communication.

#### **4.2.5 Universe sizes and discretización levels regarding RS-232 communication**

Universes of discourse in the process are defined as: �� � [−20, 20] (������ � = 40), �� ��� �� � [−�, �](�(����� � ��� ��) = 2�) and � � [−� �⁄ ,� �⁄ ] (�(����� �) = 2�/�).

RS-232 interface has been chosen for connecting the embedded IT2-FLC with the truck system emulated by using Matlab® over Simulink. Since RS-232 interface can send and receive up to one Byte per time (28=256 levels), a universe of discourse between [0, 249] is defined in order to transmit just one byte per input, so UA=DA=250. Resolution of input universes is determined as: Input\_res=UPlant/DA. Thus, Ex\_res=40/250=0.16, Eφ\_res=2π/250≃0.025 and Δφ\_res=2π /250≃0.025.

Output universe is defined between [-π/3, π/3]. If UC is the output universe size in the embedded IT2-FLC, calculating output resolution by using DA=UC, θ\_res= 2π/(3·250)=0.00837=0.48º. Since computational models chosen particularly offer a low accuracy and output is limited to ±40º, we consider that 0.48º is a low resolution. Thus, a universe of discourse between [0, 999] is defined, so UC=1000 and θ\_res= 0.00209=0.12º.

It is necessary to transmit from the PC serial port to the IT2-FLC embedded on the DSC56F8013 three bytes, one per input of IT2-FLC. Then, it is necessary to receive one data as its output. Since the output universe of the embedded IT2-FLC has been to a thousand points. In order to receive the output of the IT2-FLC from DSC56F8013, it is necessary to divide this result. So, units, tens and cents are transmitted as three independent bytes those must be concatenated by a Matlab RS-232 function before to be injected to the truck plant.

Universes of discourse for the embedded IT2-FLC are defined in the previous paragraphs as and. So, it is necessary a scalization as:

$$V\_{\rm FLC} = V\_{\rm Plant} \cdot U\_{\rm FLC} / U\_{\rm Plant} + U\_{\rm FLC} / 2 \tag{5}$$

Where VFLC and UFLC are respectively the scaled value and the universe size that handles the embedded FLC and UPlant and VPlant are respectively the universe size and the un-scaled value that handles the truck plant.

#### **4.2.6 Discretization levels in the consequent regarding available memory resources**

According (Leottau & Melgarejo, 2010a), the RAM usage expressed in Words for the inference engine with centroid type-reduction is 2·DC+2·R and for all discussed fuzzyfiers is *2·MA.* Since DSC56F8013 offers 1KWord in RAM, the maximum discretization levels in the consequent regarding RAM resources can be calculated as:

$$D\_{C(max)} = \left( RAM\_{available} - 2 \cdot M\_A - 2 \cdot R\right) / 2 = 486 \tag{6}$$

On the other hand, the DATA memory usage for the inference engine with centroid TR and MBA is 2·DC·MC and *2·MA*·DA for the fuzzyfier*.* Since DSC56F8013 offers 4KWord in Flash memory, the maximum discretization levels in the consequent regarding DATA memory resources can be calculated as:

$$D\_{\mathbb{C}(\max)} = \left( Flashh\_{\text{available}} - 2 \cdot M\_{\text{A}}^{\top} \cdot D\_{\text{A}} \right) / \left( 2 \cdot M\_{\text{C}} \right) = 312 \tag{7}$$

Thus, with centroid TR and MBA for fuzzyfier and inference engine, *Dc* must be less than 312. On the other hand, RAM usage for inference engine with CoS TR is 2·R=16 *Words.*  Equation 7 evidences that *Dc* must be than three hundred twelve points. So, *Dc* is defined as one hundred taking into account the study carried out in (Leottau & Melgarejo, 2010a). This study evidence that by using Dc=100*,* results maintaining a trade-off between accuracy, memory consumption and processing time.

#### **5. Tests, results and discussion**

#### **5.1 Obtained IT2-FLC for the truck backer-upper**

The obtained IT2-FLC for tracking the trajectory of the truck Backer-Upper is presented in this sub-section. The IT2-FLC has been designed following the procedure described throughout section 3. Preliminary response of *x* and � obtained in step 3.3 is presented in Figure 6. This response has been obtained with the IT2-FSs resulting in the initialization step. These IT2-FSs and resulting T1-FSs tuned in step 3.4 are shown in Figure 7.a. The final IT2-FSs obtained after step 3.5 are shown in Figure 7.b.

Fig. 6. IT2-FLC design procedure. Preliminary response of *x* and obtained in step 3.3.

#### **5.2 Tests**

374 Recent Advances in Mobile Robotics

As it is mentioned in section 3, in a context of simulation, a block of Matlab® fuzzy toolbox or it2fuzzy toolbox is included in the Simulink model for simulating the T1-FLC and the IT2- FLC respectively (Figure 3). In order to interconnect the truck system plant with the IT2-FLC embedded in the DSC56F8013, these blocks must be replaced by a Matlab function that

Output universe is defined between [-π/3, π/3]. If UC is the output universe size in the embedded IT2-FLC, calculating output resolution by using DA=UC, θ\_res= 2π/(3·250)=0.00837=0.48º. Since computational models chosen particularly offer a low accuracy and output is limited to ±40º, we consider that 0.48º is a low resolution. Thus, a universe of discourse between [0, 999] is defined, so UC=1000 and θ\_res= 0.00209=0.12º. It is necessary to transmit from the PC serial port to the IT2-FLC embedded on the DSC56F8013 three bytes, one per input of IT2-FLC. Then, it is necessary to receive one data as its output. Since the output universe of the embedded IT2-FLC has been to a thousand points. In order to receive the output of the IT2-FLC from DSC56F8013, it is necessary to divide this result. So, units, tens and cents are transmitted as three independent bytes those must be concatenated by a Matlab RS-232 function before to be injected to the truck plant. Universes of discourse for the embedded IT2-FLC are defined in the previous paragraphs as

Where VFLC and UFLC are respectively the scaled value and the universe size that handles the embedded FLC and UPlant and VPlant are respectively the universe size and the un-scaled

**4.2.6 Discretization levels in the consequent regarding available memory resources**  According (Leottau & Melgarejo, 2010a), the RAM usage expressed in Words for the

*2·MA.* Since DSC56F8013 offers 1KWord in RAM, the maximum discretization levels in the

2·DC+2·R

���� = ������ · ����⁄������ + ����/2 (5)

and for all discussed fuzzyfiers is

**4.2.5 Universe sizes and discretización levels regarding RS-232 communication**  Universes of discourse in the process are defined as: �� � [−20, 20] (������ � = 40), �� ��� �� � [−�, �](�(����� � ��� ��) = 2�) and � � [−� �⁄ ,� �⁄ ] (�(����� �) = 2�/�). RS-232 interface has been chosen for connecting the embedded IT2-FLC with the truck system emulated by using Matlab® over Simulink. Since RS-232 interface can send and receive up to one Byte per time (28=256 levels), a universe of discourse between [0, 249] is defined in order to transmit just one byte per input, so UA=DA=250. Resolution of input universes is determined as: Input\_res=UPlant/DA. Thus, Ex\_res=40/250=0.16,

**4.2.4 Interconnecting the emulated mobile robot plant with the embedded IT2-FLC**  The truck system is emulated by using Matlab® over Simulink, thus it is necessary interconnect the DSC56F8013 with the computer in order to communicate the embedded IT2-FLC with the truck system. Since 56F8013 evaluation board include a RS-232 interface, for easy connection to a host processor the serial port is used. Since emulation is carried out over a general propose processor which serial port baud-rate is limited to 115200, it is

selected a sped of 57600 bauds in order to not to force the platform.

carries out the RS-232 serial port communication.

Eφ\_res=2π/250≃0.025 and Δφ\_res=2π /250≃0.025.

and. So, it is necessary a scalization as:

value that handles the truck plant.

inference engine with centroid type-reduction is

consequent regarding RAM resources can be calculated as:

Figure 8 resumes the approach outlined throughout this chapter applied for designing and implementing an IT2-FLC for the truck system. After modelling and defining parameters steps in 3.1 and 3.2, initialization and the first design in step 3.3 is carried out by simulation with Matlab® over Simulink. Subsequent to reduction of dimensionality in step 3.4, the tuning of the T1-FLC is carried out by using Excel®. Then, tuned T1-FLC is tested over simulation and emulation. Later than returning to Type-2 and the final fine tuning in step

An Embedded Type-2 Fuzzy Controller for a Mobile Robot Application 377

Fig. 8. Conceptual diagram of proposed approach for implementing the embedded IT2-FLC

As it is mentioned in section 2, there are several type reducer and defuzzyfier methods. In order to determine the impact of that in the final performance of designed T1-FLC and IT2- FLC, a test is carried out changing the TR or defuzzyfier method in each FLC. Under simulation, some of available methods in its respective Matlab® toolbox (*fuzzy* for type-1 and *it2fuzzy* for type-2) are tested: centroid and center of sets (CoS) by the T2-FLC and centroid, bisector and mean of maximum (Mom) by T1-FLC. Under emulation, considered

4. Repeat the same simulation (steps 2-3) five times with a different initial noise seed in

7. Repeat the procedure for the T1-FLC and the IT2-FLC over simulation and over

Results for *x* and ߮ are presented in Figure 9 to Figure 12. Figure 9 and 10 show the response of FLCs tested without noise, under simulation and emulation respectively. Figure 11 shows the response of simulated FLCs and Figure 12 shows the response of hardware implemented FLCs, both cases tested under the worst noise condition, emulating a high uncertainty environment. Since all FLCs are tested under three different noise environments and five

for the truck system.

2. Set a noise environment.

emulation.

**5.3 Results** 

methods in section 2 are tested: Centroid and CoS. The tests procedure is described as follows:

1. Set a TR or defuzzification method for the FLC.

3. Run one complete simulation or emulation and register ITSE.

order to obtain the average and standard deviation of ITSE. 5. Repeat steeps 2-4 for the three noise environments considered. 6. Repeat steeps 1-5 for the TR and defuzzification methods considered.

Fig. 7. IT2-FLC design procedure. a) IT2-FSs first design in step 3.3 (Blue lines) and tuned T1-FSs resulting in step 3.4. (Red dotted lines) b) IT2-FSs final implementation after step 3.5 (Blue lines) and newly tuned T1-FSs resulting in step 3.4. (Red dotted lines).

3.5, the resulting IT2-FLC is tested over simulation and emulation too. As it is mentioned before, the simulations are carried out by using fuzzy and it2-fuzzy toolboxes and emulations are carried out over the 56F8013 board whose programming is debugged and loaded by using the Codewarrior® suite for DSC56800E v8.2.3.

Different tests are achieved for evaluating the final performance of designed T1-FLC and IT2- FLC. A combined trajectory in *x* is used as reference form emulating different operating points to the FLC. In addition, random noise generators are inserted in the inputs and outputs of FLC in order to emulate some sources of uncertainty such as sensors measurements and changes of actuators characteristics. The FLCs are tested under three different noise environments: (1) without noise, (2) moderate noise with amplitude as 0.2% of maximum value that respective variable can take and (3) high noise with amplitudes as 1%. E.g. *Ex* input can take values within |0, 20|, then the amplitude of moderate noise is: ������� �20 x 0.2% = 0.04. In order to emulate a stochastic nature of uncertainty, every noise environment is tested five times changing the initial seed of the random noise generator.

Fig. 8. Conceptual diagram of proposed approach for implementing the embedded IT2-FLC for the truck system.

As it is mentioned in section 2, there are several type reducer and defuzzyfier methods. In order to determine the impact of that in the final performance of designed T1-FLC and IT2- FLC, a test is carried out changing the TR or defuzzyfier method in each FLC. Under simulation, some of available methods in its respective Matlab® toolbox (*fuzzy* for type-1 and *it2fuzzy* for type-2) are tested: centroid and center of sets (CoS) by the T2-FLC and centroid, bisector and mean of maximum (Mom) by T1-FLC. Under emulation, considered methods in section 2 are tested: Centroid and CoS.

The tests procedure is described as follows:


#### **5.3 Results**

376 Recent Advances in Mobile Robotics

a) b)

loaded by using the Codewarrior® suite for DSC56800E v8.2.3.

changing the initial seed of the random noise generator.

(Blue lines) and newly tuned T1-FSs resulting in step 3.4. (Red dotted lines).

Fig. 7. IT2-FLC design procedure. a) IT2-FSs first design in step 3.3 (Blue lines) and tuned T1-FSs resulting in step 3.4. (Red dotted lines) b) IT2-FSs final implementation after step 3.5

3.5, the resulting IT2-FLC is tested over simulation and emulation too. As it is mentioned before, the simulations are carried out by using fuzzy and it2-fuzzy toolboxes and emulations are carried out over the 56F8013 board whose programming is debugged and

Different tests are achieved for evaluating the final performance of designed T1-FLC and IT2- FLC. A combined trajectory in *x* is used as reference form emulating different operating points to the FLC. In addition, random noise generators are inserted in the inputs and outputs of FLC in order to emulate some sources of uncertainty such as sensors measurements and changes of actuators characteristics. The FLCs are tested under three different noise environments: (1) without noise, (2) moderate noise with amplitude as 0.2% of maximum value that respective variable can take and (3) high noise with amplitudes as 1%. E.g. *Ex* input can take values within |0, 20|, then the amplitude of moderate noise is: ������� �20 x 0.2% = 0.04. In order to emulate a stochastic nature of uncertainty, every noise environment is tested five times

Results for *x* and ߮ are presented in Figure 9 to Figure 12. Figure 9 and 10 show the response of FLCs tested without noise, under simulation and emulation respectively. Figure 11 shows the response of simulated FLCs and Figure 12 shows the response of hardware implemented FLCs, both cases tested under the worst noise condition, emulating a high uncertainty environment. Since all FLCs are tested under three different noise environments and five

An Embedded Type-2 Fuzzy Controller for a Mobile Robot Application 379

a)

b)

c) Fig. 10. Without noise, trajectory for *x* of hardware implemented: a*)* T1-FLC with centroid defuzzification. b*)* IT2-FLC with centroid type-reduction. c*)* IT2-FLC with CoS type-

reduction. (*time=Time\_samples/Ts*, where*Ts=0.1s*)

different initial seeds for each noise environment, obtained results for ITSE as performance indices are presented as an average and its standard deviation. It is shown in tables 3 and 4.

Fig. 9. Without noise and under simulation, trajectory for *x* of: *a)* T1-FLC with Centroid defuzzification. *b)* IT2-FLC with Centroid type-reduction. *c)* IT2-FLC with CoS typereduction. (*time=Time\_samples/Ts*, where*Ts=0.1s*)

378 Recent Advances in Mobile Robotics

different initial seeds for each noise environment, obtained results for ITSE as performance indices are presented as an average and its standard deviation. It is shown in tables 3 and 4.

a)

b)

c)

Fig. 9. Without noise and under simulation, trajectory for *x* of: *a)* T1-FLC with Centroid defuzzification. *b)* IT2-FLC with Centroid type-reduction. *c)* IT2-FLC with CoS type-

reduction. (*time=Time\_samples/Ts*, where*Ts=0.1s*)

Fig. 10. Without noise, trajectory for *x* of hardware implemented: a*)* T1-FLC with centroid defuzzification. b*)* IT2-FLC with centroid type-reduction. c*)* IT2-FLC with CoS typereduction. (*time=Time\_samples/Ts*, where*Ts=0.1s*)

An Embedded Type-2 Fuzzy Controller for a Mobile Robot Application 381

a)

b)

c) Fig. 12. Under the worst noise condition, trajectory for *x* of Hardware implemented: a*)* T1- FLC with Centroid defuzzification. b*)* IT2-FLC with centroid type-reduction. c*)* IT2-FLC

with CoS type-reduction. (*time=Time\_samples/Ts*, where*Ts=0.1s*)

Fig. 11. Under the worst noise condition and simulation, trajectory for *x* of: *a)* T1-FLC with centroid defuzzification. *b)* IT2-FLC with centroid type-reduction. *c)* IT2-FLC with CoS typereduction. (*time=Time\_samples/Ts*, where*Ts=0.1s*)

380 Recent Advances in Mobile Robotics

a)

b)

c) Fig. 11. Under the worst noise condition and simulation, trajectory for *x* of: *a)* T1-FLC with centroid defuzzification. *b)* IT2-FLC with centroid type-reduction. *c)* IT2-FLC with CoS type-

reduction. (*time=Time\_samples/Ts*, where*Ts=0.1s*)

Fig. 12. Under the worst noise condition, trajectory for *x* of Hardware implemented: a*)* T1- FLC with Centroid defuzzification. b*)* IT2-FLC with centroid type-reduction. c*)* IT2-FLC with CoS type-reduction. (*time=Time\_samples/Ts*, where*Ts=0.1s*)

An Embedded Type-2 Fuzzy Controller for a Mobile Robot Application 383

T2-FLC shows a good immunity to type reducer changes, which is a great advantage taking into account the reduction of computational complexity and inference time when strategies

An IT2-FLS for controlling the trajectory of a truck backer-upper mobile robot application and its embedded implementation has been presented in this chapter. The IT2-FLC has been designed based on the approach proposed in (Leottau & Melgarejo, 2010b) and implemented following methodological considerations reported in (Leottau & Melgarejo,

Two computational models have been selected taking into account the available and demanded hardware and computational resources. Several tests have been carried out in order to evaluate and to compare the performance of developed T2-FLCs and a T1-FLC. Simulated and emulated results evidence that the IT2-FLC is robust to type reducer and defuzzyfier changes and exhibits better performance than a T1-FLC when noise is added to

As future work, it is considered to test the developed IT2-FLC with a real mobile robot platform in order to carry out a comparative study of simulation, emulation and real platform performance. By this way, it is possible to extend the design and implementation methodology to other applications, involving in a more formal way a modelling of

Baturone, I.; Barriga, A.; Sanchez, S.; Jimenez, C.J. & Lopez, D.R. (2000). *Microelectronics* 

Castro, J.; Castillo, O. & Melin, P. (2007). An Interval Type-2 Fuzzy Logic Toolbox for

Figueroa, J.; Posada, J.; Soriano, J.; Melgarejo, M. & Rojas, S. (2005). A Type-2 Fuzzy Logic

Freescale Semiconductor. (January 2010). *Digital Signal Controller 56800/E Reference*.

Frontline Systems. (March 2010). *Smooth Nonlinear Optimization*. 17.03.2010, Available from:

Hagras, H. (2004). A Type-2 Fuzzy Logic Controller for Autonomous Mobile Robots,

Hagras, H. (2007). Type-2 FLC's: a New Generation of Fuzzy Controllers, *IEEE* 

Hagras, H. (2008). *Developing a type-2 FLC through embedded type-1 FLCs,* Proceedings of the

Control Applications, *Proceedings of the 2007 IEEE International Conference on Fuzzy* 

Controller For Tracking Mobile Objects In The Context Of Robotic Soccer Game, *Proceedings of the 2005 IEEE International Conference on Fuzzy Systems, FUZZ-IEEE* 

*Proceedings of the 2004 IEEE International Conference on Fuzzy Systems, FUZZ-IEEE* 

2008 IEEE International Conference on Fuzzy Systems, FUZZ-IEEE 2008, Hong

*Design of Fuzzy Logic-Based Systems*, CRC Press LLC, London.

29.01.2010, Available from: Http://www.Freescale.Com/Dsc

*Computational Intelligence Magazine*, February 2007, pp. 30-43.

Dorf, R. & Bishop, R. (1998). *Modern Control Systems*, Addison-Wesley.

2010a) to the hardware implementation of IT2-FLS over DSC technology.

inputs and outputs emulating some sources of uncertainty.

*Systems, FUZZ-IEEE 2007,* London, UK.

http://www.solver.com/technology4.htm

*2004*, Budapest, Hungary.

as CoS or heights are implemented.

**6. Conclusions** 

uncertainty.

**7. References** 

*2005*.

Kong.


† Does not reach the reference.

†† Response turns unstable.

††† Does not reach the reference and presents oscillations.

Since tests have evidenced that the T1-FLCs with MoM and Bisector defuzzyfier and the embedded T1- FLC with CoS defuzzyfier do not present a successfully performance, response for these FLCs have not shown in Figs 9 to 12.


Table 4. Results of ITSE for the IT2-FLC.

#### **5.4 Discussion**

Without noise and using centroid TR, Simulated IT2-FLC ITSE is about 39.2% better than simulated T1-FLC ITSE and embedded IT2-FLC ITSE is about 82.8% better than embedded T1-FLC ITSE. Under moderate noise and using centroid TR it can be said that: Simulated T2- FLC ITSE is about 39.4% better than simulated T1-FLC ones and embedded T2-FLC ITSE is about 83.69% better than embedded T1-FLC. Under high noise and using centroid TR Simulated T2-FLC ITSE is about 54.9% better than simulated T1-FLC ones and embeddedT2- FLC ITSE is about 54.78% better than embedded T1-FLC ones.

ITSE of simulated IT2-FLC with centroid type reduction is between 18% (without noise) and 42% (with high noise) better than the IT2-FLC with CoS. Besides, ITSE of hardware implemented IT2-FLC with centroid type reduction is between 15% (without noise) and 25% (with high noise) better than the IT2-FLC with CoS.

The ITSE of the simulated IT2-FLC with Cent type reducer is between 0.4% (with moderate noise) and 28% (with high noise) better than the embedded IT2-FLC whit the same type reducer. Besides, the ITSE of the simulated IT2-FLC with CoS type reducer is between 0.3% (without noise) and 6.7% (with high noise) better than the embedded IT2-FLC whit the same type reducer. This difference is caused by the integer numeric format that handles the hardware platform which causes lost in the accuracy of embedded FLCs regarding their models tested over simulation.

Taking into account the comparison presented above, it is possible to consider that the IT2- FLC outperforms the T1-FLC in all cases. In addition, by looking Figure 11 and 12, it can be observed that the T2-FLC presents fewer oscillations around the reference, being the IT2- FLC implemented with centroid type reduction, those with better performance.

T2-FLC shows a good immunity to type reducer changes, which is a great advantage taking into account the reduction of computational complexity and inference time when strategies as CoS or heights are implemented.

### **6. Conclusions**

382 Recent Advances in Mobile Robotics

**Zero** 18.48 - † - †† - 68.025 - ††† - **Mod.** 22.27 2.10 † - †† - 84.84 5.625 ††† - **High** 98.93 14.71 † - †† - 137.38 13.94 ††† -

Since tests have evidenced that the T1-FLCs with MoM and Bisector defuzzyfier and the embedded T1- FLC with CoS defuzzyfier do not present a successfully performance, response for these FLCs have not

**Noise level Cent. CoS Cent. Hardware CoS Hardware** 

**Zero** 11.23 - 13.73 - 11.7 - 13.77 - **Mod.** 13.78 0.93 18.02 1.70 13.84 0.85 18.37 0.87 **High** 44.57 4.81 77.41 10.49 62.122 6.45 82.92 8.76

Without noise and using centroid TR, Simulated IT2-FLC ITSE is about 39.2% better than simulated T1-FLC ITSE and embedded IT2-FLC ITSE is about 82.8% better than embedded T1-FLC ITSE. Under moderate noise and using centroid TR it can be said that: Simulated T2- FLC ITSE is about 39.4% better than simulated T1-FLC ones and embedded T2-FLC ITSE is about 83.69% better than embedded T1-FLC. Under high noise and using centroid TR Simulated T2-FLC ITSE is about 54.9% better than simulated T1-FLC ones and embeddedT2-

ITSE of simulated IT2-FLC with centroid type reduction is between 18% (without noise) and 42% (with high noise) better than the IT2-FLC with CoS. Besides, ITSE of hardware implemented IT2-FLC with centroid type reduction is between 15% (without noise) and 25%

The ITSE of the simulated IT2-FLC with Cent type reducer is between 0.4% (with moderate noise) and 28% (with high noise) better than the embedded IT2-FLC whit the same type reducer. Besides, the ITSE of the simulated IT2-FLC with CoS type reducer is between 0.3% (without noise) and 6.7% (with high noise) better than the embedded IT2-FLC whit the same type reducer. This difference is caused by the integer numeric format that handles the hardware platform which causes lost in the accuracy of embedded FLCs regarding their

Taking into account the comparison presented above, it is possible to consider that the IT2- FLC outperforms the T1-FLC in all cases. In addition, by looking Figure 11 and 12, it can be observed that the T2-FLC presents fewer oscillations around the reference, being the IT2-

FLC implemented with centroid type reduction, those with better performance.

*ITSE Desv. ITSE Desv. ITSE Desv. ITSE Desv.* 

*ITSE Desv. ITSE Desv. ITSE Desv. ITSE Desv. ITSE Desv.* 

**Hardware**

**CoS Hardware** 

**Cent. Bisector MoM Cent.** 

**Noise level** 

† Does not reach the reference. †† Response turns unstable.

shown in Figs 9 to 12.

**5.4 Discussion** 

††† Does not reach the reference and presents oscillations.

Table 3. Results of ITSE for the T1-FLC.

Table 4. Results of ITSE for the IT2-FLC.

FLC ITSE is about 54.78% better than embedded T1-FLC ones.

(with high noise) better than the IT2-FLC with CoS.

models tested over simulation.

An IT2-FLS for controlling the trajectory of a truck backer-upper mobile robot application and its embedded implementation has been presented in this chapter. The IT2-FLC has been designed based on the approach proposed in (Leottau & Melgarejo, 2010b) and implemented following methodological considerations reported in (Leottau & Melgarejo, 2010a) to the hardware implementation of IT2-FLS over DSC technology.

Two computational models have been selected taking into account the available and demanded hardware and computational resources. Several tests have been carried out in order to evaluate and to compare the performance of developed T2-FLCs and a T1-FLC. Simulated and emulated results evidence that the IT2-FLC is robust to type reducer and defuzzyfier changes and exhibits better performance than a T1-FLC when noise is added to inputs and outputs emulating some sources of uncertainty.

As future work, it is considered to test the developed IT2-FLC with a real mobile robot platform in order to carry out a comparative study of simulation, emulation and real platform performance. By this way, it is possible to extend the design and implementation methodology to other applications, involving in a more formal way a modelling of uncertainty.

### **7. References**


**0**

**19**

*Mexico*

**LQR Control Methods for Trajectory Execution in**

Omnidirectional mobile robots present the advantage of being able to move in any direction without having to rotate around the vertical axis first. While simple straight-line paths are relatively easy to achieve on this kind of robots, in many highly dynamic applications straight-line paths might just not be a feasible solution. This may be the case because of two main reasons: (1) there may be static and moving obstacles between the initial and desired final position of the robot, and (2) the dynamic effects of the inertia of the robot may force it to execute a curved path. This chapter will address these two situations and present a segment-wise optimal solution for the path execution problem which is based on a

It must be emphasized that, rather than attempting to perform an exact path tracking, the approach presented here deals with the problem of visiting a sequence of target circular regions without specifying the path that will connect them. The freedom given to the connecting paths brings the opportunity for optimization. In fact, the path that the robot will take from one circular region to the next will emerge as the solution of an optimal control

Omnidirectional wheels have the property of sliding laterally with almost zero force while providing full traction in the rolling direction. This effect is achieved by adding a set of smaller

**1. Introduction**

Linear-Quadratic Regulator.

Fig. 1. Omnidirectional wheel

problem, hence the term *segment-wise optimal* solution.

wheels around the periphery of the main wheel, as depicted in Fig. 1.

**Omnidirectional Mobile Robots**

*Mobile Robotics & Automated Systems Lab, Universidad La Salle*

Luis F. Lupián and Josué R. Rabadán-Martin


### **LQR Control Methods for Trajectory Execution in Omnidirectional Mobile Robots**

Luis F. Lupián and Josué R. Rabadán-Martin *Mobile Robotics & Automated Systems Lab, Universidad La Salle Mexico*

#### **1. Introduction**

384 Recent Advances in Mobile Robotics

John, R. & Coupland, S. (2007). Type-2 Fuzzy Logic A Historical View*, IEEE Computational* 

Karnik, N. & Mendel, J. (2001). Centroid of a Type-2 Fuzzy Set*, Information Sciences*, 132, pp.

Leottau, L. & Melgarejo, M. (2010a). A Methodological Proposal for Implementing Interval

Leottau, L. & Melgarejo, M. (2010b). A Simple Approach for Designing a Type-2 Fuzzy

*Information Processing Society's NAFIPS 2010*, Toronto, Canada, July 2010. Nguyen, D. & Widrow, B. (1989). The Truck Backer-Upper: An Example of self-learning in

Martinez, R.; Castillo, O. & Aguilar, L. (2008). Optimization with Genetic Algorithms of

Melgarejo, M.; Garcia, A. & Pena-Reyes, C. (2004). Computational Model and architectural

Melgarejo, M. & Pena-Reyes, C. A. (2007). Implementing Interval Type-2 Fuzzy Processors,

Melin, P. & Castillo, O. (2003). A new method for adaptive model-based control of non-

Mendel, J. (2007). Advances In Type-2 Fuzzy Sets and Systems, *Information Sciences*, 177,1,

Torres, P. & Saez, D. (2008). Type-2 fuzzy logic identification applied to the modelling of a

Wu, D. & Mendel, J. (2009). Enhanced Karnik-Mendel Algorithms, *IEEE Transactions on* 

Wu, D. & Tan, W. (2004). A type-2 fuzzy logic controller for the liquid-level process,

Wang, L. X. (1997). *A Course in Fuzzy Systems and Control*, Prentice Hall, New Jersey.

Type-2 Fuzzy Processors over Digital Signal Controllers, *Journal of Applied Computer* 

Controller for a Mobile Robot Application, *Proceedings of the North American Fuzzy* 

Neural Networks. *Proceedings of the International Joint Conference in Neural Networks*,

Interval Type-2 Fuzzy Logic controllers for an autonomous wheeled mobile robot: A comparison under different kinds of perturbations, *Proceedings of the 2008 IEEE* 

proposal for a hardware Type-2 Fuzzy System, *Proceedings of the 2nd IASTED conference on Neural Networks and Computational Intelligence*, Grindewald,

linear plants using type-2 fuzzy logic and neural Networks, *Proceedings of the 2003 IEEE International Conference on Fuzzy Systems, FUZZ-IEEE 2003,* St. Louis, MO. Mendel, J. (2001). Uncertain Rule-Based Fuzzy Logic Systems: Introduction and New

robot hand, *Proceedings of the 2008 IEEE International Conference on Fuzzy Systems,* 

Proceedings of the 2004 IEEE International Conference on Fuzzy Systems, FUZZ-

Hitec Servo-Motors, *Servo-motors catalogue*. 05.08.2010, Available from: http://www.hitecrcd.com/products/servos/index.html

Kuo, B. & Golnaraghi, F. (1996). *Automatic Control Systems*, Prentice Hall.

*International Conference on Fuzzy Systems, FUZZ-IEEE 2008*.

*IEEE Computational Intelligence Magazine*, 2,1, pp. 63-71.

Directions, Prentice Hall, New Jersey.

*Intelligence Magazine*, 2,1, pp. 57-62.

pp. II-357-363, June 1989.

Switzerland.

pp. 84-110.

IEEE 2004.

*FUZZ-IEEE 2008*.

*Fuzzy Systems*, 17, pp. 923-934.

*Science Methods*, v.2-1, June 2010, pp.61-81.

195-220.

Omnidirectional mobile robots present the advantage of being able to move in any direction without having to rotate around the vertical axis first. While simple straight-line paths are relatively easy to achieve on this kind of robots, in many highly dynamic applications straight-line paths might just not be a feasible solution. This may be the case because of two main reasons: (1) there may be static and moving obstacles between the initial and desired final position of the robot, and (2) the dynamic effects of the inertia of the robot may force it to execute a curved path. This chapter will address these two situations and present a segment-wise optimal solution for the path execution problem which is based on a Linear-Quadratic Regulator.

It must be emphasized that, rather than attempting to perform an exact path tracking, the approach presented here deals with the problem of visiting a sequence of target circular regions without specifying the path that will connect them. The freedom given to the connecting paths brings the opportunity for optimization. In fact, the path that the robot will take from one circular region to the next will emerge as the solution of an optimal control problem, hence the term *segment-wise optimal* solution.

Omnidirectional wheels have the property of sliding laterally with almost zero force while providing full traction in the rolling direction. This effect is achieved by adding a set of smaller wheels around the periphery of the main wheel, as depicted in Fig. 1.

Fig. 1. Omnidirectional wheel

**2. State-space dynamic model**

of the robot.

Fig. 3. Omnidirectional robot

The analysis presented pertains to a specific class of omnidirectional mobile robots that are of cylindrical shape with *n* omnidirectional wheels distributed around the periphery of the body of the robot, with the axes of the wheels intersecting at the geometrical vertical axis of the robot. Figure 3 shows an instance of this class of omnidirectional mobile robots for the case of *n* = 5 omnidirectional wheels. It is not necessary to have a uniform distribution of the wheels around the periphery of the robot. That is, the angle that separates the axis of one wheel to the next does not need to be the same. However, there is an obvious restriction that must be met in order to maintain stability, which is that the projection of the center of mass of the robot onto the ground must be contained within the convex hull of the set of contact points of the *n* wheels with the ground. For simplicity of the analysis, the mass of the robot is assumed to be distributed uniformly, so the center of mass is contained within the geometrical vertical axis

LQR Control Methods for Trajectory Execution in Omnidirectional Mobile Robots 387

In the literature, there are several proposals for the dynamic model of an omnidirectional mobile robot. One of the main problems with these models is that they do not provide a complete state-space representation, so it is not possible to perform state-space control by using one of them. Most of these proposals are based on the force coupling matrix (Gloye & Rojas, 2006), which provides a direct relationship between the torques applied by the driving

*<sup>M</sup>* (*F*<sup>1</sup> <sup>+</sup> *<sup>F</sup>*<sup>2</sup> <sup>+</sup> ··· <sup>+</sup> *Fn*)

(*f*<sup>1</sup> + *f*<sup>2</sup> + ··· + *fn*)

where *a* is the acceleration of the robot with respect to the inertial reference, *ω*˙ is the angular acceleration, *n* is the number of omnidirectional wheels around the periphery of the robot, *Fi* is the vector force applied by motor *i*, *fi* is the signed scalar value of *Fi* (positive for counter clock-wise rotation), *M* is the mass of the robot, *R* is its radius and *I* is its moment of inertia.

(1)

motors and the accelerations in the *x*, *y* and angular directions. In compact form, these equations may be written as follows

*<sup>a</sup>* <sup>=</sup> <sup>1</sup>

*<sup>ω</sup>*˙ <sup>=</sup> *<sup>R</sup> I*

By using several omnidirectional wheels distributed around the periphery of a cylindrical robot one can achieve the effect of driving the robot in a direction dependent on a vector sum of forces. This idea is illustrated in Fig. 2. The forces applied at the wheels by the motors give translational and rotational movement to the mobile robot.

#### Fig. 2. Omnidirectional drive

Omnidirectional movement on a mobile robot has many applications, and thus has received attention from the scientific community over several years. Borenstein and Evans developed a control strategy that allows trajectory control on a mobile robot that uses conventional non-omnidirectional wheels and thus requires the drive wheels to rotate with respect to the vertical axis (Borenstein & Evans, 1997). With regard to omnidirectional mobile robots based on omnidirectional wheels there are several works that deserve attention. Ashmore and Barnes presented a detailed analysis of the kinematics of this kind of mobile robots and show that under certain circumstances a curved path may be faster than the straight line between two points (Ashmore & Barnes, 2002). Balkcom et al. characterized time-optimal trajectories for omnidirectional mobile robots (Balkcom et al., 2006a;b). Kalmár-Nagy et al. developed a control method to generate minimum-time trajectories for omnidirectional mobile robots (Kalmár-Nagy et al., 2004; 2002), then Purwin and D'Andrea presented the results of applying this method to a RoboCup F180 omnidirectional mobile robot (Purwin & D'Andrea, 2005).

Section 2 deals with the first necessary step towards applying LQR control for trajectory execution, which is formulating a state-space model for the dynamics of the omnidirectional mobile robot (Lupián & Rabadán-Martin, 2009).

This state-space model is non linear with respect to the control due to the fact that the robot rotates around the vertical axis and the pan angle is one of the state variables. In Sec. 3 of this chapter we show how to overcome this problem to successfully apply a Linear Quadratic Regulator for the case of three, four and five-wheeled omnidirectional mobile robots. In Sec. 4 we present a method to generate a segment-wise optimal path by solving an LQR control problem for each segment between the initial state through a sequence of target regions that ends at the desired final state of the omnidirectional mobile robot. Finally, Sec. 5 presents the results of several simulation experiments that apply the methods described in this chapter.

#### **2. State-space dynamic model**

2 Will-be-set-by-IN-TECH

By using several omnidirectional wheels distributed around the periphery of a cylindrical robot one can achieve the effect of driving the robot in a direction dependent on a vector sum of forces. This idea is illustrated in Fig. 2. The forces applied at the wheels by the motors give

Omnidirectional movement on a mobile robot has many applications, and thus has received attention from the scientific community over several years. Borenstein and Evans developed a control strategy that allows trajectory control on a mobile robot that uses conventional non-omnidirectional wheels and thus requires the drive wheels to rotate with respect to the vertical axis (Borenstein & Evans, 1997). With regard to omnidirectional mobile robots based on omnidirectional wheels there are several works that deserve attention. Ashmore and Barnes presented a detailed analysis of the kinematics of this kind of mobile robots and show that under certain circumstances a curved path may be faster than the straight line between two points (Ashmore & Barnes, 2002). Balkcom et al. characterized time-optimal trajectories for omnidirectional mobile robots (Balkcom et al., 2006a;b). Kalmár-Nagy et al. developed a control method to generate minimum-time trajectories for omnidirectional mobile robots (Kalmár-Nagy et al., 2004; 2002), then Purwin and D'Andrea presented the results of applying this method to a RoboCup F180 omnidirectional mobile robot (Purwin & D'Andrea, 2005). Section 2 deals with the first necessary step towards applying LQR control for trajectory execution, which is formulating a state-space model for the dynamics of the omnidirectional

This state-space model is non linear with respect to the control due to the fact that the robot rotates around the vertical axis and the pan angle is one of the state variables. In Sec. 3 of this chapter we show how to overcome this problem to successfully apply a Linear Quadratic Regulator for the case of three, four and five-wheeled omnidirectional mobile robots. In Sec. 4 we present a method to generate a segment-wise optimal path by solving an LQR control problem for each segment between the initial state through a sequence of target regions that ends at the desired final state of the omnidirectional mobile robot. Finally, Sec. 5 presents the results of several simulation experiments that apply the methods described in this chapter.

translational and rotational movement to the mobile robot.

Fig. 2. Omnidirectional drive

mobile robot (Lupián & Rabadán-Martin, 2009).

The analysis presented pertains to a specific class of omnidirectional mobile robots that are of cylindrical shape with *n* omnidirectional wheels distributed around the periphery of the body of the robot, with the axes of the wheels intersecting at the geometrical vertical axis of the robot. Figure 3 shows an instance of this class of omnidirectional mobile robots for the case of *n* = 5 omnidirectional wheels. It is not necessary to have a uniform distribution of the wheels around the periphery of the robot. That is, the angle that separates the axis of one wheel to the next does not need to be the same. However, there is an obvious restriction that must be met in order to maintain stability, which is that the projection of the center of mass of the robot onto the ground must be contained within the convex hull of the set of contact points of the *n* wheels with the ground. For simplicity of the analysis, the mass of the robot is assumed to be distributed uniformly, so the center of mass is contained within the geometrical vertical axis of the robot.

Fig. 3. Omnidirectional robot

In the literature, there are several proposals for the dynamic model of an omnidirectional mobile robot. One of the main problems with these models is that they do not provide a complete state-space representation, so it is not possible to perform state-space control by using one of them. Most of these proposals are based on the force coupling matrix (Gloye & Rojas, 2006), which provides a direct relationship between the torques applied by the driving motors and the accelerations in the *x*, *y* and angular directions.

In compact form, these equations may be written as follows

$$\begin{aligned} a &= \frac{1}{M}(F\_1 + F\_2 + \dots + F\_n) \\ \dot{\omega} &= \frac{R}{I}(f\_1 + f\_2 + \dots + f\_n) \end{aligned} \tag{1}$$

where *a* is the acceleration of the robot with respect to the inertial reference, *ω*˙ is the angular acceleration, *n* is the number of omnidirectional wheels around the periphery of the robot, *Fi* is the vector force applied by motor *i*, *fi* is the signed scalar value of *Fi* (positive for counter clock-wise rotation), *M* is the mass of the robot, *R* is its radius and *I* is its moment of inertia.

the angular velocity. Those are the six state variables we are interested in controlling, so it

LQR Control Methods for Trajectory Execution in Omnidirectional Mobile Robots 389

However, for simulation purposes we introduce variables *μi*, which correspond to the angular positions of each of the omnidirectional wheels of the robot. The decision to include these state variables was initially motivated by the fact that the dynamic response of the robot would be simulated on a virtual environment developed in OpenGL, and the motion of the wheels would make this simulation more realistic. On the physical robot, however, these state-variables will have a more important role because, unlike the other six state variables, the angular positions of the wheels can be measured directly through motor encoders, so one

In order to express the state equations in a compact and clear form, the state vector will be

*x y β*

*x*˙ *y*˙ *β*˙

*μ*<sup>1</sup> ... *μ<sup>n</sup>*

*z* = � *z*1 *z*2 �

*A*<sup>11</sup> *A*<sup>12</sup> *<sup>A</sup>*<sup>21</sup> *<sup>A</sup>*22� �*z*<sup>1</sup>

*<sup>u</sup>* <sup>=</sup> <sup>1</sup> *M*

*A*<sup>11</sup> =

where *u* is the control vector composed of the scalar forces *fi* divided by the mass of the robot

and the matrices are defined as follows. Matrix *A*<sup>11</sup> simply expresses the relationships among

�

⎡ ⎢ ⎢ ⎢ ⎣ *f*1 *f*2 . . . *fn*

03×<sup>3</sup> *I*3×<sup>3</sup> 03×<sup>3</sup> 03×<sup>3</sup>

0 0 ··· 0 0 0 ··· 0 0 0 ··· 0 − sin *θ*<sup>1</sup> − sin *θ*<sup>2</sup> ··· − sin *θ<sup>n</sup>* cos *<sup>θ</sup>*<sup>1</sup> cos *<sup>θ</sup>*<sup>2</sup> ··· cos *<sup>θ</sup><sup>n</sup> MR*

*MR*

�

*<sup>I</sup>* ··· *MR*

*I*

⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

⎤ ⎥ ⎥ ⎥

*x y β x*˙ *y*˙ *β*˙

�T

�T

�T

*z*2 � + � *B*1 *B*2 �

�T

(5)

(6)

(9)

(10)

*u* (7)

<sup>⎦</sup> , (8)

would suffice to use them to describe the state of the robot.

can implement a state observer to estimate the remaining variables.

*z*<sup>1</sup> = �

*z*<sup>11</sup> = �

*z*<sup>12</sup> = �

*z*<sup>2</sup> = �

In terms of *z*<sup>1</sup> y *z*<sup>2</sup> the state vector *z* can be expressed as

� *z*˙1 *z*˙2 � = �

and the state-space model becomes

the first six state variables, and is given by

Matrix *B*<sup>1</sup> is obtained from Eq. 3 and becomes

*B*<sup>1</sup> =

⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣

*I*

partitioned as follows:

Taking into account the driving motor distribution shown in Fig. 4, Eq. 1 may be re-written as

$$\begin{aligned} Ma\_x &= -f\_1 \sin \theta\_1 - f\_2 \sin \theta\_2 - \dots - f\_n \sin \theta\_n \\ Ma\_y &= f\_1 \cos \theta\_1 + f\_2 \cos \theta\_2 + \dots + f\_n \cos \theta\_n \\ MR\dot{\omega} &= \frac{1}{a} (f\_1 + f\_2 + \dots + f\_n) \end{aligned} \tag{2}$$

where *x* and *y* are the axes of the inertial frame of reference, *ax* is the *x* component of the acceleration, *ay* is the *y* component of the acceleration, *α* is such that *I* = *αMR*2, and *θ<sup>i</sup>* is the angular position of driving motor *i* with respect to the robot's referential frame as shown in Fig. 4. Equation 2 is expressed in matrix form as

$$
\begin{bmatrix} a\_x \\ a\_y \\ R\dot{\omega} \end{bmatrix} = \frac{1}{M} \begin{bmatrix} -\sin\theta\_1 & -\sin\theta\_2 & \cdots & -\sin\theta\_n \\ \cos\theta\_1 & \cos\theta\_2 & \cdots & \cos\theta\_n \\ \frac{MR}{I} & \frac{MR}{I} & \cdots & \frac{MR}{I} \end{bmatrix} \begin{bmatrix} f\_1 \\ f\_2 \\ \vdots \\ f\_n \end{bmatrix} \tag{3}
$$

Equation 3 gives a general idea about the dynamic behavior of the mobile robot. However, it can not be considered a complete state-space model since it does not deal with the inertial effects of the mass of the robot, so we define a state vector that is complete enough to describe the dynamic behavior of the mobile robot (Lupián & Rabadán-Martin, 2009). We define the state vector *z* as

$$z = \begin{bmatrix} x & y & \beta & \dot{x} & \dot{y} & \dot{\beta} & \mu\_1 & \cdots & \mu\_n \end{bmatrix}^T \tag{4}$$

where (*x*, *y*) is the position of the robot with respect to the inertial reference, (*x*˙, *y*˙) is the vector velocity of the robot with respect to the field plane, *β* is the angular position and *β*˙ is the angular velocity. Those are the six state variables we are interested in controlling, so it would suffice to use them to describe the state of the robot.

However, for simulation purposes we introduce variables *μi*, which correspond to the angular positions of each of the omnidirectional wheels of the robot. The decision to include these state variables was initially motivated by the fact that the dynamic response of the robot would be simulated on a virtual environment developed in OpenGL, and the motion of the wheels would make this simulation more realistic. On the physical robot, however, these state-variables will have a more important role because, unlike the other six state variables, the angular positions of the wheels can be measured directly through motor encoders, so one can implement a state observer to estimate the remaining variables.

In order to express the state equations in a compact and clear form, the state vector will be partitioned as follows:

$$\begin{aligned} z\_1 &= \begin{bmatrix} x & y & \beta & \dot{x} & \dot{y} & \dot{\beta} \end{bmatrix}^\mathrm{T} \\ z\_{11} &= \begin{bmatrix} x & y & \beta \end{bmatrix}^\mathrm{T} \\ z\_{12} &= \begin{bmatrix} \dot{x} & \dot{y} & \dot{\beta} \end{bmatrix}^\mathrm{T} \\ z\_{2} &= \begin{bmatrix} \mu\_{1} & \dots & \mu\_{n} \end{bmatrix}^\mathrm{T} \end{aligned} \tag{5}$$

In terms of *z*<sup>1</sup> y *z*<sup>2</sup> the state vector *z* can be expressed as

$$z = \begin{bmatrix} z\_1 \\ z\_2 \end{bmatrix} \tag{6}$$

and the state-space model becomes

4 Will-be-set-by-IN-TECH

Taking into account the driving motor distribution shown in Fig. 4, Eq. 1 may be re-written as

*Max* = −*f*<sup>1</sup> sin *θ*<sup>1</sup> − *f*<sup>2</sup> sin *θ*<sup>2</sup> −···− *fn* sin *θ<sup>n</sup> May* = *f*<sup>1</sup> cos *θ*<sup>1</sup> + *f*<sup>2</sup> cos *θ*<sup>2</sup> + ··· + *fn* cos *θ<sup>n</sup>*

where *x* and *y* are the axes of the inertial frame of reference, *ax* is the *x* component of the acceleration, *ay* is the *y* component of the acceleration, *α* is such that *I* = *αMR*2, and *θ<sup>i</sup>* is the angular position of driving motor *i* with respect to the robot's referential frame as shown in

> − sin *θ*<sup>1</sup> − sin *θ*<sup>2</sup> ··· − sin *θ<sup>n</sup>* cos *<sup>θ</sup>*<sup>1</sup> cos *<sup>θ</sup>*<sup>2</sup> ··· cos *<sup>θ</sup><sup>n</sup> MR*

*<sup>I</sup>* ··· *MR*

*I*

⎤ ⎦ ⎡ ⎢ ⎢ ⎢ ⎣

*f*1 *f*2 . . . *fn* ⎤ ⎥ ⎥ ⎥ ⎦

�<sup>T</sup> (4)

*MR*

Equation 3 gives a general idea about the dynamic behavior of the mobile robot. However, it can not be considered a complete state-space model since it does not deal with the inertial effects of the mass of the robot, so we define a state vector that is complete enough to describe the dynamic behavior of the mobile robot (Lupián & Rabadán-Martin, 2009). We define the

*x y <sup>β</sup> <sup>x</sup>*˙ *<sup>y</sup>*˙ *β μ* ˙ <sup>1</sup> ··· *<sup>μ</sup><sup>n</sup>*

where (*x*, *y*) is the position of the robot with respect to the inertial reference, (*x*˙, *y*˙) is the vector velocity of the robot with respect to the field plane, *β* is the angular position and *β*˙ is

(2)

(3)

*<sup>α</sup>* (*f*<sup>1</sup> <sup>+</sup> *<sup>f</sup>*<sup>2</sup> <sup>+</sup> ··· <sup>+</sup> *fn*)

*MRω*˙ <sup>=</sup> <sup>1</sup>

Fig. 4. Equation 2 is expressed in matrix form as

⎤ <sup>⎦</sup> <sup>=</sup> <sup>1</sup> *M* ⎡ ⎣

*I*

⎡ ⎣ *ax ay Rω*˙

Fig. 4. Driving motor distribution

*z* = �

state vector *z* as

$$
\begin{bmatrix} \dot{z}\_1\\ \dot{z}\_2 \end{bmatrix} = \begin{bmatrix} A\_{11} & A\_{12} \\ A\_{21} & A\_{22} \end{bmatrix} \begin{bmatrix} z\_1\\ z\_2 \end{bmatrix} + \begin{bmatrix} B\_1\\ B\_2 \end{bmatrix} u \tag{7}
$$

where *u* is the control vector composed of the scalar forces *fi* divided by the mass of the robot

$$u = \frac{1}{M} \begin{bmatrix} f\_1 \\ f\_2 \\ \vdots \\ f\_n \end{bmatrix},\tag{8}$$

and the matrices are defined as follows. Matrix *A*<sup>11</sup> simply expresses the relationships among the first six state variables, and is given by

$$A\_{11} = \begin{bmatrix} \mathbf{0}\_{3 \times 3} & I\_{3 \times 3} \\ \mathbf{0}\_{3 \times 3} & \mathbf{0}\_{3 \times 3} \end{bmatrix} \tag{9}$$

Matrix *B*<sup>1</sup> is obtained from Eq. 3 and becomes

$$B\_1 = \begin{bmatrix} 0 & 0 & \cdots & 0 \\ 0 & 0 & \cdots & 0 \\ 0 & 0 & \cdots & 0 \\ -\sin\theta\_1 & -\sin\theta\_2 & \cdots & -\sin\theta\_n \\ \cos\theta\_1 & \cos\theta\_2 & \cdots & \cos\theta\_n \\ \frac{MR}{I} & \frac{MR}{I} & \cdots & \frac{MR}{I} \end{bmatrix} \tag{10}$$

Fig. 5. Change of variable in control vector

translates to *<sup>n</sup>*

**3.1 Transformation matrix for** *n* = 3

Fig. 6. Omnidirectional drive for *n* = 3

transformation.

Moreover, in order to ensure that the angular acceleration will remain unchanged it is necessary that the resulting scalar force *f* = *f*<sup>1</sup> + *f*<sup>2</sup> + ... + *fn* is the same in both cases, which

LQR Control Methods for Trajectory Execution in Omnidirectional Mobile Robots 391

*n* ∑ *i*=1

*ui* (15)

*u*ˇ*<sup>i</sup>* =

What we need from Eqs. 14 and 15 is a one-to-one mapping that can take us from *u* to *u*ˇ back and forth. Since *u* is *n*-dimensional and we have three equations the system is under-determined whenever *n* > 3. Although the pseudo-inverse would provide a linear transformation from one domain to the other, this transformation would be rank-deficient and thus would not be one-to-one. Our solution requires then to add *n* − 3 *complementary equations*. In order to avoid unnecessary numerical problems, these equations should be chosen so that the linear transformation is well-conditioned for any value of the angle *β*.

The simplest case comes when the number of omnidirectional wheels of the robot is *n* = 3, see Fig. 6. For this particular case the number of equations provided by Eqs. 14 and 15 is equal to the number of control variables, so there is no need for additional equations to complete the

∑ *i*=1

Matrix *B*<sup>1</sup> expresses the correct influence of each force over the respective acceleration only for the case in which the angular position *β* of the robot is zero. However, in order to take into account the fact that the robot will rotate as time goes by this matrix should also depend on *β* as follows

$$B\_1(\boldsymbol{\beta}) = \begin{bmatrix} 0 & 0 & \cdots & 0 \\ 0 & 0 & \cdots & 0 \\ 0 & 0 & \cdots & 0 \\ -\sin\left(\theta\_1 + \beta\right) & -\sin\left(\theta\_2 + \beta\right) & \cdots & -\sin\left(\theta\_n + \beta\right) \\ \cos\left(\theta\_1 + \beta\right) & \cos\left(\theta\_2 + \beta\right) & \cdots & \cos\left(\theta\_n + \beta\right) \\ \frac{MR}{I} & \frac{MR}{I} & \cdots & \frac{MR}{I} \end{bmatrix} \tag{11}$$
  $B\_1(\boldsymbol{\beta}) = \begin{bmatrix} 0\_{3 \times n} \\ B\_{12}(\boldsymbol{\beta}) \end{bmatrix}$ 

*A*<sup>12</sup> and *A*<sup>22</sup> are zero matrices of size 6 × *n* and *n* × *n* respectively. Matrix *A*<sup>21</sup> expresses the angular motion of the wheels with respect to the motion of the mobile robot and, like *B*1, it is dependent on the angular position *β* of the robot:

$$\begin{aligned} A\_{21}(\boldsymbol{\beta}) &= \frac{1}{r} \begin{bmatrix} 0\_{\boldsymbol{n}\times 3} & A\_{212}(\boldsymbol{\beta}) \end{bmatrix} \\ A\_{212}(\boldsymbol{\beta}) &= \frac{1}{r} \begin{bmatrix} \sin\left(\theta\_1 + \beta\right) & -\cos\left(\theta\_1 + \beta\right) & -R \\ \sin\left(\theta\_2 + \beta\right) & -\cos\left(\theta\_2 + \beta\right) & -R \\ \vdots & \vdots & \vdots \\ \sin\left(\theta\_n + \beta\right) & -\cos\left(\theta\_n + \beta\right) & -R \end{bmatrix} \end{aligned} \tag{12}$$

where *r* is the radius of the omnidirectional wheels.

Taking into account that both *A*<sup>21</sup> and *B*<sup>1</sup> depend on *β* the state model in Eq. 7 may be reformulated as

$$
\begin{bmatrix} \dot{z}\_1\\ \dot{z}\_2 \end{bmatrix} = \begin{bmatrix} A\_{11} & 0\_{6 \times n} \\ A\_{21}(\beta) & 0\_{n \times n} \end{bmatrix} \begin{bmatrix} z\_1\\ z\_2 \end{bmatrix} + \begin{bmatrix} B\_1(\beta) \\ 0\_{n \times n} \end{bmatrix} u \tag{13}
$$

#### **3. Global linearization of the state-space dynamic model**

Since the angular position *β* is one of the state variables this implies that the model in Eq. 13 is non-linear, and that represents an important difficulty for the purpose of controlling the robot. This is why it became necessary to find a way to linearize the model.

The solution to this problem required a shift in perspective in relation to the model. The only reason why the angle *β* has a non-linear effect on the dynamics of state variables in *z*<sup>1</sup> is because as the robot rotates also the driving forces, which are the control variables, rotate.

Let *F* = *F*<sup>1</sup> + *F*<sup>2</sup> + ... + *Fn* be the resulting vector force applied by the omnidirectional wheels to the robot when it is at the angular position *β* and the control vector is *u*. Let *u*ˇ be the control vector that will produce the same resulting vector force *F* when the robot is at the angular position *β* = 0. This idea is explained by Fig. 5.

The control vectors *u* and *u*ˇ are then related by

$$\begin{aligned} \left[\sin\left(\theta\_{1}\right) \quad \cdots \quad \sin\left(\theta\_{n}\right)\right] \check{u} &= \left[\sin\left(\theta\_{1} + \beta\right) \quad \cdots \quad \sin\left(\theta\_{n} + \beta\right)\right] u \\ \left[\cos\left(\theta\_{1}\right) \quad \cdots \quad \cos\left(\theta\_{n}\right)\right] \check{u} &= \left[\cos\left(\theta\_{1} + \beta\right) \quad \cdots \quad \cos\left(\theta\_{n} + \beta\right)\right] u \end{aligned} \tag{14}$$

Fig. 5. Change of variable in control vector

6 Will-be-set-by-IN-TECH

Matrix *B*<sup>1</sup> expresses the correct influence of each force over the respective acceleration only for the case in which the angular position *β* of the robot is zero. However, in order to take into account the fact that the robot will rotate as time goes by this matrix should also depend on *β*

> 0 0 ··· 0 0 0 ··· 0 0 0 ··· 0 − sin (*θ*<sup>1</sup> + *β*) − sin (*θ*<sup>2</sup> + *β*) ··· − sin (*θ<sup>n</sup>* + *β*) cos (*θ*<sup>1</sup> + *β*) cos (*θ*<sup>2</sup> + *β*) ··· cos (*θ<sup>n</sup>* + *β*)

*<sup>I</sup>* ··· *MR*

*I*

⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

(11)

(12)

*MR*

*A*<sup>12</sup> and *A*<sup>22</sup> are zero matrices of size 6 × *n* and *n* × *n* respectively. Matrix *A*<sup>21</sup> expresses the angular motion of the wheels with respect to the motion of the mobile robot and, like *B*1, it is

�

. .

Taking into account that both *A*<sup>21</sup> and *B*<sup>1</sup> depend on *β* the state model in Eq. 7 may be

Since the angular position *β* is one of the state variables this implies that the model in Eq. 13 is non-linear, and that represents an important difficulty for the purpose of controlling the robot.

The solution to this problem required a shift in perspective in relation to the model. The only reason why the angle *β* has a non-linear effect on the dynamics of state variables in *z*<sup>1</sup> is because as the robot rotates also the driving forces, which are the control variables, rotate. Let *F* = *F*<sup>1</sup> + *F*<sup>2</sup> + ... + *Fn* be the resulting vector force applied by the omnidirectional wheels to the robot when it is at the angular position *β* and the control vector is *u*. Let *u*ˇ be the control vector that will produce the same resulting vector force *F* when the robot is at the angular

sin (*θ*<sup>1</sup> + *β*) − cos (*θ*<sup>1</sup> + *β*) −*R* sin (*θ*<sup>2</sup> + *β*) − cos (*θ*<sup>2</sup> + *β*) −*R*

sin (*θ<sup>n</sup>* + *β*) − cos (*θ<sup>n</sup>* + *β*) −*R*

� �*z*<sup>1</sup> *z*2 � + � *B*1(*β*) 0*n*×*<sup>n</sup>*

.

. .

�

sin (*θ*<sup>1</sup> + *β*) ··· sin (*θ<sup>n</sup>* + *β*)

cos (*θ*<sup>1</sup> + *β*) ··· cos (*θ<sup>n</sup>* + *β*)

. . ⎤ ⎥ ⎥ ⎥ ⎦

*u* (13)

� *u*

> � *u*

(14)

0*n*×<sup>3</sup> *A*212(*β*)

. .

� *<sup>A</sup>*<sup>11</sup> <sup>06</sup>×*<sup>n</sup> A*21(*β*) 0*n*×*<sup>n</sup>*

as follows

reformulated as

*B*1(*β*) =

*B*1(*β*) =

⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣

*MR I*

�

*r* �

> ⎡ ⎢ ⎢ ⎢ ⎣

*r*

**3. Global linearization of the state-space dynamic model**

This is why it became necessary to find a way to linearize the model.

� *u*ˇ = �

� *u*ˇ = �

� <sup>03</sup>×*<sup>n</sup> B*12(*β*)

dependent on the angular position *β* of the robot:

*<sup>A</sup>*21(*β*) = <sup>1</sup>

*<sup>A</sup>*212(*β*) = <sup>1</sup>

where *r* is the radius of the omnidirectional wheels.

� *z*˙1 *z*˙2 � =

position *β* = 0. This idea is explained by Fig. 5. The control vectors *u* and *u*ˇ are then related by

sin (*θ*1) ··· sin (*θn*)

cos (*θ*1) ··· cos (*θn*)

�

�

Moreover, in order to ensure that the angular acceleration will remain unchanged it is necessary that the resulting scalar force *f* = *f*<sup>1</sup> + *f*<sup>2</sup> + ... + *fn* is the same in both cases, which translates to *<sup>n</sup>*

$$\sum\_{i=1}^{n} \check{u}\_{i} = \sum\_{i=1}^{n} u\_{i} \tag{15}$$

What we need from Eqs. 14 and 15 is a one-to-one mapping that can take us from *u* to *u*ˇ back and forth. Since *u* is *n*-dimensional and we have three equations the system is under-determined whenever *n* > 3. Although the pseudo-inverse would provide a linear transformation from one domain to the other, this transformation would be rank-deficient and thus would not be one-to-one. Our solution requires then to add *n* − 3 *complementary equations*. In order to avoid unnecessary numerical problems, these equations should be chosen so that the linear transformation is well-conditioned for any value of the angle *β*.

#### **3.1 Transformation matrix for** *n* = 3

The simplest case comes when the number of omnidirectional wheels of the robot is *n* = 3, see Fig. 6. For this particular case the number of equations provided by Eqs. 14 and 15 is equal to the number of control variables, so there is no need for additional equations to complete the transformation.

Fig. 6. Omnidirectional drive for *n* = 3

We can then define the transformation matrix Ω4(*β*) according to

sin (*θ*1) sin (*θ*2) sin (*θ*3) sin (*θ*4) cos (*θ*1) cos (*θ*2) cos (*θ*3) cos (*θ*4) 1111 1 −1 1 −1

> ⎡ ⎢ ⎢ ⎣

1 −101 −1

<sup>1</sup> <sup>−</sup>1 0 <sup>−</sup>1 1�

⎤ ⎥ ⎥ ⎦

LQR Control Methods for Trajectory Execution in Omnidirectional Mobile Robots 393

For the particular case of *n* = 5, shown in Fig. 8, an adequate choice for the complementary

*u*ˇ = �

� *u*ˇ = � −1

sin (*θ*<sup>1</sup> + *β*) sin (*θ*<sup>2</sup> + *β*) sin (*θ*<sup>3</sup> + *β*) sin (*θ*<sup>4</sup> + *β*) cos (*θ*<sup>1</sup> + *β*) cos (*θ*<sup>2</sup> + *β*) cos (*θ*<sup>3</sup> + *β*) cos (*θ*<sup>4</sup> + *β*) 1111 1 −1 1 −1

1 −101 −1

<sup>1</sup> <sup>−</sup>1 0 <sup>−</sup>1 1�

⎤ ⎥ ⎥ ⎥ ⎥ ⎦ *u*ˇ =

sin (*θ*<sup>1</sup> + *β*) sin (*θ*<sup>2</sup> + *β*) sin (*θ*<sup>3</sup> + *β*) sin (*θ*<sup>4</sup> + *β*) sin (*θ*<sup>5</sup> + *β*) cos (*θ*<sup>1</sup> + *β*) cos (*θ*<sup>2</sup> + *β*) cos (*θ*<sup>3</sup> + *β*) cos (*θ*<sup>4</sup> + *β*) cos (*θ*<sup>5</sup> + *β*) 11111 1 −10 1 −1 1 −1 0 −1 1

� *u*

*u*

⎤ ⎥ ⎥ ⎦

(20)

(21)

⎤ ⎥ ⎥ ⎥ ⎥ ⎦

*u* (22)

Ω4(*β*) =

⎡ ⎢ ⎢ ⎣

**3.3 Transformation matrix for** *n* = 5

�

�

Fig. 8. Omnidirectional drive for *n* = 5

⎡ ⎢ ⎢ ⎢ ⎢ ⎣

⎡ ⎢ ⎢ ⎢ ⎢ ⎣

Equations 14, 15 and 21 can be put in matrix form as follows

sin (*θ*1) sin (*θ*2) sin (*θ*3) sin (*θ*4) sin (*θ*5) cos (*θ*1) cos (*θ*2) cos (*θ*3) cos (*θ*4) cos (*θ*5) 11111 1 −10 1 −1 1 −1 0 −1 1

equations is shown in Eqs. 21.

Equations 14 and 15 can be written in matrix form as follows

$$\begin{bmatrix} \sin\left(\theta\_1\right) & \sin\left(\theta\_2\right) & \sin\left(\theta\_3\right) \\ \cos\left(\theta\_1\right) & \cos\left(\theta\_2\right) & \cos\left(\theta\_3\right) \\ 1 & 1 & 1 \end{bmatrix} \text{if} \quad = \begin{bmatrix} \sin\left(\theta\_1 + \beta\right) & \sin\left(\theta\_2 + \beta\right) & \sin\left(\theta\_3 + \beta\right) \\ \cos\left(\theta\_1 + \beta\right) & \cos\left(\theta\_2 + \beta\right) & \cos\left(\theta\_3 + \beta\right) \\ 1 & 1 & 1 \end{bmatrix} u \tag{16}$$

We can then define the transformation matrix Ω3(*β*) according to

$$\Omega\_3(\boldsymbol{\beta}) = \begin{bmatrix} \sin\left(\theta\_1\right) & \sin\left(\theta\_2\right) & \sin\left(\theta\_3\right) \\ \cos\left(\theta\_1\right) & \cos\left(\theta\_2\right) & \cos\left(\theta\_3\right) \\ 1 & 1 & 1 \end{bmatrix}^{-1} \begin{bmatrix} \sin\left(\theta\_1 + \boldsymbol{\beta}\right) & \sin\left(\theta\_2 + \boldsymbol{\beta}\right) & \sin\left(\theta\_3 + \boldsymbol{\beta}\right) \\ \cos\left(\theta\_1 + \boldsymbol{\beta}\right) & \cos\left(\theta\_2 + \boldsymbol{\beta}\right) & \cos\left(\theta\_3 + \boldsymbol{\beta}\right) \\ 1 & 1 & 1 \end{bmatrix} \tag{17}$$

#### **3.2 Transformation matrix for** *n* = 4

For the particular case of *n* = 4 (Fig. 7), it is easy to see that Eq. 18 satisfies the requirement of completing a well-conditioned 4 × 4 transformation since it is orthogonal to two of the other equations and still sufficiently linearly independent from the third one.

$$
\begin{bmatrix} 1 & -1 & 1 & -1 \end{bmatrix} \check{u} = \begin{bmatrix} 1 & -1 & 1 & -1 \end{bmatrix} u \tag{18}
$$

Fig. 7. Omnidirectional drive for *n* = 4

Equations 14, 15 and 18 can be put in matrix form as follows

$$
\begin{bmatrix}
\sin\left(\theta\_1\right) & \sin\left(\theta\_2\right) & \sin\left(\theta\_3\right) & \sin\left(\theta\_4\right) \\
\cos\left(\theta\_1\right) & \cos\left(\theta\_2\right) & \cos\left(\theta\_3\right) & \cos\left(\theta\_4\right) \\
1 & 1 & 1 & 1 \\
1 & -1 & 1 & -1
\end{bmatrix}
\vec{u} = 
$$

$$
\begin{bmatrix}
\sin\left(\theta\_1 + \beta\right) & \sin\left(\theta\_2 + \beta\right) & \sin\left(\theta\_3 + \beta\right) & \sin\left(\theta\_4 + \beta\right) \\
\cos\left(\theta\_1 + \beta\right) & \cos\left(\theta\_2 + \beta\right) & \cos\left(\theta\_3 + \beta\right) & \cos\left(\theta\_4 + \beta\right) \\
1 & 1 & 1 & 1 & 1 \\
1 & -1 & 1 & -1
\end{bmatrix} u \quad \text{(19)}
$$

We can then define the transformation matrix Ω4(*β*) according to

$$\Omega\_{4}(\beta) = \begin{bmatrix} \sin(\theta\_{1}) & \sin(\theta\_{2}) & \sin(\theta\_{3}) & \sin(\theta\_{4}) \\ \cos(\theta\_{1}) & \cos(\theta\_{2}) & \cos(\theta\_{3}) & \cos(\theta\_{4}) \\ 1 & 1 & 1 & 1 \\ 1 & -1 & 1 & -1 \end{bmatrix}^{-1}$$

$$\begin{bmatrix} \sin(\theta\_{1} + \beta) & \sin(\theta\_{2} + \beta) & \sin(\theta\_{3} + \beta) & \sin(\theta\_{4} + \beta) \\ \cos(\theta\_{1} + \beta) & \cos(\theta\_{2} + \beta) & \cos(\theta\_{3} + \beta) & \cos(\theta\_{4} + \beta) \\ 1 & 1 & 1 & 1 & 1 \\ 1 & -1 & 1 & -1 \end{bmatrix} \tag{20}$$

#### **3.3 Transformation matrix for** *n* = 5

8 Will-be-set-by-IN-TECH

sin (*θ*<sup>1</sup> + *β*) sin (*θ*<sup>2</sup> + *β*) sin (*θ*<sup>3</sup> + *β*) cos (*θ*<sup>1</sup> + *β*) cos (*θ*<sup>2</sup> + *β*) cos (*θ*<sup>3</sup> + *β*) 111

sin (*θ*<sup>1</sup> + *β*) sin (*θ*<sup>2</sup> + *β*) sin (*θ*<sup>3</sup> + *β*) cos (*θ*<sup>1</sup> + *β*) cos (*θ*<sup>2</sup> + *β*) cos (*θ*<sup>3</sup> + *β*) 111

�

⎤ ⎥ ⎦

*u* (18)

⎤ ⎥ ⎥ ⎥ ⎥ ⎦

*u* (19)

*u* (16)

⎤ ⎥ <sup>⎦</sup> (17)

⎡ ⎢ ⎣

⎤ ⎥ ⎦ <sup>−</sup><sup>1</sup> ⎡ ⎢ ⎣

For the particular case of *n* = 4 (Fig. 7), it is easy to see that Eq. 18 satisfies the requirement of completing a well-conditioned 4 × 4 transformation since it is orthogonal to two of the other

1 −1 1 −1

sin (*θ*<sup>1</sup> + *β*) sin (*θ*<sup>2</sup> + *β*) sin (*θ*<sup>3</sup> + *β*) sin (*θ*<sup>4</sup> + *β*) cos (*θ*<sup>1</sup> + *β*) cos (*θ*<sup>2</sup> + *β*) cos (*θ*<sup>3</sup> + *β*) cos (*θ*<sup>4</sup> + *β*) 1111 1 −1 1 −1

� *u*ˇ = �

⎤ ⎥ ⎥ ⎥ ⎥ ⎦ *u*ˇ =

Equations 14 and 15 can be written in matrix form as follows

⎤ ⎥ ⎦ *u*ˇ =

We can then define the transformation matrix Ω3(*β*) according to

equations and still sufficiently linearly independent from the third one.

1 −1 1 −1

sin (*θ*1) sin (*θ*2) sin (*θ*3) cos (*θ*1) cos (*θ*2) cos (*θ*3) 111

�

sin (*θ*1) sin (*θ*2) sin (*θ*3) cos (*θ*1) cos (*θ*2) cos (*θ*3) 111

**3.2 Transformation matrix for** *n* = 4

Fig. 7. Omnidirectional drive for *n* = 4

sin (*θ*1) sin (*θ*2) sin (*θ*3) sin (*θ*4) cos (*θ*1) cos (*θ*2) cos (*θ*3) cos (*θ*4) 1111 1 −1 1 −1

⎡ ⎢ ⎢ ⎢ ⎢ ⎣

Equations 14, 15 and 18 can be put in matrix form as follows

⎡ ⎢ ⎢ ⎢ ⎢ ⎣

⎡ ⎢ ⎣

Ω3(*β*) =

⎡ ⎢ ⎣

> For the particular case of *n* = 5, shown in Fig. 8, an adequate choice for the complementary equations is shown in Eqs. 21.

$$
\begin{bmatrix} 1 & -1 & 0 & 1 & -1 \end{bmatrix} \check{u} = \begin{bmatrix} 1 & -1 & 0 & 1 & -1 \end{bmatrix} u
$$

$$
\begin{bmatrix} 1 & -1 & 0 & -1 & 1 \end{bmatrix} \check{u} = \begin{bmatrix} 1 & -1 & 0 & -1 & 1 \end{bmatrix} u
\tag{21}
$$

Fig. 8. Omnidirectional drive for *n* = 5

Equations 14, 15 and 21 can be put in matrix form as follows

$$
\begin{bmatrix}
\sin(\theta\_1) & \sin(\theta\_2) & \sin(\theta\_3) & \sin(\theta\_4) & \sin(\theta\_5) \\
\cos(\theta\_1) & \cos(\theta\_2) & \cos(\theta\_3) & \cos(\theta\_4) & \cos(\theta\_5) \\
1 & 1 & 1 & 1 & 1 \\
1 & -1 & 0 & 1 & -1 \\
1 & -1 & 0 & -1 & 1
\end{bmatrix} \check{u} =$$

$$
\begin{bmatrix}
\sin(\theta\_1 + \beta) & \sin(\theta\_2 + \beta) & \sin(\theta\_3 + \beta) & \sin(\theta\_4 + \beta) & \sin(\theta\_5 + \beta) \\
\cos(\theta\_1 + \beta) & \cos(\theta\_2 + \beta) & \cos(\theta\_3 + \beta) & \cos(\theta\_4 + \beta) & \cos(\theta\_5 + \beta) \\
1 & 1 & 1 & 1 & 1 & 1 \\
1 & -1 & 0 & 1 & -1 \\
1 & -1 & 0 & -1 & 1
\end{bmatrix} u \quad (22)$$

**4. Segment-wise optimal trajectory control**

case of soccer).

Fig. 9. Sequence of target regions

specified trajectory.

Our solution to the trajectory execution control problem requires the specification of the desired trajectory as a sequence of target regions as shown in Fig. 9. Each of the target regions is depicted as a red circle. This figure shows the robot in its initial state near the top left corner. The robot will have to visit each of the target regions in sequence until it reaches the final target which contains the red ball near the bottom right corner. As soon as the center of the robot enters the current target region it will start moving towards the next one. In this way, the trajectory is segmented by the target regions. The desired precision of the trajectory is determined by the size of the next target region. A small target requires higher precision than a larger one. Each target region makes additional specifications for the desired final state of the robot as it reaches the target. These specifications include the desired final scalar speed and the desired final heading direction. This last specification is useful for highly dynamic applications, such as playing soccer, since it allows the robot to rotate as it moves along the trajectory in order to align its manipulating device with the location of the target (a ball in the

LQR Control Methods for Trajectory Execution in Omnidirectional Mobile Robots 395

Each of the segments of the trajectory can be treated as a different control problem in which the final state of the previous segment is the initial state of the current one. Each segment has its own desired final state and in this way the mobile robot is forced to loosely follow the

Once the trajectory has been segmented by this sequence of target regions the problem becomes how to force the robot to move from one target to the next. Our proposal is to solve an infinite-horizon LQR control problem for each of the segments. Although this approach may provide a sub-optimal solution rather than the optimal solution, that would be obtained from the corresponding finite-horizon LQR problem formulation, it has the advantage of requiring the solution of an algebraic Riccati equation rather than the more computationally demanding

We can then define the transformation matrix Ω5(*β*) according to

$$
\Omega\_5(\boldsymbol{\beta}) = \begin{bmatrix}
\sin(\theta\_1) & \sin(\theta\_2) & \sin(\theta\_3) & \sin(\theta\_4) & \sin(\theta\_5) \\
\cos(\theta\_1) & \cos(\theta\_2) & \cos(\theta\_3) & \cos(\theta\_4) & \cos(\theta\_5) \\
1 & 1 & 1 & 1 & 1 \\
1 & -1 & 0 & 1 & -1 \\
1 & -1 & 0 & -1 & 1
\end{bmatrix}^{-1}
$$

$$
\begin{bmatrix}
\sin(\theta\_1 + \beta) & \sin(\theta\_2 + \beta) & \sin(\theta\_3 + \beta) & \sin(\theta\_4 + \beta) & \sin(\theta\_5 + \beta) \\
\cos(\theta\_1 + \beta) & \cos(\theta\_2 + \beta) & \cos(\theta\_3 + \beta) & \cos(\theta\_4 + \beta) & \cos(\theta\_5 + \beta) \\
1 & 1 & 1 & 1 & 1 & 1 \\
1 & -1 & 0 & 1 & -1 \\
1 & -1 & 0 & -1 & 1
\end{bmatrix} \tag{23}
$$

Further generalization is possible by taking into account that the set of *n* − 3 complementary equations must be chosen such that the transformation matrix is full rank and of minimum condition number.

#### **3.4 Global linearization using the transformation matrix** Ω*n*(*β*)

The change of variable from *u* to *u*ˇ for any number of wheels *n* can now be expressed as

$$\begin{aligned} \check{u} &= \Omega\_{\mathbb{R}}(\beta)u \\ \mu &= \Omega\_{\mathbb{R}}(\beta)^{-1}\check{u} \end{aligned} \tag{24}$$

Matrix Ω*n*(*β*) has the property of canceling the non-linear effect of *β* on matrix *B*1(*β*) since *B*1(0) = *B*1(*β*)Ω*n*(*β*)−<sup>1</sup> hence the model can be linearized at least with respect to the state variables in *z*1, which are in fact the only variables we need to control. It is important to note that this is not a local linearization but a global one, so it is equally accurate in all of the control space.

By applying this change of variable to the state-space model of Eq. 13 we obtain the following model

$$
\begin{bmatrix} \dot{z}\_1\\ \dot{z}\_2 \end{bmatrix} = \begin{bmatrix} A\_{11} & 0\_{6 \times n} \\ A\_{21}(\beta) & 0\_{n \times n} \end{bmatrix} \begin{bmatrix} z\_1\\ z\_2 \end{bmatrix} + \begin{bmatrix} B\_1(\beta) \\ 0\_{n \times n} \end{bmatrix} \Omega\_n(\beta)^{-1} \mathbb{1}
$$
 
$$
\begin{bmatrix} \dot{z}\_1\\ \dot{z}\_2 \end{bmatrix} = \begin{bmatrix} A\_{11} & 0\_{6 \times n} \\ A\_{21}(\beta) & 0\_{n \times n} \end{bmatrix} \begin{bmatrix} z\_1\\ z\_2 \end{bmatrix} + \begin{bmatrix} B\_1(\beta)\Omega\_n(\beta)^{-1} \\ 0\_{n \times n} \Omega\_n(\beta)^{-1} \end{bmatrix} \mathbb{1} \tag{25}
$$
 
$$
\begin{bmatrix} \dot{z}\_1\\ \dot{z}\_2 \end{bmatrix} = \begin{bmatrix} A\_{11} & 0\_{6 \times n} \\ A\_{21}(\beta) & 0\_{n \times n} \end{bmatrix} \begin{bmatrix} z\_1\\ z\_2 \end{bmatrix} + \begin{bmatrix} B\_1(0) \\ 0\_{n \times n} \end{bmatrix} \mathbb{1} $$

If we separate this model according to the state vector partition proposed in Eq. 5 it is easy to see that the transformed model is linear with respect to state variables in *z*<sup>1</sup> and control variables *u*ˇ

$$\begin{aligned} \dot{z}\_1 &= A\_{11} z\_1 + B\_1(0) \text{it} \\ \dot{z}\_2 &= A\_{21}(\beta) z\_1 \end{aligned} \tag{26}$$

From the state-space model in Eq. 26 it is possible to formulate a wide variety of linear state-space controllers for state variables in *z*1.

#### **4. Segment-wise optimal trajectory control**

10 Will-be-set-by-IN-TECH

sin (*θ*<sup>1</sup> + *β*) sin (*θ*<sup>2</sup> + *β*) sin (*θ*<sup>3</sup> + *β*) sin (*θ*<sup>4</sup> + *β*) sin (*θ*<sup>5</sup> + *β*) cos (*θ*<sup>1</sup> + *β*) cos (*θ*<sup>2</sup> + *β*) cos (*θ*<sup>3</sup> + *β*) cos (*θ*<sup>4</sup> + *β*) cos (*θ*<sup>5</sup> + *β*) 11111 1 −10 1 −1 1 −1 0 −1 1

Further generalization is possible by taking into account that the set of *n* − 3 complementary equations must be chosen such that the transformation matrix is full rank and of minimum

The change of variable from *u* to *u*ˇ for any number of wheels *n* can now be expressed as

*u*ˇ = Ω*n*(*β*)*u*

Matrix Ω*n*(*β*) has the property of canceling the non-linear effect of *β* on matrix *B*1(*β*) since *B*1(0) = *B*1(*β*)Ω*n*(*β*)−<sup>1</sup> hence the model can be linearized at least with respect to the state variables in *z*1, which are in fact the only variables we need to control. It is important to note that this is not a local linearization but a global one, so it is equally accurate in all of the control

By applying this change of variable to the state-space model of Eq. 13 we obtain the following

� �*z*<sup>1</sup> *z*2 � + � *B*1(*β*) 0*n*×*<sup>n</sup>*

� �*z*<sup>1</sup> *z*2 � + �

� �*z*<sup>1</sup> *z*2 � + � *B*1(0) 0*n*×*<sup>n</sup>*

If we separate this model according to the state vector partition proposed in Eq. 5 it is easy to see that the transformed model is linear with respect to state variables in *z*<sup>1</sup> and control

*z*˙1 = *A*11*z*<sup>1</sup> + *B*1(0)*u*ˇ

From the state-space model in Eq. 26 it is possible to formulate a wide variety of linear

*z*˙2 = *A*21(*β*)*z*<sup>1</sup>

� *<sup>A</sup>*<sup>11</sup> <sup>06</sup>×*<sup>n</sup> A*21(*β*) 0*n*×*<sup>n</sup>*

� *<sup>A</sup>*<sup>11</sup> <sup>06</sup>×*<sup>n</sup> A*21(*β*) 0*n*×*<sup>n</sup>*

� *<sup>A</sup>*<sup>11</sup> <sup>06</sup>×*<sup>n</sup> A*21(*β*) 0*n*×*<sup>n</sup>* ⎤ ⎥ ⎥ ⎥ ⎥ ⎦ −1

*<sup>u</sup>* <sup>=</sup> <sup>Ω</sup>*n*(*β*)−1*u*<sup>ˇ</sup> (24)

�

*B*1(*β*)Ω*n*(*β*)−<sup>1</sup> <sup>0</sup>*n*×*n*Ω*n*(*β*)−<sup>1</sup>

> � *u*ˇ

Ω*n*(*β*)−1*u*ˇ

� *u*ˇ ⎤ ⎥ ⎥ ⎥ ⎥ ⎦

(23)

(25)

(26)

We can then define the transformation matrix Ω5(*β*) according to

**3.4 Global linearization using the transformation matrix** Ω*n*(*β*)

� *z*˙1 *z*˙2 � =

� *z*˙1 *z*˙2 � =

� *z*˙1 *z*˙2 � =

state-space controllers for state variables in *z*1.

sin (*θ*1) sin (*θ*2) sin (*θ*3) sin (*θ*4) sin (*θ*5) cos (*θ*1) cos (*θ*2) cos (*θ*3) cos (*θ*4) cos (*θ*5) 11111 1 −10 1 −1 1 −1 0 −1 1

Ω5(*β*) =

condition number.

space.

model

variables *u*ˇ

⎡ ⎢ ⎢ ⎢ ⎢ ⎣

> ⎡ ⎢ ⎢ ⎢ ⎢ ⎣

Our solution to the trajectory execution control problem requires the specification of the desired trajectory as a sequence of target regions as shown in Fig. 9. Each of the target regions is depicted as a red circle. This figure shows the robot in its initial state near the top left corner. The robot will have to visit each of the target regions in sequence until it reaches the final target which contains the red ball near the bottom right corner. As soon as the center of the robot enters the current target region it will start moving towards the next one. In this way, the trajectory is segmented by the target regions. The desired precision of the trajectory is determined by the size of the next target region. A small target requires higher precision than a larger one. Each target region makes additional specifications for the desired final state of the robot as it reaches the target. These specifications include the desired final scalar speed and the desired final heading direction. This last specification is useful for highly dynamic applications, such as playing soccer, since it allows the robot to rotate as it moves along the trajectory in order to align its manipulating device with the location of the target (a ball in the case of soccer).

Fig. 9. Sequence of target regions

Each of the segments of the trajectory can be treated as a different control problem in which the final state of the previous segment is the initial state of the current one. Each segment has its own desired final state and in this way the mobile robot is forced to loosely follow the specified trajectory.

Once the trajectory has been segmented by this sequence of target regions the problem becomes how to force the robot to move from one target to the next. Our proposal is to solve an infinite-horizon LQR control problem for each of the segments. Although this approach may provide a sub-optimal solution rather than the optimal solution, that would be obtained from the corresponding finite-horizon LQR problem formulation, it has the advantage of requiring the solution of an algebraic Riccati equation rather than the more computationally demanding

poles are implicitly determined by the choice of matrices *Q* and *R*, rather than being explicitly

LQR Control Methods for Trajectory Execution in Omnidirectional Mobile Robots 397

Our segment-wise optimal solution to the trajectory execution control problem allows then to formulate control strategies that can easily adapt to the circumstances of the application. So, for example, the robot may be forced to move quickly in segments of the trajectory that do not require precision, and it may be left free to rotate in those segments in which its orientation is not of concern. In this way, its limited energy is consumed only for those objectives that are

A benchmark sequence of target regions was used for the purpose of testing our proposed solution. This sequence is shown in Fig. 9, and has a total of nine segments. Several obstacles were placed on the testing field to give more realism to the 3D OpenGL simulation of the resulting trajectory. A four wheel omnidirectional robot was used for this simulation with wheels distributed around its periphery according to Table 1. Wheels 1 and 4 are the frontal wheels. They are intentionally separated wider apart than the rest in order to allow for a

> *θ*1 *θ*2 *θ*3 *θ*4 60◦ 135◦ −135◦ −60◦

The specific optimality-index parameters used for each of the segments of the trajectory are shown in Table 2. Throughout the whole trajectory, parameter *wm* is kept at a moderate value in order to conserve energy without degrading the time-domain performance too much. Parameter *w<sup>β</sup>* is kept at a low value for all segments except for the final two. This saves the energy that would otherwise be required to maintain the orientation of the robot towards a specific angle in the first seven segments where that would not be of interest, but then forces the robot to align itself with the location of the final target region in order to capture the object of interest located at the center of such region. Parameter *wβ*˙ is kept at a low value throughout the whole trajectory. Parameter *wxy* is set to a high value in segments 2, 3, 4, 5 and 9. The first four of these segments correspond to the part of the trajectory that takes the robot through the narrow opening in the obstacle barrier, where high precision is required in order to avoid a collision. High precision is also desirable in the last segment to ensure a successful capture of the object of interest. Finally, parameter *wv* is given a moderate value throughout the first seven segments of the trajectory and then is lowered in the last two segments in order to allow for the energy to be used for the more important objective of orienting the robot towards the final target region. In this analysis what matters is the relative value of the weights. However, in order to give a more precise idea, in this analysis *moderate* means 1, *low* means 0.1 and *high*

The resulting trajectory is shown in Fig. 10. The continuous black line shows the path of the robot, while the smaller black straight lines show the orientation of the robot at each point. This gives an idea of how the robot rotates as it moves along the trajectory. One surprising result of this simulation experiment is that the robot started the second segment almost in reverse direction and slowly rotated to approach a forward heading motion. The final section of the trajectory shows how the object of interest gets captured by the robot. Figure 11 shows

specified.

means 5.

important at the moment.

**5. Experimental results**

manipulating device to be installed between them.

Table 1. Omnidirectional wheels distribution

differential Riccati equation. Computational efficiency is of course an important concern in this application since the solution will have to be obtained in real time in a high speed mobile robot environment.

The infinite-horizon LQR control problem consists on finding the state-feedback matrix *K* such that *u*ˇ = −*Kz*<sup>1</sup> minimizes the performance index *J* given by

$$J = \int\_0^\infty (z\_1 \mathbf{^T Q} z\_1 + \check{\mathbf{u}} \, ^\mathbf{T} \mathbf{R} \check{\mathbf{u}}) dt \tag{27}$$

taking into account that *z*<sup>1</sup> and *u*ˇ are restricted by the dynamics of the mobile robot given by Eq. 26.

The performance index *J* specifies the total cost of the control strategy, which depends on an integral quadratic measure of the state *z*<sup>1</sup> and control *u*ˇ. *Q* and *R* represent positive definite matrices that give a weighted measure of the cost of each state variable and control variable, respectively.

For simplicity, in our solution *Q* and *R* are defined to be diagonal matrices

$$Q = \begin{bmatrix} w\_{xy} & 0 & 0 & 0 & 0 & 0 \\ 0 & w\_{xy} & 0 & 0 & 0 & 0 \\ 0 & 0 & w\_{\beta} & 0 & 0 & 0 \\ 0 & 0 & 0 & w\_{v} & 0 & 0 \\ 0 & 0 & 0 & 0 & w\_{v} & 0 \\ 0 & 0 & 0 & 0 & 0 & w\_{\beta} \end{bmatrix} \tag{28}$$
 
$$R = w\_{m}I\_{n \times n}$$

where


This set of cost weights has to be specified for each of the segments of the trajectory. The weights specify the relative cost of each variable, and by an appropriate choice of their values one can easily adjust the optimality index for different control strategies. For example, if *wm* is very large in comparison to the other weights then our strategy will be to save energy, if *wxy* is large in comparison to the rest then the strategy dictates that the robot should reach the target region as soon as possible without regard to a specific target final speed, angle *β* or energy consumption. It would make sense to keep *w<sup>β</sup>* very low during most of the trajectory except for those segments in which the robot is approaching the final target region or, in the case of soccer applications, orienting itself to receive a pass from a teammate robot.

The LQR approach is much more natural from the point of view of the designer as compared to other linear state-feedback controller design techniques such as *pole placement*. Although pole placement may help in specifying a desired time-domain transient behavior, the resulting feedback control may turn out to be higher than what the actuators can actually achieve. In LQR, on the other hand, the solution strikes a balance between the transient behavior of the state variables and the energy consumed by the actuators. In the case of LQR the resulting poles are implicitly determined by the choice of matrices *Q* and *R*, rather than being explicitly specified.

Our segment-wise optimal solution to the trajectory execution control problem allows then to formulate control strategies that can easily adapt to the circumstances of the application. So, for example, the robot may be forced to move quickly in segments of the trajectory that do not require precision, and it may be left free to rotate in those segments in which its orientation is not of concern. In this way, its limited energy is consumed only for those objectives that are important at the moment.

#### **5. Experimental results**

12 Will-be-set-by-IN-TECH

differential Riccati equation. Computational efficiency is of course an important concern in this application since the solution will have to be obtained in real time in a high speed mobile

The infinite-horizon LQR control problem consists on finding the state-feedback matrix *K* such

taking into account that *z*<sup>1</sup> and *u*ˇ are restricted by the dynamics of the mobile robot given by

The performance index *J* specifies the total cost of the control strategy, which depends on an integral quadratic measure of the state *z*<sup>1</sup> and control *u*ˇ. *Q* and *R* represent positive definite matrices that give a weighted measure of the cost of each state variable and control variable,

> *wxy* 0 0000 0 *wxy* 0000 0 0 *w<sup>β</sup>* 000 0 00 *wv* 0 0 0 0 00 *wv* 0 0 0 000 *wβ*˙

This set of cost weights has to be specified for each of the segments of the trajectory. The weights specify the relative cost of each variable, and by an appropriate choice of their values one can easily adjust the optimality index for different control strategies. For example, if *wm* is very large in comparison to the other weights then our strategy will be to save energy, if *wxy* is large in comparison to the rest then the strategy dictates that the robot should reach the target region as soon as possible without regard to a specific target final speed, angle *β* or energy consumption. It would make sense to keep *w<sup>β</sup>* very low during most of the trajectory except for those segments in which the robot is approaching the final target region or, in the

The LQR approach is much more natural from the point of view of the designer as compared to other linear state-feedback controller design techniques such as *pole placement*. Although pole placement may help in specifying a desired time-domain transient behavior, the resulting feedback control may turn out to be higher than what the actuators can actually achieve. In LQR, on the other hand, the solution strikes a balance between the transient behavior of the state variables and the energy consumed by the actuators. In the case of LQR the resulting

case of soccer applications, orienting itself to receive a pass from a teammate robot.

<sup>T</sup>*Qz*<sup>1</sup> + *u*ˇ

<sup>T</sup>*Ru*ˇ)*dt* (27)

(28)

⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

that *u*ˇ = −*Kz*<sup>1</sup> minimizes the performance index *J* given by

*Q* =

• *wxy* : cost weight of the XY position of the robot • *wv* : cost weight of the XY speed of the robot

• *w<sup>β</sup>* : cost weight of the angular position of the robot • *wβ*˙ : cost weight of the angular speed of the robot • *wm* : cost weight of the torque of the driving motors

⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣

*R* = *wm In*×*<sup>n</sup>*

*J* = � ∞ 0 (*z*<sup>1</sup>

For simplicity, in our solution *Q* and *R* are defined to be diagonal matrices

robot environment.

Eq. 26.

where

respectively.

A benchmark sequence of target regions was used for the purpose of testing our proposed solution. This sequence is shown in Fig. 9, and has a total of nine segments. Several obstacles were placed on the testing field to give more realism to the 3D OpenGL simulation of the resulting trajectory. A four wheel omnidirectional robot was used for this simulation with wheels distributed around its periphery according to Table 1. Wheels 1 and 4 are the frontal wheels. They are intentionally separated wider apart than the rest in order to allow for a manipulating device to be installed between them.

Table 1. Omnidirectional wheels distribution


The specific optimality-index parameters used for each of the segments of the trajectory are shown in Table 2. Throughout the whole trajectory, parameter *wm* is kept at a moderate value in order to conserve energy without degrading the time-domain performance too much. Parameter *w<sup>β</sup>* is kept at a low value for all segments except for the final two. This saves the energy that would otherwise be required to maintain the orientation of the robot towards a specific angle in the first seven segments where that would not be of interest, but then forces the robot to align itself with the location of the final target region in order to capture the object of interest located at the center of such region. Parameter *wβ*˙ is kept at a low value throughout the whole trajectory. Parameter *wxy* is set to a high value in segments 2, 3, 4, 5 and 9. The first four of these segments correspond to the part of the trajectory that takes the robot through the narrow opening in the obstacle barrier, where high precision is required in order to avoid a collision. High precision is also desirable in the last segment to ensure a successful capture of the object of interest. Finally, parameter *wv* is given a moderate value throughout the first seven segments of the trajectory and then is lowered in the last two segments in order to allow for the energy to be used for the more important objective of orienting the robot towards the final target region. In this analysis what matters is the relative value of the weights. However, in order to give a more precise idea, in this analysis *moderate* means 1, *low* means 0.1 and *high* means 5.

The resulting trajectory is shown in Fig. 10. The continuous black line shows the path of the robot, while the smaller black straight lines show the orientation of the robot at each point. This gives an idea of how the robot rotates as it moves along the trajectory. One surprising result of this simulation experiment is that the robot started the second segment almost in reverse direction and slowly rotated to approach a forward heading motion. The final section of the trajectory shows how the object of interest gets captured by the robot. Figure 11 shows

successfully executed as shown in Fig. 12. Although the resulting 8 is not perfectly smooth the advantage here is that the path is achieved without the need for a computationally intensive path tracking algorithm but rather only loosely specified by a small set of target regions.

LQR Control Methods for Trajectory Execution in Omnidirectional Mobile Robots 399

This work presents the formulation of a complete state-space model for the omnidirectional mobile robot. Although the model is nonlinear with respect to the control vector, it is shown how it can be globally linearized by a change of variables on the control vector. It has been shown how to apply this linearization for the case of *n* = 3, *n* = 4 and *n* = 5 omnidirectional wheels. The distinction among number of wheels is far from trivial since there is the need of introducing *complementary equations* that will make the proposed change of variable

The model used for analysis in this paper assumes an idealized omnidirectional robot with uniform density. Nonetheless, the same fundamental ideas that were tested under simulation

There are still two major considerations to take into account before our proposed control method can be applied in the real world. First, our solution assumes availability of all state variables in *z*1. However, in reality these state variables would have to be estimated from the available measurements using an approach similar to that presented in (Lupián & Avila, 2008). These measurements would come from the encoders of the driving motors and from the global vision system. The global vision system provides a measurement of variables *x*, *y* and *β*, but this measurement comes with a significant delay due to the computational overhead, so in actuality these variables would have to be predicted (Gloye et al., 2003). The second major consideration is that this solution will be implemented on a digital processor so, rather than a

The benchmark sequence of target regions used in the "Simulation results" section assumes the environment is static. In a real highly dynamic application the sequence of target regions will have to be updated as the trajectory execution progresses. This dynamic path-planning problem would be the task of a higher-level artificial intelligence algorithm which is left open

The first of the two simulation experiments can be better appreciated on the 3D OpenGL animation developed for this purpose. The reader can access a video of this animation online

continuous-time model, the problem should be formulated in discrete-time.

Fig. 12. Simulation of the segment-wise optimal 8-shaped trajectory

in this work would still apply for a real robot.

**6. Conclusion**

invertible.

for future research.

at (Lupián, 2009).


Table 2. Optimality index parameters for each segment

Fig. 10. Simulation of the segment-wise optimal trajectory execution

how the speeds and torques of each driving motor evolve against time. From these graphs we can interpret that there are only two required high energy transients. The first one comes at the initial time, when the robot must accelerate from a motionless state. The second high energy transient comes at the transition from the first to the second segment, where the robot must abruptly change direction while moving at a relatively high speed.

Fig. 11. Speed and torque vs. time for each wheel

A second experiment that we performed to test the segment-wise optimal solution to the trajectory execution problem is that of performing an 8-shaped trajectory, which was successfully executed as shown in Fig. 12. Although the resulting 8 is not perfectly smooth the advantage here is that the path is achieved without the need for a computationally intensive path tracking algorithm but rather only loosely specified by a small set of target regions.

Fig. 12. Simulation of the segment-wise optimal 8-shaped trajectory

#### **6. Conclusion**

14 Will-be-set-by-IN-TECH

**Par. 123456789** *wxy* 155551115 *wv* 1 1 1 1 1 1 1 0.1 0.1 *w<sup>β</sup>* 0.1 0.1 0.1 0.1 0.1 0.1 1 5 5 *wβ*˙ 1 0.1 0.1 0.1 0.1 1 0.1 0.1 0.1 *wm* 111111111

Table 2. Optimality index parameters for each segment

Fig. 10. Simulation of the segment-wise optimal trajectory execution

must abruptly change direction while moving at a relatively high speed.

Fig. 11. Speed and torque vs. time for each wheel

how the speeds and torques of each driving motor evolve against time. From these graphs we can interpret that there are only two required high energy transients. The first one comes at the initial time, when the robot must accelerate from a motionless state. The second high energy transient comes at the transition from the first to the second segment, where the robot

A second experiment that we performed to test the segment-wise optimal solution to the trajectory execution problem is that of performing an 8-shaped trajectory, which was This work presents the formulation of a complete state-space model for the omnidirectional mobile robot. Although the model is nonlinear with respect to the control vector, it is shown how it can be globally linearized by a change of variables on the control vector. It has been shown how to apply this linearization for the case of *n* = 3, *n* = 4 and *n* = 5 omnidirectional wheels. The distinction among number of wheels is far from trivial since there is the need of introducing *complementary equations* that will make the proposed change of variable invertible.

The model used for analysis in this paper assumes an idealized omnidirectional robot with uniform density. Nonetheless, the same fundamental ideas that were tested under simulation in this work would still apply for a real robot.

There are still two major considerations to take into account before our proposed control method can be applied in the real world. First, our solution assumes availability of all state variables in *z*1. However, in reality these state variables would have to be estimated from the available measurements using an approach similar to that presented in (Lupián & Avila, 2008). These measurements would come from the encoders of the driving motors and from the global vision system. The global vision system provides a measurement of variables *x*, *y* and *β*, but this measurement comes with a significant delay due to the computational overhead, so in actuality these variables would have to be predicted (Gloye et al., 2003). The second major consideration is that this solution will be implemented on a digital processor so, rather than a continuous-time model, the problem should be formulated in discrete-time.

The benchmark sequence of target regions used in the "Simulation results" section assumes the environment is static. In a real highly dynamic application the sequence of target regions will have to be updated as the trajectory execution progresses. This dynamic path-planning problem would be the task of a higher-level artificial intelligence algorithm which is left open for future research.

The first of the two simulation experiments can be better appreciated on the 3D OpenGL animation developed for this purpose. The reader can access a video of this animation online at (Lupián, 2009).

**0**

**20**

<sup>1</sup>*Greece* <sup>2</sup>*Cyprus*

**Feedback Equivalence and Control of Mobile**

G.P. Moustris1, K.M. Deliparaschos2 and S.G. Tzafestas1

<sup>1</sup>*National Technical University of Athens*

<sup>2</sup>*Cyprus University of Technology*

**Robots Through a Scalable FPGA Architecture**

The control of mobile robots is an intense research field, having produced a substantial volume of research literature in the past three decades. Mobile robots present a challenge in both theoretical control design as well as hardware implementation. From a theoretical point of view, mobile robots exert highly non-linear kinematics and dynamics, non-holonomy and complex operational environment. On the other hand, hardware designs strive for scalability,

The main control problems in robot motion control can be classified in three general cases; stabilization to a point, trajectory tracking and path following. Point stabilization refers to the stabilization of the robot to a specific configuration in its state space (pose). For example, if the robot at *t* = *t*<sup>0</sup> is at *p*<sup>0</sup> = (*x*0, *y*0, *θ*0), then find a suitable control law that steers it to a goal point *pg*=(*xg*, *yg*, *θg*). Apparently *p*<sup>0</sup> must be an equilibrium point of the closed loop system, exerting asymptotic stability (although practical stability can also be sought for). In the path following (or path tracking) problem, the robot is presented with a reference path, either feasible or infeasible, and has to follow it by issuing the appropriate control commands. A path is defined as a geometric curve in the robot's application space. The trajectory tracking problem is similar, although there is a notable difference; the trajectory is a path with an associated timing law i.e. the robot has to be on specific points at specific time instances. These three problems present challenges and difficulties exacerbated by the fact that the robot models are highly non-linear and non-holonomic (although robots that lift the non-holonomic rolling constraint do exist and are called *omni-directional robots*. However the most interesting mobile robots present this constraint). The non-holonomy means that there are constraints in the robot velocities e.g. the non-slipping condition, which forces the robot to move tangentially to its path or equivalently, the robot's heading is always collinear to its velocity vector (this can readily be attested by every driver who expects his car to move at the direction it is heading and not sideways i.e. slip). For a more motivating example of holonomy, consider a prototypical and pedagogical kimenatic model for motion analysis and control; the unicycle

**1. Introduction**

downsizing, computational power and low cost.

robot, described by the equations,

Σ : ⎡ ⎣ *x*˙ *y*˙ ˙ *θ*

⎤ ⎦ = ⎡ ⎣

cos *θ* sin *θ* 0

⎤ ⎦ *υ* + ⎡ ⎣ 0 0 1 ⎤

⎦ *ω* (1)

#### **7. References**


URL: *http://www.youtube.com/watch?v=-rBP3VwDPAs*


### **Feedback Equivalence and Control of Mobile Robots Through a Scalable FPGA Architecture**

G.P. Moustris1, K.M. Deliparaschos2 and S.G. Tzafestas1

<sup>1</sup>*National Technical University of Athens* <sup>2</sup>*Cyprus University of Technology* <sup>1</sup>*Greece* <sup>2</sup>*Cyprus*

#### **1. Introduction**

16 Will-be-set-by-IN-TECH

400 Recent Advances in Mobile Robotics

Ashmore, M. & Barnes, N. (2002). Omni-drive robot motion on curved paths: The fastest path

Balkcom, D. J., Kavathekar, P. A. & Mason, M. T. (2006b). Time-optimal trajectories for an

Borenstein, J. & Evans, J. (1997). The omnimate mobile robot: Design, implementation,

Gloye, A. & Rojas, R. (2006). *Künstliche Intelligenz*, Springer, chapter Holonomic Control of a

Gloye, A., Simon, M., Egorova, A., Wiesel, F., Tenchio, O., Schreiber, M., Behnke, S. & Rojas, R. (2003). Predicting away robot control latency, *Technical report B-08-03*, FU-Berlin. Kalmár-Nagy, T., D'Andrea, R. & Ganguly, P. (2004). Near-optimal dynamic trajectory

Kalmár-Nagy, T., Ganguly, P. & D'Andrea, R. (2002). Real-time trajectory generation for

Lupián, L. F. (2009). Simulación de robot F180 en AIR-GL, World Wide Web electronic

Lupián, L. F. & Avila, R. (2008). Stabilization of a wheeled inverted pendulum by a

Lupián, L. F. & Rabadán-Martin, J. R. (2009). Segment-wise optimal trajectory execution

Purwin, O. & D'Andrea, R. (2005). Trajectory generation for four wheeled omnidirectional

omni-directional vehicle, *Int. J. Rob. Res.* 25(10): 985–999.

URL: *http://www.youtube.com/watch?v=-rBP3VwDPAs*

*2009 6th Latin American*, Valparaíso, Chile, pp. 1 – 6.

vehicles, *American Control Conference* pp. 4979–4984.

between two points is not a straight-line, *AI '02: Proceedings of the 15th Australian Joint Conference on Artificial Intelligence*, Springer-Verlag, London, UK, pp. 225–236. Balkcom, D. J., Kavathekar, P. A. & Mason, M. T. (2006a). The minimum-time trajectories for an

omni-directional vehicle, *Seventh International Workshop on the Algorithmic Foundations*

and experimental results, *International Conference on Robotics and Automation, IEEE*,

generation and control of an omnidirectional vehicle, *Robotics and Autonomous*

omnidirectional vehicles, *American Control Conference*, Anchorage, AK, pp. 286–291.

continuous-time infinite-horizon LQG optimal controller, *Robotic Symposium, IEEE*

control for four-wheeled omnidirectional mobile robots, *Robotics Symposium (LARS),*

**7. References**

*of Robotics*, New York City.

*Systems* 46: 47–64.

publication.

Albuquerque, NM, pp. 3505–3510.

Robot with an Omnidirectional Drive.

*Latin American*, Bahia, Brazil, pp. 65–70.

The control of mobile robots is an intense research field, having produced a substantial volume of research literature in the past three decades. Mobile robots present a challenge in both theoretical control design as well as hardware implementation. From a theoretical point of view, mobile robots exert highly non-linear kinematics and dynamics, non-holonomy and complex operational environment. On the other hand, hardware designs strive for scalability, downsizing, computational power and low cost.

The main control problems in robot motion control can be classified in three general cases; stabilization to a point, trajectory tracking and path following. Point stabilization refers to the stabilization of the robot to a specific configuration in its state space (pose). For example, if the robot at *t* = *t*<sup>0</sup> is at *p*<sup>0</sup> = (*x*0, *y*0, *θ*0), then find a suitable control law that steers it to a goal point *pg*=(*xg*, *yg*, *θg*). Apparently *p*<sup>0</sup> must be an equilibrium point of the closed loop system, exerting asymptotic stability (although practical stability can also be sought for). In the path following (or path tracking) problem, the robot is presented with a reference path, either feasible or infeasible, and has to follow it by issuing the appropriate control commands. A path is defined as a geometric curve in the robot's application space. The trajectory tracking problem is similar, although there is a notable difference; the trajectory is a path with an associated timing law i.e. the robot has to be on specific points at specific time instances.

These three problems present challenges and difficulties exacerbated by the fact that the robot models are highly non-linear and non-holonomic (although robots that lift the non-holonomic rolling constraint do exist and are called *omni-directional robots*. However the most interesting mobile robots present this constraint). The non-holonomy means that there are constraints in the robot velocities e.g. the non-slipping condition, which forces the robot to move tangentially to its path or equivalently, the robot's heading is always collinear to its velocity vector (this can readily be attested by every driver who expects his car to move at the direction it is heading and not sideways i.e. slip). For a more motivating example of holonomy, consider a prototypical and pedagogical kimenatic model for motion analysis and control; the unicycle robot, described by the equations,

$$
\Sigma: \begin{bmatrix} \dot{\boldsymbol{x}} \\ \dot{\boldsymbol{y}} \\ \dot{\boldsymbol{\theta}} \end{bmatrix} = \begin{bmatrix} \cos \theta \\ \sin \theta \\ \boldsymbol{0} \end{bmatrix} \boldsymbol{v} + \begin{bmatrix} 0 \\ 0 \\ 1 \end{bmatrix} \boldsymbol{\omega} \tag{1}
$$

Due to the increased number of calculations necessary for the path tracking control, a high performance processing system to efficiently handle the task is required. By using a System-on-a-Chip (SoC) realised on an FPGA device, we utilize the hardware/software re-configurability of the FPGA to satisfy the needs of fuzzy logic path tracking for autonomous robots for high-performance onboard processing and flexible hardware for different tasks. FPGAs provide several advantages over single processor hardware, on the one hand, and Application Specific Integrated Circuits (ASIC) on the other. FPGA chips are field-upgradable and do not require the time and expense involved with ASIC redesign. Being reconfigurable, FPGA chips are able to keep up with future modifications that might be necessary. They offer a simpler design cycle, re-programmability, and have a faster time-to-market since no fabrication (layout, masks, or other manufacturing steps) time is required, when compared to

Feedback Equivalence and Control of Mobile Robots Through a Scalable FPGA Architecture 403

The use of FPGAs in robotic applications is noted in (Kongmunvattana & Chongstivatana, 1998; Leong & Tsoi, 2005; Li et al., 2003; Reynolds et al., 2001). A review of the application of FPGA's in robotic systems is provided be Leong and Tsoi in (Leong & Tsoi, 2005). A notable case study is the use of FPGA's in the Mars Pathfinder, Mars Surveyor '98, and Mars Surveyor

In this chapter we analyse a SoC implementation for the non-holonomic robot path tracking task using a fuzzy logic controller, along with a non-linear feedback-equivalence transformation which reduces path tracking to straight line tracking. The major components of the SoC are a parametrized Digital Fuzzy Logic Controller (DFLC) soft IP core Deliparaschos et al. (2006) Deliparaschos & Tzafestas (2006), implementing the fuzzy tracking algorithm, and Xilinx's Microblaze soft processor core as the top level flow controller. The system was tied to a differential drive robot and experiments were performed in order to asses the efficacy and performance of the overall control scheme. This was facilitated using an image analysis algorithm, presented in the following sections, which calculated the robot's position from a video stream captured using an overhead camera. The analysis was made

The mobile robot used in this work, is the Khepera II differential drive robot, described by the

'01 Lander crafts, analysed in (Reynolds et al., 2001).

Fig. 1. Overview of the general system.

off-line. The overall system setup can be seen in Fig 1.

**2. Kinematics & odometry of the Khepera II robot**

ASICs.

equations,

This model is linear on the inputs and describes a linear combination of vector fields which evolve on a 3D configuration space *<sup>M</sup>* <sup>=</sup> **<sup>R</sup>**<sup>3</sup> <sup>×</sup> *<sup>S</sup>*<sup>1</sup> (a three dimensional manifold). The robot is controlled by two inputs *υ* and *ω*, expressing the linear and angular velocities respectively. The generalized velocities live on the manifold's tangent space *TqM* at each point q, thus the system's equations express the available directions of movement. The non-holonomic no-slipping constraint is expressed by the Pfaffian equation,

$$0 = \dot{\mathfrak{x}}\sin(\theta) - \dot{\mathfrak{y}}\cos(\theta) \tag{2}$$

which can be put into the general form,

$$G(q)\dot{q} = 0\tag{3}$$

Here *<sup>q</sup>* <sup>∈</sup> *<sup>M</sup>* is the state vector and *<sup>G</sup>*(*q*) <sup>∈</sup> **<sup>R</sup>**1×<sup>3</sup> is the constraint matrix (although in this case is just a vector). Each row vector (covector) of G lives in the manifold's cotangent space *T*∗ *<sup>q</sup> M* at q, which is the dual space of *TqM*. Equation 3 describes restrictions on the available directions of movement. Apparently, since the velocities must satisfy (3), it is evident that they live in the null space of G. One can move from Eq.(3) to Eq.(1) by solving (3) with respect to the velocities *q*˙. Since the system is underdetermined (note that G is a one by three "matrix"), two generalized velocities can vary freely, which are precisely the two inputs of (1). The non-holonomy of the system derives from the fact that Eq.(2) is not integrable i.e. does not express the total derivative of some function. By the Frobenius theorem, if Δ is the distribution spanned by the two vector fields of Σ, the system is holonomic if Δ is involutive under Lie bracketing, a condition that is not satisfied by (1).

Due to these challenges, the path following problem has been attacked by several researchers from many angles, ranging from classical control approaches (Altafini, 1999; Kamga & Rachid, 1997; Kanayama & Fahroo, 1997), to nonlinear control methodologies (Altafini, 2002; Egerstedt et al., 1998; Koh & Cho, 1994; Samson, 1995; Wit et al., 2004) to intelligent control strategies (Abdessemed et al., 2004; Antonelli et al., 2007; Baltes & Otte, 1999; Cao & Hall, 1998; Deliparaschos et al., 2007; El Hajjaji & Bentalba, 2003; Lee et al., 2003; Liu & Lewis, 1994; Maalouf et al., 2006; Moustris & Tzafestas, 2005; Rodriguez-Castano et al., 2000; Sanchez et al., 1997; Yang et al., 1998). Of course, boundaries often blend since various approaches are used simultaneously. Fuzzy logic path trackers have been used by several researchers (Abdessemed et al., 2004; Antonelli et al., 2007; Baltes & Otte, 1999; Cao & Hall, 1998; Deliparaschos et al., 2007; El Hajjaji & Bentalba, 2003; Jiangzhou et al., 1999; Lee et al., 2003; Liu & Lewis, 1994; Moustris & Tzafestas, 2011; 2005; Ollero et al., 1997; Raimondi & Ciancimino, 2008; Rodriguez-Castano et al., 2000; Sanchez et al., 1997) since fuzzy logic provides a more intuitive way for analysing and formulating the control actions, which bypasses most of the mathematical load needed to tackle such a highly nonlinear control problem. Furthermore, the fuzzy controller, which can be less complex in its implementation, is inherently robust to noise and parameter uncertainties.

The implementation of Fuzzy Logic Controllers (FLC) in software suffers from speed limitations due to the sequential program execution and the fact that standard processors do not directly support many fuzzy operations (i.e., minimum or maximum). In an effort to reduce the lack of fuzzy operations several modified architectures of standard processors supporting fuzzy computation exist (Costa et al., 1997; Fortuna et al., 2003; Salapura, 2000). Software solutions running on these devices speed up fuzzy computations by at least one order of magnitude over standard processors, but are still not fast enough for some real-time applications. Thus, a dedicated hardware implementation must be used (Hung, 1995).

Due to the increased number of calculations necessary for the path tracking control, a high performance processing system to efficiently handle the task is required. By using a System-on-a-Chip (SoC) realised on an FPGA device, we utilize the hardware/software re-configurability of the FPGA to satisfy the needs of fuzzy logic path tracking for autonomous robots for high-performance onboard processing and flexible hardware for different tasks.

FPGAs provide several advantages over single processor hardware, on the one hand, and Application Specific Integrated Circuits (ASIC) on the other. FPGA chips are field-upgradable and do not require the time and expense involved with ASIC redesign. Being reconfigurable, FPGA chips are able to keep up with future modifications that might be necessary. They offer a simpler design cycle, re-programmability, and have a faster time-to-market since no fabrication (layout, masks, or other manufacturing steps) time is required, when compared to ASICs.

The use of FPGAs in robotic applications is noted in (Kongmunvattana & Chongstivatana, 1998; Leong & Tsoi, 2005; Li et al., 2003; Reynolds et al., 2001). A review of the application of FPGA's in robotic systems is provided be Leong and Tsoi in (Leong & Tsoi, 2005). A notable case study is the use of FPGA's in the Mars Pathfinder, Mars Surveyor '98, and Mars Surveyor '01 Lander crafts, analysed in (Reynolds et al., 2001).

Fig. 1. Overview of the general system.

2 Will-be-set-by-IN-TECH

This model is linear on the inputs and describes a linear combination of vector fields which evolve on a 3D configuration space *<sup>M</sup>* <sup>=</sup> **<sup>R</sup>**<sup>3</sup> <sup>×</sup> *<sup>S</sup>*<sup>1</sup> (a three dimensional manifold). The robot is controlled by two inputs *υ* and *ω*, expressing the linear and angular velocities respectively. The generalized velocities live on the manifold's tangent space *TqM* at each point q, thus the system's equations express the available directions of movement. The non-holonomic

Here *<sup>q</sup>* <sup>∈</sup> *<sup>M</sup>* is the state vector and *<sup>G</sup>*(*q*) <sup>∈</sup> **<sup>R</sup>**1×<sup>3</sup> is the constraint matrix (although in this case is just a vector). Each row vector (covector) of G lives in the manifold's cotangent space

*<sup>q</sup> M* at q, which is the dual space of *TqM*. Equation 3 describes restrictions on the available directions of movement. Apparently, since the velocities must satisfy (3), it is evident that they live in the null space of G. One can move from Eq.(3) to Eq.(1) by solving (3) with respect to the velocities *q*˙. Since the system is underdetermined (note that G is a one by three "matrix"), two generalized velocities can vary freely, which are precisely the two inputs of (1). The non-holonomy of the system derives from the fact that Eq.(2) is not integrable i.e. does not express the total derivative of some function. By the Frobenius theorem, if Δ is the distribution spanned by the two vector fields of Σ, the system is holonomic if Δ is involutive under Lie

Due to these challenges, the path following problem has been attacked by several researchers from many angles, ranging from classical control approaches (Altafini, 1999; Kamga & Rachid, 1997; Kanayama & Fahroo, 1997), to nonlinear control methodologies (Altafini, 2002; Egerstedt et al., 1998; Koh & Cho, 1994; Samson, 1995; Wit et al., 2004) to intelligent control strategies (Abdessemed et al., 2004; Antonelli et al., 2007; Baltes & Otte, 1999; Cao & Hall, 1998; Deliparaschos et al., 2007; El Hajjaji & Bentalba, 2003; Lee et al., 2003; Liu & Lewis, 1994; Maalouf et al., 2006; Moustris & Tzafestas, 2005; Rodriguez-Castano et al., 2000; Sanchez et al., 1997; Yang et al., 1998). Of course, boundaries often blend since various approaches are used simultaneously. Fuzzy logic path trackers have been used by several researchers (Abdessemed et al., 2004; Antonelli et al., 2007; Baltes & Otte, 1999; Cao & Hall, 1998; Deliparaschos et al., 2007; El Hajjaji & Bentalba, 2003; Jiangzhou et al., 1999; Lee et al., 2003; Liu & Lewis, 1994; Moustris & Tzafestas, 2011; 2005; Ollero et al., 1997; Raimondi & Ciancimino, 2008; Rodriguez-Castano et al., 2000; Sanchez et al., 1997) since fuzzy logic provides a more intuitive way for analysing and formulating the control actions, which bypasses most of the mathematical load needed to tackle such a highly nonlinear control problem. Furthermore, the fuzzy controller, which can be less complex in its implementation, is inherently robust to

The implementation of Fuzzy Logic Controllers (FLC) in software suffers from speed limitations due to the sequential program execution and the fact that standard processors do not directly support many fuzzy operations (i.e., minimum or maximum). In an effort to reduce the lack of fuzzy operations several modified architectures of standard processors supporting fuzzy computation exist (Costa et al., 1997; Fortuna et al., 2003; Salapura, 2000). Software solutions running on these devices speed up fuzzy computations by at least one order of magnitude over standard processors, but are still not fast enough for some real-time applications. Thus, a dedicated hardware implementation must be used (Hung, 1995).

0 = *x*˙ sin(*θ*) − *y*˙ cos(*θ*) (2)

*G*(*q*)*q*˙ = 0 (3)

no-slipping constraint is expressed by the Pfaffian equation,

which can be put into the general form,

bracketing, a condition that is not satisfied by (1).

noise and parameter uncertainties.

*T*∗

In this chapter we analyse a SoC implementation for the non-holonomic robot path tracking task using a fuzzy logic controller, along with a non-linear feedback-equivalence transformation which reduces path tracking to straight line tracking. The major components of the SoC are a parametrized Digital Fuzzy Logic Controller (DFLC) soft IP core Deliparaschos et al. (2006) Deliparaschos & Tzafestas (2006), implementing the fuzzy tracking algorithm, and Xilinx's Microblaze soft processor core as the top level flow controller. The system was tied to a differential drive robot and experiments were performed in order to asses the efficacy and performance of the overall control scheme. This was facilitated using an image analysis algorithm, presented in the following sections, which calculated the robot's position from a video stream captured using an overhead camera. The analysis was made off-line. The overall system setup can be seen in Fig 1.

#### **2. Kinematics & odometry of the Khepera II robot**

The mobile robot used in this work, is the Khepera II differential drive robot, described by the equations,

we get the familiar unicycle model, i.e.

⎡ ⎣ *x*˙ *y*˙ ˙ *θ*

By adding (9) together and solving for the linear velocity *vs*, we get,

Subtracting (9) we come up with the robot's angular velocity *ω*,

Fig. 3. Depiction of the velocities of the Differential Drive model

⎤ ⎦ = ⎡ ⎣

cos *θ* sin *θ* 0

⎤ ⎦ *us* +

Feedback Equivalence and Control of Mobile Robots Through a Scalable FPGA Architecture 405

For the physical interpretation of the inputs *us*, *u<sup>θ</sup>* , consider that the robot performs a turn of radius R with respect to the axis' middle point (point *K*), centred at the point O (Fig 3). The point O is called the *Instantaneous Centre of Rotation* or ICR. If *ω* is the robot's angular velocity (actually the angular velocity of *K*), then its linear velocity is *vs* = *ωR*. It also holds that,

*vr* = *ω*(*R* + *L*/2)

*vs* <sup>=</sup> *vr* <sup>+</sup> *vl*

Observe that *vs*, *v<sup>θ</sup>* are actually *us*, *u<sup>θ</sup>* , thus the new input variables in Eq.7 are actually the robot's linear and angular velocities. What we have proved so far is that the unicycle (or the Dubins Car, which is the unicycle with a constant speed) is related to the Differential Drive by an input transformation, hence they are equivalent. This means that the Differential Drive can *emulate* these models. Consequently, we can develop a controller for either system and apply it to the others by using this transformation (this is feasible if the input transformation is actually *bijective*. If it is not, then the controller can by ported to one direction i.e. from

*<sup>v</sup><sup>θ</sup>* <sup>=</sup> *vr* <sup>−</sup> *vl*

⎡ ⎣ 0 0 1 ⎤

*vl* <sup>=</sup> *<sup>ω</sup>*(*<sup>R</sup>* <sup>−</sup> *<sup>L</sup>*/2) (9)

<sup>2</sup> (10)

*<sup>L</sup>* <sup>=</sup> *<sup>ω</sup>* (11)

⎦ *u<sup>θ</sup>* (8)

$$
\begin{bmatrix}
\dot{\mathbf{x}} \\
\dot{y} \\
\dot{\theta}
\end{bmatrix} = \begin{bmatrix}
\frac{r}{2}\cos\theta \\
\frac{r}{2}\sin\theta \\
\frac{r}{2} \\
\frac{r}{L}
\end{bmatrix} u\_r + \begin{bmatrix}
\frac{r}{2}\cos\theta \\
\frac{r}{2}\sin\theta \\
\end{bmatrix} u\_l \tag{4}
$$

Here, *x*, *y* are the coordinates of the middle point of the axis, *L* is the axis length (the distance between the two wheels), *r* is the wheel radius and *ul*, *ur* the angular wheel velocities. A diagram of the model is seen in Fig.(2). Equations 4 can be transformed to a model more akin to the unicycle by first noting that the linear velocity of a point on the wheel's circumference is *v* = *rω* (*ω<sup>i</sup> ui*). It can be easily shown that the linear velocity of the wheel's center equals the velocity of its circumference. Thus, denoting the centres' velocities as *vr*, *vl*, then,

$$v\_l = ru\_l$$

$$v\_l = ru\_l\tag{5}$$

and substituting them into Eq.4, the system takes the form,

$$
\begin{bmatrix} \dot{\mathbf{x}} \\ \dot{\mathbf{y}} \\ \dot{\theta} \end{bmatrix} = \begin{bmatrix} \cos \theta / 2 \\ \sin \theta / 2 \\ 1 / L \end{bmatrix} v\_l + \begin{bmatrix} \cos \theta / 2 \\ \sin \theta / 2 \\ -1 / L \end{bmatrix} v\_r \tag{6}
$$

Fig. 2. Depiction of the generalized coordinated for the Differential Drive model If we further apply a new input transformation,

$$\begin{aligned} u\_s &= \frac{v\_r + v\_l}{2} \\ u\_\theta &= \frac{v\_r - v\_l}{2} \end{aligned} \tag{7}$$

we get the familiar unicycle model, i.e.

4 Will-be-set-by-IN-TECH

⎤ ⎥ ⎥ ⎥ ⎥ ⎦

Here, *x*, *y* are the coordinates of the middle point of the axis, *L* is the axis length (the distance between the two wheels), *r* is the wheel radius and *ul*, *ur* the angular wheel velocities. A diagram of the model is seen in Fig.(2). Equations 4 can be transformed to a model more akin to the unicycle by first noting that the linear velocity of a point on the wheel's circumference is *v* = *rω* (*ω<sup>i</sup> ui*). It can be easily shown that the linear velocity of the wheel's center equals

> *vr* = *rur vl* = *rul*

> > ⎤ ⎥ ⎦ *vl* +

⎡ ⎢ ⎣

cos *θ*/2 sin *θ*/2 −1/*L*

⎤ ⎥ ⎦

the velocity of its circumference. Thus, denoting the centres' velocities as *vr*, *vl*, then,

cos *θ*/2 sin *θ*/2 1/*L*

Fig. 2. Depiction of the generalized coordinated for the Differential Drive model

*us* <sup>=</sup> *vr* <sup>+</sup> *vl* 2 *<sup>u</sup><sup>θ</sup>* <sup>=</sup> *vr* <sup>−</sup> *vl* 2

If we further apply a new input transformation,

*ur* +

⎡ ⎢ ⎢ ⎢ ⎢ ⎣

*r* <sup>2</sup> cos *<sup>θ</sup> r* <sup>2</sup> sin *<sup>θ</sup>* − *r L*

⎤ ⎥ ⎥ ⎥ ⎥ ⎦

*ul* (4)

*vr* (6)

(5)

(7)

⎡ ⎢ ⎣

and substituting them into Eq.4, the system takes the form,

⎡ ⎢ ⎣ *x*˙ *y*˙ ˙ *θ*

⎤ ⎥ <sup>⎦</sup> <sup>=</sup> ⎡ ⎢ ⎣

*x*˙ *y*˙ ˙ *θ* ⎤ ⎥ <sup>⎦</sup> <sup>=</sup> ⎡ ⎢ ⎢ ⎢ ⎢ ⎣ *r* <sup>2</sup> cos *<sup>θ</sup> r* <sup>2</sup> sin *<sup>θ</sup> r L*

$$
\begin{bmatrix} \dot{\mathbf{x}} \\ \dot{y} \\ \dot{\theta} \end{bmatrix} = \begin{bmatrix} \cos \theta \\ \sin \theta \\ 0 \end{bmatrix} u\_s + \begin{bmatrix} 0 \\ 0 \\ 1 \end{bmatrix} u\_\theta \tag{8}
$$

For the physical interpretation of the inputs *us*, *u<sup>θ</sup>* , consider that the robot performs a turn of radius R with respect to the axis' middle point (point *K*), centred at the point O (Fig 3). The point O is called the *Instantaneous Centre of Rotation* or ICR. If *ω* is the robot's angular velocity (actually the angular velocity of *K*), then its linear velocity is *vs* = *ωR*. It also holds that,

$$\begin{aligned} v\_r &= \omega (R + L/2) \\ v\_l &= \omega (R - L/2) \end{aligned} \tag{9}$$

By adding (9) together and solving for the linear velocity *vs*, we get,

$$v\_s = \frac{v\_r + v\_l}{2} \tag{10}$$

Subtracting (9) we come up with the robot's angular velocity *ω*,

$$v\_{\theta} = \frac{v\_{r} - v\_{l}}{L} = \omega \tag{11}$$

$$\omega = \underbrace{\int\_{\omega\_{\theta}}^{\omega\_{\theta}} \int\_{\omega\_{\theta}}^{\omega\_{\theta}} \int\_{\omega\_{\theta}}^{\omega\_{\theta}} \omega \, d\omega} \tag{12}$$

Fig. 3. Depiction of the velocities of the Differential Drive model

Observe that *vs*, *v<sup>θ</sup>* are actually *us*, *u<sup>θ</sup>* , thus the new input variables in Eq.7 are actually the robot's linear and angular velocities. What we have proved so far is that the unicycle (or the Dubins Car, which is the unicycle with a constant speed) is related to the Differential Drive by an input transformation, hence they are equivalent. This means that the Differential Drive can *emulate* these models. Consequently, we can develop a controller for either system and apply it to the others by using this transformation (this is feasible if the input transformation is actually *bijective*. If it is not, then the controller can by ported to one direction i.e. from

To calculate the robot's *x*, *y* position, we must solve the kinematics for the interval Δ*t*, setting

Feedback Equivalence and Control of Mobile Robots Through a Scalable FPGA Architecture 407

Δ*θ*

Δ*θ*

In the case that the robot moves in a straight line, hence Δ*θ* = 0, taking the limit of 18 gives

<sup>Δ</sup>*x*(*t*) = <sup>Δ</sup>*<sup>S</sup>* cos(*θt*−<sup>1</sup> +

<sup>Δ</sup>*y*(*t*) = <sup>Δ</sup>*<sup>S</sup>* sin(*θt*−<sup>1</sup> +

*x*(*t*) = *x*<sup>0</sup> +

*y*(*t*) = *y*<sup>0</sup> +

value, something which was also seen in actual experiments and must be minimized.

The Strip-Wise Affine Map (SWAM) is the first step towards constructing a feedback equivalence relation which transforms the robot's model to a suitable form, under specific requirements. The equivalence relation however, exerts the useful property of *form invariance* on the mobile robot equations. The SWAM is defined for a robot model *and* a reference path, being applied to tracking control. To begin with, consider a reference polygonal path in the original physical domain, i.e. the actual domain where the robot dwells. Denote this physical domain as *Dp* (w-plane) and the transformed canonical domain as *Dc* (z-plane). Then, the strip-wise affine map is a homeomorphism Φ : *Dc* → *Dp* that sends the real line of the canonical domain to the reference polygonal path in the physical domain. The SWAM is a piecewise linear homeomorphism between the two spaces (Groff, 2003; Gupta & Wenger,

To get the absolute coordinates of the robot at *t*, Eq.19 must be integrated, leading to the

Using the previous formulas (Eq.16 and Eq.20) we have succeeded in reconstructing the robot's state vector, i.e. the states (*x*, *y*, *θ*). Note that *time* is absent from the odometry equations. This has been chosen deliberately since it reduces the estimation error significantly. To consider this further, suppose that we get the wheel velocities from the robot and use the odometry equations involving the time Δ*t*. The use of the velocities in the formulas inserts two types of errors; the first is the estimation of the velocities themselves from the robot. In the time between two queries to the robot, which is 12.5 msec, the velocity cannot be computed with adequate precision; the second error derives from the calculation of the interval Δ*t*, which is inserted into the equations. This interval is not constant since there is always a small computational overhead in the software in order to setup and issue the command, communication delays etc. Furthermore, the queries to the robot are implemented in MATLAB using a *timer object*. The timer period however, is not guaranteed and is affected by the processes running on the computer at each instant. Thus, Δ*t* can vary from its nominal

*t* ∑ *τ*=0

*t* ∑ *τ*=0 Δ*x*(*τ*)

Δ*y*(*τ*)

<sup>2</sup> ) cos(*θt*−<sup>1</sup> <sup>+</sup>

<sup>2</sup> ) sin(*θt*−<sup>1</sup> <sup>+</sup>

Δ*θ* 2 )

Δ*θ* 2 ) Δ*θ* 2 )

Δ*θ* 2 ) (18)

(19)

(20)

the input constant. The solution can be easily shown to be,

the equations,

odometric equations,

**3. Strip-Wise Affine Map**

Δ*x*(*t*) = 2

Δ*y*(*t*) = 2

Δ*S* <sup>Δ</sup>*<sup>θ</sup>* sin(

Δ*S* <sup>Δ</sup>*<sup>θ</sup>* sin(

model A to model B, and not the other way around). As mentioned earlier, the Dubins Car is a simplified model where the velocity is considered constant. Furthermore, the path following problem involves the tracking of a purely geometric curve, where indeed the velocity is irrelevant. Hence, the model (8) can be transformed to a more suitable control form by using the curvature *κ uκ*. The curvature is related to the angular and linear velocities by the well known formula *ω* = *κv*, or, using our nomenclature,

$$
\mathfrak{u}\_{\theta} = \mathfrak{u}\_{\mathsf{K}} \mathfrak{u}\_{\mathsf{S}} \tag{12}
$$

Since the linear velocity *us* = *v* is constant, by applying this transformation to the system (8), we get,

$$
\begin{bmatrix}
\dot{\mathbf{x}} \\
\dot{y} \\
\dot{\theta}
\end{bmatrix} = \begin{bmatrix}
v\cos\theta \\
v\sin\theta \\
0
\end{bmatrix} + \begin{bmatrix}
0 \\
0 \\
v\end{bmatrix} u\_k \tag{13}
$$

This model is of control-affine form, with a non-vanishing drift term, where the only input is the curvature *uκ*. By controlling the curvature in this model, we expect to control the actual system i.e. the Khepera robot, which is a differential drive and has two inputs. Thus, starting with the curvature, in order to calculate the true input vector to the robot, we need a second equation. This is of course the velocity equation *us* = *v*, which is considered known. By combining (10), (11), (12), the wheel velocities are calculated as,

$$\begin{aligned} v\_I &= v(1 + \mu\_\kappa L/2) \\ v\_I &= v(1 - \mu\_\kappa L/2) \end{aligned} \tag{14}$$

Equation 14 produces the linear wheel velocities of the robot, given its linear velocity and curvature. Since the linear velocity is constant, the only input is the curvature which is output by the fuzzy controller implemented on the FPGA.

In order to calculate its position, MATLAB queries the robot about its wheel encoder readings every 12.5 msec. The robot returns the 32bit encoder position, expressed in pulse units, with each unit corresponding to 0.08 mm. Consequently, by multiplying the units with 0.08 we get the *total length* each wheel has travelled since the beginning of the experiment. Now let *SR*(*t*), *SL*(*t*) be the travel length of the right and left wheels at time *t*, and *SR*(*t* − 1), *SL*(*t* − 1) be the length at *t* − 1. We assume that in the interval Δ*t* the robot moves with a constant curvature, and thus traces an arc. This translates to constant wheel velocities (Eq. 14). Then, using (11) we have,

$$
\omega = \frac{\Delta \theta}{\Delta t} = \frac{\upsilon\_l - \upsilon\_l}{L} = \frac{\Delta S\_R - \Delta S\_L}{\Delta t L} \Leftrightarrow \Delta \theta = \frac{\Delta S\_R - \Delta S\_L}{L} \tag{15}
$$

If the robot's initial heading *θ*0, with respect to the world frame, is known, then at time *t* it holds that,

$$\theta(t) = \theta\_0 + \sum\_{\tau=0}^{t} \Delta \theta(t) = \theta\_0 + \sum\_{\tau=0}^{t} \frac{\Delta S\_R(\tau) - \Delta S\_L(\tau)}{L} \tag{16}$$

Using 10, the length travelled by the point *K* in Δ*t* is found to be,

$$v\_s = \frac{\Delta S}{\Delta t} = \frac{v\_r + v\_l}{2} \Leftrightarrow \Delta S = \frac{\Delta S\_R + \Delta S\_L}{2} \tag{17}$$

To calculate the robot's *x*, *y* position, we must solve the kinematics for the interval Δ*t*, setting the input constant. The solution can be easily shown to be,

$$\begin{aligned} \Delta x(t) &= 2 \frac{\Delta S}{\Delta \theta} \sin(\frac{\Delta \theta}{2}) \cos(\theta\_{t-1} + \frac{\Delta \theta}{2}) \\ \Delta y(t) &= 2 \frac{\Delta S}{\Delta \theta} \sin(\frac{\Delta \theta}{2}) \sin(\theta\_{t-1} + \frac{\Delta \theta}{2}) \end{aligned} \tag{18}$$

In the case that the robot moves in a straight line, hence Δ*θ* = 0, taking the limit of 18 gives the equations,

$$\begin{aligned} \Delta \mathbf{x}(t) &= \Delta S \cos(\theta\_{t-1} + \frac{\Delta \theta}{2}) \\ \Delta y(t) &= \Delta S \sin(\theta\_{t-1} + \frac{\Delta \theta}{2}) \end{aligned} \tag{19}$$

To get the absolute coordinates of the robot at *t*, Eq.19 must be integrated, leading to the odometric equations,

$$\begin{aligned} \mathbf{x}(t) &= \mathbf{x}\_0 + \sum\_{\tau=0}^t \Delta \mathbf{x}(\tau) \\ \mathbf{y}(t) &= \mathbf{y}\_0 + \sum\_{\tau=0}^t \Delta \mathbf{y}(\tau) \end{aligned} \tag{20}$$

Using the previous formulas (Eq.16 and Eq.20) we have succeeded in reconstructing the robot's state vector, i.e. the states (*x*, *y*, *θ*). Note that *time* is absent from the odometry equations. This has been chosen deliberately since it reduces the estimation error significantly. To consider this further, suppose that we get the wheel velocities from the robot and use the odometry equations involving the time Δ*t*. The use of the velocities in the formulas inserts two types of errors; the first is the estimation of the velocities themselves from the robot. In the time between two queries to the robot, which is 12.5 msec, the velocity cannot be computed with adequate precision; the second error derives from the calculation of the interval Δ*t*, which is inserted into the equations. This interval is not constant since there is always a small computational overhead in the software in order to setup and issue the command, communication delays etc. Furthermore, the queries to the robot are implemented in MATLAB using a *timer object*. The timer period however, is not guaranteed and is affected by the processes running on the computer at each instant. Thus, Δ*t* can vary from its nominal value, something which was also seen in actual experiments and must be minimized.

#### **3. Strip-Wise Affine Map**

6 Will-be-set-by-IN-TECH

model A to model B, and not the other way around). As mentioned earlier, the Dubins Car is a simplified model where the velocity is considered constant. Furthermore, the path following problem involves the tracking of a purely geometric curve, where indeed the velocity is irrelevant. Hence, the model (8) can be transformed to a more suitable control form by using the curvature *κ uκ*. The curvature is related to the angular and linear velocities by the well

Since the linear velocity *us* = *v* is constant, by applying this transformation to the system (8),

⎤ ⎦ + ⎡ ⎣ 0 0 *v* ⎤

*v* cos *θ v* sin *θ* 0

This model is of control-affine form, with a non-vanishing drift term, where the only input is the curvature *uκ*. By controlling the curvature in this model, we expect to control the actual system i.e. the Khepera robot, which is a differential drive and has two inputs. Thus, starting with the curvature, in order to calculate the true input vector to the robot, we need a second equation. This is of course the velocity equation *us* = *v*, which is considered known. By

*vr* = *v*(1 + *uκL*/2)

Equation 14 produces the linear wheel velocities of the robot, given its linear velocity and curvature. Since the linear velocity is constant, the only input is the curvature which is output

In order to calculate its position, MATLAB queries the robot about its wheel encoder readings every 12.5 msec. The robot returns the 32bit encoder position, expressed in pulse units, with each unit corresponding to 0.08 mm. Consequently, by multiplying the units with 0.08 we get the *total length* each wheel has travelled since the beginning of the experiment. Now let *SR*(*t*), *SL*(*t*) be the travel length of the right and left wheels at time *t*, and *SR*(*t* − 1), *SL*(*t* − 1) be the length at *t* − 1. We assume that in the interval Δ*t* the robot moves with a constant curvature, and thus traces an arc. This translates to constant wheel velocities (Eq. 14). Then,

*<sup>L</sup>* <sup>=</sup> <sup>Δ</sup>*SR* <sup>−</sup> <sup>Δ</sup>*SL*

Δ*θ*(*t*) = *θ*<sup>0</sup> +

If the robot's initial heading *θ*0, with respect to the world frame, is known, then at time *t* it

*t* ∑ *τ*=0

<sup>2</sup> <sup>⇔</sup> <sup>Δ</sup>*<sup>S</sup>* <sup>=</sup> <sup>Δ</sup>*SR* <sup>+</sup> <sup>Δ</sup>*SL*

*u<sup>θ</sup>* = *uκus* (12)

*vl* <sup>=</sup> *<sup>v</sup>*(<sup>1</sup> <sup>−</sup> *<sup>u</sup>κL*/2) (14)

<sup>Δ</sup>*tL* <sup>⇔</sup> <sup>Δ</sup>*<sup>θ</sup>* <sup>=</sup> <sup>Δ</sup>*SR* <sup>−</sup> <sup>Δ</sup>*SL*

Δ*SR*(*τ*) − Δ*SL*(*τ*)

⎦ *u<sup>κ</sup>* (13)

*<sup>L</sup>* (15)

*<sup>L</sup>* (16)

<sup>2</sup> (17)

known formula *ω* = *κv*, or, using our nomenclature,

⎡ ⎣ *x*˙ *y*˙ ˙ *θ*

combining (10), (11), (12), the wheel velocities are calculated as,

by the fuzzy controller implemented on the FPGA.

*<sup>ω</sup>* <sup>=</sup> <sup>Δ</sup>*<sup>θ</sup>*

*θ*(*t*) = *θ*<sup>0</sup> +

<sup>Δ</sup>*<sup>t</sup>* <sup>=</sup> *vr* <sup>−</sup> *vl*

*t* ∑ *τ*=0

<sup>Δ</sup>*<sup>t</sup>* <sup>=</sup> *vr* <sup>+</sup> *vl*

Using 10, the length travelled by the point *K* in Δ*t* is found to be,

*vs* <sup>=</sup> <sup>Δ</sup>*<sup>S</sup>*

⎤ ⎦ = ⎡ ⎣

we get,

using (11) we have,

holds that,

The Strip-Wise Affine Map (SWAM) is the first step towards constructing a feedback equivalence relation which transforms the robot's model to a suitable form, under specific requirements. The equivalence relation however, exerts the useful property of *form invariance* on the mobile robot equations. The SWAM is defined for a robot model *and* a reference path, being applied to tracking control. To begin with, consider a reference polygonal path in the original physical domain, i.e. the actual domain where the robot dwells. Denote this physical domain as *Dp* (w-plane) and the transformed canonical domain as *Dc* (z-plane). Then, the strip-wise affine map is a homeomorphism Φ : *Dc* → *Dp* that sends the real line of the canonical domain to the reference polygonal path in the physical domain. The SWAM is a piecewise linear homeomorphism between the two spaces (Groff, 2003; Gupta & Wenger,

to infinity, on the second case. The edges have a direction of *θ*−<sup>∞</sup> and *θ*+<sup>∞</sup> respectively. The

Feedback Equivalence and Control of Mobile Robots Through a Scalable FPGA Architecture 409

*<sup>f</sup>*−∞(*x*)=(*w*<sup>1</sup> <sup>+</sup> *<sup>S</sup>* · (*<sup>x</sup>* <sup>−</sup> *<sup>a</sup>*1) · *<sup>e</sup>j*·*θ*−<sup>∞</sup> )*ψ*0, *<sup>f</sup>*+∞(*x*)=(*wn* <sup>+</sup> *<sup>S</sup>* · (*<sup>x</sup>* <sup>−</sup> *an*) · *<sup>e</sup>j*·*θ*+<sup>∞</sup> )*ψ<sup>n</sup>*

Here *ψ*<sup>0</sup> is an open-left pulse with a single falling edge at x=*a*1, and *ψ<sup>n</sup>* is the open-right pulse with a single rising edge at x=*an*. Combining the above and using the conventions *θ<sup>k</sup>* = *arg*(*wk*<sup>+</sup><sup>1</sup> − *wk*), *a*<sup>0</sup> = −∞, *an*+<sup>1</sup> = +∞, *w*<sup>0</sup> the point at infinity corresponding to *a*0, *wn*+<sup>1</sup> the point at infinity corresponding to *an*+1, *θ*<sup>0</sup> = *arg*(*w*<sup>1</sup> − *w*0) = *θ*−∞, *θ<sup>n</sup>* =

where the functions *f*−∞, *f*+∞, correspond to k=0 and k=n respectively. In order to extend this transformation to the entire complex plane, let *z* = *x* + *jy* be a complex variable in the

where *θ<sup>s</sup>* is the *shifting angle* in [-*π*/2, *π*/2].The complex variable *w* = *u* + *jv* in the physical domain, is identified with the transformation Φ(*z*), i.e. *w* = *u* + *jv* = Φ(*z*). This transformation is the *direct strip-wise affine map* and produces a linear displacement of the polygon along the direction *θs*. Each edge of the polygon produces an affine transformation that applies only in the "strip" that the edge sweeps as it is being translated. Thus, the transformation Φ(*z*) can be described as a "strip-wise affine" transformation. The invertibility of the map depends firstly on the geometry of the chain and secondly on the shifting angle. It can be shown (Moustris & Tzafestas, 2008) that necessary and sufficient conditions for the mapping to be invertible are that the chain must be a strictly monotone polygonal chain (Preparata & Supowit, 1981) and the shifting angle must not coincide with the angle of any of the chain's edges i.e. the chain must not be shifted along one of its edges. The inverse strip-wise affine map can be expressed in matrix form by treating Φ(*z*) as an **R**<sup>2</sup> to **R**<sup>2</sup> mapping since it cannot be solved analytically with respect to *z*. The inverse mapping equations can be

(*wk* <sup>+</sup> *<sup>S</sup>* · (*<sup>x</sup>* <sup>−</sup> *ak*) · *<sup>e</sup>j*·*θ<sup>k</sup>* )*ψ<sup>k</sup>* (26)

<sup>Φ</sup>(*z*) = *<sup>y</sup>* · *<sup>S</sup>* · *<sup>e</sup>j*·*θ<sup>s</sup>* <sup>+</sup> *<sup>f</sup>*(*x*) (27)

*arg*(*wn*+<sup>1</sup> − *wn*) = *θ*+∞, the extended transformation takes the following form,

*n* ∑ *k*=0

*f*(*x*) =

� *x y* �

*<sup>k</sup>* sin *<sup>θ</sup><sup>s</sup>* <sup>−</sup> *<sup>w</sup><sup>I</sup>*

− *n* ∑ *k*=0

⎡ ⎣ = A−<sup>1</sup>

sin *θ<sup>s</sup>* − cos *θ<sup>s</sup>*

*n* ∑ *k*=0

*ψ<sup>k</sup>* cos *θ<sup>k</sup>*

Besides Eq.28, one also needs to know the activated pulse since the rectangle pulses are functions of the variable *x*, and thus (28) does not provide a complete solution to the inversion problem. If this information is provided, the sums in Eq.28 degenerate and the equation provides the inverse system. The activated pulse can be calculated algorithmically by doing

*ψ<sup>k</sup>* sin *θ<sup>k</sup>*

� *u v* � − �

*<sup>k</sup>* cos *<sup>θ</sup>s*)*ψk*, *<sup>D</sup>* <sup>=</sup> *<sup>S</sup>* <sup>∑</sup>*<sup>n</sup>*

*C*/*J* − *ak* −*D*/*J*

*<sup>k</sup>*=<sup>0</sup> (*w<sup>R</sup>*

⎤ ⎦ /*S*

*<sup>k</sup>*=<sup>0</sup> sin(*θ<sup>s</sup>* <sup>−</sup> *<sup>θ</sup>k*)*ψk*. A−<sup>1</sup> is the inverse system matrix, i.e.

*n* ∑ *k*=0

�

*<sup>k</sup>* sin *<sup>θ</sup><sup>k</sup>* <sup>−</sup> *<sup>w</sup><sup>I</sup>*

canonical domain and consider the mapping,

calculated as,

where *C* = *S* ∑*<sup>n</sup>*

*<sup>k</sup>*=<sup>0</sup> (*w<sup>R</sup>*

A−<sup>1</sup> =

map Jacobian given by *J* = *S*<sup>2</sup> ∑*<sup>n</sup>*

(25)

(28)

*<sup>k</sup>* cos *θk*)*ψ<sup>k</sup>* and *J* is the

sin(*θ<sup>s</sup>* − *θk*)*ψ<sup>k</sup>* (29)

formulas that account for these branches are given by:

1997) . It acts by inducing a strip decomposition on the planes and then applying an affine map between them. Note that the map acts on the entire domains, not just on a bounded region.

Fig. 4. Illustration of the Strip-Wise Affine Map

In more rigorous terms, let *A* = {*w*1, *w*2,..., *wn*} , *wi* ∈ **C** be a polygonal chain on the *complex* physical domain. This represents the original reference path. Each vertex *wi* of the chain is projected to a point *ai* on the real axis in the canonical domain according to its normalized length,

$$a\_{\mathbf{i}} = \sum\_{k=1}^{\mathrm{i}} \frac{S\_{\mathbf{k}}}{S} \text{ , } \mathbf{i} = 1 \text{ } 2 \text{ } 3 \text{ } \dots \text{ } \mathbf{n} \tag{21}$$

where *Sk* <sup>=</sup> <sup>|</sup>*wk* <sup>−</sup> *wk*−1<sup>|</sup> , *<sup>S</sup>* <sup>=</sup> <sup>∑</sup>*<sup>n</sup> <sup>k</sup>*=<sup>1</sup> *Sk* and *<sup>S</sup>*<sup>1</sup> = 0. The polygon edge from *wk*−<sup>1</sup> to *wk* is linearly projected onto [*ak*−1, *ak*]. The transformation of [*ak*−1, *ak*] onto its respective polygon edge is done using the function

$$f\_{k-1}(\mathbf{x}) = w\_{k-1} + \mathbf{S} \cdot (\mathbf{x} - a\_{k-1}) \cdot e^{j \cdot \arg(w\_k - w\_{k-1})} \tag{22}$$

Each interval [*ak*−1, *ak*] is transformed by a respective transformation *<sup>f</sup>*1, *<sup>f</sup>*2, , *fn*−1. Now, consider the following rectangle pulse function on the canonical real axis,

$$\psi\_k = \begin{cases} 1 \text{ / x} \in [a\_{k'} a\_{k+1}) \\ 0 \text{ / elsewhere} \end{cases} \tag{23}$$

The pulse is a complex function of *<sup>z</sup>* = *<sup>x</sup>* + *jy* in the canonical domain. Each function *fk*−<sup>1</sup> is multiplied by the corresponding pulse and the products are summed to account for the general transformation that projects the interval [0,1) onto the polygonal chain,

$$f(\mathbf{x}) = \sum\_{k=1}^{n-1} f\_k(\mathbf{x}) \psi\_k \tag{24}$$

Extension to the intervals (-∞,0) and [1,+ ∞) can be performed by appending an edge that begins from infinity ending at *w*1, for the first case, and another edge starting from *wn* escaping 8 Will-be-set-by-IN-TECH

1997) . It acts by inducing a strip decomposition on the planes and then applying an affine map between them. Note that the map acts on the entire domains, not just on a bounded

In more rigorous terms, let *A* = {*w*1, *w*2,..., *wn*} , *wi* ∈ **C** be a polygonal chain on the *complex* physical domain. This represents the original reference path. Each vertex *wi* of the chain is projected to a point *ai* on the real axis in the canonical domain according to its normalized

linearly projected onto [*ak*−1, *ak*]. The transformation of [*ak*−1, *ak*] onto its respective polygon

Each interval [*ak*−1, *ak*] is transformed by a respective transformation *<sup>f</sup>*1, *<sup>f</sup>*2, , *fn*−1. Now,

The pulse is a complex function of *<sup>z</sup>* = *<sup>x</sup>* + *jy* in the canonical domain. Each function *fk*−<sup>1</sup> is multiplied by the corresponding pulse and the products are summed to account for the

> *n*−1 ∑ *k*=1

Extension to the intervals (-∞,0) and [1,+ ∞) can be performed by appending an edge that begins from infinity ending at *w*1, for the first case, and another edge starting from *wn* escaping

1 , *<sup>x</sup>* <sup>∈</sup> [*ak*, *ak*<sup>+</sup>1)

*<sup>S</sup>* , i <sup>=</sup> 1, 2,3, ..., n (21)

0 , elsewhere (23)

*fk*(*x*)*ψ<sup>k</sup>* (24)

*<sup>k</sup>*=<sup>1</sup> *Sk* and *<sup>S</sup>*<sup>1</sup> = 0. The polygon edge from *wk*−<sup>1</sup> to *wk* is

*fk*−1(*x*) = *wk*−<sup>1</sup> <sup>+</sup> *<sup>S</sup>* · (*<sup>x</sup>* <sup>−</sup> *ak*−1) · *<sup>e</sup>j*·arg(*wk*−*wk*−<sup>1</sup>) (22)

*ai* =

consider the following rectangle pulse function on the canonical real axis,

*ψ<sup>k</sup>* =

general transformation that projects the interval [0,1) onto the polygonal chain,

*f*(*x*) =

*i* ∑ *k*=1

*Sk*

region.

length,

Fig. 4. Illustration of the Strip-Wise Affine Map

where *Sk* <sup>=</sup> <sup>|</sup>*wk* <sup>−</sup> *wk*−1<sup>|</sup> , *<sup>S</sup>* <sup>=</sup> <sup>∑</sup>*<sup>n</sup>*

edge is done using the function

to infinity, on the second case. The edges have a direction of *θ*−<sup>∞</sup> and *θ*+<sup>∞</sup> respectively. The formulas that account for these branches are given by:

$$\begin{aligned} f\_{-\infty}(\mathbf{x}) &= (w\_1 + \mathbf{S} \cdot (\mathbf{x} - a\_1) \cdot \mathbf{e}^{l \cdot \theta\_{-\infty}}) \psi\_{0\prime} \\ f\_{+\infty}(\mathbf{x}) &= (w\_n + \mathbf{S} \cdot (\mathbf{x} - a\_n) \cdot \mathbf{e}^{j \cdot \theta\_{+\infty}}) \psi\_n \end{aligned} \tag{25}$$

Here *ψ*<sup>0</sup> is an open-left pulse with a single falling edge at x=*a*1, and *ψ<sup>n</sup>* is the open-right pulse with a single rising edge at x=*an*. Combining the above and using the conventions *θ<sup>k</sup>* = *arg*(*wk*<sup>+</sup><sup>1</sup> − *wk*), *a*<sup>0</sup> = −∞, *an*+<sup>1</sup> = +∞, *w*<sup>0</sup> the point at infinity corresponding to *a*0, *wn*+<sup>1</sup> the point at infinity corresponding to *an*+1, *θ*<sup>0</sup> = *arg*(*w*<sup>1</sup> − *w*0) = *θ*−∞, *θ<sup>n</sup>* = *arg*(*wn*+<sup>1</sup> − *wn*) = *θ*+∞, the extended transformation takes the following form,

$$f(\mathbf{x}) = \sum\_{k=0}^{n} \left( w\_k + \mathbf{S} \cdot (\mathbf{x} - a\_k) \cdot e^{j \cdot \theta\_k} \right) \psi\_k \tag{26}$$

where the functions *f*−∞, *f*+∞, correspond to k=0 and k=n respectively. In order to extend this transformation to the entire complex plane, let *z* = *x* + *jy* be a complex variable in the canonical domain and consider the mapping,

$$\Phi(z) = y \cdot \mathbb{S} \cdot e^{j \cdot \theta\_s} + f(x) \tag{27}$$

where *θ<sup>s</sup>* is the *shifting angle* in [-*π*/2, *π*/2].The complex variable *w* = *u* + *jv* in the physical domain, is identified with the transformation Φ(*z*), i.e. *w* = *u* + *jv* = Φ(*z*). This transformation is the *direct strip-wise affine map* and produces a linear displacement of the polygon along the direction *θs*. Each edge of the polygon produces an affine transformation that applies only in the "strip" that the edge sweeps as it is being translated. Thus, the transformation Φ(*z*) can be described as a "strip-wise affine" transformation. The invertibility of the map depends firstly on the geometry of the chain and secondly on the shifting angle. It can be shown (Moustris & Tzafestas, 2008) that necessary and sufficient conditions for the mapping to be invertible are that the chain must be a strictly monotone polygonal chain (Preparata & Supowit, 1981) and the shifting angle must not coincide with the angle of any of the chain's edges i.e. the chain must not be shifted along one of its edges. The inverse strip-wise affine map can be expressed in matrix form by treating Φ(*z*) as an **R**<sup>2</sup> to **R**<sup>2</sup> mapping since it cannot be solved analytically with respect to *z*. The inverse mapping equations can be calculated as,

$$
\begin{bmatrix} x \\ y \end{bmatrix} = \mathbf{A}^{-1} \begin{bmatrix} u \\ v \end{bmatrix} - \begin{bmatrix} \mathbf{C}/J - a\_k \\ -D/J \end{bmatrix} \tag{28}
$$

where *C* = *S* ∑*<sup>n</sup> <sup>k</sup>*=<sup>0</sup> (*w<sup>R</sup> <sup>k</sup>* sin *<sup>θ</sup><sup>s</sup>* <sup>−</sup> *<sup>w</sup><sup>I</sup> <sup>k</sup>* cos *<sup>θ</sup>s*)*ψk*, *<sup>D</sup>* <sup>=</sup> *<sup>S</sup>* <sup>∑</sup>*<sup>n</sup> <sup>k</sup>*=<sup>0</sup> (*w<sup>R</sup> <sup>k</sup>* sin *<sup>θ</sup><sup>k</sup>* <sup>−</sup> *<sup>w</sup><sup>I</sup> <sup>k</sup>* cos *θk*)*ψ<sup>k</sup>* and *J* is the map Jacobian given by *J* = *S*<sup>2</sup> ∑*<sup>n</sup> <sup>k</sup>*=<sup>0</sup> sin(*θ<sup>s</sup>* <sup>−</sup> *<sup>θ</sup>k*)*ψk*. A−<sup>1</sup> is the inverse system matrix, i.e.

$$\mathbf{A}^{-1} = \begin{bmatrix} \sin \theta\_{\mathrm{s}} & -\cos \theta\_{\mathrm{s}} \\ -\sum\_{k=0}^{n} \psi\_{k} \sin \theta\_{k} \sum\_{k=0}^{n} \psi\_{k} \cos \theta\_{k} \\ \end{bmatrix} / S \sum\_{k=0}^{n} \sin(\theta\_{\mathrm{s}} - \theta\_{k}) \psi\_{k} \tag{29}$$

Besides Eq.28, one also needs to know the activated pulse since the rectangle pulses are functions of the variable *x*, and thus (28) does not provide a complete solution to the inversion problem. If this information is provided, the sums in Eq.28 degenerate and the equation provides the inverse system. The activated pulse can be calculated algorithmically by doing

Now, let *pref* be a reference path in the physical domain and let *uc*(*qc*, *Ic*, *t*) be a straight line tracking controller for Σ*c*, where *Ic* denotes the line segment *yc* = {0/*xc* ∈ [0, 1]} of the C-plane, i.e. the reference path in the canonical domain. This controller is transformed to a

Feedback Equivalence and Control of Mobile Robots Through a Scalable FPGA Architecture 411

However, since Σ*<sup>c</sup>* is the Dubins Car in the canonical domain, the straight line tracking controller *uc*(*qc*, *Ic*, *t*) for Σ*<sup>c</sup>* is actually a straight line tracker for the Dubins Car, and in order to build a path tracker for the Dubins Car, one has but to build a straight line tracker and use Eq.(33) to promote it to a path tracker for strictly monotone polygonal chains. Furthermore, one could also use existing path trackers for the Dubins Car to track the straight line in the canonical domain. In this case, these controllers can be simplified since in general, straight

The controller used in this work for the path following task, is based on a Fuzzy Logic Controller (FLC) developed by the authors, which has been deployed in previous experiments. The original Fuzzy Logic tracker is described in (Moustris & Tzafestas, 2005), and further modified in (Deliparaschos et al., 2007) in order to be implemented on a FPGA. Specifically the tracker is a zero-order Takagi-Sugeno FLC with the two inputs partitioned in nine triangular membership functions each, while the output is partitioned in five singletons

Fig. 5. Input and output membership functions of the fuzzy controller

which are presented in Table 1. The implication operator is the min operator.

The FL tracker uses two angles as inputs, and outputs the control input *uκ*. It is assumed that the path is provided as a sequence of points on **R**2. The fuzzy rule base consists of 81 rules,

*up*(*qp*, *pref* , *t*) = Ω(*uc*(*qc*, *Ic*, *t*), *qc*) = Ω(*uc*(Ψ−1(*qp*), Φ−1(*pref*), *t*), Ψ−1(*qp*)) (33)

path tracking controller for Σ*<sup>p</sup>* under the equation,

line tracking is simpler than path tracking.

**4. Fuzzy Logic Controller**

(Fig. 5).

a point-in-strip test. Consider an axis orthogonal to the direction *θ<sup>s</sup>* such that the projection of *w*<sup>1</sup> corresponds to 0. Furthermore let the projections of each *wi* onto that axis be denoted as *bi*, and the projection of the current mapping point denoted as *bc*. The projections of *wi* apparently partition the axis into consecutive line segments [*bibi*+1] which are into one-to-one correspondence with the edges of the polygonal chain. Then, in order to find the current pulse one needs to find the segment into which the point *bc* resides. This can be performed optimally by a binary search algorithm in *O*(*logn*).

Since the SWAM transforms the robot's application space, it also affects its model's equations. Denoting all the state variables in the physical and canonical domains with a subscript of *p* and *c* respectively, then the *u-v* plane (physical domain) is mapped to the *x-y* plane (canonical domain), i.e. the state vector *qp* = [*xp*, *yp*, *θp*] *<sup>T</sup>* is transformed to *q*� *<sup>p</sup>* = [*xc*, *yc*, *θp*] *<sup>T</sup>* . The homeomorphism Φ defines an equivalence relation between the two states. Notice that the state *θ<sup>p</sup>* remains unaffected. By introducing a new extended homeomorphism Ψ that also maps the heading angle *θp*, one can send the *canonical state-space* to the *physical state-space*, i.e. *qp* = Ψ(*qc*). This transformation acts on all state variables and the new system state is *qc* = [*xc*, *yc*, *θc*] *<sup>T</sup>*. The map is then defined by,

$$
\begin{bmatrix} x\_p \\ y\_p \\ \theta\_p \end{bmatrix} = \begin{bmatrix} y\_\mathcal{C} S \cos \theta\_\mathcal{s} + \text{Re}(f(\mathbf{x}\_\mathcal{c})) \\ y\_\mathcal{C} S \sin \theta\_\mathcal{s} + \text{Im}(f(\mathbf{x}\_\mathcal{c})) \\ \tan^{-1}(\frac{\sum\_{\kappa=0}^n \sin \theta\_\kappa \psi\_\kappa + \sin \theta\_\mathcal{s} \tan \theta\_\mathcal{c}}{\sum\_{\kappa=0}^n \cos \theta\_\kappa \psi\_\kappa + \cos \theta\_\mathcal{s} \tan \theta\_\mathcal{c}}) \end{bmatrix} = \boldsymbol{\Psi}(q\_\mathcal{c}) \tag{30}
$$

and the new system is,

$$
\tilde{\Sigma}: \begin{bmatrix} \dot{\mathbf{x}}\_{\mathcal{L}} \\ \dot{y}\_{\mathcal{C}} \\ \dot{\theta}\_{\mathcal{C}} \end{bmatrix} = \begin{bmatrix} v\_{\mathcal{C}} \cos \theta\_{\mathcal{C}} \\ v\_{\mathcal{C}} \sin \theta\_{\mathcal{C}} \\ 0 \end{bmatrix} + \begin{bmatrix} 0 \\ 0 \\ S^3 \gamma^3 J^{-1} v\_{\mathcal{C}} \end{bmatrix} \kappa\_p \tag{31}
$$

*J* is the Jacobian of Φ and *γ* = �1 + sin 2*θ<sup>c</sup>* ∑*<sup>n</sup> <sup>κ</sup>*=<sup>0</sup> cos(*θ<sup>s</sup>* − *θκ* )*ψκ*. The input *κ<sup>p</sup>* of the system remains unaffected. However, since it expresses the curvature of the physical system, it can also be transformed under Ψ. Thus by including the transformation of the input and extending the homeomorphism Ψ to Ψˆ = (Ψ, Ω) , where

$$\kappa\_p = \Omega(\kappa\_\circ \, q\_\circ) = \mathcal{S}^{-3} I \gamma^{-3} \kappa\_\circ$$

is the input map that sends the controls from the canonical input space to the controls in the physical input space, one gets the new extended system,

$$\Sigma\_{\mathbb{C}} : \begin{bmatrix} \dot{\boldsymbol{x}}\_{\mathbb{C}} \\ \dot{\boldsymbol{y}}\_{\mathbb{C}} \\ \dot{\boldsymbol{\theta}}\_{\mathbb{C}} \end{bmatrix} = \begin{bmatrix} \boldsymbol{v}\_{\mathbb{C}} \cos \theta\_{\mathbb{C}} \\ \boldsymbol{v}\_{\mathbb{C}} \sin \theta\_{\mathbb{C}} \\ \boldsymbol{0} \end{bmatrix} + \begin{bmatrix} \boldsymbol{0} \\ \boldsymbol{0} \\ \boldsymbol{v}\_{\mathbb{C}} \end{bmatrix} \kappa\_{\mathbb{C}} \tag{32}$$

The systems Σ*<sup>p</sup>* and Σ*<sup>c</sup>* are feedback-equivalent (Gardner & Shadwick, 1987; 1990) since they are related by a state and input transformation. The input transformation is actually a feedback transformation of Σ*<sup>c</sup>* that feeds the states *qc* back to the input. A more careful look at Eq.(32) shows that it expresses the Dubins Car in the canonical domain, thus Ψˆ presents a kind of *form invariance* on the model.

Now, let *pref* be a reference path in the physical domain and let *uc*(*qc*, *Ic*, *t*) be a straight line tracking controller for Σ*c*, where *Ic* denotes the line segment *yc* = {0/*xc* ∈ [0, 1]} of the C-plane, i.e. the reference path in the canonical domain. This controller is transformed to a path tracking controller for Σ*<sup>p</sup>* under the equation,

$$u\_p(q\_{p\prime}p\_{ref},t) = \Omega(u\_\varepsilon(q\_{\varepsilon\prime}I\_{\varepsilon\prime}t),q\_\varepsilon) = \Omega(u\_\varepsilon(\Psi^{-1}(q\_p)\_\prime\Phi^{-1}(p\_{ref})\_\prime t), \Psi^{-1}(q\_p))\tag{33}$$

However, since Σ*<sup>c</sup>* is the Dubins Car in the canonical domain, the straight line tracking controller *uc*(*qc*, *Ic*, *t*) for Σ*<sup>c</sup>* is actually a straight line tracker for the Dubins Car, and in order to build a path tracker for the Dubins Car, one has but to build a straight line tracker and use Eq.(33) to promote it to a path tracker for strictly monotone polygonal chains. Furthermore, one could also use existing path trackers for the Dubins Car to track the straight line in the canonical domain. In this case, these controllers can be simplified since in general, straight line tracking is simpler than path tracking.

#### **4. Fuzzy Logic Controller**

10 Will-be-set-by-IN-TECH

a point-in-strip test. Consider an axis orthogonal to the direction *θ<sup>s</sup>* such that the projection of *w*<sup>1</sup> corresponds to 0. Furthermore let the projections of each *wi* onto that axis be denoted as *bi*, and the projection of the current mapping point denoted as *bc*. The projections of *wi* apparently partition the axis into consecutive line segments [*bibi*+1] which are into one-to-one correspondence with the edges of the polygonal chain. Then, in order to find the current pulse one needs to find the segment into which the point *bc* resides. This can be performed

Since the SWAM transforms the robot's application space, it also affects its model's equations. Denoting all the state variables in the physical and canonical domains with a subscript of *p* and *c* respectively, then the *u-v* plane (physical domain) is mapped to the *x-y* plane (canonical

homeomorphism Φ defines an equivalence relation between the two states. Notice that the state *θ<sup>p</sup>* remains unaffected. By introducing a new extended homeomorphism Ψ that also maps the heading angle *θp*, one can send the *canonical state-space* to the *physical state-space*, i.e. *qp* = Ψ(*qc*). This transformation acts on all state variables and the new system state is

> *ycS* cos *θ<sup>s</sup>* + Re(*f*(*xc*)) *ycS* sin *θ<sup>s</sup>* + Im(*f*(*xc*))

*<sup>κ</sup>*=<sup>0</sup> sin *θκψκ* + sin *θ<sup>s</sup>* tan *θ<sup>c</sup>*

*<sup>κ</sup>*=<sup>0</sup> cos *θκψκ* + cos *θ<sup>s</sup>* tan *θ<sup>c</sup>*

⎡ ⎢ ⎣

0 0 *S*3*γ*<sup>3</sup> *J*

<sup>−</sup>1*vc*

⎤ ⎥ <sup>⎦</sup> <sup>+</sup>

remains unaffected. However, since it expresses the curvature of the physical system, it can also be transformed under Ψ. Thus by including the transformation of the input and

*κ<sup>p</sup>* = Ω(*κc*, *qc*) = *S*−<sup>3</sup> *Jγ*−3*κ<sup>c</sup>*

is the input map that sends the controls from the canonical input space to the controls in the

The systems Σ*<sup>p</sup>* and Σ*<sup>c</sup>* are feedback-equivalent (Gardner & Shadwick, 1987; 1990) since they are related by a state and input transformation. The input transformation is actually a feedback transformation of Σ*<sup>c</sup>* that feeds the states *qc* back to the input. A more careful look at Eq.(32) shows that it expresses the Dubins Car in the canonical domain, thus Ψˆ presents a

*vc* cos *θ<sup>c</sup> vc* sin *θ<sup>c</sup>* 0

⎤ ⎥ <sup>⎦</sup> <sup>+</sup> ⎡ ⎢ ⎣

0 0 *vc* ⎤ ⎥ ⎦

*<sup>T</sup>* is transformed to *q*�

)

⎤ ⎥ ⎦

*<sup>κ</sup>*=<sup>0</sup> cos(*θ<sup>s</sup>* − *θκ* )*ψκ*. The input *κ<sup>p</sup>* of the system

⎤ ⎥ ⎥ ⎥ *<sup>p</sup>* = [*xc*, *yc*, *θp*]

<sup>⎦</sup> <sup>=</sup> <sup>Ψ</sup>(*qc*) (30)

*κ<sup>p</sup>* (31)

*κ<sup>c</sup>* (32)

*<sup>T</sup>* . The

optimally by a binary search algorithm in *O*(*logn*).

domain), i.e. the state vector *qp* = [*xp*, *yp*, *θp*]

⎡ ⎣ *xp yp θp* ⎤ ⎦ =

*<sup>T</sup>*. The map is then defined by,

tan−1( <sup>∑</sup>*<sup>n</sup>*

∑*n*

⎡ ⎢ ⎣

*vc* cos *θ<sup>c</sup> vc* sin *θ<sup>c</sup>* 0

⎡ ⎢ ⎢ ⎢ ⎣

Σ˜ :

extending the homeomorphism Ψ to Ψˆ = (Ψ, Ω) , where

physical input space, one gets the new extended system,

Σ*<sup>c</sup>* :

⎡ ⎢ ⎣ *x*˙*c y*˙*c* ˙ *θc*

⎤ ⎥ <sup>⎦</sup> <sup>=</sup> ⎡ ⎢ ⎣

*J* is the Jacobian of Φ and *γ* = �1 + sin 2*θ<sup>c</sup>* ∑*<sup>n</sup>*

kind of *form invariance* on the model.

⎡ ⎢ ⎣ *x*˙*c y*˙*c* ˙ *θc*

⎤ ⎥ <sup>⎦</sup> <sup>=</sup>

*qc* = [*xc*, *yc*, *θc*]

and the new system is,

The controller used in this work for the path following task, is based on a Fuzzy Logic Controller (FLC) developed by the authors, which has been deployed in previous experiments. The original Fuzzy Logic tracker is described in (Moustris & Tzafestas, 2005), and further modified in (Deliparaschos et al., 2007) in order to be implemented on a FPGA. Specifically the tracker is a zero-order Takagi-Sugeno FLC with the two inputs partitioned in nine triangular membership functions each, while the output is partitioned in five singletons (Fig. 5).

Fig. 5. Input and output membership functions of the fuzzy controller

The FL tracker uses two angles as inputs, and outputs the control input *uκ*. It is assumed that the path is provided as a sequence of points on **R**2. The fuzzy rule base consists of 81 rules, which are presented in Table 1. The implication operator is the min operator.

S ahead of the one nearest to the robot (point P). Of course, the nearest point P can be easily found since its coordinates are (*xc*, 0), where *xc* is the x-axis coordinate of the robot in the canonical space. In the case of a constant S, the angle *φ* varies from [−*π*/2, *π*/2] and the tuple (*φ*1, *φ*2) is constrained in a strip. This can also lead to a rule reduction in the FLC rule base since some rules are never activated ((Moustris & Tzafestas, 2011)). For the control of the robot, the FLC and the SWAM were calculated on-line using a dedicated FPGA SoC, as

Feedback Equivalence and Control of Mobile Robots Through a Scalable FPGA Architecture 413

This section discusses the System on Chip (SoC) implemented on an FPGA chip for the robot path tracking task using fuzzy logic. The SoC design was implemented on the Spartan-3 MB development kit (DS-KIT-3SMB1500) by Digilent Inc. The Spartan-3 MB system board utilizes the 1.5 million-gate Xilinx Spartan-3 device (XC3S1500-4FG676) in the 676-pin fine-grid array package. A high level and a detailed architecture view of the SoC is shown in Fig.7 and 8

A design on an FPGA could be thought as a "hard" implementation of program execution. The Processor based systems often involve several layers of abstraction to help schedule tasks and share resources among multiple processes. The driver layer controls hardware resources and the operating system manages memory and processor bandwidth. Any given processor core can execute only one instruction at a time, and processor based systems are

described in the next section.

**5. DFLC & SoC architecture**

Fig. 7. Overview of the SoC's hardware architecture

respectively.


**pvb pbig pmid ps zero ns nmid nbig nvb**

Table 1. FLC Rule Base

In each control loop the closest path point is picked up and the two input angles are calculated. These angles are the angle *φ*<sup>1</sup> of the closest point with respect to the current robot heading and the direction *φ*<sup>2</sup> of the tangent of the path at that point, as depicted in Fig. 6a. Using the SWAM, we can move the tracking task to the canonical domain where the path is a straight line. In this case, the oriented straight line splits the plane into two half-planes, which present two general cases for the placement of the robot. Due to the symmetry of the cases only one will be analyzed. Consider that the robot resides in the positive half-plane (Fig. 6b) and that the distance from the closest path point P is D.

Fig. 6. Illustration of the controller inputs for path tracking in the general case (a) and in the straight line case (b).

Furthermore, one can consider that the robot tracks not the closest point P but the one found distance S ahead. The "sightline" of the robot to that point forms an angle *φ* with the path. In this situation, by varying the angle *φ*<sup>2</sup> one can discern four cases for the relation between the angles *φ*, *φ*<sup>1</sup> and *φ*<sup>2</sup> with the three of them being the same, namely,

$$\begin{aligned} \varphi\_1 - \varphi\_2 &= -\varphi, \varphi\_2 \in [-\pi + \varrho, 0] \cup [0, \varrho] \cup [\varrho, \pi] \\ \varphi\_1 - \varphi\_2 &= 2\pi - \varrho, \varphi\_2 \in [-\pi, -\pi + \varrho] \end{aligned} \tag{34}$$

When the robot resides in the positive half-plane, the angle *φ* is also positive. On the contrary, when it is in the negative half-plane, the angle changes sign although Eqs.(34) remain the same. With respect to the point being tracked, we discern two cases; either fixing the sightline, i.e. fixing the angle *φ*, or fixing the look-ahead distance S, i.e. tracking the point that is distance S ahead of the one nearest to the robot (point P). Of course, the nearest point P can be easily found since its coordinates are (*xc*, 0), where *xc* is the x-axis coordinate of the robot in the canonical space. In the case of a constant S, the angle *φ* varies from [−*π*/2, *π*/2] and the tuple (*φ*1, *φ*2) is constrained in a strip. This can also lead to a rule reduction in the FLC rule base since some rules are never activated ((Moustris & Tzafestas, 2011)). For the control of the robot, the FLC and the SWAM were calculated on-line using a dedicated FPGA SoC, as described in the next section.

#### **5. DFLC & SoC architecture**

12 Will-be-set-by-IN-TECH

In each control loop the closest path point is picked up and the two input angles are calculated. These angles are the angle *φ*<sup>1</sup> of the closest point with respect to the current robot heading and the direction *φ*<sup>2</sup> of the tangent of the path at that point, as depicted in Fig. 6a. Using the SWAM, we can move the tracking task to the canonical domain where the path is a straight line. In this case, the oriented straight line splits the plane into two half-planes, which present two general cases for the placement of the robot. Due to the symmetry of the cases only one will be analyzed. Consider that the robot resides in the positive half-plane (Fig. 6b) and that

Fig. 6. Illustration of the controller inputs for path tracking in the general case (a) and in the

Furthermore, one can consider that the robot tracks not the closest point P but the one found distance S ahead. The "sightline" of the robot to that point forms an angle *φ* with the path. In this situation, by varying the angle *φ*<sup>2</sup> one can discern four cases for the relation between the

*ϕ*<sup>1</sup> − *ϕ*<sup>2</sup> = −*ϕ* ,*ϕ*<sup>2</sup> ∈ [−*π* + *ϕ*, 0] ∪ [0, *ϕ*] ∪ [*ϕ*, *π*]

When the robot resides in the positive half-plane, the angle *φ* is also positive. On the contrary, when it is in the negative half-plane, the angle changes sign although Eqs.(34) remain the same. With respect to the point being tracked, we discern two cases; either fixing the sightline, i.e. fixing the angle *φ*, or fixing the look-ahead distance S, i.e. tracking the point that is distance

*<sup>ϕ</sup>*<sup>1</sup> <sup>−</sup> *<sup>ϕ</sup>*<sup>2</sup> <sup>=</sup> <sup>2</sup>*<sup>π</sup>* <sup>−</sup> *<sup>ϕ</sup>* ,*ϕ*<sup>2</sup> <sup>∈</sup> [−*π*, <sup>−</sup>*<sup>π</sup>* <sup>+</sup> *<sup>ϕ</sup>*] (34)

angles *φ*, *φ*<sup>1</sup> and *φ*<sup>2</sup> with the three of them being the same, namely,

Table 1. FLC Rule Base

straight line case (b).

the distance from the closest path point P is D.

**pvb pbig pmid ps zero ns nmid nbig nvb p180** pb pb pb pm ze nm nb nb pb **p135** pb pb pb pb pm nm nb Pb pb **p90** pb pb pb pm pm pm pb pb Pb **p45** pb pb pb pm pm ze nb pb pb **z** pb pb Pb pm ze nm nb nb nb **n45** nb nb pb ze nm nb nb nb nb **n90** nb nb nb nm nm nb nb nb nb **n135** nb nb pb pm nm nb nb nb nb **n180** pb pb pb pm zero nm nb nb pb

This section discusses the System on Chip (SoC) implemented on an FPGA chip for the robot path tracking task using fuzzy logic. The SoC design was implemented on the Spartan-3 MB development kit (DS-KIT-3SMB1500) by Digilent Inc. The Spartan-3 MB system board utilizes the 1.5 million-gate Xilinx Spartan-3 device (XC3S1500-4FG676) in the 676-pin fine-grid array package. A high level and a detailed architecture view of the SoC is shown in Fig.7 and 8 respectively.

Fig. 7. Overview of the SoC's hardware architecture

A design on an FPGA could be thought as a "hard" implementation of program execution. The Processor based systems often involve several layers of abstraction to help schedule tasks and share resources among multiple processes. The driver layer controls hardware resources and the operating system manages memory and processor bandwidth. Any given processor core can execute only one instruction at a time, and processor based systems are

Fig. 9. Illustration of HW vs SW computation process

output with 8-bit and 12-bit degree of truth resolution respectively.

experiments of the tracking scheme.

order to reduce the computation time.

tracking algorithm, and a Xilinx's Microblaze soft processor core which acts as the top level flow controller. The FPGA board hosting the SoC controls the Kephera robot, used in the

Feedback Equivalence and Control of Mobile Robots Through a Scalable FPGA Architecture 415

In our application the DFLC facilitates scaling and can be configured for different number of inputs and outputs, number of triangular or trapezoidal fuzzy sets per input, number of singletons per output, antecedent method (t-norm, s-norm), divider type, and number of pipeline registers for the various components in the model. This parametrization enabled the creation of a generic DFLC soft IP core that was used to produce a fuzzy controller of different specifications without the need of redesigning the IP from the beginning. The fuzzy logic controller architecture assumes overlap of two fuzzy sets among adjoining fuzzy sets, and requires 2*<sup>n</sup>* (*n* is the number of inputs) clock cycles at the core frequency speed in order to sample the input data (input sample rate of 78.2960ns), since it processes one active rule per clock cycle. In its present form the SoC design achieves a core frequency speed of 51.1 MHz. To achieve this timing result, the latency of the chip architecture involves 9 pipeline stages each one requiring 19.574ns. The featured DFLC IP is based on a simple algorithm similar to the zero-order Takagi-Sugeno inference scheme and the weighted average defuzzification method. By using the chosen parameters of Table 2, it employs two 12-bit inputs and one 12-bit output, 9 triangular membership functions (MFs) per input and 5 singleton MFs at the

The FPGA SoC implements the autonomous control logic of the Kephera II robot. It receives odometry information from the robot and issues steering commands output by the FL tracker. The encoding and decoding of the information packets (i.e., encoding of steering control into data frames) is handled by the MATLAB application. Therefore the MATLAB application implements the actual framer/deframer for the I/O communication with the Kephera robot and downloads the tracking path to the SoC. The top-level program that supervises these tasks, treats synchronization and timing requirements, is written in C and executes in the Microblaze soft processor core. The SWAM algorithm is also implemented on the FPGA, in

The MATLAB application displays information about the robot's pose and speed, as well as some other data used for the path tracking control. It also calculates the robot's position relative to the world and the local coordinate frames. Another important function of the MATLAB application is to provide a path for the robot to track. The current work deals only

Fig. 8. Architecture of the DFLC IP core

continually at risk of time critical tasks preempting one another. FPGAs on the other hand do not use operating systems and minimize reliability concerns with true parallel execution and deterministic hardware dedicated to every task (see Fig.9).

Today's FPGAs contain hundreds of powerful DSP slices with up to 4.7 Tera-MACS throughput; 2 million logic cells with clock speeds of up to 600MHz, and up to 2.4 Tera-bps high-speed on-chip bandwidth capable to outperform DSP and RISC processors by a factor of 100 to 1,000. Taking advantage of hardware parallelism, FPGAs exceed the computing power of digital signal processors (DSPs) by breaking the paradigm of sequential execution and accomplishing more per clock cycle.

The main unit of the SoC is a parametrized Digital Fuzzy Logic Controller (DFLC) soft IP core Deliparaschos et al. (2006)Deliparaschos & Tzafestas (2006) that implements the fuzzy

Fig. 9. Illustration of HW vs SW computation process

14 Will-be-set-by-IN-TECH

continually at risk of time critical tasks preempting one another. FPGAs on the other hand do not use operating systems and minimize reliability concerns with true parallel execution and

Today's FPGAs contain hundreds of powerful DSP slices with up to 4.7 Tera-MACS throughput; 2 million logic cells with clock speeds of up to 600MHz, and up to 2.4 Tera-bps high-speed on-chip bandwidth capable to outperform DSP and RISC processors by a factor of 100 to 1,000. Taking advantage of hardware parallelism, FPGAs exceed the computing power of digital signal processors (DSPs) by breaking the paradigm of sequential execution

The main unit of the SoC is a parametrized Digital Fuzzy Logic Controller (DFLC) soft IP core Deliparaschos et al. (2006)Deliparaschos & Tzafestas (2006) that implements the fuzzy

Fig. 8. Architecture of the DFLC IP core

and accomplishing more per clock cycle.

deterministic hardware dedicated to every task (see Fig.9).

tracking algorithm, and a Xilinx's Microblaze soft processor core which acts as the top level flow controller. The FPGA board hosting the SoC controls the Kephera robot, used in the experiments of the tracking scheme.

In our application the DFLC facilitates scaling and can be configured for different number of inputs and outputs, number of triangular or trapezoidal fuzzy sets per input, number of singletons per output, antecedent method (t-norm, s-norm), divider type, and number of pipeline registers for the various components in the model. This parametrization enabled the creation of a generic DFLC soft IP core that was used to produce a fuzzy controller of different specifications without the need of redesigning the IP from the beginning. The fuzzy logic controller architecture assumes overlap of two fuzzy sets among adjoining fuzzy sets, and requires 2*<sup>n</sup>* (*n* is the number of inputs) clock cycles at the core frequency speed in order to sample the input data (input sample rate of 78.2960ns), since it processes one active rule per clock cycle. In its present form the SoC design achieves a core frequency speed of 51.1 MHz. To achieve this timing result, the latency of the chip architecture involves 9 pipeline stages each one requiring 19.574ns. The featured DFLC IP is based on a simple algorithm similar to the zero-order Takagi-Sugeno inference scheme and the weighted average defuzzification method. By using the chosen parameters of Table 2, it employs two 12-bit inputs and one 12-bit output, 9 triangular membership functions (MFs) per input and 5 singleton MFs at the output with 8-bit and 12-bit degree of truth resolution respectively.

The FPGA SoC implements the autonomous control logic of the Kephera II robot. It receives odometry information from the robot and issues steering commands output by the FL tracker. The encoding and decoding of the information packets (i.e., encoding of steering control into data frames) is handled by the MATLAB application. Therefore the MATLAB application implements the actual framer/deframer for the I/O communication with the Kephera robot and downloads the tracking path to the SoC. The top-level program that supervises these tasks, treats synchronization and timing requirements, is written in C and executes in the Microblaze soft processor core. The SWAM algorithm is also implemented on the FPGA, in order to reduce the computation time.

The MATLAB application displays information about the robot's pose and speed, as well as some other data used for the path tracking control. It also calculates the robot's position relative to the world and the local coordinate frames. Another important function of the MATLAB application is to provide a path for the robot to track. The current work deals only

a co-processor and is connected to the Microblaze via the FSL bus. The architecture of the present SoC consists mainly of the DFLP that communicates with the Microblaze Processor through the Fast Simplex Bus (FSL), the utilized block RAMs (BRAM) through the LMB bus, and other peripherals such as the general purpose input/output ports (GPIO), and UART modules via the OPB bus. Here, the DFLP incorporates the fuzzy tracking algorithm, whereas

Feedback Equivalence and Control of Mobile Robots Through a Scalable FPGA Architecture 417

The parametrized zero-order TSK type Fuzzy Logic Controller (FLC) core exchanges data with the MicroBlaze processor via the FSL bus. The scope of the FLC core is to serve as high-speed fuzzy inference co-processor to the Microblaze. The DFLC core was implemented with the

> **Property Value** Inputs 2 Input resolution 12 Outputs 1 Output resolution 12 bit Antecedent Membership Functions (MF's) 9 Triangular MF's Degree of Truth resolution 8 bit

> > Consequent MF's 5 Singletons MF resolution 8 bit

Defuzzification method Weighted average

Number of fuzzy inference rules 81 Rule activation method MIN Aggregation method SUM Implication method PROD MF overlapping degree 2

Besides these two main cores and buses, the design consists of 16 KB local memory, 32 MB DDR, timer, interrupt controller, a UART, a debug peripheral (MDM) and a couple of General Purpose Inputs/Outputs (GPIOs). A Multi-CHannel (MCH) On-chip Peripheral Bus (OPB) Double Data Rate (DDR) Synchronous DRAM (SDRAM) memory controller (MCH OPB DDR with support for asynchronous DDR clock) is used in this design. This allows the MicroBlaze system to run at a lower speed of 51 MHz, which is more reasonable for Spartan-3, while the DDR is running at 75 MHz, which is the minimum required frequency for the Micron DDR chip. The on-chip Digital Clock Manager (DCM) is used to create the various clock frequencies and phases required to make this system work, all based on the 75 MHz oscillator on the 3SMB board. The FLC core runs at the same speed as the OPB and MicroBlaze, which is 51 MHz. Based on the place and route report, the design occupies 4174 out of 13312 slices of the Xilinx

On the beginning of the co-design process one starts with an architecture independent description of the system functionality. Since the description of the system functionality is independent of the HW and SW, several system modelling representations may be used, such as finite state machines (FSMs) for example. The modelled system can then be described by means of a high level language, which is next compiled into an internal representation

the Microblaze processor mainly executes the C code for the flow control.

following parameters (see Table3).

Table 3. DFLC core characteristics

Spartan 3 FPGA (XC3S1500-4FG676).

**6. Hardware/software co-design**


Table 2. DFLC soft IP core chosen parameters

with the path tracking task and not path planning. To compensate for this, the path is drawn in MATLAB, encoded properly and downloaded to the SoC. Then, the SoC begins the tracking control.

The Microblaze soft processor core is licensed as part of the Xilinx Embedded Development Kit (EDK) and is a soft core, meaning that it is implemented using general logic primitives rather than a hard dedicated block in the FPGA. The Microblaze is based on a RISC architecture which is very similar to the DLX architecture described in (Patterson & Hennessy, 1997)(Sailer et al., 1996). It features a 3-stage pipeline with most instruction completing in a single cycle. Both the instruction and data words are 32 bits. The core alone can obtain a speed of up to 100MHz on the Spartan 3 FPGA family. The Microblaze processor can connect to the OPB bus for access to a wide range of different modules, it can communicate via the LMB bus for a fast access to local memory, normally block RAM (BRAM) inside the FPGA.

Moreover, the Fast Simplex Link (FSL) offers the ability to connect user soft core IP's acting as co-processors to accelerate time critical algorithms. The FSL channels are dedicated unidirectional point-to-point data streaming interfaces. Each FSL channel provides a low latency interface to the processor pipeline allowing extending the processor's execution unit with custom soft core co-processors. In this work the DFLP IP core is playing the role of such a co-processor and is connected to the Microblaze via the FSL bus. The architecture of the present SoC consists mainly of the DFLP that communicates with the Microblaze Processor through the Fast Simplex Bus (FSL), the utilized block RAMs (BRAM) through the LMB bus, and other peripherals such as the general purpose input/output ports (GPIO), and UART modules via the OPB bus. Here, the DFLP incorporates the fuzzy tracking algorithm, whereas the Microblaze processor mainly executes the C code for the flow control.

The parametrized zero-order TSK type Fuzzy Logic Controller (FLC) core exchanges data with the MicroBlaze processor via the FSL bus. The scope of the FLC core is to serve as high-speed fuzzy inference co-processor to the Microblaze. The DFLC core was implemented with the following parameters (see Table3).


Table 3. DFLC core characteristics

16 Will-be-set-by-IN-TECH

ip\_no 2 Number of inputs ip\_sz 12 Input bus resolution (bits) op\_no 1 Number of outputs op\_sz 12 Output bus resolution (bits) MF\_ip\_no 9 Number of input MFs (same for all inputs) dy\_ip 8 Input MFs degree of truth resolution (bits) MF\_op\_no 5 Number of output MFs (singletons) MF\_op\_sz 12 Output MFs resolution (bits) sel\_op 0 Antecedent method connection:

div\_type 1 Divider Model:

cpr1\_no 1 addr\_gen\_p cpr2\_no 1 cons\_map\_p cpr3\_no 3 trap\_gen\_p cpr4\_no 0 rule\_sel\_p cpr5\_no 2 minmax\_p cpr6\_no 1 mult cpr7\_no 0 int\_uns cpr8\_no 0 int\_sig cpr9\_no 2 div\_array

with the path tracking task and not path planning. To compensate for this, the path is drawn in MATLAB, encoded properly and downloaded to the SoC. Then, the SoC begins the tracking

The Microblaze soft processor core is licensed as part of the Xilinx Embedded Development Kit (EDK) and is a soft core, meaning that it is implemented using general logic primitives rather than a hard dedicated block in the FPGA. The Microblaze is based on a RISC architecture which is very similar to the DLX architecture described in (Patterson & Hennessy, 1997)(Sailer et al., 1996). It features a 3-stage pipeline with most instruction completing in a single cycle. Both the instruction and data words are 32 bits. The core alone can obtain a speed of up to 100MHz on the Spartan 3 FPGA family. The Microblaze processor can connect to the OPB bus for access to a wide range of different modules, it can communicate via the LMB bus

Moreover, the Fast Simplex Link (FSL) offers the ability to connect user soft core IP's acting as co-processors to accelerate time critical algorithms. The FSL channels are dedicated unidirectional point-to-point data streaming interfaces. Each FSL channel provides a low latency interface to the processor pipeline allowing extending the processor's execution unit with custom soft core co-processors. In this work the DFLP IP core is playing the role of such

for a fast access to local memory, normally block RAM (BRAM) inside the FPGA.

Table 2. DFLC soft IP core chosen parameters

control.

**PSR Signal Path Route** psr1\_no 1 ip\_set→psr1\_no→trap\_gen\_p psr2\_no 4 s\_rom→psr2\_no→mult psr3\_no 1 s\_rom→psr3\_no→rul\_sel\_p psr4\_no 1 cpr5→psr→int\_uns **CPR Component (Entity) Name**

0: min, 1: prod, 2: max, 3: probor

0: restoring array, 1: LUT reciprocal approx.

**Parameters (VHDL generics) Value Generic Description**

Besides these two main cores and buses, the design consists of 16 KB local memory, 32 MB DDR, timer, interrupt controller, a UART, a debug peripheral (MDM) and a couple of General Purpose Inputs/Outputs (GPIOs). A Multi-CHannel (MCH) On-chip Peripheral Bus (OPB) Double Data Rate (DDR) Synchronous DRAM (SDRAM) memory controller (MCH OPB DDR with support for asynchronous DDR clock) is used in this design. This allows the MicroBlaze system to run at a lower speed of 51 MHz, which is more reasonable for Spartan-3, while the DDR is running at 75 MHz, which is the minimum required frequency for the Micron DDR chip. The on-chip Digital Clock Manager (DCM) is used to create the various clock frequencies and phases required to make this system work, all based on the 75 MHz oscillator on the 3SMB board. The FLC core runs at the same speed as the OPB and MicroBlaze, which is 51 MHz. Based on the place and route report, the design occupies 4174 out of 13312 slices of the Xilinx Spartan 3 FPGA (XC3S1500-4FG676).

#### **6. Hardware/software co-design**

On the beginning of the co-design process one starts with an architecture independent description of the system functionality. Since the description of the system functionality is independent of the HW and SW, several system modelling representations may be used, such as finite state machines (FSMs) for example. The modelled system can then be described by means of a high level language, which is next compiled into an internal representation

core. The DFLC core is scalable in terms of the number of inputs/bus resolution, number of input/output fuzzy sets per input and membership resolution. More specifically A VHDL package stores the above generic parameters together with the number of necessary pipeline stages for each block. An RTL simulation was performed to ensure the correct functionality of the fuzzy controller. The DFLC core was independently synthesized with Synopsys Synplify logic synthesis tool (as it produced better synthesis results and meet timing constraints), whereas the rest of the SoC cores were synthesised with Xilinx synthesis tool XST. The Xilinx EDK studio was used for the integration flow of different SoC cores (i.e., DFLC, Microblaze, etc) and Xilinx ISE tool for the placement and routing of the SoC on the FPGA. More analytically, the place and route tool accepts the input netlist file (.edf), previously created by the synthesis tool and goes through the following steps. First, the translation program translates the input netlist together with the design constraints to a database file. After the translation program has run successfully, the logical design is mapped to the Xilinx FPGA device. Lastly, the the mapped design is placed and routed onto the chosen FPGA family and a device configuration file (bitstream) is created. Xilinx's SDK used for C programming and debugging the SoC's Microblaze soft processor. RTL and timing simulation to verify the

Feedback Equivalence and Control of Mobile Robots Through a Scalable FPGA Architecture 419

correct functionality was handled with the use of Mentor's Modelsim simulator.

The Component Pipeline Registers (CPR) blocks in Fig.8 indicate the number of pipeline stages for each component; the Path Synchronization Registers (PSR) blocks point to registers used for synchronizing the data paths, while the "U" blocks represent the different

The U\_fpga\_fc component is embedded in the flc\_ip top structural entity wrapper which is compliant with the FSL standard and provides all the necessary peripheral logic to the DFLC soft IP core in order to send/receive data to/from the FSL bus. The flc\_ip wrapper architecture is shown in Fig.8 while the chosen (generic) parameters (VHDL package definition file) for the parameterized DFLC IP (U\_fpga\_fc) and its characteristics are summarized in Table 2 and

The U\_fpga\_fc alone was synthesized using Synplify Pro synthesizer tool, while the rest of the design components were synthesized with Xilinx Synthesis Tool (XST) through the EDK Platform Studio. The produced .edf file for the U\_fpga\_fc is been seeing by the flc\_ip wrapper as a blackbox during the XST flow. The placement and routing of the SoC design into the

According to the device utilization report from the place and route tool (see Table 4), the SoC design (including the DFLC) occupies 4,494 (16%) LUTs, 15 Block Multipliers (MULT18X18s), and 18 Block RAMs. The implemented design uses two Digital Clock Manager (DCM) Modules (DCM\_0 for the system clock and DCM\_1 for clocking the external DDR RAM) that produce the different clocks in the FPGA. The DFLC core itself occupies 1303 or 4% LUTs, 8 Block Multipliers, 12 64x1 ROMs (ROM64X1) and 54 256x1 ROMs (ROM256X1). The SoC achieves a minimum clock operating period of 19.574ns or a maximum frequency of ∼51.1 MHz respectively (the DFLC with the chosen parameters reports a frequency of 85MHz when

**7. FPGA design and performance evaluation**

Table 3 respectively.

implemented alone).

components of the DFLC Deliparaschos & Tzafestas (2006).

FPGA was done through the EDK by calling the Xilinx ISE tool.

such as a data control flow description. This description which serves as a unified system representation allows to perform HW/SW functional partitioning. After the completion of the partitioning, the HW and SW blocks are synthesized and evaluation is then performed. If the evaluation does not meet the required objectives, another HW/SW partition is generated and evaluated (Rozenblit & Buchenrieder, 1996)(Kumar, 1995).

A general HW/SW co-design schema followed in this SoC implementation is illustrated in Fig.10).

Fig. 10. HW/SW Co-design Flow

The DFLC core implementation follows a sequential design manner (see Fig.11) (Navabi, 1998). The starting point of the design process was the functional modelling of the fuzzy controller in a high level description (i.e., MATLAB/Simulink). This serves a two purpose role, first to evaluate the model and second to generate a set of test vectors for RTL and timing verification. The model was coded in register transfer level (RTL) with the use of hardware description language VHDL. Extensive use of VHDL generic and generate statements was used through out the coding of the different blocks, in order to achieve a parameterized DFLC core. The DFLC core is scalable in terms of the number of inputs/bus resolution, number of input/output fuzzy sets per input and membership resolution. More specifically A VHDL package stores the above generic parameters together with the number of necessary pipeline stages for each block. An RTL simulation was performed to ensure the correct functionality of the fuzzy controller. The DFLC core was independently synthesized with Synopsys Synplify logic synthesis tool (as it produced better synthesis results and meet timing constraints), whereas the rest of the SoC cores were synthesised with Xilinx synthesis tool XST. The Xilinx EDK studio was used for the integration flow of different SoC cores (i.e., DFLC, Microblaze, etc) and Xilinx ISE tool for the placement and routing of the SoC on the FPGA. More analytically, the place and route tool accepts the input netlist file (.edf), previously created by the synthesis tool and goes through the following steps. First, the translation program translates the input netlist together with the design constraints to a database file. After the translation program has run successfully, the logical design is mapped to the Xilinx FPGA device. Lastly, the the mapped design is placed and routed onto the chosen FPGA family and a device configuration file (bitstream) is created. Xilinx's SDK used for C programming and debugging the SoC's Microblaze soft processor. RTL and timing simulation to verify the correct functionality was handled with the use of Mentor's Modelsim simulator.

#### **7. FPGA design and performance evaluation**

18 Will-be-set-by-IN-TECH

such as a data control flow description. This description which serves as a unified system representation allows to perform HW/SW functional partitioning. After the completion of the partitioning, the HW and SW blocks are synthesized and evaluation is then performed. If the evaluation does not meet the required objectives, another HW/SW partition is generated

A general HW/SW co-design schema followed in this SoC implementation is illustrated in

The DFLC core implementation follows a sequential design manner (see Fig.11) (Navabi, 1998). The starting point of the design process was the functional modelling of the fuzzy controller in a high level description (i.e., MATLAB/Simulink). This serves a two purpose role, first to evaluate the model and second to generate a set of test vectors for RTL and timing verification. The model was coded in register transfer level (RTL) with the use of hardware description language VHDL. Extensive use of VHDL generic and generate statements was used through out the coding of the different blocks, in order to achieve a parameterized DFLC

and evaluated (Rozenblit & Buchenrieder, 1996)(Kumar, 1995).

Fig.10).

Fig. 10. HW/SW Co-design Flow

The Component Pipeline Registers (CPR) blocks in Fig.8 indicate the number of pipeline stages for each component; the Path Synchronization Registers (PSR) blocks point to registers used for synchronizing the data paths, while the "U" blocks represent the different components of the DFLC Deliparaschos & Tzafestas (2006).

The U\_fpga\_fc component is embedded in the flc\_ip top structural entity wrapper which is compliant with the FSL standard and provides all the necessary peripheral logic to the DFLC soft IP core in order to send/receive data to/from the FSL bus. The flc\_ip wrapper architecture is shown in Fig.8 while the chosen (generic) parameters (VHDL package definition file) for the parameterized DFLC IP (U\_fpga\_fc) and its characteristics are summarized in Table 2 and Table 3 respectively.

The U\_fpga\_fc alone was synthesized using Synplify Pro synthesizer tool, while the rest of the design components were synthesized with Xilinx Synthesis Tool (XST) through the EDK Platform Studio. The produced .edf file for the U\_fpga\_fc is been seeing by the flc\_ip wrapper as a blackbox during the XST flow. The placement and routing of the SoC design into the FPGA was done through the EDK by calling the Xilinx ISE tool.

According to the device utilization report from the place and route tool (see Table 4), the SoC design (including the DFLC) occupies 4,494 (16%) LUTs, 15 Block Multipliers (MULT18X18s), and 18 Block RAMs. The implemented design uses two Digital Clock Manager (DCM) Modules (DCM\_0 for the system clock and DCM\_1 for clocking the external DDR RAM) that produce the different clocks in the FPGA. The DFLC core itself occupies 1303 or 4% LUTs, 8 Block Multipliers, 12 64x1 ROMs (ROM64X1) and 54 256x1 ROMs (ROM256X1). The SoC achieves a minimum clock operating period of 19.574ns or a maximum frequency of ∼51.1 MHz respectively (the DFLC with the chosen parameters reports a frequency of 85MHz when implemented alone).

**Logic Utilization**

**Logic Distribution**

(Two LUTs used per Dual Port RAM)

Table 4. SoC design summary

placed at the center of the robot.

**8. Experimental results**

Number of Slice Flip Flops 3,288 out of 26,624 12% Number of 4 input LUTs 4,494 out of 26,624 16%

Number of occupied Slices 4,174 out of 13,312 31%

Total Number of 4 input LUTs 5,893 out of 26,624 22%

Number of bonded IOBs 62 out of 487 12%

Number of Block RAMs 18 out of 32 56% Number of MULT18X18s 15 out of 32 46% Number of GCLKs 6 out of 8 75% Number of DCMs 2 out of 4 50% Number of BSCANs 1 out of 1 100%

IOB Flip Flops 94 IOB Dual-Data Rate Flops 23

Number of Slices containing only related logic 4,174 out of 4,174 100% Number of Slices containing unrelated logic 0 out of 4,174 0%

Feedback Equivalence and Control of Mobile Robots Through a Scalable FPGA Architecture 421

Number used as logic 4,494 Number used as a route-thru 165 Number used for Dual Port RAMs 432

Number used as 16x1 ROMs 432 Number used as Shift registers 370

Total equivalent gate count for design 1,394,323 Additional JTAG gate count for IOBs 2,976

The experiments consist of tracking predefined paths and analysing the displacement error. The paths are drawn by hand in the MATLAB application and downloaded to the FPGA. Then the control algorithm running on the board, calculates the steering command (curvature), relays it to MATLAB, which in turn passes it to the robot. Conversely, the MATLAB application receives odometric data from the robot which are then relayed to the FPGA. Essentially, the MATLAB application acts as an intermediary between the board and the robot, transforming commands and data to a suitable form for each party. Note also that the actual odometry is being performed by MATLAB (estimation of the robot's pose (*x*, *y*, *θ*) using the data from the robot's encoders). A key detail in the above process is that odometry provides an estimation of the actual pose. Thus in order to analyse the efficacy of the tracking scheme, we need to know the actual pose of the robot. Position detection of the robot is achieved using a camera hanging above the robot's activity terrain and utilizing a video tracking algorithm which extracts the robot's trajectory in post-processing. This algorithm tracks a red LED

The video tracking algorithm uses the high contrast of the LED with its surrounding space. Specifically, each video frame is transformed from the *RGB* color space to the generalized *rgb*

Fig. 11. HW/SW Hardware design flow


Table 4. SoC design summary

#### **8. Experimental results**

20 Will-be-set-by-IN-TECH

Fig. 11. HW/SW Hardware design flow

The experiments consist of tracking predefined paths and analysing the displacement error. The paths are drawn by hand in the MATLAB application and downloaded to the FPGA. Then the control algorithm running on the board, calculates the steering command (curvature), relays it to MATLAB, which in turn passes it to the robot. Conversely, the MATLAB application receives odometric data from the robot which are then relayed to the FPGA. Essentially, the MATLAB application acts as an intermediary between the board and the robot, transforming commands and data to a suitable form for each party. Note also that the actual odometry is being performed by MATLAB (estimation of the robot's pose (*x*, *y*, *θ*) using the data from the robot's encoders). A key detail in the above process is that odometry provides an estimation of the actual pose. Thus in order to analyse the efficacy of the tracking scheme, we need to know the actual pose of the robot. Position detection of the robot is achieved using a camera hanging above the robot's activity terrain and utilizing a video tracking algorithm which extracts the robot's trajectory in post-processing. This algorithm tracks a red LED placed at the center of the robot.

The video tracking algorithm uses the high contrast of the LED with its surrounding space. Specifically, each video frame is transformed from the *RGB* color space to the generalized *rgb*

Fig. 13. (UP) The odometry (blue), camera (red) and reference (dashed black) paths for the first experiment. (DOWN) Minimum distance of the odometry and camera paths to the

Feedback Equivalence and Control of Mobile Robots Through a Scalable FPGA Architecture 423

Fig. 14. (UP) The odometry (blue), camera (red) and reference (dashed black) paths for the second experiment. (DOWN) Minimum distance of the odometry and camera paths to the

reference path versus path length experiment.

reference path versus path length.

space. This space expresses the percentage of each color at each pixel, i.e.,

$$\begin{aligned} r &= \frac{R+G+B}{R} \\ g &= \frac{R+G+B}{G} \\ b &= \frac{R+G+B}{B} \end{aligned} \tag{35}$$

It is the hyper-space of the so-called *rg* chromaticity space, which consists of the first two equations. Following the transformation, in order to enhance the contrast, each image pixel is raised to the 3rd power,

$$(r',\mathbf{g'},\mathbf{b'})\_{(\mathfrak{u},\mathfrak{v})} = (r^3,\mathbf{g^3},\mathbf{b^3})\_{(\mathfrak{u},\mathfrak{v})} \tag{36}$$

The new image is then re-transformed according to the *rgb* transform, essentially computing the color percentage of the percentage. Then, we apply a thresholding on the *r* channel, producing a binary image. The threshold was empirically set to "0.6". This procedure produces the "patch" of pixels corresponding to the red LED. The next step is, of course, to calculate a single pixel value from this patch, and thus get the robot's position. To this end, we calculate the median (*row*, *col*) value of the patch's pixels. This algorithm is applied to the first video frame, and is rather slow since the image dimensions are large (1280×720 pixels). In order to speed up the process, the algorithm processes an image region of interest in each consecutive frame. This ROI is a 17×17 pixel square, centred at the point extracted from the previous frame. The square dimensions are appropriate since the robot is not expected to have moved far between frames. The precision of the algorithm is about ±2 pixels, translating to 2.4 mm.

Previous to all runs, the camera was calibrated using the Camera Calibration Toolbox by J.Y. Bouguet, extracting the camera's intrinsic and extrinsic parameters. Two runs were performed; one tracking a straight line and one tracking a curved path (snapshots of the two videos are seen in Fig. 12). For the first run, the reference, odometry and camera paths are presented in Fig. 13(UP). The minimum distance versus the path length of the odometry and the camera paths are shown in Fig.13(DOWN). Likewise, for the second experiment the results are presented in Fig14.

Fig. 12. Snapshots of the first (LEFT) and the second (RIGHT) experiments. The red line is the robot's path calculated off-line from the video camera.

22 Will-be-set-by-IN-TECH

*<sup>r</sup>* <sup>=</sup> *<sup>R</sup>* <sup>+</sup> *<sup>G</sup>* <sup>+</sup> *<sup>B</sup> R <sup>g</sup>* <sup>=</sup> *<sup>R</sup>* <sup>+</sup> *<sup>G</sup>* <sup>+</sup> *<sup>B</sup> G <sup>b</sup>* <sup>=</sup> *<sup>R</sup>* <sup>+</sup> *<sup>G</sup>* <sup>+</sup> *<sup>B</sup> B*

It is the hyper-space of the so-called *rg* chromaticity space, which consists of the first two equations. Following the transformation, in order to enhance the contrast, each image pixel is

The new image is then re-transformed according to the *rgb* transform, essentially computing the color percentage of the percentage. Then, we apply a thresholding on the *r* channel, producing a binary image. The threshold was empirically set to "0.6". This procedure produces the "patch" of pixels corresponding to the red LED. The next step is, of course, to calculate a single pixel value from this patch, and thus get the robot's position. To this end, we calculate the median (*row*, *col*) value of the patch's pixels. This algorithm is applied to the first video frame, and is rather slow since the image dimensions are large (1280×720 pixels). In order to speed up the process, the algorithm processes an image region of interest in each consecutive frame. This ROI is a 17×17 pixel square, centred at the point extracted from the previous frame. The square dimensions are appropriate since the robot is not expected to have moved far between frames. The precision of the algorithm is about ±2 pixels, translating to

Previous to all runs, the camera was calibrated using the Camera Calibration Toolbox by J.Y. Bouguet, extracting the camera's intrinsic and extrinsic parameters. Two runs were performed; one tracking a straight line and one tracking a curved path (snapshots of the two videos are seen in Fig. 12). For the first run, the reference, odometry and camera paths are presented in Fig. 13(UP). The minimum distance versus the path length of the odometry and the camera paths are shown in Fig.13(DOWN). Likewise, for the second experiment the results

Fig. 12. Snapshots of the first (LEFT) and the second (RIGHT) experiments. The red line is the

robot's path calculated off-line from the video camera.

)(*u*,*v*) = (*r*

(35)

3, *<sup>g</sup>*3, *<sup>b</sup>*<sup>3</sup>)(*u*,*v*) (36)

space. This space expresses the percentage of each color at each pixel, i.e.,

(*r*� , *g*� , *b*�

raised to the 3rd power,

2.4 mm.

are presented in Fig14.

Fig. 13. (UP) The odometry (blue), camera (red) and reference (dashed black) paths for the first experiment. (DOWN) Minimum distance of the odometry and camera paths to the reference path versus path length experiment.

Fig. 14. (UP) The odometry (blue), camera (red) and reference (dashed black) paths for the second experiment. (DOWN) Minimum distance of the odometry and camera paths to the reference path versus path length.

Egerstedt, M., Hu, X. & Stotsky, A. (1998). Control of a car-like robot using a dynamic model,

Feedback Equivalence and Control of Mobile Robots Through a Scalable FPGA Architecture 425

El Hajjaji, A. & Bentalba, S. (2003). Fuzzy path tracking control for automatic steering of

Fortuna, L., Presti, M. L., Vinci, C. & Cucuccio, A. (2003). Recent trends in fuzzy control

Gardner, R. B. & Shadwick, W. F. (1987). Feedback equivalence of control systems, *Systems &*

Gardner, R. B. & Shadwick, W. F. (1990). Feedback equivalence for general control systems,

Groff, R. E. (2003). *Piecewise Linear Homeomorphisms for Approximation of Invertible Maps*, PhD

Gupta, H. & Wenger, R. (1997). Constructing piecewise linear homeomorphisms of simple

Jiangzhou, L., Sekhavat, S. & Laugier, C. (1999). Fuzzy variable-structure control

Kanayama, Y. & Fahroo, F. (1997). A new line tracking method for nonholonomic vehicles,

Koh, K. & Cho, H. (1994). A path tracking control system for autonomous mobile robots: an

Kongmunvattana, A. & Chongstivatana, P. (1998). A FPGA-based behavioral control system

Kumar, S. (1995). *A unified representation for hardware/software codesign*, PhD thesis, University

Lee, T., Lam, H., Leung, F. & Tam, P. (2003). A practical fuzzy logic controller for the path tracking of wheeled mobile robots, *Control Systems Magazine, IEEE* 23(2): 60– 65. Leong, P. & Tsoi, K. (2005). Field programmable gate array technology for robotics

Li, T., Chang, S. & Chen, Y. (2003). Implementation of human-like driving skills by

Liu, K. & Lewis, F. (1994). Fuzzy logic-based navigation controller for an autonomous mobile

Maalouf, E., Saad, M. & Saliah, H. (2006). A higher level path tracking controller for

*Proceedings. 1999 IEEE/IEEJ/JSAI International Conference on*, pp. 465–470. Kamga, A. & Rachid, A. (1997). A simple path tracking controller for car-like mobile robots,

for nonholonomic vehicle path tracking, *Intelligent Transportation Systems, 1999.*

*Robotics and Automation, 1997. Proceedings., 1997 IEEE International Conference on*,

for a mobile robot, *Circuits and Systems, 1998. IEEE APCCAS 1998. The 1998 IEEE*

applications, *Robotics and Biomimetics (ROBIO). 2005 IEEE International Conference on*,

autonomous fuzzy behavior control on an FPGA-based car-like mobile robot,

robot, *Systems, Man, and Cybernetics, 1994. 'Humans, Information and Technology'., 1994*

a four-wheel differentially steered mobile robot, *Robotics and Autonomous Systems*

Hung, D. L. (1995). Dedicated digital fuzzy hardware, *IEEE Micro* 15(4): 31–39.

experimental investigation, *Mechatronics* 4(8): 799–820.

*Industrial Electronics, IEEE Transactions on* 50(5): 867– 880.

*IEEE International Conference on*, Vol. 2, pp. 1782–1789 vol.2.

vehicles, *Robotics and Autonomous Systems* 43(4): 203–213.

*Symposium on Circuits and Systems*, Vol. 3, pp. 459–461.

pp. 3273–3278 vol.4.

*Control Letters* 8(5): 463–465.

Vol. 4, pp. 2908–2913 vol.4.

*ECC97 Proc.*

pp. 295–298.

54(1): 23–33.

*Systems & Control Letters* 15(1): 15–23.

thesis, The University of Michigan.

polygons, *J. Algorithms* 22(1): 142–157.

*Asia-Pacific Conference on*, pp. 759–762.

of Virginia. UMI Order No. GAX96-00485.

*Robotics and Automation, 1998. Proceedings. 1998 IEEE International Conference on*, Vol. 4,

of electrical drives: an industry point of view, *Proceedings of the 2003 International*

As one can see, the performance of the tracking scheme is satisfactory maintaining the minimum distance to the reference path at about 50mm in the worst case. However, by taking a closer look at Figures 13 and 14, it is clear that the performance degradation is attributed not to the algorithm *per se* but to the odometry. The error accumulation of odometric data forces the robot to diverge from the actual path. But the actual odometry solution is very close to the reference path, meaning that based solely on odometry (as is the case in these experiments), the tracker maintains the robot very close to the reference path (the minimum distance is below 10mm in both cases). This implies that if a better localization technique is used, our tracking scheme would perform with more accuracy.

#### **9. Conclusions**

In this chapter we have analysed and demonstrated the applicability of the strip-wise affine transform in the path tracking task for mobile robots. The transformation was translated to hardware and implemented into an FPGA chip with the use of VHDL and advanced EDA software. The scalability of the fuzzy controller core allowed easy parameter adaptation of the theoretic fuzzy tracker model. The experiments show that the tracking scheme performs satisfactory but is degraded by the accumulation of errors of the odometry used in estimating the robots position.

#### **10. References**


24 Will-be-set-by-IN-TECH

As one can see, the performance of the tracking scheme is satisfactory maintaining the minimum distance to the reference path at about 50mm in the worst case. However, by taking a closer look at Figures 13 and 14, it is clear that the performance degradation is attributed not to the algorithm *per se* but to the odometry. The error accumulation of odometric data forces the robot to diverge from the actual path. But the actual odometry solution is very close to the reference path, meaning that based solely on odometry (as is the case in these experiments), the tracker maintains the robot very close to the reference path (the minimum distance is below 10mm in both cases). This implies that if a better localization technique is

In this chapter we have analysed and demonstrated the applicability of the strip-wise affine transform in the path tracking task for mobile robots. The transformation was translated to hardware and implemented into an FPGA chip with the use of VHDL and advanced EDA software. The scalability of the fuzzy controller core allowed easy parameter adaptation of the theoretic fuzzy tracker model. The experiments show that the tracking scheme performs satisfactory but is degraded by the accumulation of errors of the odometry used in estimating

Abdessemed, F., Benmahammed, K. & Monacelli, E. (2004). A fuzzy-based reactive controller for a non-holonomic mobile robot, *Robotics and Autonomous Systems* 47(1): 31–46. Altafini, C. (1999). A Path-Tracking criterion for an LHD articulated vehicle, *The International*

Altafini, C. (2002). Following a path of varying curvature as an output regulation problem,

Antonelli, G., Chiaverini, S. & Fusco, G. (2007). A Fuzzy-Logic-Based approach for mobile robot path tracking, *Fuzzy Systems, IEEE Transactions on* 15(2): 211–221. Baltes, J. & Otte, R. (1999). A fuzzy logic controller for car-like mobile robots, *Computational*

Cao, M. & Hall, E. L. (1998). Fuzzy logic control for an automated guided vehicle,

Costa, A., Gloria, A. D., Giudici, F. & Olivieri, M. (1997). Fuzzy logic microcontroller, *IEEE*

Deliparaschos, K. M., Nenedakis, F. I. & Tzafestas, S. G. (2006). Design and implementation of

Deliparaschos, K. M. & Tzafestas, S. G. (2006). A parameterized T-S digital fuzzy logic

Deliparaschos, K., Moustris, G. & Tzafestas, S. (2007). Autonomous SoC for fuzzy robot path tracking, *Proceedings of the European Control Conference 2007*, Kos, Greece.

*Intelligence in Robotics and Automation, 1999. CIRA '99. Proceedings. 1999 IEEE*

*Intelligent Robots and Computer Vision XVII: Algorithms, Techniques, and Active Vision*

a fast digital fuzzy logic controller using FPGA technology, *Journal of Intelligent and*

processor: soft core VLSI design and FPGA implementation, *International Journal of*

used, our tracking scheme would perform with more accuracy.

*Journal of Robotics Research* 18(5): 435–441.

*Automatic Control, IEEE Transactions on* 47(9): 1551–1556.

*International Symposium on*, Monterey, CA , USA, pp. 89–94.

*Factory Automation, Robotics and Soft Computing* 3: 7–15.

**9. Conclusions**

the robots position.

3522(1): 303–312.

*Micro* 17(1): 66–74.

*Robotics Systems* 45(1): 77–96.

**10. References**


**21** 

*Korea* 

**Control Architecture Design and** 

KiSung You, HwangRyol Ryu and Chintae Choi *Research Institute of Industrial Science and Technology* 

**Localization for a Gas Cutting Robot** 

Conventional control architecture which has been employed in mobile robot control software is divided into two categories such as knowledge-based and behavior-based control architecture. Early implementation of control architecture was mainly focused on building for sensing the environment, modeling it, planning based on this perceived model and executing the planned action to achieve a certain task. This design approach is called sense-model-plan-act (SMPA) or knowledge-based. A mobile robot making use of a knowledge-based controller tries to achieve its goal by following closely the sense-modelplan-act procedure. SMPA controller also needs initial knowledge, required to model its task environment prior for the robots to executing the planned task. Hence, if the initial knowledge is suited to its working environment, the resulting tasks guarantee success. Although the resulting overall behavior is predictable, the controller often suffers from being slow and becomes complex as it deals with a dynamic environment because most of the controller processing time is consumed in building a model, doing general perception and planning. Therefore, it is suitable controller for robots to require high-level intelligence

Brooks proposed a radically different approach in the design of mobile robot control architecture to address the drawback of knowledge-based control architecture. This control architecture functions a horizontal computation scheme so that each behavior is a fixed action pattern with respect to the sensory information. When the mobile robot is confronted by a dynamic environment, a behavior-based robot can react fast because of the direct coupling between its behaviors and the sensed states. The robot controller can be built incrementally, thus making it highly modular and easy to construct. However, this reactive approach still suffers from planning the productive and efficient actions in an unstructured environment because they are only confined to reactions to sensors and the changing states

In this chapter, we address the functional safety problems potentially embedded in the control system of the developed mobile robot and introduce a concept of Autonomous Poor Strip Cutting Robot (APSCR) control architecture with a focus on safety in order to design the walkthrough procedure for each behavior. In section 2, we explain the working environment where the robot will be manually or autonomously operated. Section 3 explains the control architecture of APSCR. In section 4, we explain the localization system of APSCR. Finally, section 5 shows some experimental result and in section 6 conclusions will be lastly addressed.

**1. Introduction** 

of other modules.

and work in static and predictable environment.


KiSung You, HwangRyol Ryu and Chintae Choi *Research Institute of Industrial Science and Technology Korea* 

#### **1. Introduction**

26 Will-be-set-by-IN-TECH

426 Recent Advances in Mobile Robotics

Moustris, G. P. & Tzafestas, S. G. (2011). Switching fuzzy tracking control for mobile robots under curvature constraints, *Control Engineering Practice* 19(1): 45–53. Moustris, G. & Tzafestas, S. (2005). A robust fuzzy logic path tracker for non-holonomic mobile robots., *International Journal on Artificial Intelligence Tools* 14(6): 935–966. Moustris, G. & Tzafestas, S. (2008). Reducing a class of polygonal path tracking to straight line

Navabi, Z. (1998). *VHDL: analysis and modeling of digital systems*, McGraw-Hill Professional. Ollero, A., Garcia-Cerezo, A., Martinez, J. L. & Mandow, A. (1997). Fuzzy tracking methods

Patterson, D. A. & Hennessy, J. L. (1997). *Computer Organization and Design: The*

Preparata, F. P. & Supowit, K. J. (1981). Testing a simple polygon for monotonicity, *Info. Proc.*

Raimondi, F. & Ciancimino, L. (2008). Intelligent neuro-fuzzy dynamic path following for

Reynolds, R., Smith, P., Bell, L. & Keller, H. (2001). The design of mars lander cameras

Rodriguez-Castano, A., Heredia, G. & Ollero, A. (2000). Fuzzy path tracking and position

Rozenblit, J. & Buchenrieder, K. (1996). *Codesign: Computer-aided Software/Hardware*

Sailer, P. M., Sailer, P. M. & Kaeli, D. R. (1996). *The DLX Instruction Set Architecture Handbook*,

Salapura, V. (2000). A fuzzy RISC processor, *IEEE Transactions on Fuzzy Systems* 8(6): 781–790. Samson, C. (1995). Control of chained systems application to path following and time-varying

Sanchez, O., Ollero, A. & Heredia, G. (1997). Adaptive fuzzy control for automatic path

Yang, X., He, K., Guo, M. & Zhang, B. (1998). An intelligent predictive control approach to

*1998. 1998 IEEE International Conference on*, Vol. 4, pp. 3301–3306 vol.4.

*Hardware/Software Interface*, 2 edn, Morgan Kaufmann.

*Measurement, IEEE Transactions on* 50(1): 63–71.

1st edn, Morgan Kaufmann Publishers Inc.

*Simulation* 79(2): 133–148.

*Lett.* 12(4): 161–164.

7(3): 257–264.

40(1): 64–77.

*Workshop on*, pp. 744–750.

*Engineering*, I.E.E.E.Press.

*Robot. Syst.* 21(8): 439–449.

Jersey.

tracking via nonlinear strip-wise affine transformation, *Mathematics and Computers in*

for mobile robots, *in* M. Jamshidi, A. Titli, L. Zadeh & S. Boverie (eds), *Applications of fuzzy logic: Towards high machine intelligence quotient systems*, Prentice-Hall, New

car-like vehicle, *Advanced Motion Control, 2008. AMC '08. 10th IEEE International*

for mars pathfinder, mars surveyor '98 and mars surveyor '01, *Instrumentation and*

estimation of autonomous vehicles using differential GPS, *Mathware Soft Comput*

point-stabilization of mobile robots, *Automatic Control, IEEE Transactions on*

tracking of outdoor mobile robots. application to romeo 3R, *Fuzzy Systems, 1997., Proceedings of the Sixth IEEE International Conference on*, Vol. 1, pp. 593–599 vol.1. Wit, J., Crane, C. D. & Armstrong, D. (2004). Autonomous ground vehicle path tracking, *J.*

path tracking problem of autonomous mobile robot, *Systems, Man, and Cybernetics,*

Conventional control architecture which has been employed in mobile robot control software is divided into two categories such as knowledge-based and behavior-based control architecture. Early implementation of control architecture was mainly focused on building for sensing the environment, modeling it, planning based on this perceived model and executing the planned action to achieve a certain task. This design approach is called sense-model-plan-act (SMPA) or knowledge-based. A mobile robot making use of a knowledge-based controller tries to achieve its goal by following closely the sense-modelplan-act procedure. SMPA controller also needs initial knowledge, required to model its task environment prior for the robots to executing the planned task. Hence, if the initial knowledge is suited to its working environment, the resulting tasks guarantee success. Although the resulting overall behavior is predictable, the controller often suffers from being slow and becomes complex as it deals with a dynamic environment because most of the controller processing time is consumed in building a model, doing general perception and planning. Therefore, it is suitable controller for robots to require high-level intelligence and work in static and predictable environment.

Brooks proposed a radically different approach in the design of mobile robot control architecture to address the drawback of knowledge-based control architecture. This control architecture functions a horizontal computation scheme so that each behavior is a fixed action pattern with respect to the sensory information. When the mobile robot is confronted by a dynamic environment, a behavior-based robot can react fast because of the direct coupling between its behaviors and the sensed states. The robot controller can be built incrementally, thus making it highly modular and easy to construct. However, this reactive approach still suffers from planning the productive and efficient actions in an unstructured environment because they are only confined to reactions to sensors and the changing states of other modules.

In this chapter, we address the functional safety problems potentially embedded in the control system of the developed mobile robot and introduce a concept of Autonomous Poor Strip Cutting Robot (APSCR) control architecture with a focus on safety in order to design the walkthrough procedure for each behavior. In section 2, we explain the working environment where the robot will be manually or autonomously operated. Section 3 explains the control architecture of APSCR. In section 4, we explain the localization system of APSCR. Finally, section 5 shows some experimental result and in section 6 conclusions will be lastly addressed.

requirements and be designed to respond to the severe faults that might be done by

We must take into consideration robot's functionalities and safety with respect to all cutting operations because the working environment is extremely bad to the robot for itself. For the purpose of fulfilling all the tasks, we analyzed the survey results in technically critical requirements in the engineer's point of view and took into consideration task achievements in the customer's point of view as well. Below is a list of important information required for

Robot must cut a poor strip at single cutting operation, which means not to move back

When a flame is not detected due to the malfunction of flame detection sensor or

Poor strips should be cut within 10 ~15 minutes regardless of the thickness of the poor

Poor strip (approximately 1.5 m x 60 m) cutting operation should be done within 1

We take into consideration repulsiveness of torching at the moment of emitting the gas

Electrical spark must be sealed off because gas tank and oxygen tank are loaded on the

We also take the design of the robot into consideration in the engineer's point of views as

Robot should move along the guide fence installed at the edge of the conveyor stand.

Robot body should sustain a fully stretching arm (full reach: 3.8 m) so that links of arm

Cutting operation is divided by two steps: pre-heating and blowing. Each step must be

For the safety protection against gas explosion, a flame detection sensor should be

Object detection mechanism, implemented by high sensitive ultrasonic sensor must be

Weight balance mechanism should be taken into consideration since robot's total

Intensity of flame shouldn't be strong enough to cut the bottom plate under a poor

We had collected many opinions and suggestions regarding to robot's functionalities and behaviors. Above lists are a part of surveys, which workers thought of the fundamental

considered in order to avoid collision on moving next cutting point.

clogged by the heavy dust, robot must stop all the operations.

Robot should be equipped with a backfire protection device.

Maximum thickness of the poor strips is 150 mm.

Torch tip should be designed to rotate automatically.

accurately controlled by a system or worker.

interrupted by a hardware and software.

weight is approximately 700 kg.

 Backfire protection device should be mounted in the pipe of gas. Workers should control a flow of gas and oxygen manually.

segment should not hang downwards with no firmness.

tasks and operations which the robot developer should consider.

It should be an automatic ignition system.

operators or malfunctioned by the mechanical faults.

the robot design in the customer's point of view.

to the points where cutting is done.

strips in the width direction.

hour.

robot.

strip.

follows:

and oxygen.

**2.2 Technical requirement** 

### **2. Working environment explanation**

This section makes an explanation of the robot's working environment in Hot Rolling facility, Gwangyang steel work and describes the challenges and difficulties which the robot shall confront based on the results of CCR (customer critical requirements) and TCR (technical critical requirements) are.

#### **2.1 Robot's working environment**

Hot Rolling, used mainly to produce sheet metal or simple cross sections from billets is the most efficient process of primary forming used for the mass production of steel. The first operation of hot rolling process is to heat the stock to the proper deformation temperature. During heating, a scale forms on the stock surface which must be systematically removed and thus descaling is performed mechanically by crushing during a hot forming operation or by spraying with water under high pressure.

Fig. 1. Picture of poor strip produced during hot rolling.

During rolling, deformation of material occurs in between in the forms of rotation, driven rolls. The transporting force during rolling is the friction between rolls and a processed material. Due to this tremendous force applied to the slabs into wrong direction, the poor strips are produced as shown in Figure 1. Due to these poor strips, the whole facility has to be stopped until those poor strips are to be taken away from the roller conveyor by cutting them into the right size and weight so that the heavy-weight crane can hold and carry them out. In order for the workers to cut the poor strips (generally the poor strip is 1.5m x 60m), they have to put on the fire resistant work wear and do the oxygen cutting on the surface of the poor strip whose temperature is over 200 °C.

During inspecting the working environment, we gathered the strong demands and voices about the development of new type of automation which can provide the safety and the cutting operation. We carefully determined the requirements in the respects to the workers or technical barrier. We concluded that the APSCR must be equipped with all the safety requirements and be designed to respond to the severe faults that might be done by operators or malfunctioned by the mechanical faults.

#### **2.2 Technical requirement**

428 Recent Advances in Mobile Robotics

This section makes an explanation of the robot's working environment in Hot Rolling facility, Gwangyang steel work and describes the challenges and difficulties which the robot shall confront based on the results of CCR (customer critical requirements) and TCR

Hot Rolling, used mainly to produce sheet metal or simple cross sections from billets is the most efficient process of primary forming used for the mass production of steel. The first operation of hot rolling process is to heat the stock to the proper deformation temperature. During heating, a scale forms on the stock surface which must be systematically removed and thus descaling is performed mechanically by crushing during a hot forming operation

During rolling, deformation of material occurs in between in the forms of rotation, driven rolls. The transporting force during rolling is the friction between rolls and a processed material. Due to this tremendous force applied to the slabs into wrong direction, the poor strips are produced as shown in Figure 1. Due to these poor strips, the whole facility has to be stopped until those poor strips are to be taken away from the roller conveyor by cutting them into the right size and weight so that the heavy-weight crane can hold and carry them out. In order for the workers to cut the poor strips (generally the poor strip is 1.5m x 60m), they have to put on the fire resistant work wear and do the oxygen cutting on the surface of

During inspecting the working environment, we gathered the strong demands and voices about the development of new type of automation which can provide the safety and the cutting operation. We carefully determined the requirements in the respects to the workers or technical barrier. We concluded that the APSCR must be equipped with all the safety

**2. Working environment explanation** 

or by spraying with water under high pressure.

Fig. 1. Picture of poor strip produced during hot rolling.

the poor strip whose temperature is over 200 °C.

(technical critical requirements) are.

**2.1 Robot's working environment**

We must take into consideration robot's functionalities and safety with respect to all cutting operations because the working environment is extremely bad to the robot for itself. For the purpose of fulfilling all the tasks, we analyzed the survey results in technically critical requirements in the engineer's point of view and took into consideration task achievements in the customer's point of view as well. Below is a list of important information required for the robot design in the customer's point of view.


We also take the design of the robot into consideration in the engineer's point of views as follows:


We had collected many opinions and suggestions regarding to robot's functionalities and behaviors. Above lists are a part of surveys, which workers thought of the fundamental tasks and operations which the robot developer should consider.

In the context of developing safety-oriented software, safety protection functionalities in control architecture become a key to making a robot function as was designed with a focus on the safety in this project. The safety protection functionalities are embodied within the

The task-achieving behaviors, which are described above provide the functional mechanism for APSCR to provide the poor strip cutting procedure autonomously. The following Figure 5 shows how behaviors modules depend on each other and are grouped according to their

To be able to operate the task-achieving behaviors sufficiently as defined in Figure 6, the APSCR starts with checking its system by *BEHAVIOR\_SelfWalkThrough* and do the alignment job in order to previously position cutting procedure by calling the following functions *CheckDistance10cm* and *InitialAlignemtnwithStand* in Before Cutting. After initial setup is done, the robot starts to measure the thickness of the poor strip (*MeasureDepthOfPoorStrip*), finds the starting position in which the edge of the poor strip is detected by the roller mounted on the torch holder (*SetStartingPostion4Cutting*), cuts the poor strip with controlling the speed of Boom and Derrick and maintaining the gap between the torch tip and surface of the strip (*StartFiring, Preheating4Cutting, SettingCuttingParameter*), and finally detects the finishing edge of the strip (*SetfinishigPosition4cutting*). After finishing the cutting procedure, while monitoring the alignment between the robot and the poor strip stand and making obstacle avoidance modules running (*CheckSensorStatus4Navigation*), the robot moves to the next cutting position

Fig. 4. Structure of boom(robot arm) and touch holder.

task-achieving behaviors as follows. *BEHAVIOR\_SelfWalkThrough BEHAVIOR\_CheckDistance10cm BEHAVIOR\_InitialAlignmentWithLane BEHAVIOR\_MeasureDepthOfPoorStrip BEHAVIOR\_SetStartingPosition4Cutting* 

 *BEHAVIOR\_Preheating4Cutting BEHAVIOR\_SettingCuttingParameter* 

 *BEHAVIOR\_SetFinishingPosition4Cutting BEHAVIOR\_MovingNextCuttingPosition BEHAVIOR\_CheckSensorStatus4Navigation BEHAVIOR\_CheckSensorStatus4Cutting* 

*BEHAVIOR\_StartFiring* 

functions.

**3.1 Design of safety interaction for software interrupts** 

Fig. 2. APSCR stretches its arm up to 3.8 m.

#### **3. System configuration for APSCR**

When we develop industrial robots under an unpredictable environment such as steelworks, there are many critical requirements in the sense that hardware parts would comprise of the supplementary functionalities with a help of software parts. On the contrary, software and hardware are separately developed and combined without considering control architecture. The proposed control architecture mainly makes use of the advantages of conventional reactive control architecture. In this section we focus on the software architecture assuming that the hardware architecture is well designed and describe how the safety walkthrough procedure is sufficiently cooperated within the reactive controller by utilizing hardware and software interrupts as well. Figures 3 through 4 are pictures illustrating the proposed gas cutting robot as mentioned in section 2.

Fig. 3. Layout of a gas cutting robot.

Fig. 4. Structure of boom(robot arm) and touch holder.

#### **3.1 Design of safety interaction for software interrupts**

In the context of developing safety-oriented software, safety protection functionalities in control architecture become a key to making a robot function as was designed with a focus on the safety in this project. The safety protection functionalities are embodied within the task-achieving behaviors as follows.

*BEHAVIOR\_SelfWalkThrough* 

430 Recent Advances in Mobile Robotics

When we develop industrial robots under an unpredictable environment such as steelworks, there are many critical requirements in the sense that hardware parts would comprise of the supplementary functionalities with a help of software parts. On the contrary, software and hardware are separately developed and combined without considering control architecture. The proposed control architecture mainly makes use of the advantages of conventional reactive control architecture. In this section we focus on the software architecture assuming that the hardware architecture is well designed and describe how the safety walkthrough procedure is sufficiently cooperated within the reactive controller by utilizing hardware and software interrupts as well. Figures 3 through 4 are

pictures illustrating the proposed gas cutting robot as mentioned in section 2.

Fig. 2. APSCR stretches its arm up to 3.8 m.

**3. System configuration for APSCR** 

Fig. 3. Layout of a gas cutting robot.


The task-achieving behaviors, which are described above provide the functional mechanism for APSCR to provide the poor strip cutting procedure autonomously. The following Figure 5 shows how behaviors modules depend on each other and are grouped according to their functions.

To be able to operate the task-achieving behaviors sufficiently as defined in Figure 6, the APSCR starts with checking its system by *BEHAVIOR\_SelfWalkThrough* and do the alignment job in order to previously position cutting procedure by calling the following functions *CheckDistance10cm* and *InitialAlignemtnwithStand* in Before Cutting. After initial setup is done, the robot starts to measure the thickness of the poor strip (*MeasureDepthOfPoorStrip*), finds the starting position in which the edge of the poor strip is detected by the roller mounted on the torch holder (*SetStartingPostion4Cutting*), cuts the poor strip with controlling the speed of Boom and Derrick and maintaining the gap between the torch tip and surface of the strip (*StartFiring, Preheating4Cutting, SettingCuttingParameter*), and finally detects the finishing edge of the strip (*SetfinishigPosition4cutting*). After finishing the cutting procedure, while monitoring the alignment between the robot and the poor strip stand and making obstacle avoidance modules running (*CheckSensorStatus4Navigation*), the robot moves to the next cutting position

(*MovingNextCuttingPosition*). Each of these task-achieving behaviors comprises of the primitive action modules to support the functional mechanism of the robot and sort out the interactions between the behaviors. Every primitive action module has its own timer or interrupts because

As shown in Figure 6, every primitive action modules are implemented in the manner that each sensor considers a different property and I/O interface. Controller integrates all modules discussed previously using the safety-oriented reactive control architecture. One of the significant advantages for encapsulating the behavior modules is that when programming, we can hide a system level architecture such as sensor calibration, hardware or software interrupts, and so on. More importantly, we do not have to program the detailed safety modules of relevance. The motion controller for a gas cutting robot is composed of a microprocessor and peripheral circuits with a digital input and output (DIO), an analog to digital converter (ADC), a digital to analog converter (DAC), a serial communication circuit, and timers. The microprocessor of the controller is the Atmega128 made in Atmel Co., Ltd.

Localization system for a gas cutting robot is a unique sensor system for indoor localization of industrial mobile robots. It analyzes infrared ray image which is reflected from a passive landmark with an independent ID. The output of position and heading angle of a robot is given with very precise resolution and high speed. It is seldom affected by surroundings such as an infrared ray, a fluorescent light and sunshine. It is composed of an IR Projector

each behavior or primitive action should be considerably taken for the safety.

Fig. 6. Autonomous poor strip cutting procedure.

**3.2 Design of motion controller** 

**4. Localization for a gas cutting robot** 

Fig. 5. Interaction between sensors and actuators.

432 Recent Advances in Mobile Robotics

Fig. 5. Interaction between sensors and actuators.

Fig. 6. Autonomous poor strip cutting procedure.

(*MovingNextCuttingPosition*). Each of these task-achieving behaviors comprises of the primitive action modules to support the functional mechanism of the robot and sort out the interactions between the behaviors. Every primitive action module has its own timer or interrupts because each behavior or primitive action should be considerably taken for the safety.

#### **3.2 Design of motion controller**

As shown in Figure 6, every primitive action modules are implemented in the manner that each sensor considers a different property and I/O interface. Controller integrates all modules discussed previously using the safety-oriented reactive control architecture. One of the significant advantages for encapsulating the behavior modules is that when programming, we can hide a system level architecture such as sensor calibration, hardware or software interrupts, and so on. More importantly, we do not have to program the detailed safety modules of relevance. The motion controller for a gas cutting robot is composed of a microprocessor and peripheral circuits with a digital input and output (DIO), an analog to digital converter (ADC), a digital to analog converter (DAC), a serial communication circuit, and timers. The microprocessor of the controller is the Atmega128 made in Atmel Co., Ltd.

#### **4. Localization for a gas cutting robot**

Localization system for a gas cutting robot is a unique sensor system for indoor localization of industrial mobile robots. It analyzes infrared ray image which is reflected from a passive landmark with an independent ID. The output of position and heading angle of a robot is given with very precise resolution and high speed. It is seldom affected by surroundings such as an infrared ray, a fluorescent light and sunshine. It is composed of an IR Projector

provides an apparatus and method for recognizing a position of a mobile robot, the apparatus having an inclination correction function to precisely recognizing the position of the mobile robot by correcting position information of an image of a landmark photographed by an infrared camera by measuring inclination information of the mobile

According to aspect of the present localization system, the landmark is used as recognizing coordinates and azimuth information of a mobile robot. The landmark including a position recognition part is formed of a mark in any position and at least two marks on an X axis and Y axis centered on the landmark. The landmark may further include an area recognition part formed of a combination of a plurality of marks to distinguish an individual landmark from others. According to another aspect of the present localization system, there is an apparatus provided for recognizing a position of a mobile robot. The apparatus including: an infrared lighting unit irradiating an infrared ray to a landmark formed of a plurality of marks reflecting the infrared; an infrared camera photographing the landmark and obtaining a binary image; a mark detector labeling a partial image included in the binary image and detecting the mark by using a number and/or dispersion of labeled pixels for each the partial image; and a position detector detecting coordinates and an azimuth of the mobile robot by using centric coordinates of the detected mark. The landmark may include a position recognition part formed of a mark in any position and at least two marks located on an X axis and Y axis centered on the mark. Also, it is provided a method of recognizing a position of a mobile robot having an inclination correction function, the method including: (a) obtaining a binary image by irradiating an infrared ray to a landmark including a position recognition part formed of a mark in any position and at least two marks located on an X axis and Y axis centered on the mark to reflect the infrared ray and photographing the landmark; (b) detecting two-axis inclination information of the mobile robot to the ground and obtaining a binary image again when the detected two-axis inclination information is more than a predetermined threshold; (c) labeling a partial image included in the binary image and detecting the mark by using a number and/or dispersion of labeled pixels for each the partial image; and (d) detecting coordinates and an azimuth of the mobile robot by using centric coordinates of the detected mark. Coordinates and an azimuth of the mobile robot can be detected by correcting the centric coordinates of the detected mark by using a coordinate transformation matrix according to the two axis inclination information.

robot, such as a roll angle and a pitch angle, by using two axis inclinometer.

Artificial landmark to enhance reflective gain is shown in Figure 7.

Fig. 7. Retro high gain reflective landmarks

 (a) Landmark (b) enlarged landmark

**4.2 Technical solution** 

part and an image processing unit. It can calculate high resolution and high speed localization of position and heading angle. Also, landmark is used by being attached on ceiling. This localization system doesn't require any device for any synchronization or communication between a robot and a landmark. The area that localization system covers is extended by only adding landmarks to ceiling. Then each section can be distinguished easily each other by using landmarks with different IDs. This system automatically measure and calibrate distance between landmarks and ceiling height. The greatest advantage of this system is that it is nearly not affected in environment such as lamp and sunlight and works excellent localization function at night as well as in the daytime.

#### **4.1 Background art**

In general, to control an indoor mobile robot, it is required to recognize a position of the robot. There are two self-localization calculation methods performed by a robot itself using a camera. First, there is a method of using an artificial landmark. a landmark having a certain meaning is installed on a ceiling or a wall, the landmark is photographed by a CMOS image sensor, the landmark is extracted from an image, and coordinates on a screen are allowed to be identical with coordinates of a mobile robot, thereby calculating a self-localization of the mobile robot by itself On the other hand, the landmark is installed on the top of the mobile robot and the CMOS image sensor is installed on the ceiling. Second, there is a method of using a natural landmark. A ceiling is photographed by a camera; information of structures such as lightings installed on the ceiling and straight lines and edges of interfaces between the ceiling and walls is extracted, thereby calculating a self-localization of a mobile robot by itself using the information. However, when using the artificial landmark, the artificial landmark may be affected by lightings and color information of the landmark may be distorted by sunlight. Also, when using the natural landmark, since the natural landmark is much affected by brightness of an ambient light and there is required odometer information or another robot position reader when recording a position of a feature of the landmark, a large memory is required and an additional device is essential. Particularly, when there is no illumination, it is very difficult to use the natural landmark. Accordingly, it is required a new self-localization recognition method of a mobile robot that is capable of be disaffected by lightings and reduces a calculation time of the image processing. Also, when using the two conventional methods described above, since coordinates of a camera and coordinates of a landmark attached to a ceiling calculate position information of a mobile robot while assuming that there is no rotation of in directions excluding the direction of gravity, the robot position information calculated using an image obtained by the camera may have many errors when the mobile robot goes over a small mound or is inclined by an external force or an inertial force of rapid acceleration or deceleration. On the other hand, though there may be an initial correction for an inclination occurring when attaching one of a CMOS or CCD sensors used for a camera device to a robot, the initial correction is merely for an error occurring when it is initially installing in the robot but not for an error caused by an inclination occurring while the robot actually is driving.

An aspect of the present localization system provides a landmark for recognizing a position of a mobile robot, the landmark capable of allowing one of position and area information of the mobile robot to be detected from a landmark image photographed by a camera and being recognized regardless of indoor lightings. Also it provides an apparatus and method for recognizing a position of a mobile robot, in which an image of the landmark is obtained by an infrared camera and the position and area information of a gas cutting robot can be obtained without particular preprocessing the obtained image. This proposed method provides an apparatus and method for recognizing a position of a mobile robot, the apparatus having an inclination correction function to precisely recognizing the position of the mobile robot by correcting position information of an image of a landmark photographed by an infrared camera by measuring inclination information of the mobile robot, such as a roll angle and a pitch angle, by using two axis inclinometer.

#### **4.2 Technical solution**

434 Recent Advances in Mobile Robotics

part and an image processing unit. It can calculate high resolution and high speed localization of position and heading angle. Also, landmark is used by being attached on ceiling. This localization system doesn't require any device for any synchronization or communication between a robot and a landmark. The area that localization system covers is extended by only adding landmarks to ceiling. Then each section can be distinguished easily each other by using landmarks with different IDs. This system automatically measure and calibrate distance between landmarks and ceiling height. The greatest advantage of this system is that it is nearly not affected in environment such as lamp and sunlight and works

In general, to control an indoor mobile robot, it is required to recognize a position of the robot. There are two self-localization calculation methods performed by a robot itself using a camera. First, there is a method of using an artificial landmark. a landmark having a certain meaning is installed on a ceiling or a wall, the landmark is photographed by a CMOS image sensor, the landmark is extracted from an image, and coordinates on a screen are allowed to be identical with coordinates of a mobile robot, thereby calculating a self-localization of the mobile robot by itself On the other hand, the landmark is installed on the top of the mobile robot and the CMOS image sensor is installed on the ceiling. Second, there is a method of using a natural landmark. A ceiling is photographed by a camera; information of structures such as lightings installed on the ceiling and straight lines and edges of interfaces between the ceiling and walls is extracted, thereby calculating a self-localization of a mobile robot by itself using the information. However, when using the artificial landmark, the artificial landmark may be affected by lightings and color information of the landmark may be distorted by sunlight. Also, when using the natural landmark, since the natural landmark is much affected by brightness of an ambient light and there is required odometer information or another robot position reader when recording a position of a feature of the landmark, a large memory is required and an additional device is essential. Particularly, when there is no illumination, it is very difficult to use the natural landmark. Accordingly, it is required a new self-localization recognition method of a mobile robot that is capable of be disaffected by lightings and reduces a calculation time of the image processing. Also, when using the two conventional methods described above, since coordinates of a camera and coordinates of a landmark attached to a ceiling calculate position information of a mobile robot while assuming that there is no rotation of in directions excluding the direction of gravity, the robot position information calculated using an image obtained by the camera may have many errors when the mobile robot goes over a small mound or is inclined by an external force or an inertial force of rapid acceleration or deceleration. On the other hand, though there may be an initial correction for an inclination occurring when attaching one of a CMOS or CCD sensors used for a camera device to a robot, the initial correction is merely for an error occurring when it is initially installing in the robot but not for

excellent localization function at night as well as in the daytime.

an error caused by an inclination occurring while the robot actually is driving.

An aspect of the present localization system provides a landmark for recognizing a position of a mobile robot, the landmark capable of allowing one of position and area information of the mobile robot to be detected from a landmark image photographed by a camera and being recognized regardless of indoor lightings. Also it provides an apparatus and method for recognizing a position of a mobile robot, in which an image of the landmark is obtained by an infrared camera and the position and area information of a gas cutting robot can be obtained without particular preprocessing the obtained image. This proposed method

**4.1 Background art** 

According to aspect of the present localization system, the landmark is used as recognizing coordinates and azimuth information of a mobile robot. The landmark including a position recognition part is formed of a mark in any position and at least two marks on an X axis and Y axis centered on the landmark. The landmark may further include an area recognition part formed of a combination of a plurality of marks to distinguish an individual landmark from others. According to another aspect of the present localization system, there is an apparatus provided for recognizing a position of a mobile robot. The apparatus including: an infrared lighting unit irradiating an infrared ray to a landmark formed of a plurality of marks reflecting the infrared; an infrared camera photographing the landmark and obtaining a binary image; a mark detector labeling a partial image included in the binary image and detecting the mark by using a number and/or dispersion of labeled pixels for each the partial image; and a position detector detecting coordinates and an azimuth of the mobile robot by using centric coordinates of the detected mark. The landmark may include a position recognition part formed of a mark in any position and at least two marks located on an X axis and Y axis centered on the mark. Also, it is provided a method of recognizing a position of a mobile robot having an inclination correction function, the method including: (a) obtaining a binary image by irradiating an infrared ray to a landmark including a position recognition part formed of a mark in any position and at least two marks located on an X axis and Y axis centered on the mark to reflect the infrared ray and photographing the landmark; (b) detecting two-axis inclination information of the mobile robot to the ground and obtaining a binary image again when the detected two-axis inclination information is more than a predetermined threshold; (c) labeling a partial image included in the binary image and detecting the mark by using a number and/or dispersion of labeled pixels for each the partial image; and (d) detecting coordinates and an azimuth of the mobile robot by using centric coordinates of the detected mark. Coordinates and an azimuth of the mobile robot can be detected by correcting the centric coordinates of the detected mark by using a coordinate transformation matrix according to the two axis inclination information. Artificial landmark to enhance reflective gain is shown in Figure 7.

 (a) Landmark (b) enlarged landmark Fig. 7. Retro high gain reflective landmarks

finally obtained coordinate value is not a coordinate value of the retinal plane but a pixel coordinate value. When pixel coordinates corresponding to *mr* is [ ]*<sup>T</sup> m uv p pp* , a transform

> *p ur p vr*

*u ku u v kv v* 

wherein α and β are values indicating a scale transformation between the two coordinates

The relationship given by Eq. (2) is effective when an array of the CCD array is formed by a perfect right angle. However, since it is actually difficult to form a perfect right angle, it is required to obtain a relationship equation considering the difficulty. As shown in Figure 8, when an angle formed by both axes forming the pixel coordinates is designated as θ, there is a relationship between coordinates on the retinal plane and the pixel coordinates, as follows.

0 0

0

0 0

 

( ) *MC w RM t* (5)

0

 

cot

*u ku k v u*

 

When applying Eq. (1) to Eq. (3), a relationship equation between the three dimensional coordinates on the camera coordinates and the pixel coordinates is finally obtained as

0 0

*C C CC*

*X Y XY u uu Z Z ZZ*

*C C CC*

csc *p ur u r*

*v k vv*

*pv r*

cot

 

*C C*

 

As described above, the intrinsic parameters are formed of five such as α, β, γ, *u*<sup>0</sup> , and 0 *v* .

Generally, points on a three dimensional space are described in different coordinates from the camera coordinates, the coordinates generally designated as world coordinates. Accordingly, a transformation equation from the world coordinates to the camera coordinates is required, the transformation equation capable of being shown by a displacement vector indicating a relative position between origin points of respective coordinates and a rotation matrix showing a rotation amount of each coordinate axis. When a point shown in world coordinates is [ ]*<sup>T</sup> M XYZ w ww w* and is converted into *Mc* in the camera coordinates, a relationship equation between Mw and Mc is shown as follows.

wherein R indicates the rotation matrix and t indicates the displacement vector. Since R includes three independent parameters and t also includes three independent parameters, the number of extrinsic parameters is six. Hereinafter, it will be described to show a camera

A pinhole model of a camera may be linearly shown by using a concept of homogeneous coordinates. When a point on two dimensional pixel coordinates is defined as [ ]*<sup>T</sup> m uv* and the coordinates on three dimensional world coordinates corresponding to the point, are

*Y Y v vv Z Z*

*C C*

csc

*p v*

Hereinafter, the extrinsic parameters will be described.

 

mode using projective geometry.

*pu u*

(2)

(3)

(4)

relationship between the two coordinates is provided as follows.

and *u*0 and 0 *v* are pixel coordinates values of the principal point c.

follows.

#### **4.3 Camera calibration**

A pinhole model generally uses cameral model when describing an image process. Figure 8 is a configuration diagram illustrating the pinhole model. Referring to Figure 8, a point *mr* of a point *Mc* on a three dimensional space corresponds to a point at which a straight line connecting the point *Mc* to a point C meets with a plane *r* . In this case, the point C is designated as an optical center, and the plane *r* is designated as a retinal plane. A straight line passing the point C and vertical to the plane *r* may exist, which is designated as an optical axis. Generally, the point C is allowed to be an origin point of camera coordinates, and the optical axis is allowed to be identical with Z axis of an orthogonal coordinate system. After the camera model is determined, a structure of the camera may be expressed with various parameters. The parameters may be divided into two kinds of parameters used for describing a camera, intrinsic parameters and extrinsic parameters.

Fig. 8. Camera Model & In case: out of square in CCD array.

The intrinsic parameters describe corresponding relationships between points on the camera coordinates, which is expressed within three dimensional coordinates and the retinal plane with two dimensional coordinates, where the points are projected. The extrinsic parameters describe a transform relationship between the camera coordinates and world coordinates. Hereinafter, the intrinsic parameters will be described.

Referring to Figure 8, it may be known that a relationship between points [ ]*<sup>T</sup> M XYZ C CC C* on the camera coordinates and a point [ ]*<sup>T</sup> m uv r rr* on the corresponding retinal plane is provided as follows.

$$\begin{cases} u\_r = f \frac{X\_\mathbb{C}}{Z\_\mathbb{C}} \\ v\_r = f \frac{Y\_\mathbb{C}}{Z\_\mathbb{C}} \end{cases} \tag{1}$$

wherein *f* indicates a focal length that is a distance between the optical center C and a point c at which the optical axis meets the retinal plane. The point c indicates a principal point. A phase formed on the retinal plane is sampled by the CCD or CMOS array, converted into a video signal, outputted from the camera, and stored in a frame buffer. Accordingly, a 436 Recent Advances in Mobile Robotics

A pinhole model generally uses cameral model when describing an image process. Figure 8 is a configuration diagram illustrating the pinhole model. Referring to Figure 8, a point *mr* of a point *Mc* on a three dimensional space corresponds to a point at which a straight line connecting the point *Mc* to a point C meets with a plane *r* . In this case, the point C is designated as an optical center, and the plane *r* is designated as a retinal plane. A straight line passing the point C and vertical to the plane *r* may exist, which is designated as an optical axis. Generally, the point C is allowed to be an origin point of camera coordinates, and the optical axis is allowed to be identical with Z axis of an orthogonal coordinate system. After the camera model is determined, a structure of the camera may be expressed with various parameters. The parameters may be divided into two kinds of parameters used

The intrinsic parameters describe corresponding relationships between points on the camera coordinates, which is expressed within three dimensional coordinates and the retinal plane with two dimensional coordinates, where the points are projected. The extrinsic parameters describe a transform relationship between the camera coordinates and world coordinates.

Referring to Figure 8, it may be known that a relationship between points [ ]*<sup>T</sup> M XYZ C CC C* on the camera coordinates and a point [ ]*<sup>T</sup> m uv r rr* on the corresponding retinal plane is

> *<sup>C</sup> <sup>r</sup> C*

(1)

*<sup>X</sup> u f <sup>Z</sup>*

 

at which the optical axis meets the retinal plane. The point c indicates a principal point.

*<sup>C</sup> <sup>r</sup> C*

*<sup>Y</sup> v f <sup>Z</sup>*

wherein *f* indicates a focal length that is a distance between the optical center C and a point c

A phase formed on the retinal plane is sampled by the CCD or CMOS array, converted into a video signal, outputted from the camera, and stored in a frame buffer. Accordingly, a

for describing a camera, intrinsic parameters and extrinsic parameters.

Fig. 8. Camera Model & In case: out of square in CCD array.

Hereinafter, the intrinsic parameters will be described.

provided as follows.

**4.3 Camera calibration** 

finally obtained coordinate value is not a coordinate value of the retinal plane but a pixel coordinate value. When pixel coordinates corresponding to *mr* is [ ]*<sup>T</sup> m uv p pp* , a transform relationship between the two coordinates is provided as follows.

$$\begin{cases} \boldsymbol{\mu}\_p = \boldsymbol{k}\_u \boldsymbol{\mu}\_r + \boldsymbol{\mu}\_0 \\ \boldsymbol{\upsilon}\_p = \boldsymbol{k}\_v \boldsymbol{\upsilon}\_r + \boldsymbol{\upsilon}\_0 \end{cases} \tag{2}$$

wherein α and β are values indicating a scale transformation between the two coordinates and *u*0 and 0 *v* are pixel coordinates values of the principal point c.

The relationship given by Eq. (2) is effective when an array of the CCD array is formed by a perfect right angle. However, since it is actually difficult to form a perfect right angle, it is required to obtain a relationship equation considering the difficulty. As shown in Figure 8, when an angle formed by both axes forming the pixel coordinates is designated as θ, there is a relationship between coordinates on the retinal plane and the pixel coordinates, as follows.

$$\begin{cases} \mu\_p = k\_u \mu\_r - k\_u \cot \theta \upsilon\_r + \mu\_0 \\ \upsilon\_p = k\_v \csc \theta \upsilon\_r + \upsilon\_0 \end{cases} \tag{3}$$

When applying Eq. (1) to Eq. (3), a relationship equation between the three dimensional coordinates on the camera coordinates and the pixel coordinates is finally obtained as follows.

$$\begin{cases} u\_p = \alpha\_u \frac{X\_\circ}{Z\_\circ} - \alpha\_u \cot \theta \frac{Y\_\circ}{Z\_\circ} + u\_0 = \alpha \frac{X\_\circ}{Z\_\circ} + \gamma \frac{Y\_\circ}{Z\_\circ} + u\_0 \\ v\_p = \alpha\_v \csc \theta \frac{Y\_\circ}{Z\_\circ} + v\_0 = \beta \frac{Y\_\circ}{Z\_\circ} + v\_0 \end{cases} \tag{4}$$

As described above, the intrinsic parameters are formed of five such as α, β, γ, *u*<sup>0</sup> , and 0 *v* . Hereinafter, the extrinsic parameters will be described.

Generally, points on a three dimensional space are described in different coordinates from the camera coordinates, the coordinates generally designated as world coordinates. Accordingly, a transformation equation from the world coordinates to the camera coordinates is required, the transformation equation capable of being shown by a displacement vector indicating a relative position between origin points of respective coordinates and a rotation matrix showing a rotation amount of each coordinate axis. When a point shown in world coordinates is [ ]*<sup>T</sup> M XYZ w ww w* and is converted into *Mc* in the camera coordinates, a relationship equation between Mw and Mc is shown as follows.

$$M\_{\mathbb{C}} = R(M\_w + t) \tag{5}$$

wherein R indicates the rotation matrix and t indicates the displacement vector. Since R includes three independent parameters and t also includes three independent parameters, the number of extrinsic parameters is six. Hereinafter, it will be described to show a camera mode using projective geometry.

A pinhole model of a camera may be linearly shown by using a concept of homogeneous coordinates. When a point on two dimensional pixel coordinates is defined as [ ]*<sup>T</sup> m uv* and the coordinates on three dimensional world coordinates corresponding to the point, are

rotation and 3 for translation), we can only obtain 2 constraints on the intrinsic parameters. Note that *<sup>T</sup>* <sup>1</sup> *A A* actually describes the image of the absolute conic(Luong and Faugeras, 1997). We use the closed-form solution in order to solve camera calibration problem among

0 0

*v u*

 

 

*<sup>T</sup> bB B B B B B* (14)

00 0

 

 

*vu v*

2 2

 

*i j ij h Bh v b* (15)

*Vb* 0 (17)

  (13)

(16)

0 . If *b* is solved, we

11 12 13

*BBB*

*BBB*

21 22 23 31 32 33

2

 

 

> 

with 11 12 21 22 31 13 32 23 33 [ ]*<sup>T</sup> ij i i i i i i i i i i i i i i i i i i v hh hh hh hh hh hh hh hh hh* .

up to a scale factor. If *n* 2 , we can impose the skewless constraint

2 33 13

11

*B*

/

/

 

can calculate *A* , *Rrrr* <sup>123</sup> and *t* as follows:

 

 

22 2

 

00 00 0 00 0 2 22 2 22 2

*vu vu v vu v*

 

 

2 22 2 22 2

 

11 12 22 13 23 33

*T T*

Therefore, the two functional constraints (11) and (12), from a given homography, can be

*T*

*b*

0 12 13 11 23 / 11

2

 12 11 22

If n images of the model plane are observed, by stacking n such equations as (17) we have

where *V* is a 2 6 *n* matrix. If *n* 3 , we will have in general a unique solution *b* defined

/

*B B vBB BB B*

0 12 13 11 23 11 22 12

*v BB BB BB B*

11 11 22 12

*B BB B*

2

*v v*

*v*

*T*

 

1 ( )

 

> 

( )( ) <sup>1</sup>

*T iiii hhhh* . Then, we have

0

1

*BAA B B B*

1

Note that *B* is symmetric, defined by a 6D vector

Let the *th i* column vector of *H* be <sup>123</sup>

rewritten as 2 homogeneous equations in *b* :

 

*T*

nonlinear optimization methods.

Let

defined as [ ] *M <sup>T</sup> XYZ* , homogeneous coordinates formed by adding 1 to the last term of the coordinates become [ 1]*<sup>T</sup> m uv* , and [ 1] *M XYZ <sup>T</sup>* .

A relationship equation between the three dimensional point M and m that is formed by projecting the point M is expressed using the described pinhole model as follows.

$$\mathbf{s}\tilde{\mathbf{m}} = A\mathbf{[R\ t]}\tilde{\mathbf{M}}\tag{6}$$

wherein s is a scale factor and R and t are a rotation matrix and a displacement vector, respectively, which are extrinsic parameters. A is a matrix of the intrinsic parameter and is designated as a calibration matrix.

$$A = \begin{bmatrix} \alpha & \gamma & u\_0 \\ 0 & \beta & v\_0 \\ 0 & 0 & 1 \end{bmatrix} \tag{7}$$

wherein α and β correspond to scale values to u and v axes, γ corresponds to a skewness of two image axes, and *u*0 and 0 *v* are principal points. We use the abbreviation *<sup>T</sup> <sup>A</sup>* for <sup>1</sup> *<sup>T</sup> <sup>A</sup>* or <sup>1</sup> *<sup>T</sup> <sup>A</sup>* .

Homography between the model plane and its image without loss of generality, we assume the model plane is on *Z* 0 of the world coordinate system. Let's denote the *th i* column of the rotation matrix *R* by *ir* . From Eq. (6), we have

$$\begin{bmatrix} s \\ v \\ 1 \end{bmatrix} = A \begin{bmatrix} r\_1 & r\_2 & r\_3 & t \end{bmatrix} \begin{bmatrix} X \\ Y \\ 0 \\ 1 \end{bmatrix} = A \begin{bmatrix} r\_1 & r\_2 & t \end{bmatrix} \begin{bmatrix} X \\ Y \\ 1 \end{bmatrix} \tag{8}$$

By abuse of notation, we still use a point on the model plane, but *<sup>T</sup> M X Y* since Z is always equal to 0. In turn, 1 *<sup>T</sup> M XY* . Therefore, a model point *M* and its image *m* is related by a homography *H* :

$$\mathbf{r} \cdot \mathbf{s} \widetilde{\mathbf{m}} = \mathbf{H} \widetilde{\mathbf{M}} \text{ with } \mathbf{H} = \begin{bmatrix} r\_1 & r\_2 & t \end{bmatrix} \tag{9}$$

As is clear, the 3 3 matrix is defined up to a scale factor. Given an image of the model plane, an homography can be estimated. Let's denote it by *Hhhh* <sup>123</sup> . From Eq. (7), we have

$$
\begin{bmatrix} h\_1 & h\_2 & h\_3 \end{bmatrix} = \lambda A \begin{bmatrix} r\_1 & r\_2 & t \end{bmatrix} \tag{10}
$$

where is an arbitrary scalar. Using the knowledge that 1*r* and 2*r* are orthonormal, we have

$$\mathbf{h}\_1^T \mathbf{A}^{-T} \mathbf{A}^{-1} \mathbf{h}\_2 = \mathbf{0} \tag{11}$$

$$\mathbf{h\_1^T A^{-T} A^{-1} h\_1 = h\_2^T A^{-T} A^{-1} h\_2} \tag{12}$$

These are the two basic constraints on the intrinsic parameters, given one homography. Because a homography has 8 degrees of freedom and there are 6 extrinsic parameters (3 for rotation and 3 for translation), we can only obtain 2 constraints on the intrinsic parameters. Note that *<sup>T</sup>* <sup>1</sup> *A A* actually describes the image of the absolute conic(Luong and Faugeras, 1997). We use the closed-form solution in order to solve camera calibration problem among nonlinear optimization methods.

Let

438 Recent Advances in Mobile Robotics

defined as [ ] *M <sup>T</sup> XYZ* , homogeneous coordinates formed by adding 1 to the last term of

A relationship equation between the three dimensional point M and m that is formed by

wherein s is a scale factor and R and t are a rotation matrix and a displacement vector, respectively, which are extrinsic parameters. A is a matrix of the intrinsic parameter and is

> <sup>0</sup> 0 00 1

wherein α and β correspond to scale values to u and v axes, γ corresponds to a skewness of two image axes, and *u*0 and 0 *v* are principal points. We use the abbreviation *<sup>T</sup> <sup>A</sup>* for

Homography between the model plane and its image without loss of generality, we assume

 <sup>123</sup> 1 2 <sup>0</sup> 1 1 1

*sm HM with H r r t* 1 2 (9)

(10)

1 2 <sup>0</sup> *T T hA A h* (11)

*TT TT hA A h hA A h* (12)

By abuse of notation, we still use a point on the model plane, but *<sup>T</sup> M X Y* since Z is always equal to 0. In turn, 1 *<sup>T</sup> M XY* . Therefore, a model point *M* and its image *m* is

As is clear, the 3 3 matrix is defined up to a scale factor. Given an image of the model plane, an homography can be estimated. Let's denote it by *Hhhh* <sup>123</sup> . From Eq. (7), we have

> *h h h Ar r t* 1 2 3 12

is an arbitrary scalar. Using the knowledge that 1*r* and 2*r* are orthonormal, we have

1

These are the two basic constraints on the intrinsic parameters, given one homography. Because a homography has 8 degrees of freedom and there are 6 extrinsic parameters (3 for

1 1 1 12 2

*X u X Y s v Ar r r t Ar r t Y* 

the model plane is on *Z* 0 of the world coordinate system. Let's denote the *th*

*A v* 

0

*u*

*sm A R t M* [ ] (6)

(7)

(8)

*i* column of

projecting the point M is expressed using the described pinhole model as follows.

the coordinates become [ 1]*<sup>T</sup> m uv* , and [ 1] *M XYZ <sup>T</sup>* .

the rotation matrix *R* by *ir* . From Eq. (6), we have

designated as a calibration matrix.

<sup>1</sup> *<sup>T</sup>*

where 

*<sup>A</sup>* or <sup>1</sup> *<sup>T</sup> <sup>A</sup>* .

related by a homography *H* :

$$\begin{aligned} B &= A^{-T} A^{-1} \equiv \begin{bmatrix} B\_{11} & B\_{12} & B\_{13} \\ B\_{21} & B\_{22} & B\_{23} \\ B\_{31} & B\_{32} & B\_{33} \end{bmatrix} \\ &= \begin{bmatrix} 1 & -\frac{\gamma}{\alpha^2} & \frac{\gamma}{\alpha^2 \beta} & \frac{v\_0 \gamma - u\_0 \beta}{\alpha^2 \beta} \\ -\frac{\gamma}{\alpha^2 \beta} & \frac{\gamma^2}{\alpha^2 \beta^2} + \frac{1}{\beta^2} & -\frac{\gamma (v\_0 \gamma - u\_0 \beta)}{\alpha^2 \beta^2} - \frac{v\_0}{\beta^2} \\ \frac{v\_0 \gamma - u\_0 \beta}{\alpha^2 \beta} & -\frac{\gamma (v\_0 \gamma - u\_0 \beta)}{\alpha^2 \beta^2} - \frac{v\_0}{\beta^2} & \frac{(v\_0 \gamma - u\_0 \beta)^2}{\alpha^2 \beta^2} + \frac{v\_0^2}{\beta^2} + 1 \end{bmatrix} \tag{13}$$

Note that *B* is symmetric, defined by a 6D vector

$$b = \begin{bmatrix} B\_{11} & B\_{12} & B\_{22} & B\_{13} & B\_{23} & B\_{33} \end{bmatrix}^T \tag{14}$$

Let the *th i* column vector of *H* be <sup>123</sup> *T iiii hhhh* . Then, we have

$$\mathbf{h}\_i^T \mathbf{B} \mathbf{h}\_j = \mathbf{v}\_{ij}^T \mathbf{b} \tag{15}$$

with 11 12 21 22 31 13 32 23 33 [ ]*<sup>T</sup> ij i i i i i i i i i i i i i i i i i i v hh hh hh hh hh hh hh hh hh* .

Therefore, the two functional constraints (11) and (12), from a given homography, can be rewritten as 2 homogeneous equations in *b* :

$$\begin{bmatrix} v\_{12}^T \\ \left(v\_{11} - v\_{22}\right)^T \end{bmatrix} b = 0 \tag{16}$$

If n images of the model plane are observed, by stacking n such equations as (17) we have

$$Vb = 0\tag{17}$$

where *V* is a 2 6 *n* matrix. If *n* 3 , we will have in general a unique solution *b* defined up to a scale factor. If *n* 2 , we can impose the skewless constraint 0 . If *b* is solved, we can calculate *A* , *Rrrr* <sup>123</sup> and *t* as follows:

$$\begin{aligned} \upsilon\_0 &= \left(B\_{12}B\_{13} - B\_{11}B\_{23}\right) / \left(B\_{11}B\_{22} - B\_{12}^2\right) \\ \mathcal{A} &= B\_{33} - \left[B\_{13}^2 + \upsilon 0 \left(B12B13 - B11B23\right)\right] / B11 \\ \alpha &= \sqrt{\mathcal{A} / B\_{11}} \\ \mathcal{B} &= \sqrt{\mathcal{A}B\_{11} / \left(B\_{11}B\_{22} - B\_{12}^2\right)} \end{aligned}$$

1 2 2 1 2

Experimentally, we found the convergence of the above alternation technique is slow. A natural extension to Eq. (20) is then to estimate the complete set of parameters by

*pxy p r x t M*

2 ( 2) ( 2 )2

*p r y pxy* 

1212

*ij ii j*

*m mAk k p p R t M*

where 1 2 ˆ , , , ,, *m Ak k R t M i i <sup>j</sup>* is the projection of point *Mj* in image *i* according to Eq. (6), followed by distortion according to Eq. (11) and (12) (Zhang, 1997). This is a nonlinear minimization problem, which is solved with the Levenberg-Marquardt Algorithm (More, 1977). A rotation is again parameterized by a 3 vector *r* . An initial guess of *A* and *R ti n i i* 1 can be obtained using the prescribed technique. An initial guess of 1 *k* and <sup>2</sup> *k* can be obtained with the technique described in the last paragraph technique, or simply

A vector allowing the coordinates and the azimuth of the mobile robot to be known may be obtained by using a calibration equation, which is disclosed in detail in several theses as follows. A transform relationship of projecting a point on world coordinates to camera pixel

When a roll angle and a pitch angle corresponding to inclination of a camera are α and β, respectively, such a degree of inclination as α and β is expressed in a matrix as follows.

> 10 0 0 cos sin 0 sin cos

cos 0 sin 0 10 sin 0 cos

> 

 (24)

(25)

ˆ (, , , , , ,, )

*n n n*

*n nn*

2

, (23)

2

. (22)

1 1

*i j*

coordinates will be described referring to Figure 9.

Fig. 9. Diagram illustrating a coordinate system when a camera rotates.

*R*

*R*

*n m*

minimizing the following functional:

by setting them to 0.

*n*

$$\begin{aligned} \gamma &= -B\_{12}\alpha^2 \beta \ne \lambda \\ \mu 0 &= \gamma v\_0 \ne \beta - B\_{13}\alpha^2 \ne \lambda \\ r\_1 &= sA^{-1}h\_1 \\ r\_2 &= sA^{-1}h\_2 \\ r\_3 &= r\_1 \times r\_2 \\ t &= sA^{-1}h\_3 \\ s &= 1 \ne \left\| A^{-1}h\_1 \right\| = 1 \ne \left\| A^{-1}h\_2 \right\| \end{aligned} \tag{18}$$

The above solution is obtained through minimizing an algebraic distance which is not physically meaningful. We can refine it through maximum likelihood inference. We are given n images of a model plane and there are m points on the model plane. Assume that the image points are corrupted by independent and identically distributed noise. The maximum likelihood estimation can be obtained by minimizing the following functional:

$$\sum\_{i=1}^{n} \sum\_{j=1}^{m} \left\| m\_{ij} - \hat{m}(A\_{\prime} R\_{i\prime} t\_{i\prime} M\_j) \right\|^2 \tag{19}$$

where ˆ (, ,, ) *mAR t M i i <sup>j</sup>* is the projection of point *Mj* in image *i* , according to Eq. (7). A rotation *R* is parameterized by a vector of 3 parameters, denoted by *r* , which is parallel to the rotation axis and whose magnitude is equal to the rotation angle. *R* and *r* are related by the Rodrigues formula (Faugeras, 1993). Minimizing Eq. (20) is a nonlinear minimization problem, which is solved with the Levenberg-Marquardt algorithm (More, 1977). It requires an initial guess of *A* , *R ti n i i* 1 which can be obtained using the technique described in the previous subsection. Up to now, we have not considered lens distortion of a camera. However, a desktop camera usually exhibits significant lens distortion, especially radial distortion. Therefore, we only consider the first two terms of radial distortion.

Let *u v* , be the ideal (nonobservable distortion-free) pixel image coordinates, and *u v* ˆ ˆ , the corresponding real observed image coordinates. The ideal points are the projection of the model points according to the pinhole model. Similarly, *x y* , and *x y* ˆ ˆ , are the ideal(distortion-free) and real(distorted) normalized image coordinates. When we represent pixels of camera coordinates to *<sup>T</sup> MC XC YC ZC* , normalized pixels is changed as follows:

$$\mathbf{M}\_n = \begin{bmatrix} X\_{\mathbb{C}} \ / Z\_{\mathbb{C}} \\ \mathbf{Y}\_{\mathbb{C}} \ / Z\_{\mathbb{C}} \end{bmatrix} = \begin{bmatrix} x\_n \\ y\_n \end{bmatrix} \tag{20}$$

where *<sup>n</sup> x* , *<sup>n</sup> y* is normalized coordinates vale. Let 2 2 *n n rx y* . If *Md* is assumed by distorted coordinates,

$$M\_d = \begin{bmatrix} \mathbf{x}\_d \\ \mathbf{y}\_d \end{bmatrix} = M\_n \left( \mathbf{1} + k\_1 r + k\_2 r^2 + \cdots \right) + t \left( M\_n \right) \cdot \tag{21}$$

Where <sup>2</sup> 1 2 1 *M kr kr <sup>n</sup>* is the radial distortion and *t M <sup>n</sup>* is the tangential distortion. Finally, *<sup>n</sup> t M* can be solved as follows:

440 Recent Advances in Mobile Robotics

/ 0/ /

2

(18)

 

1 1 1 2

ˆ (, ,, )

*ij ii j*

*m mAR t M*

where ˆ (, ,, ) *mAR t M i i <sup>j</sup>* is the projection of point *Mj* in image *i* , according to Eq. (7). A rotation *R* is parameterized by a vector of 3 parameters, denoted by *r* , which is parallel to the rotation axis and whose magnitude is equal to the rotation angle. *R* and *r* are related by the Rodrigues formula (Faugeras, 1993). Minimizing Eq. (20) is a nonlinear minimization problem, which is solved with the Levenberg-Marquardt algorithm (More, 1977). It requires an initial guess of *A* , *R ti n i i* 1 which can be obtained using the technique described in the previous subsection. Up to now, we have not considered lens distortion of a camera. However, a desktop camera usually exhibits significant lens distortion, especially

radial distortion. Therefore, we only consider the first two terms of radial distortion.

Let *u v* , be the ideal (nonobservable distortion-free) pixel image coordinates, and *u v* ˆ ˆ , the corresponding real observed image coordinates. The ideal points are the projection of the model points according to the pinhole model. Similarly, *x y* , and *x y* ˆ ˆ , are the ideal(distortion-free) and real(distorted) normalized image coordinates. When we represent pixels of camera coordinates to *<sup>T</sup> MC XC YC ZC* , normalized pixels is changed as

> / /

1 2 <sup>1</sup> *<sup>d</sup> d n n*

*M M kr kr t M*

*n*

*M*

*n n rx y* . If *Md* is assumed by distorted coordinates,

*d x*

*y* 

where *<sup>n</sup> x* , *<sup>n</sup> y* is normalized coordinates vale.

*CC n*

*CC n XZ x*

<sup>2</sup>

1 2 1 *M kr kr <sup>n</sup>* is the radial distortion and *t M <sup>n</sup>* is the tangential distortion.

*YZ y*  2

(19)

, (20)

. (21)

2 12

> 

 

 

*B uv B*

*r sA h r sA h r rr t sA h*

 

1 1

*i j*

follows:

Let 2 2

Where <sup>2</sup>

Finally, *<sup>n</sup> t M* can be solved as follows:

*n m*

0 13

1/ 1/

*s Ah Ah*

The above solution is obtained through minimizing an algebraic distance which is not physically meaningful. We can refine it through maximum likelihood inference. We are given n images of a model plane and there are m points on the model plane. Assume that the image points are corrupted by independent and identically distributed noise. The maximum likelihood estimation can be obtained by minimizing the following functional:

$$\mathbf{t}\begin{pmatrix} M\_n \end{pmatrix} = \begin{bmatrix} 2p\_1 \mathbf{x}\_n y\_n + p\_2 \left( r + 2\mathbf{x}\_n^2 \right) \\ p\_1 \left( r + 2y\_n^2 \right) + 2p\_2 \mathbf{x}\_n y\_n \end{bmatrix} \tag{22}$$

Experimentally, we found the convergence of the above alternation technique is slow. A natural extension to Eq. (20) is then to estimate the complete set of parameters by minimizing the following functional:

$$\sum\_{i=1}^{n} \sum\_{j=1}^{m} \left\| m\_{ij} - \hat{m}(A\_i k\_1, k\_2, p\_1, p\_2, R\_i, t\_i, M\_j) \right\|^2,\tag{23}$$

where 1 2 ˆ , , , ,, *m Ak k R t M i i <sup>j</sup>* is the projection of point *Mj* in image *i* according to Eq. (6), followed by distortion according to Eq. (11) and (12) (Zhang, 1997). This is a nonlinear minimization problem, which is solved with the Levenberg-Marquardt Algorithm (More, 1977). A rotation is again parameterized by a 3 vector *r* . An initial guess of *A* and *R ti n i i* 1 can be obtained using the prescribed technique. An initial guess of 1 *k* and <sup>2</sup> *k* can be obtained with the technique described in the last paragraph technique, or simply by setting them to 0.

A vector allowing the coordinates and the azimuth of the mobile robot to be known may be obtained by using a calibration equation, which is disclosed in detail in several theses as follows. A transform relationship of projecting a point on world coordinates to camera pixel coordinates will be described referring to Figure 9.

Fig. 9. Diagram illustrating a coordinate system when a camera rotates.

When a roll angle and a pitch angle corresponding to inclination of a camera are α and β, respectively, such a degree of inclination as α and β is expressed in a matrix as follows.

$$R\_{\alpha} = \begin{vmatrix} 1 & 0 & 0 \\ 0 & \cos a & \sin a \\ 0 & -\sin a & \cos a \end{vmatrix} \tag{24}$$

$$R\_{\beta} = \begin{bmatrix} \cos \beta & 0 & \sin \beta \\ 0 & 1 & 0 \\ -\sin \beta & 0 & \cos \beta \end{bmatrix} \tag{25}$$

binary coded, thereby quickly recognizing area information of the mobile robot. On the other hand, an infrared reflection coating may be applied or a reflection sheet may be attached to the marks forming the landmark in order to diffusely reflect an infrared ray in a certain wavelength band, particularly, a wavelength band of 800 to 1200 nm. Accordingly, not only in the night but also when there exist reflected lights, only an infrared ray reflected by the mark is detected by an infrared camera, thereby quickly recognizing the position of the mobile robot without using other image processing methods. In this case, the mark may be formed in the shape of only a circle of a predetermined size or may be formed in the protruded shape of one of a circle on a plane and a hemisphere from a plane. The mark formed in the shape of one of the circle and the hemisphere may be used for easily obtaining a number, dispersion, and centric coordinates of pixels when detecting the mark. Though the marks may be formed identically with each other, the marks for the position recognition part are formed different from those for the area recognition part in size and/or color, thereby easily distinguishing the position recognition part from the area recognition part. The mark forming the landmark, described above, may be applied to a conventional mobile robot position recognition apparatus without using an infrared camera, and a use of the marks is not limited to a position recognition apparatus according to an exemplary

Next, an apparatus and method for recognizing a position of a mobile robot, according to an exemplary embodiment of the proposed localization system, will be described in the order of operations. The embodiment may be applied to when a space in which the mobile robot moves or a space to which a landmark is attached has no bend and is flat. An infrared light

Fig. 10. Design of artificial landmark.

embodiment of the proposed localization system.

wherein homogeneous pixel coordinates *m* corresponding to a scale parameter are obtained as follows.

$$\text{is } \tilde{m}\_{\alpha\beta} = R\_{\alpha} R\_{\beta} A \begin{bmatrix} R & \mathfrak{t} \end{bmatrix} \tilde{M} \tag{26}$$

When assuming the displacement vector t to be known, the point M on the world coordinates may be obtained as follows.

$$M = \text{sR}^{T}\left(A^{-1}R\_{\beta}^{T}R\_{\alpha}^{T}\tilde{m}\_{\alpha\beta} - t\right) \tag{27}$$

When the point M corresponding to a reference point is known, the displacement vector t that is finally to be calculated is obtained as follows, thereby calculating a self-localization of the mobile robot.

$$t = \text{s}A^{-1} \mathcal{R}\_{\beta}^{T} \mathcal{R}\_{\alpha}^{T} \tilde{\boldsymbol{m}}\_{\alpha\beta} - \text{RM} \tag{28}$$

As described above, the vector amount allowing the coordinates and azimuth of the mobile robot to be simultaneously known may be obtained by the vector operation using the three detected marks of the position recognition part and the calibration equation, thereby embodying a microprocessor at a low price.

#### **4.4 Coordinates calculation for mobile robot**

The proposed localization system includes a landmark indicating position information such as coordinates and an azimuth of a mobile robot, a position recognition apparatus, and method. A landmark indicating a position of a mobile robot will be described with reference to Figure 10.

According to an embodiment of the proposed localization system, the landmark is attached to a ceiling of a space, in which the mobile robot moves, and is photographed by a camera installed on the mobile robot or attached to a top of the mobile robot and photographed by a camera installed on the ceiling to be used for recognizing the position of the mobile robot. A landmark according to an exemplary embodiment of the present invention includes a position recognition part formed of three marks to recognize essential position information such as coordinates, an azimuth of a mobile robot, and an area recognition part formed of a plurality of marks to distinguish an individual landmark from others to recognize additional area information of the mobile robot. The position recognition part is formed of one mark B in any position and two marks C and A located on an X axis and Y axis, respectively, centered on the mark B. The three marks B, A, and C provide the landmark with a reference point and reference coordinates. Though there is mark shown in Figure 10, the number of the marks is not limited to this and more than two marks may be used. Though the area recognition part is formed of 4×4 marks inside the position recognition part as shown in Figure 10, a position and the number of the marks forming the position recognition part may be varied according to purpose. By giving an ID corresponding to the number and the position of each of the marks forming the area recognition part, each individual landmark may be distinguished from others. As shown in Figure 10, when the area recognition part is formed of the 3×3 marks, IDs of 512 is given. In this case, the position of the mark forming the area recognition part may be determined according to the reference coordinates provided by the position recognition part and each of the IDs may be

442 Recent Advances in Mobile Robotics

*sm R R A R t M*

When assuming the displacement vector t to be known, the point M on the world

 *M T TT* <sup>1</sup> *sR A R R m t* 

When the point M corresponding to a reference point is known, the displacement vector t that is finally to be calculated is obtained as follows, thereby calculating a self-localization of

> <sup>1</sup> *T T t sA R R m RM*

As described above, the vector amount allowing the coordinates and azimuth of the mobile robot to be simultaneously known may be obtained by the vector operation using the three detected marks of the position recognition part and the calibration equation, thereby

The proposed localization system includes a landmark indicating position information such as coordinates and an azimuth of a mobile robot, a position recognition apparatus, and method. A landmark indicating a position of a mobile robot will be described with reference

According to an embodiment of the proposed localization system, the landmark is attached to a ceiling of a space, in which the mobile robot moves, and is photographed by a camera installed on the mobile robot or attached to a top of the mobile robot and photographed by a camera installed on the ceiling to be used for recognizing the position of the mobile robot. A landmark according to an exemplary embodiment of the present invention includes a position recognition part formed of three marks to recognize essential position information such as coordinates, an azimuth of a mobile robot, and an area recognition part formed of a plurality of marks to distinguish an individual landmark from others to recognize additional area information of the mobile robot. The position recognition part is formed of one mark B in any position and two marks C and A located on an X axis and Y axis, respectively, centered on the mark B. The three marks B, A, and C provide the landmark with a reference point and reference coordinates. Though there is mark shown in Figure 10, the number of the marks is not limited to this and more than two marks may be used. Though the area recognition part is formed of 4×4 marks inside the position recognition part as shown in Figure 10, a position and the number of the marks forming the position recognition part may be varied according to purpose. By giving an ID corresponding to the number and the position of each of the marks forming the area recognition part, each individual landmark may be distinguished from others. As shown in Figure 10, when the area recognition part is formed of the 3×3 marks, IDs of 512 is given. In this case, the position of the mark forming the area recognition part may be determined according to the reference coordinates provided by the position recognition part and each of the IDs may be

 

corresponding to a scale parameter are

(26)

(27)

(28)

wherein homogeneous pixel coordinates *m*

coordinates may be obtained as follows.

embodying a microprocessor at a low price.

**4.4 Coordinates calculation for mobile robot** 

obtained as follows.

the mobile robot.

to Figure 10.

Fig. 10. Design of artificial landmark.

binary coded, thereby quickly recognizing area information of the mobile robot. On the other hand, an infrared reflection coating may be applied or a reflection sheet may be attached to the marks forming the landmark in order to diffusely reflect an infrared ray in a certain wavelength band, particularly, a wavelength band of 800 to 1200 nm. Accordingly, not only in the night but also when there exist reflected lights, only an infrared ray reflected by the mark is detected by an infrared camera, thereby quickly recognizing the position of the mobile robot without using other image processing methods. In this case, the mark may be formed in the shape of only a circle of a predetermined size or may be formed in the protruded shape of one of a circle on a plane and a hemisphere from a plane. The mark formed in the shape of one of the circle and the hemisphere may be used for easily obtaining a number, dispersion, and centric coordinates of pixels when detecting the mark. Though the marks may be formed identically with each other, the marks for the position recognition part are formed different from those for the area recognition part in size and/or color, thereby easily distinguishing the position recognition part from the area recognition part. The mark forming the landmark, described above, may be applied to a conventional mobile robot position recognition apparatus without using an infrared camera, and a use of the marks is not limited to a position recognition apparatus according to an exemplary embodiment of the proposed localization system.

Next, an apparatus and method for recognizing a position of a mobile robot, according to an exemplary embodiment of the proposed localization system, will be described in the order of operations. The embodiment may be applied to when a space in which the mobile robot moves or a space to which a landmark is attached has no bend and is flat. An infrared light

using the detected mark. The ID determined according to the number and position of the marks corresponding to the area recognition part from the detected marks may be quickly obtained, and the area information of the mobile robot may be obtained. In this case, the area information of the mobile robot is allocated to the ID and is an approximate position in which the mobile robot is located. Detailed position information of the mobile robot, such as the coordinates and the azimuth, may be obtained by using centric coordinates of the detected three marks A, B, and C forming the position recognition part. According to an exemplary embodiment of the present invention, coordinates of the mobile robot may be obtained by considering any point obtained from the centric coordinates of each of the three marks A, B, and C shown in Figure 10 as reference coordinates. In this case, the any point may be a center of gravity obtained by the centric coordinates of the three marks. In this case, since the center of gravity is an average of errors with respect to the centric coordinates of the three marks, an error with respect to the coordinates of the mobile robot obtained by using the center of gravity may be reduced. An azimuth of the mobile robot may be obtained based on one direction vector obtained by three centric coordinates, for example, a direction vector obtained by summation of a vector from B to A and a vector from B to C. A vector allowing both the coordinates and the azimuth of the mobile robot to be known may be obtained by using a calibration equation, which is disclosed in detail in several theses as follows (Hartley, 1994,

*m A RM t s*

where *m*<sup>0</sup> is projected pixel coordinates corresponding to a reference position of the mobile robot, *R*0 and 0*t* are a rotation matrix and a displacement vector, respectively, s is a scale value, A is a calibration matrix, *m*<sup>1</sup> is pixel coordinates of a position to which the mobile robot rotates and moves, *R*1 is a matrix corresponding to a rotation angle amount, and 1*t* is

0 00 <sup>1</sup> *m ARM t*

1 01 1 <sup>1</sup> <sup>1</sup> *m A RRM Rt*

A coordinate value M is obtained by using Eq. (31), and the obtained value is assigned to Eq. (32). In Eq. (32), *R*1 may be calculated by using a sum of vectors of the recognized marks. Since all values in Eq. (32) excluding 1*t* are known, the displacement vector 1*t* may be calculated. That is, the displacement vector of the mobile robot may be obtained by using

1 1

As described above, the vector allowing the coordinates and the azimuth of the mobile robot to be simultaneously known may be obtained by using the detected three marks of the

*sm A R t M*

a displacement vector, Eq. (31) and (32) may be obtained by using Eq. (30).

<sup>1</sup>

(30)

*<sup>s</sup>* (31)

*<sup>s</sup>* (32)

1 1 10 *t sR A m R M* (33)

Liebowitz and Zisserman, 1998).

Eq. (32).

The calibration equation is shown as follows.

emitting diode (LED) irradiates an infra ray to the landmark and an image reflected by the mark forming the position recognition part is photographed by a camera, thereby obtaining a binary image. Namely, the mark in the image obtained by the camera is set up as a bright light close to white and is converted into the binary image by selecting a predetermined threshold brightness value. Considering the camera in detail, the camera includes a plurality of infrared LEDs, an infrared light controller, a CMOS array, and a image processing controller around a wide angle lens. The camera is installed on one of the mobile robots and a ceiling of a space in which the mobile robot moves, to obtain an image of the landmark attached to one of the ceiling, a wall, and a top of the mobile robot. A partial image brightly displayed in the binary image is labeled, and the mark is detected from the number and/or dispersion of the labeled pixel. In this case, labeling indicates a procedure of recognizing an individual image, giving a reference number to the individual image, and making a label list to know a position and a size of the partial image brightly displayed in the binary image. After the labeling, centric coordinates are obtained for each label and the mark is detected from the number and/or dispersion of the labeled pixels. There may be various methods of detecting a mark from a label list. For example, one method may limit the number of pixels forming a label. Namely, since the mark is formed in the shape of a circle and has a uniform size, only a label having a certain number of pixels is selected as a mark candidate and labels having pixels more or less than the certain number are deleted from the label list.

Another method may determine a predetermined dispersion value corresponding to a dispersion index with respect to centric coordinates from the labels and delete labels in which pixels are not clustered from the label list, thereby determining a mark candidate, since the marks are clustered in the shape of a circle. The two methods of detecting a mark from labels, described above, may be used selectively or simultaneously if necessary.

On the other hand, when only the marks of the position recognition part exist in the landmark, three marks may be detected by using the above methods. However, when there are the marks of the area recognition part, whose size is identical with the mark of the position recognition part, only the marks corresponding to the position recognition part may be separately detected from the total marks by performing an additional process as follows. Namely, three labels whose distances from each other are similar and located in the shape of a right angle are detected from the determined mark candidates, thereby detecting the marks of the position recognition part. For example, an inner product of vectors connecting labels is obtained and a label whose inner product value is closest to a largest valid inner product value is detected, thereby detecting only the marks of the position recognition part from the total marks. When indexes of labels corresponding to A, B, and C of Figure 10 are designated by i, j, and k and a largest valid value of an inner product of vectors between the labels is the indexes whose difference of magnitudes is smallest among indexes whose inner product value corresponds to a range is obtained by using Eq. (6).

$$\begin{split} D(i,j,k) = \left\| \overleftarrow{\boldsymbol{\ddot{\boldsymbol{y}}}} \right\| - \left\| \overleftarrow{\boldsymbol{k}} \right\|\_{\boldsymbol{\prime}} \text{ where } \left\{ \boldsymbol{i}, \, j, k \right\} = \text{arg}\_{i,j,k} \min D(i,j,k) \\ &= \text{arg}\_{i,j,k} \left| \overleftarrow{\boldsymbol{\ddot{\boldsymbol{y}}}} \bullet \overleftarrow{\boldsymbol{k}} \right| \prec \delta\_{\boldsymbol{\text{th}}} \end{split} \tag{29}$$

When an existence and a position of the mark has been recognized by using Eq. (29), an identifier (ID) of the mark may be easily obtained by calculating the position by using a sum of position values and detecting whether the label exists in the position. Position information such as coordinates and an azimuth and area information of the mobile robot are detected by 444 Recent Advances in Mobile Robotics

emitting diode (LED) irradiates an infra ray to the landmark and an image reflected by the mark forming the position recognition part is photographed by a camera, thereby obtaining a binary image. Namely, the mark in the image obtained by the camera is set up as a bright light close to white and is converted into the binary image by selecting a predetermined threshold brightness value. Considering the camera in detail, the camera includes a plurality of infrared LEDs, an infrared light controller, a CMOS array, and a image processing controller around a wide angle lens. The camera is installed on one of the mobile robots and a ceiling of a space in which the mobile robot moves, to obtain an image of the landmark attached to one of the ceiling, a wall, and a top of the mobile robot. A partial image brightly displayed in the binary image is labeled, and the mark is detected from the number and/or dispersion of the labeled pixel. In this case, labeling indicates a procedure of recognizing an individual image, giving a reference number to the individual image, and making a label list to know a position and a size of the partial image brightly displayed in the binary image. After the labeling, centric coordinates are obtained for each label and the mark is detected from the number and/or dispersion of the labeled pixels. There may be various methods of detecting a mark from a label list. For example, one method may limit the number of pixels forming a label. Namely, since the mark is formed in the shape of a circle and has a uniform size, only a label having a certain number of pixels is selected as a mark candidate and labels

having pixels more or less than the certain number are deleted from the label list.

from labels, described above, may be used selectively or simultaneously if necessary.

Another method may determine a predetermined dispersion value corresponding to a dispersion index with respect to centric coordinates from the labels and delete labels in which pixels are not clustered from the label list, thereby determining a mark candidate, since the marks are clustered in the shape of a circle. The two methods of detecting a mark

On the other hand, when only the marks of the position recognition part exist in the landmark, three marks may be detected by using the above methods. However, when there are the marks of the area recognition part, whose size is identical with the mark of the position recognition part, only the marks corresponding to the position recognition part may be separately detected from the total marks by performing an additional process as follows. Namely, three labels whose distances from each other are similar and located in the shape of a right angle are detected from the determined mark candidates, thereby detecting the marks of the position recognition part. For example, an inner product of vectors connecting labels is obtained and a label whose inner product value is closest to a largest valid inner product value is detected, thereby detecting only the marks of the position recognition part from the total marks. When indexes of labels corresponding to A, B, and C of Figure 10 are designated by i, j, and k and a largest valid value of an inner product of vectors between the labels is the indexes whose difference of magnitudes is smallest among indexes whose inner product value corresponds to a range is obtained by using Eq. (6).

, ,

( , , ) , , , arg min ( , , )

When an existence and a position of the mark has been recognized by using Eq. (29), an identifier (ID) of the mark may be easily obtained by calculating the position by using a sum of position values and detecting whether the label exists in the position. Position information such as coordinates and an azimuth and area information of the mobile robot are detected by

*D i j k ij kj where i j k Di j k*

, ,

*i j k th*

(29)

*ij kj*

*ijk*

arg

using the detected mark. The ID determined according to the number and position of the marks corresponding to the area recognition part from the detected marks may be quickly obtained, and the area information of the mobile robot may be obtained. In this case, the area information of the mobile robot is allocated to the ID and is an approximate position in which the mobile robot is located. Detailed position information of the mobile robot, such as the coordinates and the azimuth, may be obtained by using centric coordinates of the detected three marks A, B, and C forming the position recognition part. According to an exemplary embodiment of the present invention, coordinates of the mobile robot may be obtained by considering any point obtained from the centric coordinates of each of the three marks A, B, and C shown in Figure 10 as reference coordinates. In this case, the any point may be a center of gravity obtained by the centric coordinates of the three marks. In this case, since the center of gravity is an average of errors with respect to the centric coordinates of the three marks, an error with respect to the coordinates of the mobile robot obtained by using the center of gravity may be reduced. An azimuth of the mobile robot may be obtained based on one direction vector obtained by three centric coordinates, for example, a direction vector obtained by summation of a vector from B to A and a vector from B to C. A vector allowing both the coordinates and the azimuth of the mobile robot to be known may be obtained by using a calibration equation, which is disclosed in detail in several theses as follows (Hartley, 1994, Liebowitz and Zisserman, 1998).

The calibration equation is shown as follows.

$$\begin{aligned} \text{s } \tilde{m} &= A \begin{bmatrix} R & t \end{bmatrix} \tilde{M} \\ \implies \tilde{m} &= \frac{1}{s} A \begin{pmatrix} R \ M + t \end{pmatrix} \end{aligned} \tag{30}$$

where *m*<sup>0</sup> is projected pixel coordinates corresponding to a reference position of the mobile robot, *R*0 and 0*t* are a rotation matrix and a displacement vector, respectively, s is a scale value, A is a calibration matrix, *m*<sup>1</sup> is pixel coordinates of a position to which the mobile robot rotates and moves, *R*1 is a matrix corresponding to a rotation angle amount, and 1*t* is a displacement vector, Eq. (31) and (32) may be obtained by using Eq. (30).

$$
\tilde{m}\_0 = \frac{1}{s} \, A \left( R\_0 \, M + t\_0 \right) \tag{31}
$$

$$
\tilde{m}\_1 = \frac{1}{s} \operatorname{A} \left( \mathcal{R}\_0 \, \mathcal{R}\_1 \, \mathcal{M} + \mathcal{R}\_1 \, t\_1 \right) \tag{32}
$$

A coordinate value M is obtained by using Eq. (31), and the obtained value is assigned to Eq. (32). In Eq. (32), *R*1 may be calculated by using a sum of vectors of the recognized marks. Since all values in Eq. (32) excluding 1*t* are known, the displacement vector 1*t* may be calculated. That is, the displacement vector of the mobile robot may be obtained by using Eq. (32).

$$t\_1 = \text{s } R\_1^{-1} \text{ A}^{-1} \text{ } \mathfrak{M}\_1 - R\_0 \text{ } M \tag{33}$$

As described above, the vector allowing the coordinates and the azimuth of the mobile robot to be simultaneously known may be obtained by using the detected three marks of the

Fig. 12. Flowchart of localization algorithm.

plates. And reference images for camera calibration are show in Figure 15, which these can be acquired by calibration system as shown in Figure 26. In order to find label in reference images automatically, we use the Javis' March algorithm. This is perhaps the most simple-

position recognition part and a vector operation using the calibration equation, thereby embodying a microprocessor at a low price. Also, brief area information and detailed position information such as the coordinates and azimuth of the mobile robot may be converted into code information. The code information is transmitted to the mobile robot to perform necessary operations. An apparatus and method of recognizing a position of a mobile robot having an inclination correction function, according to another embodiment of the proposed localization system will be described referring to Figure 11.

Fig. 11. The layout of proposed localization system.

The apparatus of Figure 11 further includes two axis inclinometers. An infrared light emitting diode (LED) irradiates an infra ray to the landmark and an image reflected by the mark forming the position recognition part is photographed by a camera, thereby obtaining a binary image. Namely, the mark in the image obtained by the camera is set up as a bright light close to white and is converted into the binary image by selecting a predetermined threshold brightness value. Considering the camera in detail, as shown in Figure 11, the camera includes a plurality of infrared LEDs, an infrared light controller, a CMOS array, a vision controller, and two axis inclinometers around a wide angle lens. The camera is installed on the mobile to obtain an image of the landmark attached to one of the ceiling and a wall.

Entire flowchart for localization algorithm is depicted in Figure 12. Position information such as coordinates and an azimuth of the mobile robot is detected by using the detected mark and the two-axis inclination information. The detailed position of the mobile robot, such as the coordinates and the azimuth of the mobile robot, may be obtained by using centric coordinates of the detected three marks A, B, and C forming the position recognition part.

#### **4.5 Evaluation**

A mobile robot can identify its own position relative to landmarks, the locations of which are known in advance. The main contribution of this research is that it gives various ways of making the self-localizing error smaller by referring to special landmarks which are developed as high gain reflection material and coded array associations. In order to prove the proposed localization system, we develop the embedded system using TMS320DM640 of Texas Instrument Co., Ltd. And then, the proposed localization system has been tested on mobile robot. The schematic diagram for embedded system is depicted in Figure 13. And embedded system on mobile robot is shown in Figure 14. This localization system is composed of a microprocessor and CMOS image sensor with a digital bus, a serial communication circuit, and infrared LED driver as shown in Figure 14. Calibration system for camera distortion has 3-axis motion controller in order to control gesture of reference 446 Recent Advances in Mobile Robotics

position recognition part and a vector operation using the calibration equation, thereby embodying a microprocessor at a low price. Also, brief area information and detailed position information such as the coordinates and azimuth of the mobile robot may be converted into code information. The code information is transmitted to the mobile robot to perform necessary operations. An apparatus and method of recognizing a position of a mobile robot having an inclination correction function, according to another embodiment of

The apparatus of Figure 11 further includes two axis inclinometers. An infrared light emitting diode (LED) irradiates an infra ray to the landmark and an image reflected by the mark forming the position recognition part is photographed by a camera, thereby obtaining a binary image. Namely, the mark in the image obtained by the camera is set up as a bright light close to white and is converted into the binary image by selecting a predetermined threshold brightness value. Considering the camera in detail, as shown in Figure 11, the camera includes a plurality of infrared LEDs, an infrared light controller, a CMOS array, a vision controller, and two axis inclinometers around a wide angle lens. The camera is installed on the mobile to

Entire flowchart for localization algorithm is depicted in Figure 12. Position information such as coordinates and an azimuth of the mobile robot is detected by using the detected mark and the two-axis inclination information. The detailed position of the mobile robot, such as the coordinates and the azimuth of the mobile robot, may be obtained by using centric coordinates

A mobile robot can identify its own position relative to landmarks, the locations of which are known in advance. The main contribution of this research is that it gives various ways of making the self-localizing error smaller by referring to special landmarks which are developed as high gain reflection material and coded array associations. In order to prove the proposed localization system, we develop the embedded system using TMS320DM640 of Texas Instrument Co., Ltd. And then, the proposed localization system has been tested on mobile robot. The schematic diagram for embedded system is depicted in Figure 13. And embedded system on mobile robot is shown in Figure 14. This localization system is composed of a microprocessor and CMOS image sensor with a digital bus, a serial communication circuit, and infrared LED driver as shown in Figure 14. Calibration system for camera distortion has 3-axis motion controller in order to control gesture of reference

the proposed localization system will be described referring to Figure 11.

obtain an image of the landmark attached to one of the ceiling and a wall.

of the detected three marks A, B, and C forming the position recognition part.

Fig. 11. The layout of proposed localization system.

**4.5 Evaluation** 

Fig. 12. Flowchart of localization algorithm.

plates. And reference images for camera calibration are show in Figure 15, which these can be acquired by calibration system as shown in Figure 26. In order to find label in reference images automatically, we use the Javis' March algorithm. This is perhaps the most simple-

The accuracy of self-localizing a mobile robot with landmarks based on the indices is evaluated as shown in Figure 17 and a rational way to minimize to reduce the computational cost of selecting the best self-localizing method. The simulation results show a high accuracy and a good performance as depicted in Figure 16. Also, the localization errors for the proposed algorithm are shown in Figures 17(a) and (b), respectively. In results

Fig. 15. Reference images for camera calibration.

Fig. 16. Calibration errors for camera.

of evaluation, the peak error is less than 3 cm as shown in Figure 17.

minded algorithm for the convex hull, and yet in some cases it can be very fast. The basic idea is as follows: Start at some extreme point, which is guaranteed to be on the hull. At each step, test each of the points, and find the one which makes the largest right-hand turn. That point has to be the next one on the hull. Because this process marches around the hull in counter-clockwise order, like a ribbon wrapping itself around the points, this algorithm also called the "gift-wrapping" algorithm.

Fig. 13. Schematic diagram for embedded system.

Fig. 14. Embedded system for localization.

448 Recent Advances in Mobile Robotics

minded algorithm for the convex hull, and yet in some cases it can be very fast. The basic idea is as follows: Start at some extreme point, which is guaranteed to be on the hull. At each step, test each of the points, and find the one which makes the largest right-hand turn. That point has to be the next one on the hull. Because this process marches around the hull in counter-clockwise order, like a ribbon wrapping itself around the points, this algorithm

also called the "gift-wrapping" algorithm.

Fig. 13. Schematic diagram for embedded system.

Fig. 14. Embedded system for localization.

Fig. 15. Reference images for camera calibration.

The accuracy of self-localizing a mobile robot with landmarks based on the indices is evaluated as shown in Figure 17 and a rational way to minimize to reduce the computational cost of selecting the best self-localizing method. The simulation results show a high accuracy and a good performance as depicted in Figure 16. Also, the localization errors for the proposed algorithm are shown in Figures 17(a) and (b), respectively. In results of evaluation, the peak error is less than 3 cm as shown in Figure 17.

Fig. 16. Calibration errors for camera.

determine the preheating status depends on the human decision. The most challenging task in the oxygen cutting is to hold the flame with blowing out of the focused area with the proper speed along the way to the end. To determine the proper speed according to the slab thickness, we designed the table that maps between the speed and slab thickness. Figures 18 are the pictures of gas cutting experiments. In summary, the first procedure in the developed oxygen cutting robot for the hot slab cobble is to choose the proper torch tip based on the slab thickness. Secondly, ignition procedure is operated based on the table explained in the Table 1. Thirdly, locating the starting area by using the limit switch inside the torch holder is automatically done. At last, the human inspects the right condition for

(a) Step 1: Start (b) Step 2: Ignition (c) Step 3: Cutting

(d) Step 4: Boom control (e) Step 5: Flame control (f) Step 6: Finish

In this chapter, we proposed the safety-oriented reactive controller implemented in an innovative and robust industrial mobile robot designed for cutting a poor strip in the steelworks. This mobile robot, called "APSCR", which provides the fully-autonomous operation and wireless controlled operation, is designed to take into consideration the functional safety because it controls the gas and oxygen without human intervention. To able to support the safety guarantee, we encapsulated the robot controller such as behavior and primitive action modules so that the behaviors do not care about how safety protection mechanism works behind the hardware part or software part. In this chapter, we propose a set of indices to evaluate the accuracy of self-localizing methods using the selective reflection landmark and infrared projector, and the indices are derived from the sensitivity enhancement using 3D distortion calibration of camera. And then, the accuracy of selflocalizing a mobile robot with landmarks based on the indices is evaluated, and a rational way to reduce the computational cost of selecting the best self-localizing method is

Fig. 18. Experiment for cutting process.

**6. Conclusion** 

preheating and send command for "Moving Forward" to the robot.

Fig. 17. Localization error.

#### **5. Simulations and experiments**

One of the most important cutting procedures taken into consideration is to properly use the cutting torch according the hot slab thickness ranged between 50 mm to 300 mm because choosing the right torch tip plays the major effects on the performance of cut quality and amount of usage for oxygen and gas. For automated oxygen cutting to blow the hot slab cobble into two pieces in the way of oxygen cutting equipped in the end of the robot arm, there are several things to consider as follows:

Step1: way of being lit with spark

Step2: control of flammable gas mixed with oxygen without human intervention

Step3: status of the metal hot enough to be melt down

Setp4: Status of end of blowing out of the melting

In order for the torch to be lit with spark, we installed the stepping motor with 300mm long guided arm equipped with the engine ignition plug used in the car engine. When signal is on, it is rolled out all the way to the bottom of torch tip and sparked until the gas is lit and then begin automatically adjusting the oxygen based on the oxygen insertion table [Table 1] that is previously decided.


Table 1. Oxygen and LPG Insertion Table.

Before blowing out of the way, we must preheat starting area. In order to locate the starting area, we used limit switch mounted inside the torch holder wheel. The decision signal to determine the preheating status depends on the human decision. The most challenging task in the oxygen cutting is to hold the flame with blowing out of the focused area with the proper speed along the way to the end. To determine the proper speed according to the slab thickness, we designed the table that maps between the speed and slab thickness. Figures 18 are the pictures of gas cutting experiments. In summary, the first procedure in the developed oxygen cutting robot for the hot slab cobble is to choose the proper torch tip based on the slab thickness. Secondly, ignition procedure is operated based on the table explained in the Table 1. Thirdly, locating the starting area by using the limit switch inside the torch holder is automatically done. At last, the human inspects the right condition for preheating and send command for "Moving Forward" to the robot.

450 Recent Advances in Mobile Robotics


(a) Horizontal error (b) Vertical error

One of the most important cutting procedures taken into consideration is to properly use the cutting torch according the hot slab thickness ranged between 50 mm to 300 mm because choosing the right torch tip plays the major effects on the performance of cut quality and amount of usage for oxygen and gas. For automated oxygen cutting to blow the hot slab cobble into two pieces in the way of oxygen cutting equipped in the end of the robot arm,

In order for the torch to be lit with spark, we installed the stepping motor with 300mm long guided arm equipped with the engine ignition plug used in the car engine. When signal is on, it is rolled out all the way to the bottom of torch tip and sparked until the gas is lit and then begin automatically adjusting the oxygen based on the oxygen insertion table [Table 1]

Slab Thickness(mm) Gas Pressure Oxygen Flow rate Oxygen LPG

1~10 2.5 0.2~0.4 550 10~30 2.5 0.2~0.4 1,100 30~50 3.0 0.2~0.4 1,900 50~70 3.5 0.2~0.4 2,800 70~100 4.0 0.2~0.4 4,300

Before blowing out of the way, we must preheat starting area. In order to locate the starting area, we used limit switch mounted inside the torch holder wheel. The decision signal to

Step2: control of flammable gas mixed with oxygen without human intervention

0

**Y Axis Error [cm]**

5


 Horizontal 0 cm Horizontal 50 cm Horizontal 100 cm Horizontal 150 cm Horizontal -50 cm Horizontal -100 cm

**X Axis Error [cm]**


 Horizontal 0 cm Horizontal 50 cm Horizontal 100 cm Horizontal 150 cm

**X Axis Error [cm]**


Fig. 17. Localization error.

**5. Simulations and experiments** 

Step1: way of being lit with spark

that is previously decided.

there are several things to consider as follows:

Table 1. Oxygen and LPG Insertion Table.

Step3: status of the metal hot enough to be melt down Setp4: Status of end of blowing out of the melting

0

**Y Axis Error [cm]**

5

(d) Step 4: Boom control (e) Step 5: Flame control (f) Step 6: Finish

Fig. 18. Experiment for cutting process.

### **6. Conclusion**

In this chapter, we proposed the safety-oriented reactive controller implemented in an innovative and robust industrial mobile robot designed for cutting a poor strip in the steelworks. This mobile robot, called "APSCR", which provides the fully-autonomous operation and wireless controlled operation, is designed to take into consideration the functional safety because it controls the gas and oxygen without human intervention. To able to support the safety guarantee, we encapsulated the robot controller such as behavior and primitive action modules so that the behaviors do not care about how safety protection mechanism works behind the hardware part or software part. In this chapter, we propose a set of indices to evaluate the accuracy of self-localizing methods using the selective reflection landmark and infrared projector, and the indices are derived from the sensitivity enhancement using 3D distortion calibration of camera. And then, the accuracy of selflocalizing a mobile robot with landmarks based on the indices is evaluated, and a rational way to reduce the computational cost of selecting the best self-localizing method is proposed. The simulation results show a high accuracy and a good performance. With the preliminary results, we have proved the robustness and reliability of the proposed control architecture. To prove that APSCR is able to use in the steelworks, we still have to perform further experiments with mechanical modification.

#### **7. References**


452 Recent Advances in Mobile Robotics

proposed. The simulation results show a high accuracy and a good performance. With the preliminary results, we have proved the robustness and reliability of the proposed control architecture. To prove that APSCR is able to use in the steelworks, we still have to perform

Faugeras, O. (1993). Three-Dimensional Computer Vision: a Geometric Viewpoint, *MIT* 

Faugeras, O., Luong, T., & Maybank, S. (1992). Camera self-calibration: theory and

Faugeras, O. , & Toscani, G. (1986). The calibration problem for stereo, *In Proceedings of the* 

Hartley, R. (1994). Self-calibration from multiple views with a rotating camera, In J.-O.

Hartley, R. (1995). In defence of the 8-point algorithm, *In Proceedings of the 5th International* 

Hartley, R. I. (1994). An algorithm for self calibration from several views, *In Proceedings of the* 

Liebowitz, D. & Zisserman, A. (1998). Metric rectification for perspective images of planes,

experiments, In G. Sandini, editor, *Proc 2nd ECCV, volume 588 of Lecture Notes in Computer Science*, pages 321–334, Santa Margherita Ligure, Italy, May 1992.

*IEEE Conference on Computer Vision and Pattern Recognition*, pages 15–20, Miami

Eklundh, editor, *Proceedings of the 3rd European Conference on Computer Vision*, volume 800-801 of Lecture Notes in Computer Science, pages 471–478, Stockholm,

*Conference on Computer Vision*, pages 1064–1070, Boston, MA, June 1995. IEEE

*IEEE Conference on Computer Vision and Pattern Recognition*, pages 908–912, Seattle,

*In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition*, pages 482–488, Santa Barbara, California, June 1998. IEEE Computer Society. Zhang, Z. (1997). Motion and structure from two perspective views: From essential

parameters to euclidean motion via fundamental matrix, *Journal of the Optical* 

further experiments with mechanical modification.

**7. References** 

*Press*, 1993.

Springer-Verlag.

Beach, FL, June 1986. IEEE.

Computer Society Press.

WA, June 1994. IEEE.

Sweden, May 1994. Springer-Verlag.

*Society of America A*, 14(11):2938–2950, 1997.

### *Edited by Andon Venelinov Topalov*

Mobile robots are the focus of a great deal of current research in robotics. Mobile robotics is a young, multidisciplinary field involving knowledge from many areas, including electrical, electronic and mechanical engineering, computer, cognitive and social sciences. Being engaged in the design of automated systems, it lies at the intersection of artificial intelligence, computational vision, and robotics. Thanks to the numerous researchers sharing their goals, visions and results within the community, mobile robotics is becoming a very rich and stimulating area. The book Recent Advances in Mobile Robotics addresses the topic by integrating contributions from many researchers around the globe. It emphasizes the computational methods of programming mobile robots, rather than the methods of constructing the hardware. Its content reflects different complementary aspects of theory and practice, which have recently taken place. We believe that it will serve as a valuable handbook to those who work in research and development of mobile robots.

ISBN 978-953-307-909-7

ISBN 978-953-51-5580-5

Recent Advances in Mobile Robotics

Recent Advances in

Mobile Robotics

*Edited by Andon Venelinov Topalov*

Photo by v\_alex / iStock