Robot Autonomy (16-662, S13)
Lecture#08 (Monday February 11)
Discrete search algorithms
Lecturer: Siddhartha Srinivasa
I.
Scribes: Kavya Suresh & David Matten
INTRODUCTION
These notes contain a detailed explanation of the algorithms involved in discrete search. In the following
sections we describe, in detail, the piano movers problem, discrete feasible planning and the different
forward searching methods.
1. Piano movers problem
2. Some basic terms
3. Discrete feasible planning
a. Introduction of discrete feasible planning
b. Discrete feasible planning algorithm model
4. Forward searching methods
a. Breadth First
b. Depth First
c. Dijkstra’s algorithm,
d. A-Star
e. Best First
f. Iterative deepening
II.
PIANO MOVERS PROBLEM
The piano movers problem is known as the classic path planning problem. Given an open subset in an ndimensional space and two subsets, C0 and C1, of U, is it possible to move from C0 to C1 while remaining
entirely inside U? (1) i.e. if you are given a rigid body and its obstacles, the goal is to find a collision free
path from the initial configuration to the final configuration. The obstacles here are not moving and the
planning is done offline i.e. the planning is done before implementation of the program. (2) In simple
words, given a set of obstacles and the initial and final position of the robot, the goal is to find a path that
moves the robot from the initial to the final position and simultaneously avoids obstacles. (1)
No point in the robot should collide or come in contact with the obstacle. Doing so will cause the motion
to fail. This demands that every point on the robot must be represented. This is called the configuration of
the robot. The C space has the same dimensions as the degrees of freedom (DOF). In the case of the
piano, the x,y,z position and roll, pitch and yaw represent the 6 DOFs. (2)
INTRODUCTION TO DISCRETE SEARCH
The piano movers problem involves planning a path from an initial position, qi, to a final position, qf. In
the continuous domain this problem is classified as PSPACE HARD. In order to simplify the problem, we
shift C-space into the discrete spectrum. This is done by overlaying a discrete grid over the C-space and
Figure 1: Converting from continuous C-space to discrete C-space, any cell that contains Cobstacle is made into an obstacle cell.
marking all discrete locations that overlap Cobstacle as containing an obstacle. An example of this can be
seen in
.
This transition creates a finite and countable number of states, x, in the original state space X. There are
actions, u, which can be performed on x, and can be represented as U(x). For a 2D discrete problem, some
simple examples of u would be the commands move up, move right, move left and move down. For any
given state x, there is a state transition function which converts x into a subsequent state, x’, given an
action and a current state. An example of a state transition function, f(x,u), where x are the coordinates (2,
1) and u is the command to move up such that U(x) is (0, 1), would be x’ = (2, 1) + (0, 1) = (2, 2).
BASIC TERMINOLOGY
1.
Work space: The workspace is defined as the space which contains obstacles
where the robot is said to move.
2.
Configuration space: The vector space that defined the system configuration is
called as the configuration space or the C space. This is the space that contains the
robot and its surroundings. (3)
The C space contains the robot’s current position, the forbidden space which is the
region of collision of the robot with the obstacles and the free space which is the region
as which the robot is at rest. (1)
III.
DISCRETE FEASIBLE PLANNING
a. Introduction to discrete feasible planning (3)
Each and every distinct situation in the world can be referred to as a state represented as x. The set of
all states is called a state space and is represented as X. This set, X, must be finite and must not
contain unwanted information as this can cause problems in planning. The planner chooses a set of
actions represented as u, and these actions, when applied on a set of current states, produce a new
state. A state transition equation is represented by f given by x’ = f(x,u), as mentioned above. An
action space is the set of all actions that can be applied, starting from x, to produce x’. The set of all
actions applied on all states is called U. U =
UxϵXU(x). The discrete planning algorithm is defined as
the set of actions which will cause the initial state xI to reach the final state xG.
b. Discrete feasible planning algorithm model (3)
1. A nonempty state space X, which is a finite set of states.
2. For each state x ϵ X, a finite action space U(x).
3. A state transition function f that produces a state f(x, u) ϵ X for every x ϵ X and u ϵ U(x). The state
transition equation is derived from f as x’ = f(x, u).
4. An initial state xI ϵ X.
5. A goal set XG C X.
IV.
FORWARD SEARCHING METHOD
Given a starting state, xi, and an ending state, xg, the goal is to find a feasible path from xi to xg. One
method of solving this problem is to perform a forward (or backward) search. A forward search starts
from an initial position and probes until it enters the goal state. A simple algorithm that performs forward
search can be seen in Figure 2. In this algorithm there is a priority queue (Q) which is a list of states (x)
sorted based on a priority function. Q has two sub-functions, Q.insert(x) and Q.getfirst().Q.insert adds a
state to the queue. For example in an assembly when people are standing in ascending order of height,
when a tall person comes he/she is added to the last spot in the line. Q.getfirst() returns the first element
from the queue, sticking to the same example, the output returned would be the shortest person (this
depends on the queue).
Q.insert(xi) and mark xi as visited
while Q is not empty do:
x = Q.getfirst()
if x ϵ xg:
return success
else for all u:
x’ = f(x, u)
if x’ not visited:
mark x’ as visited
Q.insert(x’)
return failure
Figure 2: The basic algorithm for a forward search.
The algorithm depicted above runs as follows: While the queue is not empty, pop the topmost element
from it. If x is within the goal then declare success. If x is not within the goal, for all the actions that are
feasible for the element just popped out of the queue, compute x’ as f(x,u). This algorithm keeps track of
all the states that have already been visited, marks unvisited states as visited, and inserts them into the Q.
Despite completing all of the above steps, if the goal is not reached return failure. The forward search
algorithm constantly pops elements out of the queue, checks if it is in the goal, and, if it isn’t it computes
who the children are (by looking at actions that can be performed), and adds them into the Q.
Manipulating the priority function provides different ways to solve the problem. By modifying the queue
type the search is performed in different ways. Two basic queue forms which are used are the FIFO (First
In First Out) and the LIFO (Last In First Out) structures. While these methods both produce results, the
results aren’t guaranteed to be optimal. Optimal planning algorithms exist which find the optimal path
from qi to qg.
BASIC TERMINOLOGY
1. Unvisited: The states that have not been visited even once are referred to as unvisited states.
2. Next: If x is the current state, x’ is the next state for which x’ = f(u,x) where u is an action
that exists for this transformation.
3. Dead: A dead state is a state, along with the states following it, that have been visited and no
suitable path could be found to reach the goal.
4. Alive: Alive states are states that have been visited but the next states have not been visited,
leaving a possibility of finding a feasible plan. Alive states are always stored in a queue. The
order with which the items of the queue are called out or accessed is determined
algorithmically. (3)
BREADTH FIRST
Explanation
Breadth first search utilizes a FIFO type queue. In a FIFO queue the first element that is added is
the first element that is removed. FIFO produces an algorithm called breadth first. This queue
structure is similar to a service line at a grocery store or cafeteria, where the first person in line is
the first person to be served. This algorithm grows sub-trees in all directions and stops when it
reaches the goal state. This algorithm can be slow in reaching the goal, but the path will generally
be fairly short and branches will be fairly distributed in all directions. This method will generally
look something like a bull’s-eye, as seen in Figure 3Figure 5.
Figure 3: General shape of a breadth first search. The search is done approximately equally in all
directions.
This approach asserts that the first solution that gets XI to XG is the shortest solution that makes
use of least number of steps. This does not keep account of states that have been visited
previously. This is said to be a systematic approach. (3)
Example
An example of the algorithm, seen in Figure 2, running a breadth search first search, using a FIFO
queue is seen in Figure 4. For this example, the order in which U(x) is performed is move down,
move left, move up, move right
11
8
7
12
16
20
25
4
19
24
28
3
qi
21
qg
6
2
17
22
26
9
5
15
18
23
10
14
13
29
27
Figure 4: Breadth First Search Using a FIFO queue.
1. qi in queue. Run algorithm on qi.
add 2 then 3 and then 4 to the queue.
Q: {4,3,2}
2. Take the first item off of the queue (2)
add 5 then 6 to the queue.
Q: {6,5,4,3}
3. Take the first item off of the queue (3)
add 7 to the queue.
Q: {7,6,5,4}
4. Take the first item off of the queue (4)
add 8 to the queue.
Q: {8,7,6,5}
5. Take the first item off of the queue (5)
add 9 then 10 to the queue.
Q: {10,9,8,7,6}
6. Take the first item off of the queue (6)
Q: {10,9,8,7}
8.
9.
Take the first item off of the queue (8)
add 12 to the queue.
Q: {12,11,10,9}
Take the first item off of the queue (9)
Q: {12,11,10}
10. Take the first item off of the queue (10)
add 13 to the queue.
Q: {13,12,11}
11. Take the first item off of the queue (11)
12. Q: {13,12} Take the first item off of the
queue (9)
13. This process is continued until the goal is
removed from the queue. This occurs after
step 25, where 29 is added to the queue.
7. Take the first item off of the queue (7)
add 11 to the queue.
Q: {11,10,9,8}
After step 29 the items in the queue are:
Q:{29,28,27,26,qg}
At this point, qg is removed and the algorithm returns success.
The way to compute the path is to keep track of the states’ parents, go from child to parent, and
flip it to get the path. Each child has one parent but each parent can have multiple children
It is important to note that the path found happens to be the optimal path, even though almost the
entire space needed to be explored. Although most of the space was explored, not all of it needed
to be. The path traversed slowly, but was able to provide a solution. In order to return the path,
the parent of each cell (which cell was used to get to which cell) was stored.
DEPTH FIRST
Explanation
Depth first search utilizes a LIFO type queue. In a LIFO queue the last element that is added is
the first element that is removed. This queue structure is similar to a stack of dishes, where the
last dish added to the stack is the first dish to be removed. This algorithm grows a sub-tree in one
directions and branches out until it reaches a dead end. At this point the sub tree back tracks and
continues a new sub-tree from the point closest to the end. Like the breadth first search, the depth
first search stops when it reaches the goal state. This algorithm can be fast in reaching the goal,
but the path will generally be fairly long. This method will generally look something like a cone,
as seen in Figure 5.
Figure 5: General shape of a breadth first search. The search is done almost exclusively in a
single direction.
In the depth first approach, the Q functions as a stack. The last state to enter Q is the first one to
leave. Here too, the revisited states are not accounted for. The algorithm proceeds in a “for all”
loop and hence there is no control on the path taken. This approach is desirable when the count of
X is finite. (3)
Example
An example of the algorithm, seen in Figure 2, running a depth search first search, using a LIFO
queue is seen below. For this example, seen in Figure 6Figure 4, the order in which U(x) is
performed is move up, move right, move down, and move right.
7
6
5
4
3
qi
2
8
9
10
12
14
16
11
13
15
17
qg
20
18
21
19
Figure 6: Depth First Search using a LIFO queue.
It is important to note that the path found was found in half the steps of the breadth first search,
even though the path found was far from the optimal path. Although around half of the space was
explored, this search method could easily end up exploring too far in one direction and being
incredibly slow. Had the space in Figure 6 extended infinitely to the right, the path would have
never encountered the goal. In the case of the breadth first search, even if the space was infinite,
the path returned would still be the same as that seen in Figure 4. The path traversed quickly, but
was unable to provide a near optimal solution. As in breadth first searching, the parent of each
cell (which cell was used to get to which cell) was stored to allow the recovery of the final path.
1.
2.
3.
4.
5.
6.
7.
qi in queue. Run algorithm on qi.
add 2 then 3 and then 4 to the queue.
Q: {4,3,2}
Take the last item off of the queue (4)
add 5 then 6 to the queue.
Q: {6,5,3,2}
Take the last item off of the queue (6)
add 7 then 8 to the queue.
Q: {8,7,5,3,2}
8.
Take the last item off of the queue (14)
add 15 then 16 to the queue.
Q: {16,15,13,11,7,5,3,2}
9. Take the last item off of the queue (16)
Add 17 to the queue
Q: {17,15,13,11,7,5,3,2}
10. Take the last item off of the queue (17)
add 18 to the queue.
Q: {18,15,13,11,7,5,3,2}
Take the last item off of the queue (8)
add 9 to the queue.
Q: {9,7,5,3,2}
Take the last item off of the queue (9)
add 10 to the queue.
Q: {10,7,5,3,2}
Take the last item off of the queue (10)
Add 11 then 12 to the queue
Q: {12,11,7,5,3,2}
Take the last item off of the queue (12)
add 13 then 14 to the queue.
Q: {14,13,11,7,5,3,2}
11.
Take the last item off of the queue (18)
add 19 then 20 to the queue.
Q: {20,19,15,13,11,7,5,3,2}
12. Take the last item off of the queue (20)
add 21 then qg to the queue.
Q: {qg,21,19,15,13,11,7,5,3,2}
13. Take the first item off of the queue (qg)
return success.
OPTIMAL PLANNING ALGORITHMS
Although Breadth and Depth First Search methods both find a path to the object, Breadth First Search
is slow and Depth First search doesn’t find an efficient path. By changing the queue to select an
element based on a certain criteria, the problem can be solved faster, with an optimal result. Now, a
cost, C, is created for all values of x and u. The cost defined here could be an action cost or the
amount of energy that is needed to go from x to x’. Cost can also be determined by the distance to the
nearest obstacle. Consider for example, a robot that has a noisy actuator. The goal would be to make
sure it doesn’t bump into obstacles. For every configuration, the distance to the nearest obstacle could
be the cost-to-go. Finally, given a cost, the goal is to find a path such that the sum of the cost along a
path, C(x), is minimized. This cost is created using a function, L(x, u), which takes in any x and
produces the cost to perform an action U(x). Common L(x, u) functions are based on the distance to
obstacles and the energy cost to make the given move. The goal of an optimal planning algorithm is to
find a path such that C(x) for the entire path is minimized.
DIJKSTRA’S ALGORITHM
For Dijkstra’s Algorithm, the queue is sorted based on the estimated cost-to-come from xi to x, and is
given by C(x). The goal of the algorithm is to find C*(xg), or the minimal cost to get to the goal.
This algorithm tries to find the optimum and feasible path. This is useful for finding the shortest path
for a single source. Each and every edge (e ϵ E) can be thought of as a separate planning problem.
Each edge has a cost, L(e), associated with it when an action is applied. The cost of applying an
action, u, from state x is represented as L(x,u). The total cost is computed as the sum of costs for all
edges starting from XI and going to XG. There are two types of costs, namely C(x) i.e. non optimal
cost, and optimal cost C*(x). Q is sorted according to the cost-to-come function. For each and every
state, C*(x) is defined by the optimal cost-to-come function. C*(x) = the sum of all edge cost values
divided by the sum of path from xI to x. (3)
The algorithm starts off with no estimate of the cost-to-come value (i.e. Initially C*(x) = 0), it then
incrementally builds the cost-to-come function until it converges to the optimal cost. This uses the
forward search algorithm and makes one modification to it. When a node x’ is taken, it is created by
f(x, u). If x’ is not visited, then it is put into Q and the Q is sorted based on the cost-to-come. The cost
–to-come function is updated as the current cost-to-come, plus the cost that it takes to go from x to x’
(i.e. C(x’) = C*(x) + L(e)). C(x’) is known as the best cost-to-come. Updating the cost function takes
place. (3) If a node is found that has already been visited, the cost is recalculated and compared to the
originally listed cost. The minimum of these two compared costs is the new C(x’). In this algorithm
the most computationally complicated step is the calculation of the C(x). A theorem exists pertaining
to this algorithm that states that a state is only popped when its optimal cost is computed. This means
that when the goal is popped that it is at its optimal cost. In other words, when a state becomes a dead
state, the cost function associated with it is called the optimal cost-to-come.
HEURISTIC PATH
A heuristic path can be defined when the final destination is known. Dijkstra’s algorithm can be
modified when the final destination is known. In the example below (Figure 7), the robot is required
to move from XI to XG and the blocks are the obstacles on the way. A function G is defined such that
G: x  R+. G must underestimate the true cost-to-go. The path traversed by the robot when all the
obstacles are removed is called the heuristic path. The associated cost is called the heuristic cost-togo.
XI
XG
Figure 7: Example of a heuristic path
A* ALGORITHM
The A* algorithm computes the total cost that is used to get to the goal state and reduces the number
of explored states. There are two main cost functions that are to be defined here: the cost-to-come,
which is the cost C(x) from XI to x, and G(x), the cost-to-go from x to XG. The optimal cost-to-come,
C*, and optimal cost-to-go, G*, differ in that C* can be estimated by incrementing it periodically but
G* cannot be estimated. The goal is to compute an estimate (Ĝ*(x)) that is close enough and less than
or equal to the original cost-to-go estimate. To accomplish this a heuristic path is chosen which is a
straight path connecting the start to the end. This line is usually one which contains obstacles hence
the cost-to-go function will be higher than this estimate. The contents of Q are sorted using the sum of
C*(x’) + Ĝ*(x’). If Ĝ*(x’) is 0 the Dijkstra’s algorithm will be implemented.
In A*, Q is sorted based on the heuristic cost-to-go C(x) + G(x). It can be written as
C(x) + ϵG(x).
When ϵ = 0: Dijkstras algorithm i.e. C(x)
When ϵ=∞: Breadth first algorithm i.e. G(x)
When ϵ=1: A* algorithm i.e. C(x) + G(x)
When ϵ=1.5: weighted A* i.e. C(x) + 1.5*G(x) This is no longer a heuristic pathway
One disadvantage of the A* algorithm in comparison to best first algorithm is that it is slower.
BEST FIRST SEARCH
This is similar to A* wherein it is required to compute an estimate of the optimal cost-to-go function.
In some cases however, this estimate exceeds the optimal cost-to-go function. The disadvantage of
this system is that sometimes optimal solutions are not found. The advantage is that the execution and
running times are much faster since fewer vertices are visited. This is a non-systematic search.
ITERATIVE DEEPENING
The iterative deepening method is used when the number of vertices in the next level is far more than
the number in the current level. It does a breadth first search in a depth first way. It shoots out rays
and stops till it reaches a particular depth. It is a cautious and efficient algorithm. This uses the depth
first search to find all the states that are at a certain distance, let’s say n (or less), from the initial state.
If the goal is not found it starts over and searches for n+1 (or less). It starts from n=1 and proceeds
until the goal is reached. This algorithm doesn’t have the space constraints that the breadth first
algorithm has.
V.
REFERENCES
1. Lu Yang, Zhenbing Zeng, Symbolic Solution of a Piano Movers’ Problem with Four
Parameters, In proceeding of: Automated Deduction in Geometry, 5th International
Workshop ADG, Gainesville, 2004.
2. Piotr Indyk, Lecture 11: Motion Planning by,
http://www.cs.arizona.edu/classes/cs437/fall12/lec11.prn.pdf , March 2005
3. Steven M. LaValle, (2006), Planning Algorithms, Chicago: Cambridge University
Press