AI Robotics
5. Path-Planning
A*, Robot Motion Planning, Multi-Robot
Planning
Alexander Kleiner
Dr. A. Kleiner
AI Robotics - Path-Planning
1
Contents
•
•
•
•
Search Problem Formulation
Best-First Search & A*
Robot Motion Planning
Multi-Robot Planning
Dr. A. Kleiner
AI Robotics - Path-Planning
2
Literature
Illustrations and content presented in this lecture where
taken from:
Artificial Intelligence – A Modern Approach, 2nd Edition
by Stuart Russell - Peter Norvig
Planning Algorithms
By Steven M. LaValle
Available for downloading at: http://planning.cs.uiuc.edu/
Dr. A. Kleiner
AI Robotics - Path-Planning
3
Problem-Solving by Path-planning
! Goal-based agents
Formulation: goal and problem
Given: initial state
Task: To reach the specified goal (a state)
through the execution of appropriate
actions.
! Search for a suitable action sequence and
execute the actions
Dr. A. Kleiner
AI Robotics - Path-Planning
4
Problem Formulation
• Goal formulation
World states with certain properties
• Definition of the state space
important: only the relevant aspects ! abstraction
• Definition of the actions that can change the world
state
• Determination of the search cost (search costs, offline
costs) and the execution costs (path costs, online
costs)
Note: The type of problem formulation can have a
big influence on the difficulty of finding a solution.
Dr. A. Kleiner
AI Robotics - Path-Planning
5
Problem Formulation for the Vacuum
Cleaner World
•
World state space:
2 positions, dirt or no dirt
! 8 world states
•
Successor function
(Actions):
Left (L), Right (R), or Suck (S)
•
Goal state:
no dirt in the rooms
•
Path costs:
one unit per action
Dr. A. Kleiner
AI Robotics - Path-Planning
6
The Vacuum Cleaner State Space
States for the search: The world states 1-8.
Dr. A. Kleiner
AI Robotics - Path-Planning
7
Best-First Search
Search procedures differ in the way they determine the
next node to expand.
Uninformed Search: Rigid procedure without
knowledge of the cost of a given node to the goal.
Informed Search: Knowledge of the cost of a given
node to the goal is in the form of an evaluation function
f(n), which assigns a real number to each node.
Best-First Search: Search procedure that expands the
node with the “best” f-value.
Dr. A. Kleiner
AI Robotics - Path-Planning
8
Best-First Search Implementation
• Nodes are stored in a Fringe and successively expanded
– Expanding a node means to add all its successor nodes to
the fringe
– Typically a Priority-Queue is used as data structure where
nodes are sorted after an evaluation function f(n)
• We start by adding the initial state and expand then
successively until the goal or all nodes have been expanded
• We maintain a closed list for keeping book of already visited
states
– Ignore newly expanded state if already in closed list
– Detects loops in the search space
– Closed list can be implemented as hash table
Dr. A. Kleiner
AI Robotics - Path-Planning
9
General Tree-Search Procedure
MakeNode
Dr. A. Kleiner
AI Robotics - Path-Planning
10
Explanations
Data structure for nodes in the search tree:
State: state in the state space
Node: Containing a state, pointer to predecessor, depth, and path cost, action
Depth: number of steps along the path from the initial state
Path Cost: Cost of the path from the initial state to the node
Fringe: Memory for storing expanded nodes. For example, stack or a queue
General functions to implement:
Make-Node(state): Creates a node from a state
Goal-Test(state): Returns true if state is a goal state
Successor-Fn(state): Implements the successor function, i.e. expands a set of
new nodes given all actions applicable in the state
Cost(state,action): Returns the cost for executing action in state
Insert(node, fringe): Inserts a new node into the fringe
Remove-First(fringe): Returns the first node from the fringe
Dr. A. Kleiner
AI Robotics - Path-Planning
11
Best-First-Search Algorithm
When h is always correct, we do not need to search!
Dr. A. Kleiner
AI Robotics - Path-Planning
12
Criteria for Search Algorithms
Completeness:
Is the strategy guaranteed to find a solution when there is
one?
Time Complexity:
How long does it take to find a solution?
Space Complexity:
How much memory does the search require?
Optimality:
Does the strategy find the best solution (with the lowest
path cost)?
Dr. A. Kleiner
AI Robotics - Path-Planning
13
A*: Minimization of the estimated
path costs
A* combines both path costs and state heuristic values
g(n) = actual cost from the initial state to n.
h(n) = estimated cost from n to the next goal.
f(n) = g(n) + h(n), the estimated cost of the cheapest
solution through n.
Let h*(n) be the true cost of the optimal path from n to
the next goal.
h is admissible if the following holds for all n :
h(n) ≤ h*(n)
We require that for optimality of A*, h is admissible
(straight-line distance is admissible).
Dr. A. Kleiner
AI Robotics - Path-Planning
14
A* Search Example
Dr. A. Kleiner
AI Robotics - Path-Planning
15
A* Search from Arad to Bucharest
Homework: Expand the tree by using f(n)=h(n).
Is the result optimal?
Dr. A. Kleiner
AI Robotics - Path-Planning
16
Heuristic Function Example
h1 =
h2 =
Dr. A. Kleiner
the number of tiles in the wrong position
the sum of the distances of the tiles from their goal
positions (Manhatten distance)
AI Robotics - Path-Planning
17
Empirical Evaluation
•
•
•
d = distance from goal
Average over 100 instances
IDS: Iterative Deepening Search (the best you can do without
any heuristic)
# nodes expanded
Dr. A. Kleiner
AI Robotics - Path-Planning
18
Robot Motion Planning
Introduction
A motion computed by a planning algorithm, for a digital actor to reach into a refrigerator
An application of
motion planning to the
sealing process in
automotive
manufacturing
A planning algorithm computes the motions of 100
digital actors moving across terrain with obstacles
Dr. A. Kleiner
AI Robotics - Path-Planning
19
Robot Motion Planning
Problem Formulation
The configuration space
is the space containing all possible
configurations of the robot
Suppose world
or
Obstacle region
Rigid robot
Robot configuration
Obstacle region
is defined by:
The free space is defined by:
Which is the set of all configurations q at which
A(q), the transformed robot, intersects
Dr. A. Kleiner
AI Robotics - Path-Planning
20
Robot Motion Planning
Problem / Solution Concepts
Problem: Find continuous path
With
•
and
Requirements
– Shortest path
– Minimal execution time (requiring a good fit with the motion model, least amount of
rotations, etc.)
– Maximal distance to obstacles (needed in dynamic environments, and when
sensors are unreliable)
•
Many solution concepts, generally we have
1.
2.
3.
4.
Potential Fields (more details in a later lecture)
Visibility Graphs
Grid-based Planning
Sampling-based Planning
Dr. A. Kleiner
AI Robotics - Path-Planning
21
Robot Motion Planning
Visibility Graphs
•
Approximation of obstacles as polygons (or
circles)
•
Visibility Graph S: Build visibility graph
S=(V,G), where V is the set of all vertices
from polygon obstacles or circle tangents
•
Planning with discrete methods (e.g. A*)
•
Advantage: Depends only on number of
obstacles only
•
Disadvantage: Paths very close to obstacles.
How to get good polygons?
Dr. A. Kleiner
AI Robotics - Path-Planning
22
Robot Motion Planning
Grid-based Planning
•
Planning on a subdivision of Cfree
into smaller cells
•
Simplification: grow borders of
obstacles up to the diameter of the
robot, e.g., by Gaussian Convolution
•
Construction of graph G=(V,E),
where V is the set of cells and E
represents their neighbor-relations
•
Planning with discrete methods (A*)
– Resulting path is a sequence of cells
•
Disadvantage: Memory usage grows
with the size of the environment
•
Advantage: No polygons!
Dr. A. Kleiner
AI Robotics - Path-Planning
23
Robot Motion Planning
Sampling-based Motion Planning
• Basic Idea: To avoid explicit
construction of Cobs
• Instead: probe Cfree with a sampling
scheme
• Builds a graph G=(V,E) by
connecting sampled locations
– each e∊E has to be collision free!
Sampling without obstacles
– on G a solution can be found by
discrete search methods (e.g. A*)
• Critical part: Random Sampling
• Time consuming part: Collision
Checks
Sampling with obstacles
Dr. A. Kleiner
AI Robotics - Path-Planning
24
Multi Robot Planning
Problem
• So far, we considered problems with single agents in
static environments
• When multiple robots plan and navigate at the same
time, robot-robot collisions might occur
• Obvious solution: To use a central planner that plans
trajectories for all robots simultaneously
Dr. A. Kleiner
AI Robotics - Path-Planning
25
Multi Robot Planning
Problem Formulation
1. World
and obstacle region
2. There are m robots:
3. Each robot Ai has both initial and goal configuration
,
4. The state space considers configuration of all robots simultaneously:
C is he configuration
space
A state
specifies a combination of all robot configurations
and my be expressed as:
The dimension of X is N, which is:
Dr. A. Kleiner
AI Robotics - Path-Planning
26
Multi Robot Planning
Problem Formulation
Obstacle region 1: robot-obstacle (walls, etc.);
Obstacle region 2: robot-robot:
Entire obstacle region:
Initial state:
with
Goal state:
with
Compute
Dr. A. Kleiner
such that
AI Robotics - Path-Planning
,
27
Multi Robot Planning
Complexity
• X can be considered as an ordinary C space
(and all the methods we learned can be
applied)
• However, the dimension of X grows linearly with
the number of robots!
• Complete planning algorithms require time that
is at least exponential in the dimension of X!
Dr. A. Kleiner
AI Robotics - Path-Planning
28
Decoupled planning
• First, search motion plans for each single robot
independently while ignoring plans of other robots
• Second, solve occurring conflicts by different
strategies, e.g., stop, drive around others, driver
slower, ...
• Several schemes
– Velocity turning
– Robot prioritization
• Performance
– Faster because C-space has fewer dimensions
• Completeness
– Generally incomplete
Dr. A. Kleiner
AI Robotics - Path-Planning
29
Prioritized Planning
• Straightforward approach that sorts robots by priority and
plans for higher priority robots first
• Possible priority: Shortest Paths first
• Lower priority robots plan by viewing the higher priority
robots as obstacles (but in time-space!)
– Plan path of the first robot in its Cfree-Space
– Plan trajectory of ith robot assuming that robots
1,…,i-1 are moving obstacles
• Incompleteness: Planning can fail to find a valid multi-robot
path although there exists one!Start R1
1
1
If A1 ignores the plan of A2, then
completeness is lost when using the
prioritized planning approach.
Dr. A. Kleiner
c
Start R2
3
4
2
1
G
Planning in time-space Only when c=2 conflict
AI Robotics - Path-Planning
30
Velocity Tuning
•
Decouples the problem into a single path planning part and a
multi motion timing part
•
First phase:
[
]
– Plan a path for each single robot π 0,1 → C free
– Design a timing function δ :T → [0,1] that indicates for time t the
location of the robot along path
π
– Design a function φ (t ) = π (δ (t )) that returns the configuration in Cfree
at each time t
•
Second phase:
€
€
– Follow each
€ robot’s plan after an arbitrary sequence and check
according to φ (t ) for collisions with robots having higher priorities
€
– Avoid collision by reducing the velocities
•
The method is complete when each robot has a garage
€
configuration
that no other robot can take on
Dr. A. Kleiner
AI Robotics - Path-Planning
31
Optimizing Priority Schemes
•
Idea: to interleave the
search for collision-free
paths with the search for a
solvable priority scheme
•
Randomized hill-climbing
search
•
Randomly flips priorities of
two robots (max_flips)
•
Performs random restarts to
avoid local minima
(max_tries)
•
Any-time algorithm!
•
Can also be implemented as
a Genetic Algorithm
Dr. A. Kleiner
for tries := 1 to MAX_TRIES do
select random order Π
if (tries = 1) then
Π*:=Π
for flips := 1 to MAX_FLIPS do
chose random i,j with i<j
Π’:= swap(i,j,Π)
If cost(Π’) < cost(Π)then
Π:=Π’
end for;
If cost(Π) < cost(Π*)then
Π*:=Π
end for;
return Π*
M. Benewitz et al.2001
AI Robotics - Path-Planning
32
Summary
• Sampling-based Planning methods are well
suited for Robot Motion Planning
• To find complete solutions in multi-Robot
Planning is generally intractable
• However, for many problem domains
appropriate solutions can be found by
decoupled techniques
Dr. A. Kleiner
AI Robotics - Path-Planning
33