Ac kno wledgemen ts - WordPress.com · y the rob ot itself using certain sensors as cameras, enco ders, sonars, accelerom-eters and lasers. F or the purp ose of this pro ject w e

I

Acknowledgements

My thanks go to all the members of the Department of Measurement, Control, andMicrotechnology at the University of Ulm for their support during the realization of mydiploma thesis. I specially thank my companions cand. ing. Rom�an Moscard�o Cuenca,cand. ing. Francisco Javier Ferrandis, cand. ing. Pedro S�anchez, Dipl.-Ing. CristinaTar��n, Dipl.-Ing. Ram�on Nogueroles, cand. ing. Olaf Bayerke and Dipl.-Ing. HermannBrugger. My special thanks go to my supervisors Dipl.-Ing. J�org K�umpel and Dipl.-Ing. Jan Sparbert for their attention and intensive help. I would like to thank the Headof Department Prof. Dr. Eberhard P. Hofer, who made the realization of my diplomathesis in the Department of Measurement, Control and Microtechnology at the Uni-versity of Ulm possible.I want to thank the spanish teachers from the Universidad Polit�ecnica de ValenciaProf. Dr. Pedro Albertos and Dipl.-Ing. Jes�us Pic�o for their support of the �rst gradu-ating class of \Ingenieros en Electr�onica y Autom�atica Industrial". They are openingthe doors to Europe for a lot of students. They gave me the possibility to study oneyear in Sweden and to realize my diploma thesis in Germany.Finally, I thank all my family, my parents Mariano and Sole. They encouraged andhelped me to obtain my M. S. degree in Electronic and Automatic Engineering aftermy B. S. degree in Electrical Engineering. During the realization of my diploma thesisI remembered specially my brother Jose Ignacio, a man of the world, who is a guidefor me through my whole life.

This project was realized between January and August 1998 at the Department ofMeasurement, Control and Microtechnology at the University of Ulm.

Head of Dept. (Germany) : Prof. Dr. E. P. HoferHead of Dept. (Spain) : Prof. Dr. Pedro AlbertosSupervisors : Dipl.-Ing. J�org K�umpel,

Dipl.-Ing. Jan Sparbert,Dipl.-Ing. Jes�us Pic�o

Contents

1 Introduction 1

1.1 On Robotics and Planning . . . . . . . . . . . . . . . . . . . . . . . . . 11.2 Motion Planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

2 Con�guration Space Formulation 5

2.1 Notion of Con�guration Space . . . . . . . . . . . . . . . . . . . . . . . 52.2 Notion of a Path . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

3 Cell Decomposition Methods 10

3.1 Exact Cell Decomposition . . . . . . . . . . . . . . . . . . . . . . . . . 103.2 Approximate Cell Decomposition . . . . . . . . . . . . . . . . . . . . . 15

3.2.1 Polygonal Representation of the Environment . . . . . . . . . . 153.2.2 Matrix Representation of the Environment . . . . . . . . . . . . 20

4 Neighbour Finding 25

4.1 Equal Size Decomposition . . . . . . . . . . . . . . . . . . . . . . . . . 254.2 Quadtree Decomposition . . . . . . . . . . . . . . . . . . . . . . . . . . 26

4.2.1 Adjacency and Neighbours . . . . . . . . . . . . . . . . . . . . . 264.2.2 Nearest Common Ancestor Method . . . . . . . . . . . . . . . . 28

5 Graph Search 34

5.1 A? Search Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . 345.2 Modi�ed A? Search Algorithm . . . . . . . . . . . . . . . . . . . . . . . 36

6 Experimental Results 38

6.1 Comparision between Equal Size Decomposition and Quadtree Decom-position . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

6.2 Analysis of the Graph Search for di�erent weight factors . . . . . . . . 39

7 Conclusion 49

II

CONTENTS III

A Matlab Programs 53

A.1 Programs for Equal Cell Size Decomposition . . . . . . . . . . . . . . . 53A.1.1 Planning.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53A.1.2 Data1.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54A.1.3 Grid1.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54A.1.4 Graph3.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55A.1.5 Neighbo1.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56A.1.6 Initial1.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57A.1.7 Search.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58A.1.8 Graphnode.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60A.1.9 Plotting.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61

A.2 Programs for Quadtree Decomposition . . . . . . . . . . . . . . . . . . 63A.2.1 Planning.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63A.2.2 Data1.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63A.2.3 Grid2.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64A.2.4 Qdtree.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64A.2.5 Construct.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65A.2.6 Createnode.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67A.2.7 Graph4.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67A.2.8 Cordenate-quadrant.m . . . . . . . . . . . . . . . . . . . . . . . 69A.2.9 gteq-vertex-neighbour.m . . . . . . . . . . . . . . . . . . . . . . 70A.2.10 Adjacen.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70A.2.11 Common-edge.m . . . . . . . . . . . . . . . . . . . . . . . . . . 71A.2.12 Re ect.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71A.2.13 gtsm-vertex-neighbour.m . . . . . . . . . . . . . . . . . . . . . . 71A.2.14 Mirror.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72A.2.15 distance-cost.m . . . . . . . . . . . . . . . . . . . . . . . . . . . 72A.2.16 Initial2.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73A.2.17 Search.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74A.2.18 Graphnode.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76A.2.19 Plotting.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77

Chapter 1

Introduction

1.1 On Robotics and Planning

Automation plays an ever more important role in our society. From a machine that dis-penses refreshments to a mechanical arm that paints automobiles at the assembly line,tasks that are considered tedious, dangerous or physically demanding are delegated tomachinery. Thus, on the one hand, humans are free to deal with work that requiresintelligence and, on the other hand, these tasks are performed much more e�cientlyand reliably by machines than by humans.

The need to have machines that take over an ever larger part of our work createsthe need to have intelligent machines. It takes the combination of mechanisms, sen-sors, dynamics, control and reasoning about the physical world to arrive to systemsthat can synthesize some aspects of human ability.

What usually is called a robot today, is an autonomous device that is capable of chang-ing the physical state of the world in which it operates, according to some guidelinesspeci�ed by an external operator. The level of autonomy of a robot is determined bythe level of details that it requires for its operation. For example, a robot at the humanlevel of autonomy would accept commands expressed in ordinary human language [17].

A large class of robots have the ability to move in their environment. The robot canmove as a whole, or may rest on a �xed base and move parts of it (e.g. a painter armalong the assembly line). In either case it is necessary for the robot to be able to avoidhitting the obstacles that surrounds it while it is performing some useful task.

A principal problem in robotics, which will be discussed in the main part of this

1

CHAPTER 1. INTRODUCTION 2

diploma thesis, is that of motion planning. We want to create algorithms that willenable a robot to move from one position to another without any collisions. Indeed,this capability is most important in many applications. The robot may have a map ofthe building and know at any given moment its exact position (or con�guration), butwhen asked to go to a certain point in the building, it will need to �nd a collision-freepath from its current position to its target.

Since the robot interacts with its environment, it needs to have some knowledge aboutthe environment. This can either be given to the robot o�-line as a map or can be ob-tained by the robot itself using certain sensors as cameras, encoders, sonars, accelerom-eters and lasers. For the purpose of this project we will assume, that the knowledgeabout the surrounding space, the workspace, is complete and will not discuss how itwas acquired.

A fundamental task that is performed invariably by all motion planners is the taskof answering questions of the form \does this position of the robot give rise to a colli-sion?" This seems conceptually a very easy problem, but its importance is such, thatany gain in the speed of processing leads directly to an increasing speed of the motionplanner. Indeed, most planners ask this question many times. And many planners treatcollision checking as a black box subroutine. This separation of the planning task fromthe collision checking task creates the need for an e�cient, general purpose collisionchecker, that can be used by any planner.

1.2 Motion Planning

In the main part of this diploma thesis a path planning method is presented whichcomputes collision-free paths for virtual any type of robot moving among stationaryobstacles (static workspaces). This method proceeds according to three phases:

� A preprocessing phase, such that two decompositions of the workspace can bedone: quadtree decomposition or equal cell size decomposition. (Then, a datastructure is constructed and stored as a tree data structure, for the case ofquadtree decomposition, whose nodes correspond to collision-free con�gurationsand collision con�gurations, more details about con�gurations are presented inchapter 2.)

� A second phase, in which a general connectivity graph is computed. This graphstructure is composed mainly by the following �elds: nodes, neighbours, nodecost,pointers, edgecost, coordinates and nodesearchcost. (This connectivity graph isgenerated as result of a connection between each node and their neighbours.)


� A third search phase, where any given start and goal con�gurations of the robotare connected by a sequence of nodes. The following describes this procedure:

1. Locate the cells kinit and kgoal containing the start and the goal con�guration.

2. Search the connectivity graph for a 'channel', which consists of a sequenceof connected nodes, connecting kinit and kgoal.

3. Find a path that is contained in the channel of cells.

This methodology can be applied to virtually any type of holonomic robot. This pathplanning method requires selecting certain parameters whose values depend on theconsidered scenes, that is the type of robot and its workspace. But these values turnout to be relatively easy to choose.

Experimental results show that path planning can be done in a fraction of a second(the implementation and simulation is done in Matlab,[19]) on a contemporary work-station, after relatively short preprocessing times. These times range from a few dozenseconds to a few hundred seconds for the most di�cult examples that have been treated.

A theoretical analysis of an abstracted model of our algorithm is done. The resultsgive us a good idea of how the performance of the planning method depends on cer-tain characteristics of the workspace in which the connectivity graph is constructed.Unfortunately these characteristics are hard to measure or estimate a priori.An outline of this diploma thesis is shown in the �g. 1.1.The organization of this diploma thesis is as follows:

� In chapter 2 the motion planning problem using the con�guration space is de�ned.

� In chapter 3 cell decompositon methods are explained.

� In chapter 4 neighbour�nding methods are shown.

� In chapter 5 a method for the graph search is shown.

� In chapter 6 the implementation of the algorithms is analyzed.

� Finally, in chapter 7 the conclusion is presented.


Configuration

Representationof

Space

Finding

QuadtreeDecomposition

Equal Cell SizeDecomposition

Finding

Connectivity

Path

Graph Search

Graph

Neighbour- Neighbour-

Environment

Figure 1.1: Outline of this diploma thesis.

Chapter 2

Con�guration Space Formulation

The starting point in the treatment of path planning problems begins with the envi-ronment formulation. This formulation is named the con�guration space. Therefore,con�guration can be de�ned as follows: any mathematical speci�cation of the positionand orientation of every body composing a robot, relative to a �xed coordinate system.As from this de�nition it is possible to obtain an explanation about con�guration spaceor con�guration environment. In this way it is possible to say that con�guration spaceis a set of all con�gurations of a robot. In this diploma thesis, con�guration space isdenoted by C-space and its dimension by N.

The organization of this chapter is as follows:

� In the section 2.1, a notion of con�guration space is explained, in which a methodfor computing the C-space bitmap is described. This method bases on matrixrepresentation of the environment, in other words, the con�guration space isrepresented as a discrete space, where 0s denote free space of con�guration spaceand 1s denote obstacle space of con�guration space.

� In the section 2.2, a notion of a path is described.

2.1 Notion of Con�guration Space

From a point of view simple path planning is de�ned as follows: Let A be a robot whichis either a single rigid body or a collection of rigid subpart of workspace, W, and theobstacles be closed subsets of W. The workspace is a subset of two or three dimen-sional physical space: W = RN , (N=2 or 3), and the obstacles �1; :::; �q are given, suchthat W=RNnSq

1 �i. As from this general explanation, the con�guration space bitmapis used in order to represent the environment.

5

CHAPTER 2. CONFIGURATION SPACE FORMULATION 6

In fact, there is a variety of other ways a precomputed C-space bitmap can be ex-ploited by a planner. For example, a possible use of the bitmap is to generate a moreconcise representation of the free space by grouping adjacent free con�gurations intohyperparallelepipeds of various sizes (\Cell decomposition methods" approach to pathplanning, this will see in chapter 3).

Then, a method for computing the C-space bitmap is described. In its most basicform, it applies to the case where the robot is a N-dimensional (N=2 or N=3) rigidobject translating in an N-dimensional workspace among obstacles. In this method theonly inputs are the bitmap of the workspace and an algorithm to draw the initial andgoal con�gurations on that bitmap. The output is a C-space bitmap representing boththe boundary and the interior of the C-obstacles (1s) and the free space (0s). Thus,when using C-space maps, it restricts the C-space of the robot to a discrete space,where each position can only assume a �nite number of values. Then, in two dimen-sions the description is as follows:

Consider a workspace W=[a; b] � [c; d] � R2. The workspace W is discretized intoa N � N array W, where N is large enough to represent the obstacles in W with thedesired accuracy. It de�nes

celli;j = [a+ i(b� a)

N; a+ (i+ 1)

(b� a)

N]� [c+ j

(d� c)

N; c+ (j + 1)

(d� c)

N]; (2.1)

where i,j 2 S = f0; :::; N � 1g. If there is an obstacle anywhere in the celli;j it lets thatW(i,j)=1, else W(i,j)=0. The bitmap array W can easily be constructed if the obsta-cles are input as a collection of algebraic shapes, e.g., polygons represented by theirvertices. When the data about the obstacles is obtained through sensing, the bitmaprepresentation may be more straightforward to obtain than an algebraic representation.

Considering the set of all triples (x,y,�), where (x,y) are the coordinates of a �xedreference point on the robot and � is the orientation of the robot. In general the pa-rameters x,y and � can assume any real value in [a; b]� [c; d]� [0; 2�]. It discretizes theworkspace and the orientations of the robot and de�nes

x = a+ i(b� a)

N; y = c+ j

(d � c)

Nand � = k

2�

N; (2.2)

where i,j,k 2 S. The C-space can then be stored as N � N � N binary array.

The robot A can be approximated by a set of points (i,j), i,j 2 S, which are drawnby a simple procedure given the orientation � of the robot and the coordinates (x,y).For each �xed value of (x,y,�) it considers the N � N binary array A(x;y;�) where onlythe points that belong to the robot are marked with 1s. With this conventions, a point


(x; y; �) in the (discrete) C-space is legal (free) if and only if

C(x; y; �) �N�1Xi;j=0

W (i; j)A(x;y;�)(i; j) = 0 (2.3)

It is possible to observe that whenever � is �xed and x,y are varing, the various bitmapsA(x;y;�) are all translations of each other, and in particular of A(0;0;�). Then

C(x; y; �) �N�1Xi;j=0

W (i; j)A(0;0;�)(i� x; j � y): (2.4)

More usual kinds of robots with their characteristic parameters can be illustrated asfollows: In the �g. 2.1-2.4, FW means the reference system and FA means the robotlocal reference system.

(X,Y)

A

F

Y

X

F

w

A

Figure 2.1: Rigid robot translating in the plane

θ

A

F

Fw

A

X

Y

θ(X, Y, )

Figure 2.2: Rigid robot translating and rotating in the plane


X

YZ

y

zx

A

FA

FW

(X,Y,Z, α,β,γ )

Figure 2.3: Rigid robot translating and rotating in 3 dimensions

α

β(α,β)

Figure 2.4: Articulated robot in the plane

2.2 Notion of a Path

The notion of continuity is fundamental in the de�nition of a path. Its formalizationrequires us to de�ne a topology in C-space. One classical way to do so is to specify adistance function d:[a; b]� [c; d]! R, and to let the topology in C-space be the metrictopology induced by d. In order to match out intuition of the real-world physics, thedistance between two con�gurations q and q' (q and q' are the triple (x; y; �) and(x0; y0; �0) respectively) should decrease and tend toward zero when the regions A(q)and A(q0) get closer to each other and tend to coincide. A simple distance functionthat satis�es this condition is :

d(q; q0) = maxa2A

ka(q)� a(q0)k; (2.5)

where ka(q)�a(q0)k denotes the Euclidean distance between any two points a(q) anda(q') in R2, where a is a set of places where the robot moves [17].


A path of the robot A from the con�guration qinit to the con�guration qgoal is a conti-nous map

� : [0; 1]! C (2.6)

with� (0) = qinit and � (1) = qgoal:

qinit and qgoal are the initial and goal con�gurations of the path, respectively. Thecontinuity of � entails that for all s0 2 [0; 1],

lims!s0

maxa2A

ka(� (s))� a(� (s0))k = 0 (2.7)

with s taking its values in the interval [0; 1].

Obstacles in Con�guration Space

In the previous section, the de�nition of a path does not take obstacles into considera-tion. Therefore, now a notion of a path is taken into account when there are obstaclesin the workspace.Then, every obstacle �i, i=1 to q, in the workspace maps in C-space to a region:

C�i = fq 2 C =A(q)\�i 6= ;g (2.8)

where

C� =q[

i=1

C�i (2.9)

which is called a C-obstacle, and the set of free space in C-space is

Cfree = C nq[

i=1

C�i = fq 2 C =A(q) \ (q[

i=1

�i) 6= ;g: (2.10)

A free path between two free con�gurations qinit and qgoal is a continuous map � :[0; 1]! Cfree, with � (0)=qinit and � (1)=qgoal.

Also, there are several planning methods that generate semi-free paths, rather thanfree paths. A semi-free path is a continuous map � : [0; 1]! cl(Cfree), where cl(Cfree)denotes the closure of Cfree. Therefore, as it moves along such a path, the robot maytouch obstacles.Then, the free path can be formulated as follows:

� Free path: � : [0; 1]! Cfree (no contact)

� Semi-free path: � : [0; 1]! cl(Cfree) (may have contact)

Chapter 3

Cell Decomposition Methods

The intuitive idea is to break the C-space down into a �nite number of discrete pieces.

Cell decomposition breaks free space (Cfree) down into simple regions called cells suchthat any path between any two con�gurations in a cell can be easily generated. A non-directed graph representing the adjacency relations between cells is then constructedand searched. This is called the connectivity graph or adjacency graph. The outcomeof the search is a sequence of cells called a channel. There are two major variants ofcell decomposition methods:

� Exact cell decomposition: The free space is decomposed into cells whose union isexactly the free space; this is a complete method.

� Approximate cell decomposition: Cells have prede�ned shapes (such as trianglesor trapezoidals) whose union is strictly included in the free space. The boundaryof a cell does not characterize a discontinuity of some sort and has no physicalmeaning. For example, the quadtree method recursively decomposes a rectangularcell into four smaller cells until the cells lie entirely in the free space or reach someresolution.

3.1 Exact Cell Decomposition

The principle of this approach is to �rst decompose the robot's free C-space Cfree

(or its closure). Next the connectivity graph which represents the adjacency relationamong the cells is constructed and searched. If successful, the outcome of the search isa sequence of cells, called a channel, connecting the cell containing the initial con�g-uration to the cell containing the goal con�guration. A path is �nally extracted fromthis sequence.The cells generated by the decomposition should have the following three characteris-tics:

10

CHAPTER 3. CELL DECOMPOSITION METHODS 11

1. The geometry of cells should be simple, so that it is easy to compute a pathbetween any two con�gurations in a cell.

2. It should be rather easy to test the adjacency of two cells,i.e., whether they sharea boundary.

3. It should be quite facile to �nd a path crossing the portion of the boundary sharedby two adjacent cells, see [17].

In the next subsection, the polygonal representation of the environment is explained.

Polygonal Representation of Environment

In this section the exact cell decomposition approach is introduced in the simplecase where C = R2 and the C-obstacle region C� (the union of the C-obstacles)forms a polygonal region in C. For simplifying the presentation, the robot's free spaceCfree = C n C�i is assumed to be bounded. This assumption that Cfree is boundedis not actually needed and will be retracted later. The �g. 3.1 shows an example of acon�guration space with polygonal representation.

q

q

goal

init

freeC

Figure 3.1: This �gure depicts the two-dimensional con�guration space used to illustratethe exact cell decomposition method presented in this section.

Now, the decomposition of Cfree and the associated connectivity graph are de�ned asfollows:


DEFINITION 1: A convex polygonal decomposition K of Cfree is a �nite collec-tion of convex polygons, called cells, such that the interiors of any two cells do notintersect and the union of all the cells is equal to cl(Cfree). Two cells k and k' in K areadjacent if and only if k

Tk' is a line segment of non-zero length [17].

DEFINITION 2: The connectivity graph G associated with a convex polygonal de-composition K of Cfree is a non-directed graph speci�ed as follows [17]:

� G's nodes are the cells in K.

� Two nodes in G are connected by a link if and only if the corresponding cells areadjacent.

12

3

4

5

6

8

9

10

11

12

7 q

q

goal

init

Figure 3.2: Free space has been decomposed into a collection of convex polygonal cellswhose interiors do not intersect. Each cell is labeled by a integer number and theassociated connectivity graph is displayed (bold lines) with each node placed in thecorresponding cell.

Convex Polygonal Decomposition

The �g. 3.2 shows a convex polygonal decomposition of the robot's free space shownin �g. 3.1 and the conectivity graph associated with this decomposition.


Consider an initial con�guration qinit and a goal con�guration qgoal in Cfree. The exactcell decomposition algorithm for planning a free path connecting the two con�gurationsis the following:

1. Generate a convex polygonal decomposition K of Cfree.

2. Construct the connectivity graph G associated with K.

3. Search G for a sequence of adjacent cells between qinit and qgoal.

4. If the search terminates successfully, return the generated sequence of cells; oth-erwise, return failure.

The output of the algorithm is a sequence k1, ..., kp of cells such that qinit 2 k1,qgoal 2 kp and for every j2 [1; p� 1], kj and kj+1 are adjacent. This sequence is called achannel. For instance, in �g. 3.2 qinit is in cell 1 and qgoal is in cell 6; a possible channelis the sequence of cells labeled as 1, 2, 4 and 6 [17].The steps from 2 to 4 show how a path is obtained from space decomposition. Evidently,this is only an important preview of the following chapters.

Trapezoidal Decomposition

One possible convex polygonal decomposition of Cfree is generated by sweeping a lineL parallel to the, say, y-axis across Cfree. The sweep process is interrupted wheneverL encounters a vertex X of C� . A maximum of two vertical line segments are created,connecting X to the edges of C� that are immediately above and immediately below X,as shown in �g. 3.3. The boundary of C� and erected vertical line segments determinea tropezoidal decomposition of Cfree (also called a "vertical decomposition" [7]). Eachcell of the decomposition is either a trapezoid or a triangle. Two cells are adjacent ifand only if their boundaries share a vertical segment. When such a segment is crossedthe vertical structure of the constraints imposed by C� on the motion of A changesdiscontinously. Notice that the same algorithm also applies when Cfree is not bounded[17].

During line sweeping, it is possible to concurrently create the vertical line segmentsemanating from C�'s vertices, generate the connectivity graph G, and identify the cellscontaining the initial and goal con�gurations. The whole treatment takes O (nlogn)time, where n is the total number of vertices in C�. The number of generated cells andthe number of links in G are both in O(n). Notice that the decomposition of Cfree canalso be generated during the sweep process used to construct C� .

The search of G can be done in various ways. A simple breadth-�rst search takesO(n) time. But the produced channel may be far from optimal with respect to the


q

q

goal

init

Figure 3.3: This �gure illustrates the trapezoidal decomposition of free space. A verticalline L parallel to the y-axis is swept accross

Euclidean lengths of the paths it contains. One possible alternative is to use the A�

search algorithm, (this algorithm will be studied in chapter 5), with a di�erent searchgraph G speci�ed as follows:

� The nodes of G are qinit, qgoal, and the midpoints Qj of all the portions of bound-ary shared by adjacent cells.

� Two nodes are connected by a link if and only if they belong to the same cell(they can be joined by a straight line segment in the cell).

Each link is weighted by the Euclidean length of the straight line segment joiningthe two nodes. A� can be applied with the Euclidean distance to the goal as theheuristic function. If a free path exist, the search produces one shortest free pathcontained in G. From a practical point of view, the paths generated using this searchmethod are usually signi�cantly better than those generated using the visibility graphmethod. (This method applies to C-space with polygonal C-obstacles, which constructsby connecting every pair of vertices in Cfree's boundary by a straight segment, if thissegment does not traverse the interior of a C-obstacle.) Indeed, they avoid obstaclesmore widely, while not being signi�cantly longer [17].


3.2 Approximate Cell Decomposition

For this group of cell decomposition methods, the idea consists to construct a collectionK of non-overlapping cells such that the union of all the cells approximatively coversthe free C-space. The cell characteristics should be as follows:

� Cells should have a standard, simple shape so that it is both easy to compute thedecomposition (di�erent from exact decompositon) and easy to compute a pathbetween any two con�gurations in a cell.

� It should be fairly easy to test the adjacency of two cells, i.e., whether they sharea boundary.

� It should be quite simple to �nd a path crossing the portion of the boundaryshared by two adjacent cells.

On the other hand, the principle of the approximate cell decomposition approach isgeneral and can be applied to the basic motion planning problem in its full generality, aswell as to several of its extensions. However, in practice, the time and space complexityof the methods based on this approach grows quickly with the dimension N of thecon�guration space. In general, these methods are adaptive to achieve the desiredaccuracy or resolution, although in certain mode sacri�ce exactness for simplicity ande�ciency.

3.2.1 Polygonal Representation of the Environment

A rectangloid designates a closed region of the following form in a Cartesian space RN :

f(x1; :::; xn)= x1 2 [x0

1; x00

1]; :::; xn 2 [kx0

1; x00

1]g (3.1)

The di�ereces x00i �x0i, i=1, ..., n, are called the dimensions of the rectangloid. None ofthese dimensions is zero.

Let A be a robot whose con�guration space C is RN with N=2 or 3. If C = RN ,a con�guration q is represented by the coordinates of A's reference point OA in theframe FW attached to the workspace. If C = R2, a con�guration is represented as(x,y,�), where � is the angle between the x-axes of F! and FA (the frame attached toA). If C = R3, a con�guration is represented as (x, y, z, �, �, ), where �, � and arethe Euler angles.

Without practical loss of generality, it is possible to assume that the set of possiblepositions of A is contained in a rectangloid k � RN . Cfree is represented as:

Cfree = RN n C�;


where R is equal to the interior part of rectangle, R=int(k). A rectangloid decompo-sition P of is a �nite collection of rectangles fkigi=1;:::;r such that:

is equal to the union of the ki, i.e.

=r[

i=1

ki (3.2)

and, the interiors of the ki's do not intersect, i.e. :

8i1; i2 2 [1; r]; i1 6= i2 : int(ki1) \ int(ki2) = ;: (3.3)

Each rectangloid ki is called a cell of the decomposition P of .A cell ki is classi�ed as:

� EMPTY, if and only if its interior does not intersect the C-obstacle region, i.e.int(ki) \ C� = ;:

� FULL, if and only if ki is entirely contained in the C-obstacle region, i.e. ki � C�:

� MIXED, otherwise.

The connectivity graph associated with a decomposition P of is the non-directedgraph G de�ned as follows:

� The nodes of G are the EMPTY and MIXED cells of P.

� Two nodes of G are connected by a link if and only if the corresponding cells areadjacent.

Given a rectangloid decomposition P of , a channel is de�ned as a sequence (k�j)j=1;:::;p

of EMPTY and / or MIXED cells such that any two consecutives cells k�jand k�j+1

,j2 f1; p � 1g, are adjacent. A channel that only contains EMPTY cells is called anE-channel.A channel that contains at least one MIXED cell is called and M-channel.If (k�j

)j=1;:::;p is an E-channel, then any path connecting any con�guration in k�1 toany con�guration in k�p and lying in int([p

j=1k�j) is a free path. If (k�j

)j=1;:::;p is anM-channel, there may exist a free path connecting two con�gurations in k�1 and k�p,and lying in int([p

j=1k�j), but there is no guarantee that this is the case.

Given an initial con�guration qinit 2 Cfree and a goal con�guration qgoal 2 Cfree, theproblem is to generate an E-channel (k�j

)j=1;:::;p, such that qinit 2 k�1 and qgoal 2 k�p.If such a channel is generated, let �j = @k�j

\@k�j+1, j=1,..., p-1, be the intersection of

the boundaries of two successive cells. A free path joining the initial to the goal con�g-uration can be extrated from the E-channel by linking qinit to qgoal by a polygonal linewhose vertices are points Qj 2 int(�j). For every j such that �j�1 and �j are subsets of


Mixed cell��

��

��

��

��

��

��

Free cell

qinit

qgoal qgoal

qinit

E-channel M-channel

Figure 3.4: Figure on the left shows a representation of a M-channel, and the �gure onthe right shows a representation of an E-channel.

the same face of k�j, an additional point Qj�1 located in the interior of k�j

, should beincluded among the path's vertices, since in this case the line segment Qj�1Qj is notguaranteed to lie entirely in the robot's free space. If necessary, the polygonal path canbe smoothed by �tting spline curves.

A special case of an approximate cell decomposition method should be mentioned atthis point, hierarchical path planning consists of generating an E-channel by construct-ing successive rectangloid decompositions of and searching the associated connectiv-ity graphs. Let Pi, i=1,2,..., denote the successive decompositions of . Each decompo-sition Pi is obtained from the previous one Pi�1 (with P0=fg), by decomposing oneor several MIXED cells, the other cells being unchanged. Whenever a decompositionPi is computed, the associated connectivity graph, denoted by Gi, is searched for achannel connecting qinit to qgoal.

However, this is a general explication about polygonal decomposition in which con-cepts about hierarchical path planning are applied to this kind of decomposition. Othervariants of this method are shown in the next subsections.


Equal Cell Size Decomposition

Taking as reference the previous de�nitions of space decomposition, equal size decom-position is possible to understand. This decomposition consists of generating blocks ofenvironment of equal size by constructing rectangles. Each decomposition Ki only hasin common its size. Every cell can be labeled as Empty, Full or Mixed. From this aconnectivity graph is realized with Empty and Mixed blocks, such a decomposition isshown in �g. 3.5

Mixed cellEmpty cell

Full cell

8

4

1 2

5

3

1

4

7

3

6

98

7

2

6

9

5

Figure 3.5: Equal size decomposition with square cells and its connectivity graph

Tree decomposition

In this part, a method for computing a rectangloid decomposition of a MIXED cell isdescribed. The method consists of �rst dividing a cell into smaller cells and next label-ing each newly created cell according to its intersection with the C-obstacle region.

There are many ways to decompose a rectangloid into smaller rectangloids. Perhapsthe most widely used technique is to compute a 2m-tree decomposition, where N is thedimension of the con�guration space.

A 2m-tree decomposition of is a tree of degree 2m (each node which is not a leafhas exactly 2m children). Each node of the tree is a rectangloid cell which is labeledas EMPTY,FULL, or MIXED. The root of the tree is . Only those nodes which are


MIXED cells may have children and then they have 2m of them. All the children of acell k have the same dimensions and are obtained by cutting each edge of k into twosegments of equal length.

If m=2, the tree is called a quadtree. If m=3, it is called an octree.

M

MM M

MMMMMM EFMMMM

E

order of Sons

Left to right1 2

3 4

Figure 3.6: A tree decomposition of is obtained by recursively dividing and thegenerated MIXED cells into smaller cells. The division of a cell creates four new rectan-gloid cells of equal dimensions. The �gure at the top shows the quadtree decompositionat depth 4 of a simple con�guration space. Bellow the corresponding tree is shown.

The notion of 2m-tree has been �rst developed in Solid Modeling, Computer Graphicsand Computer Vision. Its use in collision avoidance and motion planning is more recentand reported in several papers, including [1], [2], [8], [17] and [22].The depth of a node (i.e. the number of arcs joining the root to the node) in a 2m-treerepresenting determines the dimensions of the corresponding cell relative to . Theheight h of the tree (i.e. the depth of the deepest node) determines the resolution of thedecomposition of , which means the size of the smallest cells in the decomposition.In order to make a decomposition it is neccessary to specify the height h1 of the initialdecomposition P1 . All the MIXED cells whose depths are less than h1 are decomposedat this step. Later on only those MIXED cells which are located in an M-channel aredecomposed further. Usually, a minimal height hmax of the tree is also speci�ed in or-der to bound the iterative process of decomposition. Every MIXED cell whose depthis equal to hmax is labeled as FULL, yielding a truncated 2m-tree [22].


In the worst case, the number of leaves in a 2m-tree of height h is 2mh; hence, itincreases exponentially with both the dimension of and the depth of the decomposi-tion. In practice, this number is usually signi�cantly smaller, since the tree is prunedat every EMPTY and FULL nodes. Nevertheless, the number of leaves still tends toincrease exponentially with both m and h [17].

3.2.2 Matrix Representation of the Environment

Basicly, the procedure of decompositon is very similar compared to the previous method,but there is an important point, that is neccessary to analyze. In this method, theenvironment is represented as a matrix. (This matrix has entries of 0s and 1s, whichrepresent the free space and the space occupied by obstacles, respectively.) Under theseconditions, quadtree decomposition and equal size decomposition are implemented withMatlab.

Equal Cell Size Decomposition

In this part, one possible way of a cell decomposition method with matrix representationis explained, corresponding to the second implementation developed in this diplomathesis. The process of decomposition is as follows:

��

��

1

1

1

1

1

1

1 1 1 1 1

11111

1

1

0

0

0

0

0

0

0

0

1 1

1

1

1

1

1

1 11

1 11

11

1 1 1 1

111

1 0

0

0 0

0

0 0

0 0 0

0 0

0 0 0

0

0 0

0

0

00

00

00

0 0

0

0

0

1

1FULL FULL

FULLFULL

FULL FULL

EMPTY

EMPTY

EMPTY

X-coordinate

Y-coordinate

Figure 3.7: Equal cell size decomposition of a region with cell size 3

The �rst step is to �t the input matrix; in other words, if the environment hasnot a regular disposition, it is neccessary to convert this environment to a regular


matrix (regular means that the matrix has such a size, in which it is possible todivide it in equal blocks). One possibility is to expand the matrix in two directionswith 1s, until the cell size is a whole numbered divides of the matrix dimensions.The 1s serve as virtual obstacles, to avoid that the free space is enlarged.

The next step is to divide the matrix in blocks of equal size. These blocks areevaluated in such a way that if all the pixels of a block are 0s, the block is labeledas EMPTY, otherwise the block is labeled as FULL.

One example of this method is shown in �g. 3.7 .

Quadtree Decomposition

Computer Scientists have been searching for a long time the most e�cient and e�ectiveway of representing images in terms of some form of data structure. The aim is to �ndan image representation technique that is both e�cient in space and access time whilstnot losing any accuracy of the original image - whether the image is two dimensionalor the scene is three dimensional [22].

A computerized representation of a two dimensional image (environment) is usuallya collection of pixels. The attributes of this image are usually traceable back to thenature of how this atomic unit of measurement, the pixel, is shaped and how it relatesto other pixels in the image. One of the most researched and signi�cant data structuresfor two dimensinal image representation is the quadtree. Due to its naturally hierachi-cal form, its implementation is e�ectively representing the original image. For threedimensionals, the data structure used is named octree, and the procedure is similar tothat of the quadtree. Instead of working with pixels, the image is represented by voxels.

The fundamental di�erence between this method and the tree decomposition explainedin the previous subsection, is, that the environment is decomposed until the informa-tion of the leaf nodes is FULL or EMPTY. Overall a quadtree is a hierarchical datastructure used for image representation at the bit level. They encode images in termsof a tree structure and were initially designed to save storage by combining data whichhave identical or similar values.

According to Samet [22], some de�nitions concerning two-dimensional data must bede�ned before looking at quadtrees. An image in two dimensions is an array of pictureelements (pixels). If each pixel has only two states, FULL (one) or EMPTY (zero) thenthe image is said to be binary. If it is possible to assign shades of gray then the imageis said to be mixed-scale. The more familiar quadtrees deal with binary images and sotherefore we will deal primarily with binary images (The mixed node mentioned later


is not to be confused with mixed-scale). It is also assumed that the background of allimages is EMPTY.

b

0 000

000000

000000

00000 0

000

00

00

0 00 0

000

00

1111

1 1 1111

1 1 111111 1

1 11

111 1

c

12 3

54

6 79

810

13 14

19161517 18

1211

X-coordinate

a

Y-c

oo

rdin

ate

Figure 3.8: (a) a region (b) its binary array (c) maximal blocks.

Quadtrees de�ne a hierarchical data structure whose common property is that they arebased on the principle of recursive decomposition of space. There are several kinds ofquadtrees based on di�erent types of data. The most common type of a quadtree andthe example that is being used is known as a region quadtree. Fig. 3.8 and �g. 3.9 showan example of a region, its binary array, the maximal blocks created and the resultingquadtree.

The process of decomposing a binary array into a quadtree is as follows: The arrayis broken down into sub-quadrants where each quadrant is represented by a node,and a quadrant only has children if it is a mixed quadrant. The root node in e�ect,represents the entire array. The root node has four children nodes, each representingthe four quadrants of the original array (labelled NW, NE, SW, SE { or the north-west quadrant, north-east quadrant, south-west quadrant, and south-east quadrant,respectively). Each children node in turn has four children nodes associated with it,representing the sixteen sub-quadrants of the original array. The children of these sub-nodes in turn represent the sixtyfour quadrants of the array and so on, to form theresultant hierarchical quadtree.

When the quadtree is formed, the leaf nodes (the nodes representing each particu-lar pixel of the image) represents the state of that pixel (e.g. FULL or EMPTY). Ifa non-leaf node has a mixture of non-uniform stated children nodes, it is representedby a mixed node. If a non-leaf node has all uniformly stated children nodes only, it is


represented by the state of its children nodes. Its children nodes are then disregardedfrom the tree. Thus as can be seen, the entire tree structure of nodes can be disregardedif the image has one large uniformly stated region.

As supported by Samet and Tamminen [22], quadtrees and its variants have beenfound to be useful in applications such as image processing, computer graphics, pat-tern recognition, robotics and cartography.

Figure 3.9: Quadtree decomposition of the example in �g. 3.8. A circle indicates amixed node, a black square indicates a full node and a white square indicates an emptynode.

Constructing a Quadtree

There are a number of ways to construct a quadtree from a binary array. The simplestapproach is one that converts the array to a complete quadtree (i.e., for a 2n � 2n

image, each pixel amounts to one leaf-node). The resulting quadtree is subsequentlyreduced in size by repeated attempts at merging groups of four pixels or four blocks ofa uniform state that are appropriately aligned.

The environment is represented by a 2n � 2n array of unit square pixels, with eachof them having the value 0 or 1. The quadtree is an approach to image representationbased on successive subdivision of the array into quadrants. In essence, the array isrepeatedly subdivided into quadrants, subquadrants, and so on, until blocks are ob-tained (possibly single pixels) which consist entirely of either 1s or 0s. The array is thenrepresented by a tree in which the root node represents the entire array, the four sonsof the root node represent the quadrants, and the terminal nodes correspond to those


blocks of the array for which no further subdivision is necessary. For example, �g. 3.8shows a block decomposition of the region, while �g. 3.9 illustrates the correspondingquadtree. In general, FULL and EMPTY square nodes represent blocks or pixels con-sisting entirely of 1s and 0s, respectively. Circular nodes, also termed MIXED nodes,denote non-leaf nodes.

An Algorithm for Constructing a Quadtree

The quadtree construction algorithm examines each pixel in the binary array once andonly once in a manner which is analogous to a postorder tree traversal. For example,the pixels in the binary array of Figure 3.8.c are labeled in the order in which theyhave been examined. Denoting the array by A, one sees that A[1; 1] is examined �rst,followed by A[1; 2], A[2; 1] A[2; 2], A[1; 3], assuming, that the elements in the matrixA can be refered to their x-coordinate and their y-coordinate like A[x; y]. However, anode is only created if it is maximal, in other words, if it cannot participate in anyfurther merges (a merge is said to occur when four sons of a node are either all EMPTYor all FULL). For example, �g. 3.9 shows the quadtree resulting from examining thepixels. Note that since all the pixels are not of the same type (see �g. 3.8.b, wherefor instance pixel A[5; 3] is EMPTY, and A[5; 4], A[6; 3], and A[6; 4] are FULL), theirnodes can not participate in any further merges and thus the segment of the �nalquadtree corresponding to their contribution can be constructed. In contrast, pixelsA[1; 7], A[1; 8], A[2; 7] and A[2; 8] of �g. 3.8.b are of the same type, namely EMPTYand thus they will be represented by merged node 3 in Figure 3.8.c. No nodes are everconstructed corresponding to these pixels. As a �nal example of merging, one observesthat the pixels corresponding to the rows 1 to 4 and the columns 1 to 4 of �g. 3.8.b areultimately represented by one block in the block decomposition of �g. 3.8.c. However,the node corresponding to these pixels is only created when its remaining brothers havebeen processed. This is in contrast with the MIXED node corresponding to pixels ofthe rows 5 to 6 and columns 3 to 4, which was created as soon as it was determined,that its four sons are not all EMPTY or all FULL. This algorthm of decompositionwas developed by Samet [22].

Chapter 4

Neighbour Finding

In this chapter, methods in order to �nd adjacent nodes are discussed. Two methodsare used in order to seek adjacencies bewteen the nodes. The �rst is applied for equalsize decomposition and the second for quadtree decomposition.

4.1 Equal Size Decomposition

In this method, the steps of neighbour �nding of the algorithm are completely di�erentin relation to the method for quadtrees, because the treatment is less complex.In this method, at the beginning one has a matrix with 0s and 1s that represents theenvironment. After this, the decomposition is done and each block is assigned one nodewith the value 1 if it is FULL or 0 if it is EMPTY. In this way, it is possible to constructanother reduced matrix with the neccessary information. In other words, each elementof the newly created matrix represents EMPTY or FULL nodes. Then each elementrepresents a node, since all nodes are of the same size. The necessary steps in order to�nd the neighbours of each node are the following:

1. Label the EMPTY nodes with an index according to their position.

2. Grow the newly created matrix from the size n�n to the size n+2�n+2 with 1s.That means, the border of the matrix is completed with FULL nodes (1s), in theway, that in the future theses nodes will not take into account as possibles paths,since theses nodes are not real but �ctitious. This is done in order to calculatethe neighbours of the nodes, with the help of the matrix LCNM.

3. As the maximum number of neighbours for each node can be eight, the possibleneighbours can be evaluated using the following matrix:

Local Cost Neighbour Matrix(LCNM) =

8><>:

p2 1

p2

1 0 1p2 1

p2

9>=>;

25

CHAPTER 4. NEIGHBOUR FINDING 26

The position (2,2) represents the empty node and the rest of the positions repre-sent the possible neighbours, where the di�erence between 1 and

p2 corresponds

to the strategy to weigh an orthogonal path over a diagonal path. This will beexplained with more details in chapter 6.

4. For each empty node, the LCNM matrix is added to the expanded matrix. If inthis positions (eight neighbours are possible around the empty node) the resultof the sum is 1 or

p2 these nodes are neighbours. This is illustrated in �g. 4.1.

��

��

��

��

��

��

��

��

1

1

1

1

1

1

1 1 1 1 1 1 1 1 1

1

1

1

1

1

1

1111111111

1

1

1

0

0

0

0

0

0

0

0

1 1 1

1

11

1 1 1 1

1

1

1 1 1

11

1 11

1 11

11111111

1 1 1 1 1 1 1

11111111

11 + 2 2

2 2

1 +

1 +01 +

0+

1 +

1 +11 +1

01

NEIGHBORS

FULL NODES

NODE

Figure 4.1: This �gure shows the process to �nd neighbours using the LCNM matrix,which is added to the matrix at the location of the examined empty element.

4.2 Quadtree Decomposition

For quadtree decomposition, the method in order to calculate neighbours is more com-plex compared to equal size decomposition, since the nodes belong to di�erent sizes,and therefore, the calculated neighbours are greater, less or equal than in size theconsidered node.

4.2.1 Adjacency and Neighbours

Each node has four edges, also termed sides and boundaries, and four vertices, alsotermed corners. At times one speaks of edges and vertices collectively as directions. Let


the four edges of a node's block be called its N, E, S, and W edges. The four verticesof a node's block are labeled NW, NE, SW, and SE and its meaning is illustrated in�g. 4.2.Given two nodes P and Q whose corresponding blocks do not overlap, and a directionI, one can de�ne a predicate Adjacent(P,Q,I) which is true, if there exist two pixels pand q, contained in P and Q, respectively, such that either q is adjacent to an edge ofp in the I direction or q is adjacent to vertex I of p. In such a case the nodes P and Qare neighbours (speci�cally, edge-neighbours and vertex-neighbours, respectively).For example, nodes 6 and 9 in �g. 3.8 are edge-neighbours since 6 is to the west of 9,while nodes 8 and 4 are vertex-neighbours since 4 is to NE of 8. Two blocks may beadjacent both along an edge or along a vertex (e.g., 1 is both to the north and NE of6; however, 9 is to east of 6 but not to the SE of 6).Unfortunately the neighbour relation is not a function in a mathematical sense. Theproblem is that given a node P and a direction, I there is often more than one node, sayQ, that is adjacent in direction I. For instance, in �g. 3.8 nodes 8 and 10 are all westernneighbours of node 13. Similarly, nodes 10, 12, and 13 are all neighbours of node 15.This means that to specify a neighbour, more precise information is neccessary aboutits nature (i.e.,leaf or non-leaf), size and location. In particular, it is necessary to beable to distinguish between neighbours that are adjacent along an entire edge of a nodeand those that are adjacent along only a segment of a node's edge. An alternative char-acterization of the di�erence is, that in the former case one is interested in determininga node Q such that its corresponding block is the smallest block, greater than or equalto the block corresponding to P. In the latter case it speci�es the neighbour in greaterdetail by, in this case, indicating the vertex of P to which Q must be adjacent. Thesame distinction can also be made for vertex directions [22].

NW NE

SW SE

N

E

S

W

Figure 4.2: The relationship between a block's four quadrants and its boundaries.


4.2.2 Nearest Common Ancestor Method

There are a number of di�erent ways of locating neighbours in a quadtree . Theydi�er in the type of information used to perform the process. The method that will bediscussed here is the most general. It is independent of both position (i.e., coordinates)and size of the node whose neighbour is being sought. It is based on locating a nearestcommon ancestor. The basis of this concept is to ascend the quadtree until a commonancestor is located and then descend the quadtree in search of the neighbour node. Ituses only the structure of the tree, that is, it uses no information other than the fourlinks from a node to its four sons and one link to its father for a non-root node.

De�nitions and Notation

To understand the presentation of the algorithm and analysis in this section, somede�nitions and explanations of the used notations are required.Let the directions which are possible to move in order to seek neighbours are N, E, S,W, NW, NE, SW, and SE.The functions ADJ, REFLECT, COMMON EDGE and MIRROR help in the ex-

Table 4.1: ADJ(I,O)

O (quadrant)I (direction)

NW NE SW SEN T T F FE F T F TS F F T TW T F T FNW T F F FNE F T F FSW F F T FSE F F F T

pression of operations involving a block's quadrants and its edges and vertices. (Tables4.1-4.4)

ADJ(I,O) is true if, and only if, quadrant O is adjacent to the direction I edge orvertex of O's containing block, e.g. ADJ('W','SW') is true, as is ADJ('SW','SW). Therelation can also be described as being true if O is of type I or, equivalent, that I'slabel is a subset of O's label.The function REFLECT(I,O), given a direction I and a quadrant O, taking as basis


Table 4.2: REFLECT(I,O)

O (quadrant)I (direction)

NW NE SW SEN SW SE NW NEE NE NW SE SWS SW SE NW NEW NE NW SE SWNW SE SW NE NWNE SE SW NE NWSW SE SW NE NWSE SE SW NE NW

the relationship between a block's four quadrants and its boundaries, shown in the �g.4.2, this function returns the quadrant in the search direction, I, and that is adjacentto the quadrant O.

Table 4.3: COMMON EDGE(I,O)

O (quadrant)I (vertex)

NW NE SW SENW N W NE N ESW W SSE E S

COMMON EDGE returns the common direction to two quadrants(inputs), where the�rst input to this function is the vertex direction and the second is the quadrant, forexample, COMMON EDGE('SW','NW')='W', see Table 4.3. corresponds to an un-de�ned value.The last function is MIRROR(I) which only has one argument of input, that is thedirection I. This function provides, with the direction I given, the quadrants whichshare the edge or vertex in direction I, from the opposite quadrant.There are two types of seeking neighbours. The �rst corresponds to neighbours ofequal-size or greater size than the considered node (within this case there are edge-neighbours and vertex-neighbours), and the second is for neighbours of smaller size


Table 4.4: MIRROR(I)

I (direction)N E S W NW NE SW SE

SW, SE NW, SW NW, NE NE, SE SE SW NE NW

than the considered node.

Neighbours of Equal Size or Greater Size

Locating edge-neighbours is relatively straightforward. Assume, that one is trying to�nd the neighbour of equal size of node P in direction I. The basic idea is to ascend thequadtree until a common ancestor is located and then descend the quadtree in searchof the neighbouring node. It is obvious that it is always possible to ascend as far as tothe root of the quadtree and then start the descent. However, the goal is to �nd thenearest common ancestor because this minimizes the number of nodes that must bevisited. These two steps are described bellow. This procedure ignores the situation inwhich the neighbour may not exist (e.g., when the node is on the border of the image).

1. Locate the nearest common ancestor. This is the �rst ancestor node reached bya son of type O such that ADJ(I,O) is false. In other words, I's label is not asubset of O's label.

2. Retrace the path that was taken to locate the nearest common ancestor by usingthe function REFLECT to make mirrored moves respect to the edge shared bythe neighbouring nodes.

Then, when edge-neighbours do not correspond to nodes of the same size, e.g. theneighbour is larger, then only part of the path from the nearest common ancestor isretraced. Otherwise the neighbour corresponds to a node of equal size.If there is noneighbour (i.e., the node whose neighbour is being sought is adjacent to the border ofthe image in the speci�ed direction), then a \no neighbour" is returned, an example ofthis case is shown in �g. 4.3.

Locating a vertex-neighbour is considerably more complex. Assume that one is try-ing to �nd the neighbour of equal size of node P in direction I. The initial aim is tolocate the nearest common ancestor of P and its neighbour. As before one needs toascend the tree to do so. The situation must also be taken into account in which theancestors of P and its neighbour are adjacent along an edge. Let N denote the node


D

NW

NW

NW

NE

NE

NEE

B

C

GAF

A G

NWNE

B F

CSW SE

E

NW

NW

D

a) b)

NE

NE32 5

61

3 4

1

5

4

6

2

Figure 4.3: Process of locating the eastern edge-neighbour of node A. (a) Block decom-position. (b) Tree representation

that is being examined in the ascent. An example of this case is shown in �g. 4.4. Thereare three cases described bellow:

1. As long as N is a son of type O such that ADJ(I,O) is true, continue to ascendthe tree.

2. The case when the father of N and the ancestor of the desired neighbour, sayA, are adjacent along an edge. This situation and the exact direction of A aredetermined by the function COMMON EDGE applied to I and the son type ofN. Once A has been obtained, the desired neighbour is located by applying theretracing step outlined in step 3.

3. Otherwise N is a son of a type Q, such that neither of the labels of the edgescomprising vertex I is a subset of O's label. Its father, say T, is the nearestcommon ancestor. The desired neighbour is obtained by simply retracing thepath used to locate T except that it now makes diagonally opposite moves aboutthe vertex shared by the neighbouring nodes. This process is facilitated by use ofthe function REFLECT.

If a neighbour is of di�erent size respect to the considered node, the size of the neigh-bour can be known. In such case, it is possible that the neighbour �nding algorithmsreturn both the neighbouring node and a value from which the node's size can be easilycomputed. This is relatively straightforward when we know the level in the quadtree


D

NW

NE

NE

NEE

B

C

A

A G

NE

B F

CSW SE

E

NW

NW

D

a) b)

NE

NE32

1

3 4

1

5

4

2

56

FSW

G

6SW

Figure 4.4: Process of locating the vertex-neighbours in direction SE of node A (i.e.,G). (a) Block decomposition. (b) Tree representation.

of the node whose neighbour is being sought. In fact, such an algorithm need onlyincrement the level counter by one for each link that is ascended while locating thecommon ancestor and then decrements the level counter by one for each link that isdescended while locating the appropriate neighbour [22].

Neighbours of Smaller Size

When neighbours are of smaller size, the process to detect this neighbours is morecomplex. A example of this case is shown in �g. 4.5. The steps in order to calculatethem are as follows:

1. Mixed neighbours of equal size are calculated.

2. The sons of each one of these neighbours are analyzed.

3. If these sons are empty and as well correspond to the search direction, then theseare the neighbours of the node.

4. If these sons are full, then there are neighbours.

5. If these sons are mixed, then the step 2 is invoked again.


a) b)

NW NW

SWNEA

B

C

D

E

1

1NW

NE

NW SW

A

B

C

DE

3

32

2

3 3

Figure 4.5: Process of locating the western neighbours of smaller size of node A (i. (a)Block decomposition. (b) Tree representation.

Therefore, in this project and for this method, two algorithms are used in order to �ndthe neighbours:

1. Locate an edge-neighbour and a vertex-neighbour of node P, of size greater thanor equal to P, in direction I.

2. Locate a neighbour, of smaller size than node P, in direction I.

Chapter 5

Graph Search

In this chapter an algorithm for graph search in order to �nd shortest paths is pre-sented. In the previous chapters, the information was modeled by graph nodes, andalso by the connections to the neighbours of each node. Now, a search algorithm whichseeks a path from a start node to a goal node is proposed. Therefore, a graph is de�nedby a set of nodes and a set of connecting line segments between neighbour nodes. Then,with the graph and the speci�ed start node, it is possible to seek a set of nodes thatare connecting the start node with the goal node.

The search method used in the graph search is A?. This technique will be explainedbellow.

5.1 A? Search Algorithm

The A? technique is outlined below. The search begins at the starting node and thegraphnodes adjacents to this nodes are probed. On the basis of a cost function, thenode with the minimum cost is explored next. The procedure is as follows:

1. The starting node is opened and placed on a list called OPEN. This node will bereferred to as start.

2. If there are no nodes in the OPEN list, there is no safe path from the start tothe goal. (Note, this will not occur on the �rst pass.)

3. The cost function, f, is determined for all of the nodes in the OPEN list. Thenode with the minimum cost is removed from the OPEN list and placed on theCLOSED list. This node will be referred to as n. (The cost, f, is assigned bellow.)

4. If n is the goal node, trace the \pointers" back the starting node and exit. (Point-ers will be explained shortly.)

34

CHAPTER 5. GRAPH SEARCH 35

5. "Expand" node n into all of the adjacent nodes, creating a list of successors, n'.A pointer is created from each successor back to node n indicating the directionof travel back to the starting node. For each successor, n':

(a) If the successor, n', is not on the OPEN or CLOSED lists or unsafe, place iton the OPEN list. For this node, calculate the cost, g(n'), of the path fromthe starting point to the successor, n'. This cost is determined by adding thecost of moving from nodes n to n' to the cost g(n) (previously calculated).Also, an estimate of the cost of moving from the successor to the goal iscalculated, h(n'). This is typically the cost of a euclidean distance. Finally,the total cost, f, assigned to this node is: f(n')=g(n')+h(n').

(b) If the successor, n', is already on the OPEN or CLOSED list, direct thepointer along the path yielding the lowest g(n'). This is done by comparingthe previously calculated g(n') from the previous OPEN to the new g(n')using the current n. If the new g(n') is lower, the pointer are redirected ton. If not, the node is not reOPENed.

(c) If the successor, n', required pointer adjustment in step b and was on theCLOSED list, reOPEN it.

6. Go to step 2.

In the �g. 5.1 a representation of graph search is shown, in which the graphnodes rep-resents the free space where the robot can move freely. In this case the nodes haveequal size, and each node can have eight possibles neighbours. Also, is possible to seesome lines in the graphnode that notice the di�erent costs, depending on the directionof the neighbour.

The �g. 5.1 shows A? searching algorithm. The expansion operation, shown in step5, is performed for all open nodes. First, the only open node is the starting node. Ex-pansion is performed into all nodes adjacent to the open node. Next, the open nodethat has the lowest cost function evaluation is expanded, and the process is continueduntil the goal is reached.Assume that the coordinates of the start node are (0,0). Expansion into all adjacentnode is performed and the next minimum cost function evaluation is the node at (1,1).This node is expanded, but two of the expansion nodes, (1,2) and (2,2), have thesame function evaluation, 3.8. To break the tie, the node with minimum g is used,which is node (1,2). On the subsequent expansion, the goal is reached and the searchis completed [4].


-1 0 1 2 3

-1

0

1

2

3

4

h=5.00 h=4.47 h=4.12

h=4.24 h=3.61 h=3.16 h=3.00

h=3.61 h=2.82 h=2.24

h=2.24 h=1.41 h=1.00

h=2.00

h=2.00 h=1.00 h=0.00

g=1.41 g=1.00 g=1.41

g=1.00g=1.00

g=1.00 g=1.41 g=2.41

g=2.82g=2.00 g=2.41

g=3.00 g=3.41 g=3.80

g=0.00 g=2.82

g=1.41

*

*

*

*

S

G

11

11.411.41

1.41 1.411

: Found path*: Graphnode

Figure 5.1: A? Search

5.2 Modi�ed A? Search Algorithm

The A? search has been shown to yield the optimal path if a path exists [4]. If theoptimality requirement is relaxed, additional speed increases can be obtained. A newalternative is presented in [10].Replace the cost function by f = (1� w) � g + (w) � h, where w is a relative weightingfunction between g and h. w=0, 0.5, and 1 correspond to breadth-�rst, A?, and thebest-�rst strategies, respectively. Empirical results indicate the values of 0:5 < w < 1tend to yield faster results for sparse, convex obstacle �elds. However, with maze-likeobstacles, w=1 tends to yield slower results. Without prior knowledge of the obstacle�eld, w=0.5 ( A?) should be selected. It yields acceptable results with the most prob-lems [4].

This method has some advantages and drawbacks compared to classical A?. Fromthe point of view of consuming time, the classical method o�ers better results, since inthe modi�ed method there are more mathematical operations, like multiplications inthe cost function. Also, there is additional information to compute for the search, i.e.


w, in relation to classical method.On the other hand, the modi�ed method allows to give more accurate results. For ex-ample, with the relative weighting function w, it is possible to o�er di�erent kinds ofsearch, according to the choice of w. From the point of view of performance, the resultis, that the total number of expansions is greatly reduced, in modi�ed method [4].

Chapter 6

Experimental Results

In this chapter a comparision between the equal size decomposition method and thequadtree decomposition method is shown, from di�erent points of view. The compu-tational time for connectivity graph generation, graph search, and the number of pathnodes is explained. The search for di�erent weight factors (e.g. w=1, w=0.5, and w=0)is also attended. In this diploma thesis, the equal size decomposition and the quadtreedecomposition methods have been implemented in Matlab programs, supported by aUnix Workstation System. Two environments with matrix sizes of 64�64 and 32�32,respectively, are presented.The conditions of evaluation are the follows:

1. Analysis of an environment for the same start and goal con�guration, with thefollowing considerations:

� The weight factor w, used in the cost function, is 1.

� For equal cell size decomposition, four examples corresponding to block sizes5, 4, 3, 2, and 1 are taken into a account.

� For quadtree decomposition, an example of an environment is comparedwith the di�erent decompositions realized by equal size decomposition.

2. Analysis of an environment for quadtree decomposition with the same start andgoal con�guration, and di�erent weight factors is explained.

6.1 Comparision between Equal Size Decomposi-

tion and Quadtree Decomposition

In table 6.1 it is possible to appreciate the di�erent computational times in quadtreedecomposition respect to other cases. The main reason that explain this results is thefollowing: the procedure of calculating the neighbours in equal size decomposition is

38

CHAPTER 6. EXPERIMENTAL RESULTS 39

straight, since all the neighbours have the same size. For the quadtree decompositionit is necessary to apply several recursive algorithms, in other words it is rather morecomplicate, and therefore the execution time is longer than in the other cases of equalsize decomposition. Nevertheless, the case of equal size decomposition with a block size1, or the original representation with a bitmap, has rather more memory requirementsthan quadtree decomposition. This is shown in table 6.1, where for the choosen examplean over ow of memory is produced. The results are also shown in �g. 6.1-6.5.It is also possible to appreciate, for equal size decomposition, that as the block sizeincreases, the execution time of the construction of the graph structure decreases.

Seeking a Path

In this subsection, the execution time for the search for a path corresponding to bothmethods is compared. In table 6.1 it is possible to observe the results of this simulations.The main results are as follows:

1. For quadtree decomposition, the obtained result is the best, taking into accountthat all execution times are similar and that the accuracy achieved is highest.

2. For the cases of equal cell size decomposition, where the number of total nodesis less than in quadtree decompositon, the search is faster.

3. When the number of total nodes in both methods is similar, for example witha block size of 3, quadtree decomposition is faster. This is because in quadtreedecomposition there are nodes of di�erent sizes, and this, exactly, helps to �ndthe path quickly.

4. If the decomposition grade in equal size decompositon increases, the number oftotal nodes decreases, so the search is faster and the accuracy decreases.

5. If the number of nodes belonging to each path is attended, it is necessary to noticethat search times are coherente, since the number of computed nodes decreasesalso the search time decreases.

6.2 Analysis of the Graph Search for di�erent weight

factors

In this section, an environment is analyzed with the following speci�cations:

� Quadtree decomposition method is used.

� Three examples with the weight factors w=1, w=0.5, w=0 are applied.


Table 6.1: Results of the comparision of both methods.

Type of Total number Number of Search time Time fordecomposition of nodes path nodes connectivity graph

in the graph generationQuadtree 344 25 0.212 s 20.100 s

Equal cell size Over ow of memoryblock size 1

Equal cell size 569 25 0.422 s 12.530 sblock size 2




� The start and goal con�guration is the same for the three cases.

� The results are shown in �g. 6.6-6.8.

Case of w=1

The most important results in this case are the followings:

� The path is not the shortest path.

� This choice of w results in paths with many curves.

� The search time is smallest:

18 nodes are computed from the start node to the goal node, whereas forw=0 or w=0.5 the number of computed nodes is 20.

Case of w=0.5


� This path is a shortest path.

� The search time is longer than for the case w=1:


This graph shows the layout of the trajectory in the real environment

X −Position

Y −

Pos

ition

Start

Goal

10 20 30 40 50 60

10

20

30

40

50

60

Figure 6.1: This one series of �ve graphics, where di�erent decompositions are pre-sented. This �gure shows the con�guration space and the path from the start con�g-uration to the goal con�guration for quadtree decomposition with the weight factorw=1.

{ 20 nodes are computed from start node to goal node, whereas for w=1 thenumber of computed nodes is 18.

{ The evaluation of the cost function f needs more time than in the other case,since this function uses the parameters g and h, whereas for w=1 only h isused.

Case of w=0


� This path is a shortest path.



X −Position

Y −

Pos

ition

G

S

10 20 30 40 50 60

10

20

30

40

50

60

Figure 6.2: This �gure shows the con�guration space and the path from the startcon�guration to the goal con�guration for equal size decomposition with block size 2and a weight factor w=1.

� This choice of w gives more straights to the path.

� The search time is highest, because more nodes than in other cases are computed,in total 20.



X −Position

Y −

Pos

ition

Start

Goal

10 20 30 40 50 60

10

20

30

40

50

60




X −Position

Y −

Pos

ition

Goal

Start

10 20 30 40 50 60

10

20

30

40

50

60




X −Position

Y −

Pos

ition

Start

Goal

10 20 30 40 50 60

10

20

30

40

50

60




X −Position

Y −

Pos

ition

Goal

Start

5 10 15 20 25 30

5

10

15

20

25

30

Figure 6.6: This is other series of graphics. In this �gures the e�ect of the cost functionin the path is exposed. This �gure shows the con�guration space and the path withthe weight factor w = 1.



X −Position

Y −

Pos

ition

Start

Goal

5 10 15 20 25 30

5

10

15

20

25

30

Figure 6.7: This �gure shows the con�guration space and the path for the same startcon�guration and the goal con�guration, in the same way as in the previous case, withthe weight factor w = 0:5.



X −Position

Y −

Pos

ition

Goal

Start

5 10 15 20 25 30

5

10

15

20

25

30

Figure 6.8: This �gure shows the path, in the same way as in the two previous cases,with the weight factor w = 0.

Chapter 7

Conclusion

Summary

The main part of the diploma thesis (Chapters 2, 3, 4 and 5) described a four-phasemethod to solve robot motion planning in static workspaces. In the phase of the repre-sentation of the environment, a notion of con�guration space was explained, in which amethod for computing the C-space bitmap was applied. In the cell decomposition phase,a decomposition of the C-space was carried out. Two methods were implemented: equalcell size decomposition and quadtree decomposition. In the neighbour-�nding phase,several algorithms were implemented in order to determine all the possible neighboursof a considered node. And in the last phase, an algorithm for the graph search to obtainshortest paths was presented.

Finally, some examples and results were shown, in which it was possible decompo-sitions of di�erent block sizes, and the in uence of the choice of the di�erent weightfactors in the cost function. In this way, it is necessary to notice, that the values of theweight factor w, which provide a shortest path are in the range of 0 and 0.5, accord-ing to the results shown in chapter 6. Also, it is important to say, that the quadtreedecomposition method consumes less memory than the equal cell size decompositionmethod, and additionally provides more accuracy than the other decomposition.

Future Work

Dynamic Environments

A challenging research goal would be to extend the method to dynamic workspaces.One �rst question is: how should a graph computed for a given workspace be updated ifa few obstacles are removed or added? Answering this question would be useful in orderto apply this method to workspaces subject to small incremental changes. Such changes

49

CHAPTER 7. CONCLUSION 50

occur in many manufacturing (e.g., assembly) cells. While most of the geometry of sucha cell is permanent and stationary, a few objects are added or removed between anytwo consecutive manufacturing operations.

Bibliography

[1] Ahuja, N.,Chien, R.T., Yen, R. and Bridwell, N.:Interference Detection and Collision Avoidance Among Three Dimensional Ob-

jects, Proceedings of the First AAAI Conference, Stanford, 44-48, 1980.

[2] Ayala, D., Brunet, P., Juan, R., and Navazo, I.:Object Representation by Means

of Nonminimal Division Quadtrees and Octrees, ACM Transactions on Graphics,4(1), 41-59, 1985.

[3] Bell, S.B., Diaz, B.M., and Holroyd, F.C.: Digital Image Processing in Remote

Sensing. Capturing Image Syntax using Tesseral addressing and arithmetic. Taylor& amp; Francis Ltd: USA, 1988

[4] Charles W. Warren: Fast Path Planning Using Modi�ed A? Method. Departmentof Mechanical Engineering. The University of Alabama.

[5] Boaz, M., and Roach, I.: An Octree Representation for Three-Dimensional Motion

and Collision Detection, Proceedings of the SIAM Conference on GeometricalModeling and Robotics, Albany, NY, 1985.

[6] Brooks, R.A. and Lozano-P�erez, T.: A Subdivision Algorithm in Con�guration

Space for Findpath with Rotation, Proceedings of the 8th International Conferenceon Arti�cial Intelligence, Karlsruhe, FRG, 799-806, 1983.

[7] Chazelle, B.: Approximation and Decomposition of Shapes, in Algorithmic and

Geometric Aspects of Robotics,Lawrence Erlbaum Associates, Hillsdale, NJ.145-185, 1987.

[8] Faverjon, B.: Obstacles avoidance Using an Octree in the Con�guration Space of a

Manipulator, Proceedings of the IEEE International Conference on Robotics andAutomation, Atlanta, 504-512, 1984.

[9] Fujimura, K. and Samet, H.: A Hierarchical Strategy for Path Planning Among

Moving Obstacles, IEEE Transactions on Robotics and Automation, 5 (1), 61-69,1989.

51

BIBLIOGRAPHY 52

[10] Hart, P.E., Nilsson, N.J. and Raphael, B.: A formal basis for the heuristic de-

termination of minimum cost paths. IEEE Transaction of Systems Science andCybernetics, SSC 4(2):100-107, July 1968.

[11] Hayward, V.: Fast Collision Detection Schema by Recursive Decomposition of a

Manipulator Workspace, Proceedings of the IEEE International Conference onRobotics and Automation, San Francisco, 1044-1049, 1986.

[12] Hernan, M.: Fast, Three-Dimensional, Collision-Free Motion Planning, Proceed-ings of the IEEE International Conference on Robotics and Automation, San Fran-cisco, 1056-1063, 1986.

[13] Jackins, C.L. and Tanimoto, S.L.: Octrees and their Use in Representing Three-

Dimensional Objects, Computer Graphics and Information Processing, 14(3), 1980.

[14] J�anich, K: TopologySpringer-Verlag, New York, 1984.

[15] Kambhampati, S. and Davis, L.S.: Multiresolution Path Planning for Mobile

Robots, IEEE Transactions of Robotics and Automation, RA-2 (3), 135-145, 1986.

[16] Kant, K. and Zucker, S.W.: Planning Smooth Collision-Free Trajectories: Path,

Velocity and Splines in Free Space, Technical Report, Computer Vision andRobotics Laboratory, McGill University, Montreal, 1986.

[17] J.-C. Latombe, Robot Motion Planning .Kluwer Academic Publishers, Boston, 1996.

[18] Laumond, J-P.: Robot Motion Planning and Control.Springer, Toulouse, 1998.

[19] Numerical Computation Program Matlab from Mathworks.

[20] Pearl, J.: Heuristics: Intelligent Search Strategies for Computer Problem Solving.

Addison-Wesley, Reading, MA, 1984.

[21] Puech, C. and Yahia, H.: Quadtrees, Octrees, hyperoctrees: a uni�ed analytical

approach to tree data structures used in graphics, geometri modeling, and image

processing,

Proceedings of the Symposium on Computational Geometry, Baltimore, June1985.

[22] Hanan Samet,Applications of Spatial Data Strutures.Addison-Wesley, Maryland, 1990.

Appendix A

Matlab Programs

In this appendix, all the implemented programs in this diploma thesis are listed. Thescheme of this programs is according to the �g. 1.1 shown in the chapter 1. The �gureshows the main blocks that are implemented and also the two main variants developed:Equal cell size decomposition and quadtree decomposition, in this way the programsare presented.

A.1 Programs for Equal Cell Size Decomposition

A.1.1 Planning.m

%This is the General Path Planning Program, which applies the

%the Approximate Cell Decomposition Method.

%

%The program uses the functions following:

%- Data1, which acquires the input datas.

%- Maxi provides the size of decomposition

%- Grid1, which prepares the environment plan

%- Graph3 obtains the main information as datas struture

%- Initial1 provides the initial and goal configurations

%- Search_r finds the convenient path on application of A-star Method

%- Plotting1 shows graphical the results

clear all;

close all;

global matrix

global new_matrix

global graph

global maxi

53

APPENDIX A. MATLAB PROGRAMS 54

data1;

maxi=input('Give me the dimension of decomposition.. ');

grid1;

[graph]=graph3;

[start,goal,node_goal,node_start,w]=initial1;

[traye]=search_r(node_start,node_goal,w);

graphnode;

[real_coord]=plotting1(traye);

A.1.2 Data1.m

function data1

%This function give the posibility to choose the input matrix.

%The matrix dimension is (n x m).

global matrix

while isempty(matrix)

[filename,path]=uigetfile('*.mat','Input Matrix');

if filename ~=0,

eval(['load 'path filename])

else

disp('File Load Error !')

break;

end;

if isempty(matrix)

disp('The selected file is not suitable for this application,

tray again');

end

end;

A.1.3 Grid1.m

function grid1;

%This function provides new_matrix, which is a

%smaller matrix than initial matrix,

%depending on decomposition degree maxi.

%

%The variables more important in this function

%are the following:

%- Maxi is a scalar factor which measurements

% the decomposition degree.

%- The function expand2 expand initial matrix.

% The expanded matrix dimension is (maxi*p x maxi*p)

% where maxi*p is greater or equal than initial


% matrix dimension.

global matrix

global new_matrix

global maxi

expand2;

%Decomposition with the same size for each cell

[xdim,ydim]=size(matrix);

for i=1:xdim/maxi

for j=1:ydim/maxi

if isempty(find(matrix(i*maxi+1-maxi:i*maxi,

j*maxi+1-maxi:j*maxi)))

new_matrix(i,j)=0;

else

new_matrix(i,j)=1;

end;

end;

end;

A.1.4 Graph3.m

function [graph]=graph3

%This function creates and shows the Graph data

%structure with all the information

%

%The main functions and variables are the following:

%- Neighbo1 is a function which provides the obstacles

% frees index and their neighbours

%- M_neighbo is the neighbour matrix (8 x 2 x n) where

% n is the freenodes number

%- ir is a vector with the frees obstacles index (n x 1)

%- Graph is the data structure with the fields following:

% nodenr

% neighbournodes

% pointer

% visited

% nodecost

% Nodesearchcost

% coordinate

global new_matrix


tic

[neighbourNodes,ir,coordin]=neighbo1;

nodeCost=zeros(3,length(ir));

nodeSearchCost=zeros(1,2);

%graph is the Data Structure.

for u=1:length(ir)

graph(u)=struct('nodenr',ir(u),'neighbournodes',neighbourNodes(u),

'pointer',0,'visited',0,'nodecost',nodeCost(:,u),

'Nodesearchcost',nodeSearchCost,'coordinate',coordin(u,:));

end;

toc

A.1.5 Neighbo1.m

function [neighbourNodes,ir,coordin]=neighbo1

%This function provides the frees obstacles index

%and their neighbours

global new_matrix

global maxi

%ir is a vector with the real index of frees obstacles, (n x 1)

[row,col]=size(new_matrix);

[co1,co2]=find(new_matrix==0);

ir=co1+(co2-1)*row;

%coordin is a matrix with the frees obstacles coordinates, (n x 2)

coordin=[co2 co1];

%new_matrix is one matrix that cover with four externals vectors

%for four matrix sides

new_matrix=[ones(1,col+2);ones(row,1) new_matrix ones(row,1);ones(1,col+2)];

inrow=co1+1;

incol=co2+1;

%lc is the local cost matrix.

lc=[sqrt(2) 1 sqrt(2);1 0 1;sqrt(2) 1 sqrt(2)];

for i=1:length(ir)

index=find(new_matrix(inrow(i)-1:inrow(i)+1,incol(i)-1:incol(i)+1)==0

& (lc==1 | lc==sqrt(2)));

if ~isempty(index)

realin=[1 ir(i)-row-1;2 ir(i)-row;3 ir(i)-row+1;4 ir(i)-1;5 0;

6 ir(i)+1;7 ir(i)+row-1;8 ir(i)+row;9 ir(i)+row+1];


neigh=[realin(index,2) lc(index)];

else

neigh=[ir(i) 0];

end;

neighbourNodes(i)=struct('Neighbournode',neigh(:,1),'edgecost',

neigh(:,2),'edgesearchcost',[0],'nodesearchcost',1);

end;

A.1.6 Initial1.m

function [start,goal,node_goal,node_start,w]=initial1

%This function loads the start and goal configurations

global new_matrix

global graph

global matrix

figure(3);

image(matrix*40);

set(gca,'ydir','normal','xdir','normal');

figure(1);

image(new_matrix*40);


start=[];goal=[];x=[];y=[];

node_goal=0;node_start=0;

while (node_goal==0 | node_start==0)

%Initial point

[x,y,button]=ginput(1);

x=round(x);y=round(y);

start=[y(1) x(1)];

%Goal point

[x,y,button]=ginput(1);

x=round(x);y=round(y);

goal=[y(1) x(1)];

for i=1:length(graph)

if [goal(2) goal(1)]==graph(i).coordinate+1

node_goal=graph(i).nodenr;

break

end;

end;


for j=1:length(graph)

if [start(2) start(1)]==graph(j).coordinate+1

node_start=graph(j).nodenr;

break;

end;

end;

if (new_matrix(start(1),start(2))==1 | new_matrix(goal(1),goal(2))==1)

disp('The goal point or the start point is not available')

node_goal=0;node_start=0;

end

end; %While...

w=input('Give me the relative weighting function, w, between 0 and 1 .. ');

A.1.7 Search.m

function [traye]=search_r(node_start,node_goal,w)

%Algorithm in order to design paths free from obstacles, on application of

%A-star method.

%

%The main variables are the follows:

%- The nodes start and goal are the input variables along with W that is

% weighting function.

%- Traye is the output variable and represents the collection of node since

% the start configuration until the goal configuration.

%- Open is vector which has two columns, the first represents the examined

% nodes and second represents the local cost.

%- Actnode is a vector of dimension 1 x 2 , where the first column

% represents the actual node and the second its local cost.

%- Actneighnode is the same type of variable that Actnode, only it applies

% from the current node to their neighbors.

tic

global tree

global graph

point=[];

actneighnode=0;

traye=[];

node=[];

open = [];

lookup_table=table;

no_start=[node_start 0];

goal=graph(lookup_table(node_goal)).coordinate;


open=[open;no_start];

while (isempty(open) == 0),

costF=open(:,2);

[l index_1]=min(costF);

actnode=open(index_1,:);

open=delnode1(actnode,open);

graph(lookup_table(actnode(1))).visited=1;

if actnode(1)==node_goal(1)

disp('Found path') % Representation of found way

previous=graph(lookup_table(actnode(1))).pointer;

path1=actnode(1);

while previous~=0

path1=[path1;previous];

previous=graph(lookup_table(previous)).pointer;

traye=path1;

end

if isempty(traye)

traye=node_goal(1);

else

traye=[traye;node_start];

end;

toc

return;

end;

node=lookup_table(actnode(1));

for j=1:length(graph(node).neighbournodes.Neighbournode)

neighbouring=graph(node).neighbournodes.Neighbournode(j);

if ~isempty(neighbouring)

localcost=graph(node).neighbournodes.edgecost(j);

coor=graph(lookup_table(neighbouring)).coordinate;

index=lookup_table(neighbouring);

index_cost=lookup_table(actnode(1));

rel_cost=graph(index_cost).Nodesearchcost(1);

node_cost=localcost+rel_cost;

distan=sqrt((coor(1)-goal(1))^2+(coor(2)-goal(2))^2);

actneighnode(1)=neighbouring;

actneighnode(2)=[(1-w)*(node_cost)+w*(distan)];

position=find(open(:,1)==actneighnode(1));

if (isempty(position)==1 & graph(lookup_table(actneighnode(1))).visited==0)

open=[open;actneighnode];


graph(index).Nodesearchcost=[node_cost distan];

graph(index).pointer=actnode(1);

elseif (isempty(position)==0)

if graph(lookup_table(actneighnode(1))).Nodesearchcost(1)>node_cost

open = delnode1(actneighnode,open);




%Now there is a new cost G and H

end; %If cost_current

else

if (graph(lookup_table(actneighnode(1))).Nodesearchcost(1)>node_cost)

graph(lookup_table(actneighnode(1))).visited=0;

open =[open;actneighnode];



end;

end;

end;

end %For j=1:length...

end %While (empty(open))==0

% while - Loop

disp('Did not find path !!!')

traye=[];

path1=-1;

A.1.8 Graphnode.m

%Graph plotting. This function shows two possibilities

%of plotting:

%

%- The first is in order to implement the decomposition

% for Equal

% Size, when "factor is equal to maxi", because the

% reference system of coordenates is different in

% relation to the decompositio for Different size.

%- The second is in order to implement the decomposition

% for Different Size

global graph;

global maxi;


lookup_table=table;

factor=[];

if ~isempty(maxi)

factor=maxi;

else

factor=1;

end;

coor=[];

figure(2);

tic

for p=1:length(graph)

coord=graph(p).coordinate*factor;

plot(coord(1),coord(2),'.r','markersize',25);

hold on

for q=1:length(graph(p).neighbournodes.Neighbournode)

coor=graph(lookup_table(graph(p).neighbournodes.

Neighbournode(q))).coordinate*factor;

line([coord(1);coor(1)],[coord(2);coor(2)]);

hold on

end;

end;

toc

title('This Graph represents the possible connections

between nodes');

A.1.9 Plotting.m

function [real_coord]=plotting1(traye)

%This funtion converts "nodes path" to "real coordinates",

%and plotting them.

%

%This function shows two possibilities of plotting:

%- The first is in order to implement the decomposition for

% Equal Size, when "factor is equal to maxi", because the

% reference system of coordenates is different in relation

% to the decompositio for Different Size.

%- The second is in order to implement the decomposition for

% Different Size

%

%- The main variable is real_coord which takes the reals coordenates

% respectly the initial matrix

global graph

global matrix

global maxi


lookup_table=table;

factor1=[];factor2=[];

if ~isempty(maxi)

factor1=maxi;

factor2=0;

else

factor1=1;

factor2=0.5;

end;

if ~isempty(traye)

real_coord=[];

reduc_coord=[];

for i=1:length(traye)

real_coord=[real_coord;(graph(lookup_table(traye(i))).coordinate*factor1)];

end;

figure(3);

hold on

plot(real_coord(:,1)+factor2,real_coord(:,2)+factor2,'yo',

real_coord(:,1)+factor2,real_coord(:,2)+factor2,'r');

title('This graph shows the layout of the trajectory in

the real environment');


coord=graph(p).coordinate*factor1;

plot(coord(1)+factor2,coord(2)+factor2,'.r','markersize',25);

hold on


co=graph(lookup_table(graph(p).neighbournodes.Neighbournode(q))).

coordinate;

line([coord(1);co(1)],[coord(2);co(2)]);

hold on

end

end

figure(2);

hold on

plot(real_coord(:,1),real_coord(:,2),'yo',real_coord(:,1),

real_coord(:,2),'r');

else

real_coord=[];

end;

distanc=[];


for h=2:length(real_coord)

d(h)=sqrt(((real_coord(h,1)-real_coord(h-1,1))^2)+

((real_coord(h,2)-real_coord(h-1,2))^2));

distanc=[distanc;d(h)];

end

distance=sum(distanc)

A.2 Programs for Quadtree Decomposition

A.2.1 Planning.m

%This is the General Path Planning Program, which applies the

%the Approximate Cell Decomposition Method.

%

%The program uses the functions following:

%- Data1, which acquires the input datas.

%- Grid2, which prepares the environment plan

%- Quadtree does the quadtree decomposition

%- Graph4 obtains the main information as datas struture

%- Initial2 provides the initial and goal configurations

%- Search_r finds the convenient path on application of A-star Method

%- Plotting2 shows graphical the results

clear all;

close all;

global tree

global matrix

global graph

data1;

[matrix,x_mid,y_mid]=grid2;

[p,tree]=qdtree;

graph=graph4(x_mid,y_mid);

[start,goal,node_goal,node_start,w]=initial2;

[traye]=search_r(node_start,node_goal,w);

graphnode;

[real_coord]=plotting1(traye);

A.2.2 Data1.m

function data1

%This function give the posibility to choose the input matrix.


%The matrix dimension is (n x m).

global matrix

while isempty(matrix)

[filename,path]=uigetfile('*.mat','Input Matrix');

if filename ~=0,

eval(['load 'path filename])

else

disp('File Load Error !')

break;

end;

if isempty(matrix)

disp('The selected file is not suitable for this application,

tray again');

end

end;

A.2.3 Grid2.m

function [matrix,x_mid,y_mid]=grid2

%This function expands the dimension of the matrix until power of 2

%the nearest.

global matrix;

[xmax,ymax]=size(matrix);

dim2=max([ceil(log(xmax)/log(2)) ceil(log(ymax)/log(2))]);

matrix=[[matrix;ones(2^dim2-xmax,ymax)] ones(2^dim2,2^dim2-ymax)];

x=2^dim2;

y=x;

x_mid=x/2;

y_mid=y/2;

A.2.4 Qdtree.m

function [p,tree]=qdtree

%This function controls the construction of the Quadtree, in the

%way that examines each pixel in the binary array-matrix (the matrix

%represents the environment of Robot) only once and in a manner

%analogous to a postorder tree traversal, therefore if the image is

%all white or all black, it creates the appropriate one-node tree.

%



%- Tree is a data structure that contains the information of Quadtree

% Decomposition. Its fields are COLOR, SONS and PARENT.

%- P is the pointer node that points out to node-root in the tree

% structure

global counter;

global tree;

global xf;

global yf;

global matrix;

matrix=flipud(matrix);

nil=-1;counter=0;

tree(1).color=0;

tree(1).son=0;

tree(1).father=1;

[xmax,ymax]=size(matrix);

%XF and YF contain multiplicative factors to help to find the

%location of the pixels while descending the quadtree.

xf=[1 1 0 0];

yf=[1 0 1 0];

level=ceil(log(xmax)/log(2));

[p,color]=construct(level,2^level,2^level);

%Create a one-node quadtree if entire image is black or white

if p==nil

[p]=cre_node(nil,nil,color);

end

return

A.2.5 Construct.m

function [p,color]=construct(level,x,y)

%The construction of the tree is performed by

%this function, which recursively examines all

%the pixels and creates nodes whenever all four

%sons are not of the same type. The tree is

%built as CONSTRUCT returns from examining its sons.

%This function makes use of the type COLOR to

%indicate the color of a pixel or the type of a node.

%

%Another important aspects are the follows:

%- Color: takes the value 1 if is EMPTY, 2 if is FUll,


% and 3 if is MIXED.

%- Quadrant: takes the values 1,2,3, and 4 corresponding

% on the quadrant NW, NE, SW and SE.

global tree;

global counter;

global xf;

global yf;

global matrix;

nil=-1;tc=[0 0 0 0];

if level==0 %At the pixel?

if matrix(x,y)==0

color=1; %it means that the pixel is Empty

else

color=2; %it means that the pixel is Full

end

p=nil;

return

else

level=level-1;

d=2^level;

for i=1:4 % 1,2,3 and 4 represents NW,NE,SW and SE.

[p(i),tc(i)]=construct(level,x-xf(i)*d,y-yf(i)*d);

end;

if ((tc(1)~=3) & ((tc(1)==tc(2)) & (tc(1)==tc(3))

& (tc(3)==tc(4))))

%All four brothers are of the same leaf node type

color=tc(1);

p=nil;

return

else

q=cre_node(nil,nil,3); %Create a nonleaf mixed node,

%where 3 represents "mixed"

for j=1:4

if p(j)==nil %Create a maximal node for P(j) if

%there is no son in quadrant j

p(j)=cre_node(q,j,tc(j));

else

tree(q).son(j)=p(j);

tree(p(j)).father=q;

end;

end;

color=3;

p=q;

return;

end;


end;

A.2.6 Createnode.m

function [counter]=cre_node(root,t,c)

%Create a node with color c corresponding to son t, where t represents the

%value of quadrant NW, NE, SW and SE, of node root and

%return a pointer to it. When Root is Nil, the transmitted actual parameter

%value corresponding to t is Nil and is ignored.

%

%The variable more importan are:

%- Root, which is a node that at the same time is "father" of other nodes.

%- t is the index of the four sons (NW, NE, SW and SE).

%- c is the color that it can take the values 1,2 and 3 corresponding on the

% states Full, Empty or MIXED.

global counter;

global tree;

nil=-1;

counter=counter+1;

if root~=nil

tree(root).son(t)=counter;

end

tree(counter).father=root; %Created node has a father

tree(counter).color=c;

for i=1:4

tree(counter).son(i)=nil;

end

A.2.7 Graph4.m

function graph=graph4(x_mid,y_mid)

%This function creates the data structure graph with all the

%information.

%The main variable are the follows:

%- x_mid and y_mid are the central coordenates of matrix


%- neighbor is a vector which contains the neighbors by node

% considered.

%- out is a scalar which contains a neighbor by node.

%- graphnode is the index of Graph.

%- neighbourNodes is a data structure which contains information

% about the

% neighbors by node.

%- graph is the general datas structure.

global tree;

%Initialization of variables

nil=-1;

out=[];neighbor=[];x=[];y=[];

nodeCost=zeros(3,1);

nodeSearchCost=zeros(1,2);

graphnode=1;

GraphPos=1;

tic

for TreeIndex=1:length(tree)

if tree(TreeIndex).color==1 %Make entry in Graph

neighbor=[];

NeighPos=1;

[x_cor,y_cor]=cor_quad(TreeIndex,x_mid,y_mid);

for j=1:8

out=gteq_v_n(TreeIndex,j);

if out~=nil

if (tree(out).color==3)

out=gtsm_v_n(out,j);

if ~isempty(out)

neighbor(NeighPos:NeighPos+length(out)-1)=out;

NeighPos=NeighPos+length(out);

end

elseif (tree(out).color==1)

neighbor(NeighPos)=out;

NeighPos=NeighPos+1;

end

end

end

neighbor=unique(neighbor);

distance=dis_cost(x_cor,y_cor,neighbor,x_mid,y_mid);

neighbourNodes=struct('Neighbournode',neighbor,'edgecost',

distance,'edgesearchcost',[0],'nodesearchcost',1);

graph(graphnode)=struct('nodenr',TreeIndex,'neighbournodes',

neighbourNodes,'pointer',0,'visited',0,'nodecost',

nodeCost,'Nodesearchcost',nodeSearchCost,

'coordinate',[x_cor y_cor]);

graphnode=graphnode+1;

GraphPos=GraphPos+1;

end % end if


end % end for length(tree)

toc

A.2.8 Cordenate-quadrant.m

function [out1,out2]=cor_quad(node,xi,yi)

%This function provides the real coordinates. Given the

%index node gets both the coordinates.

%

%The main variables of the function are the follows:

%- Node is the quadrant index in relation to Tree data structure.

%- xi and yi are the origin of coordenates

%- out1 and out2 are the output coordenates

%- lev is the hierarchical level in Tree data structure.

%- pos represents one of the four quadrants: NW, NE, SW and SE.

global tree

x=xi; %Origin of coordenate x

y=yi; %Origin of coordenate y

nil=-1;

l=0;

position=[];

lev=[];

pos=[];

pos=find(tree(tree(node).father).son==node);

while (tree(node).father>0)

node=tree(node).father;

l=l+1;

lev=[lev;l];

position=[position;pos];

if tree(node).father>0

pos=find(tree(tree(node).father).son==node);

end

end

for i=length(lev):-1:1

if position(i)==1

x=-xi/(2^(length(lev)-i+1))+x;

y=yi/(2^(length(lev)-i+1))+y;

elseif position(i)==2


x=xi/(2^(length(lev)-i+1))+x;

y=yi/(2^(length(lev)-i+1))+y;

elseif position(i)==3

x=-xi/(2^(length(lev)-i+1))+x;

y=-yi/(2^(length(lev)-i+1))+y;

else

x=xi/(2^(length(lev)-i+1))+x;

y=-yi/(2^(length(lev)-i+1))+y;

end

end

out1=x;

out2=y;

A.2.9 gteq-vertex-neighbour.m

function out=gteq_v_n(p,i)

%Locate a vertex-neighbor of node P, of size

%greater than or equal to P, in the direction I.

%If such a node does not exist, return NIL.

global tree;

nil=-1;

%Find a common ancestor

if tree(p).father==nil

neighbor=nil;

elseif adjacen(i,find(tree(tree(p).father).son==p))==1

neighbor=gteq_v_n(tree(p).father,i);

elseif com_ed(i,find(tree(tree(p).father).son==p))~=nil

neighbor=gteq_v_n(tree(p).father,

com_ed(i,find(tree(tree(p).father).son==p)));

else

neighbor=tree(p).father;

end

if (neighbor~=nil & tree(neighbor).color==3)

%Follow opposite path to locate the neighbor

out=tree(neighbor).son(reflect(i,find(tree(tree(p).father).son==p)));

else

out=neighbor;

end

return

A.2.10 Adjacen.m


function out=adjacen(i,p)

%This function represents the adjacent between the quadrants.

%Adjacen(p,i) is if and only if, quadrant p is adjacent to the

%i_th edge or vertex of p's containing block, where the output 1

%represents "true" and the output 2 represents "false".

mat=[1 2 2 1 1 2 2 2;1 1 2 2 2 1 2 2;2 2 1 1 2 2 1 2;2 1 1 2 2 2 2 1];

out=mat(p,i);

A.2.11 Common-edge.m

function out=com_ed(i,p)

%This function returns the block that is common to quadrant P

%and its neighbor in the I-th direction.

mat=[-1*ones(1,4) -1 1 4 -1;-1*ones(1,4) 1 -1 -1 2;

-1*ones(1,4) 4 -1 -1 3;-1*ones(1,4) -1 2 3 -1];

out=mat(p,i);

A.2.12 Re ect.m

function out=reflect(i,p)

%This function shows the neighbour corresponding on direction i

%and quadrant p

mat=[3 2 3 2 4 4 4 4;4 1 4 1 3 3 3 3;1 4 1 4 2 2 2 2;2 3 2 3 1 1 1 1];

out=mat(p,i);

A.2.13 gtsm-vertex-neighbour.m

function out=gtsm_v_n(p,i)

%This function locate a neighbor of node P, of size less than to P, in

%the direction I.


global tree

out=[];

mirrorconst=mirror(i);

mirrorlength=length(mirrorconst);

for lv=1:mirrorlength

if tree (tree(p).son(mirrorconst(lv))).color==3

out=[out gtsm_v_n(tree(p).son(mirrorconst(lv)),i)];

elseif tree(tree(p).son(mirrorconst(lv))).color==1

out=[out tree(p).son(mirrorconst(lv))];

end

end

return

A.2.14 Mirror.m

function out=mirror(i)

%Mirror provides, in function direction given, the opposite directions

%corresponding on the quadrans: NW, NE, SW and SE and the directions:

%N, S, E, W, NW, NE, SW and SE., where i the direction.

if i < 5

mir=[3 1 1 2

4 3 2 4];

out=mir(:,i);

else

mir=[-1 -1 -1 -1 4 3 2 1];

out=mir(i);

end;

A.2.15 distance-cost.m

function distance=dis_cost(x,y,neighbor,x_mid,y_mid)

%This function calculates the distance between a node and each

%neighbor

%Initilization of variables

dista=[];distance=[];


if length(neighbor)>0

for i=1:length(neighbor)

[x2,y2]=cor_quad(neighbor(i),x_mid,y_mid);

dista=sqrt(((x-x2)^2)+((y-y2)^2));

distance=[distance;dista];

end

else

distance=0;

end

A.2.16 Initial2.m

function [start,goal,node_goal,node_start,w]=initial2

%This function loads the start and goal configurations

global matrix

global tree;

figure(3);

image(flipud(matrix)*40);


[x_dim,y_dim]=size(matrix);

empty_go=1;

empty_st=2;

while (empty_go~=1 | empty_st~=1)

[x1,y1,button1]=ginput(1);

node_start=quadrant(x1-0.5,y1-0.5,x_dim/2);

[x2,y2,button2]=ginput(1);

node_goal=quadrant(x2-0.5,y2-0.5,x_dim/2);

if (tree(node_start).color~=1 | tree(node_goal).color~=1)

disp('The goal point or the start point is not available')

end

start=[x1 y1]-0.5;

goal=[x2 y2]-0.5;

empty_st=tree(node_start).color;

empty_go=tree(node_goal).color;

end

w=input('Give in the relative weighting function, w, between 0 and 1 .. ');


A.2.17 Search.m

function [traye]=search_r(node_start,node_goal,w)

%Algorithm in order to design paths free from obstacles, on application of

%A-star method.

%


%- The nodes start and goal are the input variables along with W that is

% weighting function.

%- Traye is the output variable and represents the collection of node since

% the start configuration until the goal configuration.

%- Open is vector which has two columns, the first represents the examined

% nodes and second represents the local cost.

%- Actnode is a vector of dimension 1 x 2 , where the first column

% represents the actual node and the second its local cost.

%- Actneighnode is the same type of variable that Actnode, only it applies

% from the current node to their neighbors.

tic

global tree

global graph

point=[];

actneighnode=0;

traye=[];

node=[];

open = [];

lookup_table=table;

no_start=[node_start 0];

goal=graph(lookup_table(node_goal)).coordinate;

open=[open;no_start];

while (isempty(open) == 0),

costF=open(:,2);

[l index_1]=min(costF);

actnode=open(index_1,:);

open=delnode1(actnode,open);

graph(lookup_table(actnode(1))).visited=1;

if actnode(1)==node_goal(1)

disp('Found path') % Representation of found way

previous=graph(lookup_table(actnode(1))).pointer;

path1=actnode(1);

while previous~=0

path1=[path1;previous];

previous=graph(lookup_table(previous)).pointer;


traye=path1;

end

if isempty(traye)

traye=node_goal(1);

else

traye=[traye;node_start];

end;

toc

return;

end;

node=lookup_table(actnode(1));

for j=1:length(graph(node).neighbournodes.Neighbournode)

neighbouring=graph(node).neighbournodes.Neighbournode(j);

if ~isempty(neighbouring)

localcost=graph(node).neighbournodes.edgecost(j);

coor=graph(lookup_table(neighbouring)).coordinate;

index=lookup_table(neighbouring);

index_cost=lookup_table(actnode(1));

rel_cost=graph(index_cost).Nodesearchcost(1);

node_cost=localcost+rel_cost;

distan=sqrt((coor(1)-goal(1))^2+(coor(2)-goal(2))^2);

actneighnode(1)=neighbouring;

actneighnode(2)=[(1-w)*(node_cost)+w*(distan)];

position=find(open(:,1)==actneighnode(1));

if (isempty(position)==1 & graph(lookup_table(actneighnode(1))).visited==0)




elseif (isempty(position)==0)

if graph(lookup_table(actneighnode(1))).Nodesearchcost(1)>node_cost

open = delnode1(actneighnode,open);




%Now there is a new cost G and H

end; %If cost_current

else

if (graph(lookup_table(actneighnode(1))).Nodesearchcost(1)>node_cost)

graph(lookup_table(actneighnode(1))).visited=0;

open =[open;actneighnode];



end;

end;


end;

end %For j=1:length...

end %While (empty(open))==0

% while - Loop

disp('Did not find path !!!')

traye=[];

path1=-1;

A.2.18 Graphnode.m

%Graph plotting. This function shows two possibilities

%of plotting:

%

%- The first is in order to implement the decomposition

% for Equal

% Size, when "factor is equal to maxi", because the

% reference system of coordenates is different in

% relation to the decompositio for Different size.

%- The second is in order to implement the decomposition

% for Different Size

global graph;

global maxi;

lookup_table=table;

factor=[];

if ~isempty(maxi)

factor=maxi;

else

factor=1;

end;

coor=[];

figure(2);

tic


coord=graph(p).coordinate*factor;

plot(coord(1),coord(2),'.r','markersize',25);

hold on



coor=graph(lookup_table(graph(p).neighbournodes.

Neighbournode(q))).coordinate*factor;

line([coord(1);coor(1)],[coord(2);coor(2)]);

hold on

end;

end;

toc

title('This Graph represents the possible connections

between nodes');

A.2.19 Plotting.m

function [real_coord]=plotting1(traye)

%This funtion converts "nodes path" to "real coordinates",

%and plotting them.

%

%This function shows two possibilities of plotting:

%- The first is in order to implement the decomposition for

% Equal Size, when "factor is equal to maxi", because the

% reference system of coordenates is different in relation

% to the decompositio for Different Size.

%- The second is in order to implement the decomposition for

% Different Size

%

%- The main variable is real_coord which takes the reals coordenates

% respectly the initial matrix

global graph

global matrix

global maxi

lookup_table=table;

factor1=[];factor2=[];

if ~isempty(maxi)

factor1=maxi;

factor2=0;

else

factor1=1;

factor2=0.5;

end;

if ~isempty(traye)

real_coord=[];

reduc_coord=[];

for i=1:length(traye)


real_coord=[real_coord;(graph(lookup_table(traye(i))).coordinate*factor1)];

end;

figure(3);

hold on

plot(real_coord(:,1)+factor2,real_coord(:,2)+factor2,'yo',

real_coord(:,1)+factor2,real_coord(:,2)+factor2,'r');

title('This graph shows the layout of the trajectory in

the real environment');


coord=graph(p).coordinate*factor1;

plot(coord(1)+factor2,coord(2)+factor2,'.r','markersize',25);

hold on


co=graph(lookup_table(graph(p).neighbournodes.Neighbournode(q))).

coordinate;

line([coord(1);co(1)],[coord(2);co(2)]);

hold on

end

end

figure(2);

hold on

plot(real_coord(:,1),real_coord(:,2),'yo',real_coord(:,1),

real_coord(:,2),'r');

else

real_coord=[];

end;

distanc=[];

for h=2:length(real_coord)

d(h)=sqrt(((real_coord(h,1)-real_coord(h-1,1))^2)+

((real_coord(h,2)-real_coord(h-1,2))^2));

distanc=[distanc;d(h)];

end

distance=sum(distanc)


Statement

Hereby I certify that I have realized this Diploma Thesis on my own and I have onlyused the mentioned sources.

Ulm, at 27.08.98 Pablo Tapia

Documents

Ac kno wledgemen ts - WordPress.com · y the rob ot itself using certain sensors as cameras, enco ders, sonars, accelerom-eters and lasers. F or the purp ose of this pro ject w e