Professional Documents
Culture Documents
Mechanical Engineering
Manufacturing Networks
Abstract
In this article two algorithms for robot path planning are compared and
reviewed on completeness, optimality, error reporting and computational
cost. The algorithms considered are the widely used A* algorithm and the
recently developed Topology Optimization Method (TOM). We compare
both algorithms and proceed implementing them in Matlab to study several
specific cases. These cases include new and previously proposed simulations
to test the strength of the algorithms under several limiting cases. We show
that A* is always complete, optimal and reports if a path does not exist. On
the other hand, the TOM algorithm does not satisfy these criteria, but has
lower computational cost especially for larger search domains. This study
suggests that future research in robot planning could focus on combining
the A* and TOM algorithms to create a hybrid algorithm.
Contents
1 Introduction
3
4
5
7
3 A* Algorithm
3.1 Overview . . . . . . . . .
3.2 Heuristics . . . . . . . . .
3.2.1 Manhattan . . . .
3.2.2 Diagonal . . . . . .
3.2.3 Euclidian . . . . .
3.2.4 Choice of heuristic
3.3 Implementation . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
4 Comparison
4.1 Requirements for a path planning algorithm . . . . . . . . . .
4.2 Test problems proposed by Ryu et al. (2012) . . . . . . . . .
4.2.1 Case 1: Symmetrical obstacles, start and goal . . . . .
4.2.2 Case 2: Different obstacles, multiple paths . . . . . . .
4.3 Newly developed test cases . . . . . . . . . . . . . . . . . . .
4.3.1 Case 1: A* challenge . . . . . . . . . . . . . . . . . . .
4.3.2 Case 2: Elaborate Maze . . . . . . . . . . . . . . . . .
4.4 Cases with a stopover . . . . . . . . . . . . . . . . . . . . . .
4.4.1 Case 1: Symmetrical obstacles, start, stopover and goal
4.4.2 Case 2: Different obstacles, multiple paths with stopover
4.5 Cases with mesh refinement . . . . . . . . . . . . . . . . . . .
4.6 Real life case . . . . . . . . . . . . . . . . . . . . . . . . . . .
8
8
9
10
10
11
11
11
12
12
13
13
14
14
15
16
17
17
19
20
21
A MATLAB code for robot path planning using topology optimization (by Van den Goor (2012))
27
B MATLAB code for robot path planning using topology optimization with adaptations and path calculation
31
C A* Matlab code including subfunctions
37
D Map Editor
45
E Cases with
E.1 Case 1:
E.2 Case 2:
E.3 Case 3:
E.4 Case 4:
E.5 Case 5:
52
52
53
53
54
54
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
Chapter 1
Introduction
In the last few years mobile robots have found many practical applications,
including: logistics (moving packages), military (explosives removal), domestic (house cleaning), medical (assisting robots, see also RoboCup (2012)) and
many more. Mobile robots encompass a wide variety of different research
fields, including mechanics, dynamics, micro-scale engineering, manufacturing networks and many more. The focus of this article is on robot path
planning.
Robot path planning in general requires an algorithm to find a suitable path between a start and finish point (with optional stops in between)
whilst avoiding certain areas referred to as obstacles. It is essential for mobile robots to have good path planning in order to efficiently move and avoid
obstacles. Masehian and Sedighizadeh (2007) give an overview of algorithms
for robot path planning. The path planning research area can be roughly
divided in to two areas: global and local path planning. Global path planning is concerned with determining a route from start to finish on a large
scale, whilst local path planning manages the movement in the area directly
around the robot often including dynamic obstacles. The global path planner should also prevent a robot from entering an infinite loop, which can
occur when using a local path planner only.
The main challenges considering global path planning consist of compiling the the available information into an effective and appropriate configuration space and subsequently using a search algorithm to find the best path
in that space. This article concerns itself with the search algorithms used
to find the best path.
A commonly used search method is the A* (pronounced A-star ) algorithm, which was first introduced in 1968 by Hart et al.. The algorithm is an
improvement of Dijkstras algorithm and Best-First-Search (BFS) by using
the best parts of both. The advantage of the A* algorithm is that it favors
points close to the starting point (Dijkstras algorithm) as well as point close
to the goal point (BFS) using an estimation (or heuristic). Because of this,
A* still guarantees to find the shortest path possible like Dijkstras algorithm, but is significantly faster because of the heuristic and the resulting
smaller search area.
Recently, Ryu et al. (2012) proposed a new algorithm for robot path
planning. They formulate the path planning problem as an equivalent heat
conduction problem, with the start point as a heat source, the goal point
as a heat sink and all obstacles as thermal insulators. Ryu et al. (2012)
apply topology optimization (Bendse and Sigmund, 2004) to determine the
optimal heat path from heat source to heat sink. The problem parameters
are adjusted so that only a single path is obtained, preferably of single
element thickness.
The main goal of this article is to compare the TOM in its current form
as a global path planning method with the widely used A* algorithm. The
implementation will be done in MATLAB, because the Topology Optimization Method is already programmed in MATLAB by Van den Goor (2012)
using the 88-line MATLAB code by Andreassen et al. (2011). Besides this
obvious advantage, MATLAB also offers excellent debugging tools and extensive graphic handling opportunities.
These methods will be reviewed briefly together with their implementation in Sections 2 and 3, respectively. The comparison results for speed,
consistency, resulting paths and (dis)advantages of one compared to the
other will be discussed in Section 4. The article ends with a conclusion and
discussion in Section 5.
Chapter 2
Topology Optimization
Method
The TOM used in the comparison of this article has been developed by Ryu
et al. (2012). It is based on topology optimization for heat transfer problems and uses analogies between robot path planning and heat flow: the
areas where a robot can move freely (free configuration space) are the design domain for conductive heat transfer, the areas where the robot cannot
go (obstacle configuration space or obstacles) are designed as thermal insulators and the start and goal points of robot path planning are modeled as a
heat source and heat sink, respectively. To clarify, the analogies are graphically represented in Figure 2.1, where C is the robot configuration space
and the design domain for the thermal compliance topology optimization
problem.
Figure 2.1: Analogy between (a) robot path and (b) topology optimization
of a heat path (Ryu et al., 2012)
2.1
In order to create the TOM, the finite element method is first applied to the
following governing equation for two-dimensional steady state heat transfer
in a design domain (Dewitt and Incropera, 2002):
(k) + h + Q = 0
in
(2.1)
on 1
(2.2)
q = q
on 2
(2.3)
where k and h are the thermal conductivity and convective heat transfer coefficients, Q denotes heat generation, (= T T ) is an excess temperature
with T the temperature of the material and T the surrounding temperature, is a pre-defined value on the boundary 1 and q is the heat flux
defined on 2 .
Applying the finite element method results in the matrix equation:
K = F
(2.4)
(2.5)
(2.6)
= FT = T K
e me M0 0
(2.7)
(2.8)
e=1
(2.9)
Here (2.8) is the mass constraint, where me is the mass of the e-th
element and M 0 the allowed total mass.
2.2
In order to adapt the finite element heat transfer model discussed in the
previous paragraph for use in robot path planning, the design space needs
to be converted to equivalent heat flow definitions. This includes the free
design space, obstacle space as well as the start and finish point. The free
design space is modeled as an arbitrary heat flow with:
0 < r 1,
5
qr 6= 0
(2.10)
with r the mass of the free element and qr the load vector component.
The obstacle space is then modeled as thermal insulators according to:
0 = 0,
q0 = 0
(2.11)
To define the start and goal point the essential boundary conditions in
(2.2) and (2.3) will be used. The starting point is modeled as a heat source
when it is located inside the design domain and as a incoming heat flux
when it is located on the boundary of the design domain. Also the following
essential boundary condition is used (see Equation 2.2):
start = 0
(2.12)
Ne
qsink = qr + qconv , qconv =
(2.13)
10
By using the optimality criteria method of Bendse and Kikuchi (1988),
an optimization algorithm with the following convergence criterium is defined:
|n n1 |/|n |
(n : the objective function value at the n-thiteration)
(2.14)
(2.15)
(2.16)
The allowed total mass needs to be correctly chosen in order to avoid multiple and/or meaningless paths. It is required that the algorithm produces
a unique path because otherwise there has to be chosen between several
possible paths, which slows down the algorithm. Also, most of these other
possible paths will be longer than the optimal one. Here Van den Goor
(2012) uses the same method as Ryu et al. (2012) again, being:
M0 (n) = max((n), Mmin )
(2.17)
PN e
e=1 me
n2
(n) =
(2.18)
Mmin =
|PG PS |me
PN e
e=1 me
(2.19)
2.3
Implementation
Chapter 3
A* Algorithm
3.1
Overview
A* was rst presented in 1968 by Hart et al. and since has been widely used
in path nding problems. The paper gives the proof that A*, if correctly
implemented (see Section 3.2), is both admissible and minimalistic. An
algorithm is admissible if the algorithm is guaranteed to nd the shortest
path, if there is one. It is minimalistic if compared to other admissible
algorithms with the same knowledge of the search area, it will investigate
the minimal amount of nodes necessary to nd an optimal path.
(b) Step 4
Figure 3.1: Example of the A* algorithm Step 1 and 2 (a) and 4 (b), with
the green, red, black and blue squares the start, goal, free and obstacle nodes
respectively (Guerin, 2011)
The way A* does this is by rst reading the search area, adding all
This description has been made using Patel (2012)
obstacles to the closed list. This list contains all the nodes that have already
been explored or should not be explored at all. It then sets the start node
as the current node, of which it adds all its neighbors, except for the ones
in the closed list, to the open list, which holds all nodes that still need to
be checked (Figure 3.1a). Along with each neighbor several parameters are
added, namely: g(n), h(n), f (n) and their parent node, where n is a number
to identify each node. Here g(n) is the cost of moving from the starting point
to it, h(n) is an estimate of the cost of the distance between the neighbor
and the goal point using some heuristic (more on this cost in Section 3.2),
f (n) = g(n) + h(n) is the total cost and the parent node is the node from
which the node in question was discovered. Next, it checks if some of the
neighbors are already in the open list; if there are, it checks whether the
newly calculated f (n) score is lower than the one currently attached to it. If
this is the case, g(n), f (n) and parent node are updated. It then chooses the
neighbor with the smallest f (n) score, sets it as the current node, removes
it from the open list and adds it to the closed list (Figure 3.1b). These
steps are repeated until the target node is added to the closed list or until
the open list is empty. If the target is added to the closed list, the optimal
path is constructed by tracing back the path by following the parent nodes
until the start node is reached. If the open list is empty, it means no path is
possible and the algorithm terminates. The algorithm is summarized below.
1. Set start node as current node and add it to the closed list
2. Add all neighbors of the current node to open list along with g(n),
h(n), f (n) and their parent node
3. If a neighbor is already in the open list, check if the f (n) score is lower
and if so, update g(n), h(n), f (n) and its parent
4. Choose the node with the smallest f (n) score in the open list, set it as
the current node, remove it from the open list and add it to the closed
list
5. Repeat step 2 through 4, until the target node is added to the closed
list, or until the open list is empty
6. If the target node is found, retrace the nodes by identifying their parents until the start node is found; this is the shortest path
7. If the open list is empty, no path to the target is possible
3.2
Heuristics
The kind of heuristic used in the A* algorithm greatly influences its speed.
Also, for A* to be admissible, the heuristic must satisfy the following demand
9
h(n) h(n)
for all n
(3.1)
where h(n) is some estimate for the cost of the optimal path from node n
3.2.1
Manhattan
This heuristic is commonly used for square grids that only allow horizontal
and vertical movement (this also requires a different algoritme that blocks
diagonal movement). The name itself comes from the Manhattan island,
with its streets divided in blocks by avenues and streets. The heuristic cost
h(n) is calculated by:
h(n) = |xn xgoal | + |yn ygoal |
(3.2)
Here xn , xgoal , yn and ygoal are the node x-position, goal x-position,
node y-position and goal y-position, respectively.
3.2.2
Diagonal
h(n) =
(3.3)
(3.4)
(3.5)
In short, the algorithm first calculates what the distance would be if the
Manhattan heuristic is used (3.3), then the number of possible diagonal steps
are calculated (3.4) and finally the shortest diagonal distance is calculated
by adding the distance gained and subtracting the distance won by diagonal
movement from the Manhattan distance (3.5).
10
3.2.3
Euclidian
3.2.4
Choice of heuristic
The heuristic used in this article will be Diagonal, since the Manhattan
heuristic might overestimate the cost of the optimal path because it does
not use diagonal moves, thus not satisfying Equation 3.1. The Euclidian
heuristic does not overestimate the cost, however it tends to be slower than
Diagonal because of the larger amount of explored nodes and the high computational demand of the square root. For instance, Figure 3.2 represent
the paths calculated using the Euclidian and Diagonal heuristic in red, the
nodes on the open and closed list in light blue and yellow, respectively and
the unexplored area in dark blue. The calculation times in this case were
0.243 and 0.084 seconds for Euclidian and Diagonal heuristic, respectively.
(a) Euclidian
(b) Diagonal
Figure 3.2: A*
3.3
Implementation
11
Chapter 4
Comparison
In the previous chapters the basics of the A* and TOM algorithms have
been described. To compare the two methods we start with the demands
for a path planning algorithm, followed by a selection of two test cases from
Ryu et al. (2012), after which two newly devised test cases will be used
to find specific (dis)advantages of both methods. Thereafter, two cases to
test how the methods deal with a stopover are discussed, then several cases
with increasing mesh refinement are researched and to conclude a real world
scenario will be tested.
With each case three plots will be shown, the first being the problem
definition, the second the path found by the A* method as well as the nodes
in the closed (yellow) and open (light blue) lists (this is useful in explaining
the results) and the third is the path found by the TOM. A table with
the path length and calculation time of both methods will also be shown
for each test case. To reduce the effects of deviations in calculation time
caused by computational demand of the system the calculation time will be
averaged over ten iterations. Because of the importance of some settings
(see Adjustment 4 in Appendix B and Section 2.2) in the algorithm of the
TOM, these settings will also be displayed in the table. The maps used were
created using the Matlab file in Appendix D which is based on a previous
map editor by Van den Goor (2012).
4.1
In order to make an objective comparison between the algorithms some requirements for a path planning algorithm have to be defined. The algorithms
will be tested against the following criteria:
1. Completeness: the algorithm always finds a path if there is one
2. Reporting Failures: if there is no path possible or the algorithm simply
does not find one, (how fast) does the algorithm report this?
12
4.2
4.2.1
(b) A*
(c) TOM
Mmin
A*
TOM
55,11
55,94
0,61
0,57
0,0235
13
4.2.2
(b) A*
(c) TOM
4.3
Method
Mmin
A*
TOM
64,53
65,11
0,15
0,65
0,0291
Both algorithms deal with the previous cases without very signicant differences apart from the calculation times in the second case. The following
cases are designed to be hard for either the A* algorithm (4.3.1) or the TOM
(4.3.2), revealing their specic strengths and weaknesses.
14
4.3.1
Case 1: A* challenge
(b) A*
(c) TOM
Mmin
A*
TOM
76,24
76,24
1,04
0,54
0,044
15
4.3.2
(b) A*
(c) TOM
Mmin
A*
TOM
287,56
-
0,408
0,44
0,3
16
4.4
In this section we will investigate how the two algorithms deal with a user
specied stopover. Both cases are from Ryu et al. (2012) and are the same
as the cases in Section 4.2, except for the added stopover. In this section,
an extra gure is added, because the A* algorithm is implemented to rst
nd a path from the start to the stopover and then from the stopover to the
goal. Both paths are shown separately.
4.4.1
(d) TOM
Mmin
A*
TOM
76,08
76,08
0,33
0,72
0,0345
18
4.4.2
(d) TOM
19
4.5
Method
Mmin
A*
TOM
83,84
85,84
0,25
0,70
0,0349
(a) A*
(b) TOM
20
4.6
40x40
80x80
120x120
160x160
200x200
240x240
0,15
0,65
1,43
2,92
6,53
7,10
18,59
12,50
45,52
20,62
93,94
36,97
(b) A*
(c) TOM
Mmin
A*
TOM
184,84
184,84
15,27
35,28
0,0028
To verify the previous notion the test case is altered with an obstacle in
the way of the current path, forcing both algorithms to nd a way around it.
In Figure 4.9 again both cases nd a path, so they are complete. The TOM
is far from optimal, showing a wide path and some unnished ones like in
21
Section 4.5. A* is still optimal, but the calculation times yield interesting
results (Table 4.9). Where the TOM has a calculation time similar to the
previous case, A* is almost six times slower. This is also more in line with
the results from Section 4.5 and gives rise to the notion that the calculation
time of the A* algorithm depends more strongly on the shape of the map
as well as the length of the path than with the TOM. When looking at the
cases previous to the mesh renement, where all maps are 40x40, the same
seems to hold true: A* calculation times range from 0.15 to 1.04 seconds
while the TOM calculation times only range from 0.54 to 0.72. So for maps
of the same size, the TOM seems to have more consistent calculation times
than A*.
(b) A*
(c) TOM
Mmin
A*
TOM
265,85
265,85
87,07
30,45
0,016
22
Chapter 5
Conclusions and
Recommendations
5.1
Conclusions
5.2
Recommendations
The great disadvantage of the TOM is its lack of robustness, the method
proposed by Ryu et al. (2012) does not always yield the shortest path,
but more importantly it does not always yield a complete path. If the
estimation of the allowed mass would be improved it would greatly improve
the robustness of the TOM and make it far more interesting for robot path
planning.
A* already has very good aspects for path planning, but lacks fast calculation times for large maps with long paths. A possibility to remedy this
is to add stopovers in the calculation of the path, shortening the calculation
time by clearing the open list, as shown in the stopover cases (Section 4.4).
However, this is currently not possible without sacrificing the guarantee of
an optimal path.
Further research could also be done to see how both algorithms deal
with varying terrain costs, where the TOM already proves promising (see
Ryu et al. (2012) and changing maps to investigate the alternatives both
algorithms find when the original path is blocked.
Lastly an interesting possibility for further research arises from this comparison: a combination of both the TOM and A* algorithm. Using the TOM
to quickly find wide or even multiple paths and then using this narrowed
down search area for A* to find the optimal and correct path. This could
combine the guarantee of A* to find the shortest path with the speed of the
TOM, and would be especially useful for large maps.
24
Bibliography
E. Andreassen, A. Clausen, M. Schevenels, B.S. Lazarov, and O. Sigmund.
Efficient topology optimization in matlab using 88 lines of code. Structural
and Multidisciplinary Optimization, 43(1), 2011.
M.P. Bendse and N. Kikuchi. Generating optimal topologies in structural
design using a homogenization method. Computer methods in applied
mechanics and engineering, 71(2), 1988.
M.P. Bendse and O. Sigmund. Topology optimization: theory, methods and
applications. Springer, 2004.
D.P. Dewitt and F.P. Incropera. Fundamentals of heat and mass transfer.
Wiley, 2002.
F. Guerin. a star pathfinder v. 1.92.exe. Course: Grand Challenges of
Artificial Intelligence, July 2011.
P.E. Hart, N.J. Nilsson, and B. Raphael. A formal basis for the heuristic
determination of minimum cost paths. Systems Science and Cybernetics,
IEEE Transactions on, 4(2), 1968.
E. Masehian and D. Sedighizadeh. Classic and heuristic approaches in robot
motion planning - a chronological review. World Academy of Science,
Engineering and Technology, 23, 2007.
MATLAB. version 7.14.0.739 (R2012a). The MathWorks, Inc., Natick,
Massachusetts, 2012.
A. Patel. Heurstics (from amits thoughts on pathfinding), October
2012. URL http://theory.stanford.edu/~amitp/GameProgramming/
Heuristics.html#heuristics-for-grid-maps.
RoboCup.
RoboCup @Home, October 2012.
robocupathome.org.
URL http://www.
J.C. Ryu, F.C. Park, and Y.Y. Kim. Mobile robot path planning algorithm
by equivalent conduction heat flow topology optimization. Structural and
Multidisciplinary Optimization, 1(1), 2012.
25
O. Sigmund. A 99 line topology optimization code written in matlab. Structural and Multidisciplinary Optimization, 21(2), 2001.
V.J.P. Van den Goor. Trajectorie-planning van een mobiele robot met behulp van topologieoptimalisering. Bachelor End Project, Departement
of Mechanical Engineering, Eindhoven University of Technology, August
2012.
RJ Yang and CH Chuang. Optimal topology design using linear programming. Computers & Structures, 52(2), 1994.
26
Appendix A
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
28
sH(k) = max(0,rminsqrt((i1i2)2+(j1j2)2));
74
end
75
end
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
end
end
H = sparse(iH,jH,sH);
Hs = sum(H,2);
%% INITIALIZE ITERATION
me=1/nelx/nely;
Mmin = ...
me*max(abs(sinkpos(1)sourcepos(1)),abs(sinkpos(2)sourcepos(2)))
x = repmat(1,nely,nelx);
xPhys = x;
loop = 0;
change = 1;
obj = []; vol = [];
cold=0; % Old objectivity function value
conv count=0; % Number of consecutive convergences
%% START ITERATION
while change > 0.01
loop = loop + 1;
%% CALCULATE ALLOWED MASS
z=1/loop; % z=1/loop2 for large maps
Mn=max(z,Mmin);
%% FEANALYSIS
if (loop <= nc)
penal = 1;
else
penal = min(3,1+0.1*(loop10));
end
sK = ...
reshape(KE(:)*(Emin+0.001+0.999*xPhys(:)'.penal*(E0Emin)),16*nelx*nely,1);
K = sparse(iK,jK,sK); K = (K+K')/2;
U(freedofs) = K(freedofs,freedofs)\F(freedofs);
%% OBJECTIVE FUNCTION AND SENSITIVITY ANALYSIS
ce = reshape(sum((U(edofMat)*KE).*U(edofMat),2),nely,nelx);
c = sum(sum((Emin+0.001+0.999*xPhys.penal*(E0Emin)).*ce));
dc = 0.999*penal*(E0Emin)*xPhys.(penal1).*ce;
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
dv = ones(nely,nelx);
%% FILTERING/MODIFICATION OF SENSITIVITIES
if ft == 1
dc(:) = H*(x(:).*dc(:))./Hs./max(1e3,x(:));
elseif ft == 2
dc(:) = H*(dc(:)./Hs);
dv(:) = H*(dv(:)./Hs);
end
%% OPTIMALITY CRITERIA UPDATE OF DESIGN VARIABLES AND ...
PHYSICAL DENSITIES
l1 = 0.1; l2 = 1e9; move = 0.2;
cnt = 0;
while (l2l1)/(l1+l2) > 1e3
cnt = cnt+1;
if (cnt>1000)
29
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
fprintf('Terminate iteration...\n');
obj = []; vol = [];
return
end
lmid = 0.5*(l2+l1);
xnew = ...
max(0,max(xmove,min(1,min(x+move,x.*sqrt(dc./dv/lmid)))));
for a=1:numel(obsx)
xnew(obsx(a),obsy(a))=0;
end
if ft == 1
xPhys = xnew;
elseif ft == 2
xPhys(:) = (H*xnew(:))./Hs;
end
if sum(xPhys(:)) > Mn*nelx*nely, l1 = lmid; else l2 = ...
lmid; end
end
change = max(abs(xnew(:)x(:)));
x = xnew;
obj=[obj c];
vol=[vol mean(xPhys(:))];
if (abs(ccold)/abs(c))<0.01, conv count=conv count+1;
else conv count = 0;end
if (loop>3 & conv count >=3)
fprintf('Convergence, cn = %f, cn1 = %f\n',c,cold)
change = 0;
end
cold = c;
%% PRINT RESULTS
if(fast | | change <= 0.01)
fprintf(' It.:%5i Obj.:%11.4f Vol.:%7.3f ...
ch.:%7.3f\n',loop,c, ...
mean(xPhys(:)),change);
%% PLOT DENSITIES
if change<= 0.01, xPhys=round(xPhys);path = xPhys; end
imagesc(xPhys+0.25*MAP); caxis([0 1]); axis equal; ...
axis off; drawnow;
end
end
t=toc;
fprintf('Time taken: %4.3f Average/iteration: %4.4f\n',t,t/loop);
path = xPhys;
end
30
Appendix B
Mmin =
(B.1)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
32
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
Mmin = ...
me*sqrt((sinkpos(1)sourcepos(1))2+(sinkpos(2)sourcepos(2))2);
84
85
86
87
88
89
90
91
92
93
94
95
96
x = ones(nely,nelx);
xPhys = x;
loop = 0;
change = 1;
obj = []; vol = [];
cold=0; % Old objectivity function value
conv count=0; % Number of consecutive convergences
%% START ITERATION
while change > 0.01
loop = loop + 1;
%% CALCULATE ALLOWED MASS
z=1/(loop2); % z=1/loop2 for large maps
97
98
Mn=max(z,Mmin);
33
99
100
101
102
103
104
105
106
107
108
109
110
111
112
%% FEANALYSIS
if (loop <= nc)
penal = 1;
else
penal = min(3,1+0.1*(loop10));
end
sK = ...
reshape(KE(:)*(Emin+0.001+0.999*xPhys(:)'.penal*(E0Emin)),16*nelx*nely,1);
K = sparse(iK,jK,sK); K = (K+K')/2;
U(freedofs) = K(freedofs,freedofs)\F(freedofs);
%% OBJECTIVE FUNCTION AND SENSITIVITY ANALYSIS
ce = reshape(sum((U(edofMat)*KE).*U(edofMat),2),nely,nelx);
c = sum(sum((Emin+0.001+0.999*xPhys.penal*(E0Emin)).*ce));
dc = 0.999*penal*(E0Emin)*xPhys.(penal1).*ce;
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
dv = ones(nely,nelx);
%% FILTERING/MODIFICATION OF SENSITIVITIES
if ft == 1
dc(:) = H*(x(:).*dc(:))./Hs./max(1e3,x(:));
elseif ft == 2
dc(:) = H*(dc(:)./Hs);
dv(:) = H*(dv(:)./Hs);
end
%% OPTIMALITY CRITERIA UPDATE OF DESIGN VARIABLES AND ...
PHYSICAL DENSITIES
l1 = 0.1; l2 = 1e9; move = 0.2;
cnt = 0;
while (l2l1)/(l1+l2) > 1e3
cnt = cnt+1;
if (cnt>1000)
fprintf('Terminate iteration...\n');
obj = []; vol = [];
return
end
lmid = 0.5*(l2+l1);
xnew = ...
max(0,max(xmove,min(1,min(x+move,x.*sqrt(dc./dv/lmid)))));
for a=1:numel(obsx)
xnew(obsx(a),obsy(a))=0;
end
if ft == 1
xPhys = xnew;
elseif ft == 2
xPhys(:) = (H*xnew(:))./Hs;
end
if sum(xPhys(:)) > Mn*nelx*nely, l1 = lmid; else l2 = ...
lmid; end
end
change = max(abs(xnew(:)x(:)));
x = xnew;
obj=[obj c];
vol=[vol mean(xPhys(:))];
if (abs(ccold)/abs(c))<=0.01, conv count=conv count+1;
34
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
%% PRINT RESULTS
if(fast | | change <= 0.01)
fprintf(' It.:%5i Obj.:%11.4f Vol.:%7.3f ...
ch.:%7.3f\n',loop,c, mean(xPhys(:)),change);
%% PLOT DENSITIES
if change<= 0.01, xPhys=round(xPhys);path = xPhys; end
imagesc(xPhys+0.25*MAP); caxis([0 1]); axis equal; ...
axis off; drawnow;
end
end
timeTOM=toc;
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
35
end
194
end
195
end
196
end
197
end
current = sorted path(g+1,:);
198
199
200
end
201
202
203
204
205
206
207
208
209
210
pathlength TopOpt = 0;
for i = 1:(size(sorted path,1)1)
pathlength TopOpt = pathlength TopOpt + ...
distance(sorted path(i,1),sorted path(i,2),...
sorted path((i+1),1),sorted path((i+1),2));
end
211
212
213
214
215
end
36
Appendix C
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%LISTS USED FOR ALGORITHM
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%OPEN LIST STRUCTURE
%
%IS ON LIST 1/0 | X val | Y val | Parent X val | Parent Y val ...
| h(n) | g(n) | f(n) |
%
OPEN=[];
%CLOSED LIST STRUCTURE
%
%X val | Y val |
%
% CLOSED=zeros(MAX VAL,2);
CLOSED=[];
27
28
29
30
31
32
37
33
34
35
36
37
38
39
MAX X=size(MAP,1);
MAX Y=size(MAP,2);
40
41
42
43
44
45
46
47
48
49
50
51
52
53
k = 1;
for i=1:MAX X
for j=1:MAX Y
if(MAP(i,j) == 1)
CLOSED(k,1)=i;
CLOSED(k,2)=j;
k=k+1;
elseif (MAP(i,j) == 3)
xStart=i;%Starting Position
yStart=j;%Starting Position
elseif(MAP(i,j) == 4)
xTarget=i;%X Coordinate of the Target
yTarget=j;%Y Coordinate of the Target
54
end
55
end
56
57
end
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
loop = 0;
NoPathFound = 1;
%% Forward Path Construction Loop
while ((xpos = xTarget | | ypos = yTarget) && NoPathFound == 1)
loop = loop+1;
successorlist = ...
successors(xpos,ypos,xTarget,yTarget,g cost,CLOSED,MAX X,MAX Y,heuristic);
num succesors = size(successorlist,1);
82
83
84
38
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
%Find the node with lowest f(n) score in the open list
index min = min fn(OPEN,OPEN count,xTarget,yTarget); ...
%returns the index of the node in the open list with ...
the lowest f(n) score
104
105
106
107
108
109
110
111
112
113
114
115
116
117
else
118
119
end
120
121
end
39
122
123
124
125
126
127
128
129
130
131
132
133
134
135
i=size(CLOSED,1);
Optimal path=[];
xpos=CLOSED(i,1);
ypos=CLOSED(i,2);
i=1;
Optimal path(i,1)=xpos;
Optimal path(i,2)=ypos;
i=i+1;
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
if ((fast = 1) | | (i == 1))
imagesc(visual path+0.25*MAP); caxis([0 4]); axis ...
equal; axis off; drawnow;
40
end
171
end
172
173
174
175
176
177
178
179
180
181
if heuristic == 0
fprintf('\n Time taken Astar Dijkstra: %4.3f Average ...
time/iteration Astar: %4.4f \n Path length: %4.5f ...
Number of iterations: %4.5f \n ...
',timeAstar,timeAstar/loop,pathlength,loop);
title('Astar Dijkstra Algorithm Path')
182
183
184
185
elseif heuristic == 1
fprintf('\n Time taken Astar Straight: %4.3f Average ...
time/iteration Astar: %4.4f \n Path length: %4.5f ...
Number of iterations: %4.5f \n ...
',timeAstar,timeAstar/loop,pathlength,loop);
title('Astar Straight Algorithm Path')
186
187
188
189
elseif heuristic == 2
fprintf('\n Time taken Astar Manhattan: %4.3f Average ...
time/iteration Astar: %4.4f \n Path length: %4.5f ...
Number of iterations: %4.5f \n ...
',timeAstar,timeAstar/loop,pathlength,loop);
title('Astar Manhattan Algorithm Path')
190
191
192
193
elseif heuristic == 3
fprintf('\n Time taken Astar Diagonal: %4.3f Average ...
time/iteration Astar: %4.4f \n Path length: %4.5f ...
Number of iterations: %4.5f \n ...
',timeAstar,timeAstar/loop,pathlength,loop);
title('Astar Diagonal Algorithm Path')
end
194
195
196
197
198
199
200
else
fprintf('No path to target possible!')
201
202
end
203
204
205
1
2
3
4
end
41
dist=sqrt((x1x2)2 + (y1y2)2);
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
42
end
33
34
successorlist(count,5) = ...
successorlist(count,3)+successorlist(count,4)*p; ...
%fn = gn+hn
count = count+1;
35
36
37
end
38
end
39
end
40
end
41
42
43
1
2
3
4
5
6
end
end
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
temp array=[];
k=1;
flag=0;
goal index=0;
for j=1:OPEN COUNT
if (OPEN(j,1)==1)
temp array(k,:)=[OPEN(j,:) j]; %#ok<*AGROW>
if (OPEN(j,2)==xTarget && OPEN(j,3)==yTarget)
flag=1;
goal index=j;%Store the index of the goal node
end;
k=k+1;
end;
end;%Get all nodes that are on the list open
if flag == 1 % one of the successors is the goal node so send ...
this node
i min=goal index;
end
%Send the index of the smallest node
if size(temp array = 0)
[min fn,temp min]=min(temp array(:,8));%Index of the ...
smallest node in temp array
i min=temp array(temp min,9);%Index of the smallest node in ...
the OPEN array
else
i min=1;%The temp array is empty i.e No more paths are ...
available.
end;
43
1
2
3
4
5
6
7
8
9
10
11
44
Appendix D
Map Editor
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
pos=[1;1];
filename=''; pathname=''; file='';
fig = figure(); set(fig,'name','Robot Map ...
editor','numbertitle','off','Position',[600 300 700 550]);
uicontrol('String','Obstacle','Position', [3 90 60 30], ...
'Callback',{@mat,1})
uicontrol('String','Free','Position', [3 60 60 30], ...
'Callback',{@mat,0})
uicontrol('String','Start','Position', [3 30 60 30], ...
'Callback',{@mat,3})
uicontrol('String','Goal','Position', [3 0 60 30], ...
'Callback',{@mat,4})
uicontrol('String','Go','Position', [3 150 60 ...
30],'Callback',@press go)
its = uicontrol('Style', ...
'slider','Min',1,'Max',50,'Value',1,'SliderStep',[1/49 ...
1/49],'Position', [63 370 60 30],'Callback',@displayits);
uicontrol('Style','text','Position',[63 415 60 ...
15],'String','Iterations')
uicontrol('Style','text','Position',[63 400 60 ...
15],'String',get(its,'value'))
stopover = uicontrol('Style','checkbox','String','Stop ...
Over','Position',[63 440 70 30],'Callback',@stops);
quick = ...
uicontrol('Style','checkbox','String','Quick','Position',[3 ...
45
25
26
27
28
29
30
31
32
33
34
35
36
400 60 30]);
astar = ...
uicontrol('Style','checkbox','String','A*','Fontsize',10,'Position',[3 ...
370 60 30],'Callback',@heuristics);
TopOpt = ...
uicontrol('Style','checkbox','String','TopOpt','Fontsize',10,'Position',[3 ...
210 100 30]);
Subplots = ...
uicontrol('Style','checkbox','String','Subplot','Position',[3 ...
180 60 30]);
uicontrol('String','Reset','Position', [3 440 60 30], ...
'Callback',@reset map)
uicontrol('String','Load','Position', [3 500 60 30], ...
'Callback',@load map)
uicontrol('String','Load Mesh','Position', [63 500 70 30], ...
'Callback',@load map mesh)
uicontrol('String','Save','Position', [3 470 60 30], ...
'Callback',@save map)
editor = imagesc(Map);
caxis([0 4]); axis equal; axis off; drawnow;
set(editor,'ButtonDownFcn', @startDrawing);
set(fig,'WindowButtonUpFcn', @stopDrawing);
set(fig,'WindowButtonMotionFcn',@mouseMovement);
37
38
39
40
function displayits(,)
uicontrol('Style','text','Position',[63 400 60 ...
15],'String',get(its,'value'))
end
41
42
43
44
45
46
47
48
49
50
51
52
53
54
function heuristics(,,)
if get(astar,'value') == 1
uicontrol('Style','checkbox','String','Dijkstra','Position',[3 ...
340 100 30]);
uicontrol('Style','checkbox','String','Straight ...
Line','Position',[3 310 100 30]);
uicontrol('Style','checkbox','String','Manhattan','Position',[3 ...
280 100 30]);
uicontrol('Style','checkbox','String','Diagonal','Position',[3 ...
250 100 30]);
else
delete(findobj('Style','checkbox','String','Dijkstra'))
delete(findobj('Style','checkbox','String','Straight ...
Line'))
delete(findobj('Style','checkbox','String','Manhattan'))
delete(findobj('Style','checkbox','String','Diagonal'))
end
end
55
56
57
58
59
60
function stops(,,)
if get(stopover,'value') == 1
uicontrol('String','Stop','Position', [63 90 60 ...
30], 'Callback',{@mat,2})
else
delete(findobj('String','Stop'))
46
end
61
62
end
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
for a = 1:1:round(get(its,'value'))
if subplots == 1
figure()
set(gcf, 'Position', get(0,'Screensize')); % ...
Maximize figure
i = 1;
end
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
if dijkstra == 1
if subplots == 1
subplot(2,round(number subplots/2),i)
i = i+1;
else
figure
end
heuristic = 0;
if get(stopover,'value') == 1
[tAstar] = ...
Astar stopover(Map,get(quick,'value'),heuristic);
else
[tAstar] = ...
Astar(Map,get(quick,'value'),heuristic);
end
end
97
98
99
100
101
102
103
104
105
if straight == 1
if subplots == 1
subplot(2,round(number subplots/2),i)
i = i+1;
else
figure
end
heuristic = 1;
47
if get(stopover,'value') == 1
[tAstar] = ...
Astar stopover(Map,get(quick,'value'),heuristic);
else
[tAstar] = ...
Astar(Map,get(quick,'value'),heuristic);
end
106
107
108
109
110
111
end
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
if Manhattan == 1
if subplots == 1
subplot(2,round(number subplots/2),i)
i = i+1;
else
figure
end
heuristic = 2;
if get(stopover,'value') == 1
[tAstar] = ...
Astar stopover(Map,get(quick,'value'),heuristic);
else
[tAstar] = ...
Astar(Map,get(quick,'value'),heuristic);
end
end
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
if diagonal == 1
if subplots == 1
subplot(2,round(number subplots/2),i)
i = i+1;
else
figure
end
heuristic = 3;
if get(stopover,'value') == 1
[tAstar, film] = ...
Astar stopover(Map,get(quick,'value'),heuristic);
else
[tAstar, film] = ...
Astar film(Map,get(quick,'value'),heuristic);
end
end
142
143
144
145
146
147
148
149
150
151
if topopt == 1
if subplots == 1
subplot(2,round(number subplots/2),i)
i = i+1;
else
figure
end
[obj vol rpath timeTOP] = ...
top88r adapt(m,n,10,1.1,get(quick,'value'),2/(m+n),Map);
end
152
48
if get(astar,'value') == 1
times Astar(a) = tAstar;
end
if topopt == 1
times TOP(a) = timeTOP;
end
153
154
155
156
157
158
end
159
160
if topopt == 1
averageTOM = sum(times TOP)/size(times TOP,2);
fprintf('\n Average Time taken TopOpt: %4.3f ...
\n',averageTOM);
end
161
162
163
164
165
if get(astar,'value') == 1
averageAstar = sum(times Astar)/size(times Astar,2);
fprintf('\n Average Time taken Astar: %4.3f ...
\n',averageAstar);
end
166
167
168
169
170
171
172
173
end
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
file=[pathname,filename];
if file==0,return; end
if (exist(file))
Map = dlmread(file);
mapdim=size(Map);
m=mapdim(2)
n=mapdim(1)
set(editor,'cdata',Map);
axis equal;
else
fprintf('%s does not exist.\n',filename)
49
end
205
206
end
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
file=[pathname,filename];
if file==0,return; end
if (exist(file))
oldmap = dlmread(file);
mapdim=factor*size(oldmap);
n=mapdim(2)
m=mapdim(1)
Map = zeros(m,n);
for i=1:(m/factor)
for j=1:(n/factor)
if(oldmap(i,j) == 1)
for k = 1:1:(factor)
for l = 1:1:(factor)
Map(factor*i(k1),factor*j(l1)) ...
= 1;
end
end
elseif (oldmap(i,j) == 3)
Map(factor*i(factor1),factor*j(factor1)) ...
= 3;
elseif(oldmap(i,j) == 4)
Map(factor*i(factor1),factor*j(factor1)) ...
= 4;
elseif(oldmap(i,j) == 2)
Map(factor*i(factor1),factor*(factor1)) ...
= 2;
end
end
end
239
set(editor,'cdata',Map);
axis equal;
240
241
242
else
fprintf('%s does not exist.\n',filename)
243
244
245
246
247
248
249
250
251
252
253
end
end
function mouseMovement(object,eventStruct)
pos1 = get(gca,'CurrentPoint');
% Determine the mouseposition
pos = [ pos1(1,1)0.5;
pos1(1,2)0.5];
if (mousedown)
x=pos(1);
y=pos(2);
50
254
255
256
257
258
259
260
261
end
262
263
end
264
265
266
267
268
269
270
function startDrawing(object,eventStruct)
mousedown = 1; rightbutton = 0;
if strcmp(get(fig,'SelectionType'),'alt'),rightbutton ...
= 1;
end
mouseMovement(object,eventStruct)
end
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This Matlab code was written by V.J.P. van den Goor, %
% And adapted by A.R.P. Andri n to include the A* algorithm,%
% stopovers, subplots, multiple iterations and load mesh%
% Department of Mechanical Engineering, %
% Eindhoven University of Technology %
% %
% The code is intended for educational purposes and acts as a ...
GUI for %
% the robot pathplanner program of which theoretical details ...
are %
% discussed in the paper %
% "Trajectorieplanning van een mobiele robot met behulp van %
% topologieoptimalisering", Reportnumber: MN420694 %
% %
% Disclaimer: %
% The authors reserves all rights but do not guaranty that the ...
code is %
% free from errors. Furthermore, we shall not be liable in any ...
event %
% caused by the use of the program. %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
51
Appendix E
(a) A*
(b) TOM
52
E.2
(a) A*
(b) TOM
E.3
(a) A*
(b) TOM
53
E.4
(a) A*
(b) TOM
E.5
(a) A*
(b) TOM
54