Efficient Path Replanning

1
Efficient Path Replanning

• Dave Ferguson
• Carnegie Mellon University
• April, 2005

2
Path Planning
Intro Planning Replanning
Delayed D Where to next?
3
Path Planning

• Compute sequence of actions that will transition
  agent from sstart to sgoal

sstart
sgoal
Intro Planning Replanning
Delayed D Where to next?
4
Path Planning

• Many Possible Approaches
• Randomized Algorithms (RRTs, PRMs)
• Deterministic Algorithms (Dijkstras, A)

sgoal
sstart
sstart
sgoal
Intro Planning Replanning
Delayed D Where to next?
5
Useful Concepts

• Cost of a state estimated cost of a path to goal
• Process a state
• assigning a cost to the state
• using this cost to recompute costs of neighbors
• Backwards Search process states from goal
  towards start

Intro Planning Replanning
Delayed D Where to next?
6
Deterministic Path Planning
Dynamic Programming
Dijkstras
sstart
sgoal
A
Intro Planning Replanning
Delayed D Where to next?
7
Dynamic Programming

• Repeat until convergence
• for all s ? S, g(s) mins' in Succ(s) (c(s,
  s') g(s'))

s1
g(s1)

c(s, s1)
s2
c(s, s2)
sgoal
s

g(s2)
c(s, s3)
s3

g(s3)
Optimal path generated from every state
Intro Planning Replanning
Delayed D Where to next?
8
Dijkstras Algorithm

• Process states in order of increasing g-values.

sgoal
sstart
Optimal path generated from every state processed
Intro Planning Replanning
Delayed D Where to next?
9
A

• Process states in order of increasing f-values
  f(s) g(s) h(s)

s
g(s)

sgoal
sstart
h(s)
Optimal path generated from every state processed
h(s) estimates the cost of a path from sstart to s
Intro Planning Replanning
Delayed D Where to next?
10
Robotic Path Planning

• Generate graph for planning
• Plan a path over the graph
• Traverse the path

and after two steps, everything changes
Intro Planning Replanning
Delayed D Where to next?
11
Dynamic Path Planning

• What do we do when the graph changes?

1. Replan from scratch
11. Repair previous solution
Intro Planning Replanning
Delayed D Where to next?
12
Repairing the Solution

• Changing the cost of one state can affect several
  others

How do we propagate new cost to all affected
states?
Intro Planning Replanning
Delayed D Where to next?
13
Repairing the Solution

• Dynamic Programming
• - Update Cost of all states
• - Repeat until convergence

sgoal
Intro Planning Replanning
Delayed D Where to next?
14
Repairing the Solution

• Unfocussed Dynamic A
• - Update Cost of all affected states
• - Process these states in order of increasing key
  value
• g(s) old cost of s
• rhs(s) new cost of s
• key(s) min(g(s), rhs(s))
• If g(s) rhs(s), state s is not affected

sgoal
Intro Planning Replanning
Delayed D Where to next?
15
Repairing the Solution

• Focussed Dynamic A (D/D Lite)
• - Only update states that could have a bearing on
  the path from sstart
• - key(s)
  min(g(s), rhs(s)) h(s)

sgoal
Intro Planning Replanning
Delayed D Where to next?
16
D Lite

• Algorithm
• key(s) min(g(s), rhs(s)) h(s)
• While mins ? OPEN (key(s)) lt key(sstart)
• s is element in queue with minimum key value
• if rhs(s) lt g(s)
• g(s) rhs(s)
• else
• g(s) ?
• for all s' ? pred(s) ? s, rhs(s') mins in
  Succ(s') (c(s', s) g(s))
• if rhs(s') ? g(s'), insert s' into OPEN queue
  with key(s')

Intro Planning Replanning
Delayed D Where to next?
17
D Lite in action

• Algorithm
• key(s) min(g(s), rhs(s)) h(s)
• While mins ? OPEN (key(s)) lt key(sstart)
• - s is element in queue with
• minimum key value
• - if rhs(s) lt g(s)
• g(s) rhs(s)
• - else
• g(s) ?
• - for all s' ? pred(s) ? s,
• rhs(s') mins in Succ(s') (c(s', s)
  g(s))
• - if rhs(s') ? g(s'), insert s' into OPEN
  queue with key(s')

states
path direction
Intro Planning Replanning
Delayed D Where to next?
18
D Lite in action

• Algorithm
• key(s) min(g(s), rhs(s)) h(s)
• While mins ? OPEN (key(s)) lt key(sstart)
• - s is element in queue with
• minimum key value
• - if rhs(s) lt g(s)
• g(s) rhs(s)
• - else
• g(s) ?
• - for all s' ? pred(s) ? s,
• rhs(s') mins in Succ(s') (c(s', s)
  g(s))
• - if rhs(s') ? g(s'), insert s' into OPEN
  queue with key(s')

states
path direction
Intro Planning Replanning
Delayed D Where to next?
19
D Lite in action

• Algorithm
• key(s) min(g(s), rhs(s)) h(s)
• While mins ? OPEN (key(s)) lt key(sstart)
• - s is element in queue with
• minimum key value
• - if rhs(s) lt g(s)
• g(s) rhs(s)
• - else
• g(s) ?
• - for all s' ? pred(s) ? s,
• rhs(s') mins in Succ(s') (c(s', s)
  g(s))
• - if rhs(s') ? g(s'), insert s' into OPEN
  queue with key(s')

states
path direction
Intro Planning Replanning
Delayed D Where to next?
20
D and D Lite Summary

• Able to repair solution paths
• Can be far more efficient than A from scratch
  (2 orders of magnitude)
• Used all over the place

Can we do better?
Intro Planning Replanning
Delayed D Where to next?
21
D and D Lite Summary

• Can we be even more restrictive about which
  states we process?

- When costs decrease?
- When costs increase?
Intro Planning Replanning
Delayed D Where to next?
22
D and D Lite Summary

• Can we be even more restrictive about which
  states we process?

- When costs decrease?
NO
A guaranteed to process minimum number of states
Intro Planning Replanning
Delayed D Where to next?
23
D and D Lite Summary

• Can we be even more restrictive about which
  states we process?

- When costs increase?
YES!
If state is not on solution path, it can be
ignored!
Intro Planning Replanning
Delayed D Where to next?
24
Delayed D

• D treats cost decreases and cost increases
  equally
• What if we dont?

unless they involve states ON the solution path
Intro Planning Replanning
Delayed D Where to next?
25
Delayed D

• When cost of a state s changes
• If rhs(s) lt g(s), put s on OPEN queue
• If rhs(s) gt g(s), ignore s
• Two-staged algorithm REPAIR and CHECK
• Repair solution given states on OPEN queue
• Check that solution is optimal
• Now, if any state s with rhs(s) gt g(s) is on the
  solution path, put it on the OPEN queue
• Otherwise, were done.

Intro Planning Replanning
Delayed D Where to next?
26
Delayed D

• Two-staged algorithm REPAIR and CHECK

• D Lite Algorithm REPAIR
• key(s) min(g(s), rhs(s)) h(s)
• While mins ? OPEN (key(s)) key(sstart)
• - s is element in queue with
• minimum key value
• - if rhs(s) lt g(s)
• g(s) rhs(s)
• - else
• g(s) ?
• - for all s' ? Pred(s) ? s,
• rhs(s') mins in Succ(s') (c(s', s)
  g(s))
• - if (rhs(s') ? g(s'))
• insert s' into OPEN queue with
  key(s')

• Delayed D Algorithm REPAIR
• key(s) min(g(s), rhs(s)) h(s)
• While mins ? OPEN (key(s)) key(sstart)
• - s is element in queue with
• minimum key value
• - if rhs(s) lt g(s)
• g(s) rhs(s) increase FALSE
• - else
• g(s) ? increase TRUE
• - for all s' ? Pred(s) ? s,
• rhs(s') mins in Succ(s') (c(s', s)
  g(s))
• - if (rhs(s') lt g(s')) OR
• (rhs(s') gt g(s') AND increase is TRUE)
• insert s' into OPEN queue with
  key(s')

Intro Planning Replanning
Delayed D Where to next?
27
Delayed D

• Two-staged algorithm REPAIR and CHECK

Delayed D Algorithm CHECK While there are
states s on solution path with (rhs(s) ? g(s))
- add all such states s to the OPEN queue with
key(s) - REPAIR solution
Intro Planning Replanning
Delayed D Where to next?
28
Delayed D in action
Intro Planning Replanning
Delayed D Where to next?
29
Delayed D in action
Intro Planning Replanning
Delayed D Where to next?
30
Delayed D in action
Intro Planning Replanning
Delayed D Where to next?
31
Delayed D Results

• Theorem
• The Delayed D algorithm always terminates, and
  when it does, an optimal solution path can be
  followed from sstart to sgoal by always moving
  from the current state s, starting at sstart, to
  a successor s? that minimizes c(s, s?) g(s?).

Intro Planning Replanning
Delayed D Where to next?
32
Delayed D Results

• Experiments
• 500 ? 500 random grid environments
• Varied obstacle density from 0 to 20 percent
• 50 environments for each density (1050 in total)
• 3 scenarios
• Navigation in unknown environment
• Navigation in partially-known environment
• Maintaining least-cost route from fixed start
  position to goal in a dynamic environment

Intro Planning Replanning
Delayed D Where to next?
33
Delayed D Results

• Navigation in Unknown Environments

Intro Planning Replanning
Delayed D Where to next?
34
Delayed D Results

• Navigation in Partially-known Environments

Intro Planning Replanning
Delayed D Where to next?
35
Delayed D Results

• Maintaining least-cost route in dynamic
  environments

Intro Planning Replanning
Delayed D Where to next?
36
Performance
Intro Planning Replanning
Delayed D Where to next?
37
Thanks!

• Acknowledgments
• This work was partially sponsored by the U.S.
  Army Research Laboratory, under contract
  Robotics Collaborative Technology Alliance''.
  The views contained in this document are those of
  the authors and do not represent the official
  policies or endorsements of the U.S. Government.
  Dave Ferguson is supported by a National Science
  Foundation Graduate Research Fellowship.
• References
• A. Stentz, The Focussed D Algorithm for
  Real-Time Replanning, in Proceedings of the
  International Joint Conference on Artificial
  Intelligence, 1995.
• S. Koenig and M. Likhachev, Improved Fast
  Replanning for Robot Navigation in Unknown
  Terrain, in Proceedings of the IEEE
  International Conference on Robotics and
  Automation, 2002.
• D. Ferguson and A. Stentz, The Delayed D
  Algorithm for Efficient Path Replanning, in
  Proceedings of the International Conference on
  Robotics and Automation, 2005.