Anytime search in dynamic graphs Max Likhachev, Dave Ferguson, Geoff - - PowerPoint PPT Presentation

Anytime search in dynamic graphs

Max Likhachev, Dave Ferguson, Geoff Gordon, Anthony Stentz, Sebastian Thrun (2008)

Presented by: Eric Andrews Thu 3, October 2013

Outline of presentation

1

Motivation, anytime search, and incremental search

2

Generalization of A*

3

Anytime search algorithm: ARA*

4

Incremental search algorithm: LPA*

5

Anytime and incremental: Anytime D*

6

Example domains and results

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 2 / 39

Realistic planning problems

Agents planning and operating in real world. Two problems:

◮ Limited time available for planning =

⇒ producing optimal plans often infeasible.

◮ Model of world imperfect a priori, or world changes while executing

plan.

◮ A* alone not good enough!

E.g. Autonomous car in traffic trying to get from A to B.

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 3 / 39

Anytime planners: a solution to time limitations

Quickly find an initial, highly suboptimal plan. With left over time, try to (iteratively) improve plan. Two desirable traits:

◮ Ability to reuse work of previous iterations. ◮ Controllable bound (ǫ) on the suboptimality of the produced plan.

One solution: Anytime Repairing A* (ARA*)

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 4 / 39

Incremental planners: a solution to dynamic graphs

While executing the plan, we may notice that our model (graph) of the world is no longer in sync with reality. Re-planning after each (minor) change is expensive! Solution: try to reuse as much of previous search efforts as possible. One approach: Lifelong Planning A* (LPA*)

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 5 / 39

Both have downsides...

Anytime planners:

◮ Discrepancies in world model =

⇒ new plan from scratch.

◮ Interleaving planning and execution problematic.

Incremental planners:

◮ Pre-configured to search for optimal or bounded suboptimal solution. ◮ Can’t improve solution if time left.

Novelty of paper: combine both planner types to get Anytime D* (ADA*).

◮ Controllable suboptimality bound. ◮ Allows interleaving. Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 6 / 39

Section 2 Generalization of A*

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 7 / 39

Algorithm 1 A* search ComputePath function.

1: Initialize:

g(sstart) = 0 and for rest of nodes g(s) = ∞. OPEN = {sstart}.

2: function ComputePath() 3:

while sgoal is not expanded do

4:

remove s with smallest f (s) from OPEN

5:

for each successor s′ of s do

6:

if g(s′) > g(s) + c(s, s′) then

7:

g(s′) = g(s) + c(s, s′)

8:

insert/update s′ in OPEN with f (s′) = g(s′) + h(s′).

9:

end if

10:

end for

11:

end while

12: end function

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 8 / 39

Naive anytime algorithm using Weighted A*

Use inflated heuristics, f (s) = g(s) + ǫ ∗ h(s), ǫ ≥ 1 = ⇒ Bound on returned suboptimal solution is ǫ! Series of ComputePath() calls, decrease ǫ in between each. Good: anytime capability, controllable ǫ-suboptimality bound. Bad: duplicate search effort, no reuse.

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 9 / 39

Generalizing A*: v-values

New bookkeeping variable along with f and g values. No effect on operation of one-time A*. Allows reuse of search results in subsequent A* runs.

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 10 / 39

Algorithm 2 A* search ComputePath function.

1: Initialize:

g(sstart) = 0 and for rest of nodes g(s) = ∞. v(s) = ∞ for all nodes s. OPEN = {sstart}.

2: function ComputePath() 3:

while sgoal is not expanded do

4:

remove s with smallest f (s) from OPEN

5:

v(s) = g(s)

6:

for each successor s′ of s do

7:

if g(s′) > g(s) + c(s, s′) then

8:

g(s′) = g(s) + c(s, s′)

9:

insert/update s′ in OPEN with f (s′) = g(s′) + h(s′).

10:

end if

11:

end for

12:

end while

13: end function

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 11 / 39

Inconsistent states

A state s is either

◮ inconsistent v(s) = g(s) ◮ consistent v(s) = g(s)

Turns out OPEN contains all inconsistent states. So A* expands only inconsistent states. Additionally we have two types of inconsistent states:

◮ Overconsistent v(s) > g(s) ◮ Underconsistent v(s) < g(s) (later on) Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 12 / 39

Invariant of A∗ with v-values

For every state s ∈ S: g(s) =

• if s = sstart

mins′∈pred(s)(v(s′) + c(s′, s))

• therwise

Inconsistency is propagated to the children of some state s.

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 13 / 39

Generalizing A*: priority function

Good old A* expands states in the order of increasing f -values. Let’s generalize this to any priority function key(s) (satisfying certain restrictions...) Goal: restrictions allow the search to gurantee suboptimality bounds even when heuristics are inadmissible! A* with consistent heuristics, and with consistent (constant) inflated heuristics: OK!

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 14 / 39

Restriction

For any two states s, s′ ∈ S s.t.

1 There exists a path from start to goal via s′ 2 s’ is consistent or overconsistent. 3 s is overconsistent 4 g(s′) > g(s) + ǫ ∗ c∗(s, s′)

Then it must hold that key(s′) > key(s).

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 15 / 39

Section 3 Anytime search algorithm: ARA*

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 16 / 39

Towards Anytime Repairing A* (ARA*)

To reuse previous search efforts: make sure that before each execution

• f ComputePath(), OPEN contains all the inconsistent states.

CLOSED set to prevent multiple expansions of same state in single iteration. We now also need INCONS: set of all inconsistent states that are in CLOSED. key(s) = g(s) + ǫ ∗ h(s)

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 17 / 39

Algorithm 3 ARA* search ComputePath function.

1: function ComputePath() 2:

while key(sgoal) > mins∈OPEN(key(s)) do

3:

remove s with smallest key(s) from OPEN

4:

v(s) = g(s); CLOSED = CLOSED ∪ {s}

5:

for each successor s′ of s do

6:

if s′ was never visited by ARA* before then

7:

v(s′) = g(s′) = ∞

8:

end if

9:

if g(s′) > g(s) + c(s, s′) then

10:

g(s′) = g(s) + c(s, s′)

11:

if s′ / ∈ CLOSED then

12:

insert/update s′ in OPEN with f (s′) = g(s′) + h(s′).

13:

else

14:

insert s′ into INCONS

15:

end if

16:

end if

17:

end for

18:

end while

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 18 / 39

Algorithm 4 ARA main function

1: function Main() 2:

g(sstart) = 0; v(sstart) = ∞

3:

g(sgoal) = ∞; v(sgoal) = ∞

4:

OPEN = CLOSED = INCONS = ∅

5:

insert sstart into OPEN with key(sstart).

6:

ComputePath()

7:

publish current ǫ-suboptimal solution.

8:

while ǫ > 1 do

9:

decrease ǫ

10:

Move states from INCONS to OPEN

11:

Update priorities of states in OPEN according to key

12:

CLOSED = ∅

13:

ComputePath();

14:

publish current ǫ-suboptimal solution.

15:

end while

16: end function

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 19 / 39

Theoretical properties

After ComputePath() exits: for any state s with key(s) ≤ mins′∈OPEN(key(s′)), it holds that g∗(s) ≤ g(s) ≤ ǫ ∗ g∗(s).

◮ Correctness of ARA*

ComptePath() expands a state at most once and only if its v-value is lowered during its expansion.

◮ Computational savings of ARA* Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 20 / 39

ARA* search example

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 21 / 39

Section 4 Incremental search algorithm: LPA*

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 22 / 39

Edge costs can change because of imperfect model or dynamic environment. Edge cost change: update g-value of the node on the incoming side. Decreasing edges?

◮ g only decreases, thus v(s) ≥ g(s) holds. ◮ No problem for ARA*.

Increasing edges?

◮ g increases, thus can happen that v(s) < g(s). ◮ Problem for ARA*: doesn’t support underconsistent states! Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 23 / 39

What to do?

Quick fix: force any underconsistent state into consistent or

• verconsistent by setting v(s) = ∞, update children

g(s′) = mins′′∈pred(s′)v(s′′) + c(s′′, s′), and recursively force any underconsistent children and so on... Lifelong Planning A* (LPA) applies this idea, but in a more computationally cheaper way, by only fixing states that need to be fixed (are encountered during search).

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 24 / 39

LPA*

Can handle inflated heuristics OPEN consists of both under and overconsistent states. CLOSED = states that were overconsistent when expanded. Additional key restriction: For a consistent or overconsistent state s that can belong to some path from sstart to sgoal, we require that all underconsistent states s′ that could be on a path from sstart to s are expanded before s. In other words key(s) > key(s′).

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 25 / 39

Algorithm 5 LPA* ComputePath() function

1: function UpdateSetMembership(s) 2: if v(s) = g(s) then 3: If (s / ∈ CLOSED) insert / update s in OPEN with key(s) 4: else 5: If (s ∈ OPEN) remove s from OPEN 6: end if 7: end function 8: function ComputePath() 9: while key(sgoal) > mins∈OPEN(key(s)) OR v(sgoal) < g(sgoal) do 10: remove s with smallest key(s) from OPEN 11: if v(s) > g(s) (overconsistent) then 12: v(s) = g(s); CLOSED = CLOSED ∪ {s} 13: for each successor s′ of s do 14: if g(s′) > g(s) + c(s, s′) then 15: g(s′) = g(s) + c(s, s′); UpdateSetMembership(s′) 16: end if 17: end for 18: else (underconsistent) 19: v(s) = ∞; UpdateSetMembership(s) 20: for each successor s′ of s (except sstart) do 21: g(s′) = mins′′∈pred(s′)v(s′′) + c(s′′, s′) 22: UpdateSetMembership(s′) 23: end for 24: end if 25: end while 26: end function

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 26 / 39

Section 5 Anytime and incremental: Anytime D*

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 27 / 39

Anytime D* (AD*)

Solve complex planning problems with limited deliberation time (ARA*) in dynamic domains (LPA*). So it’s both an anytime and an incremental planner. Try to reuse previous search efforts for computational efficiency. Controllable ǫ-suboptimality bound.

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 28 / 39

How it works

It turns out we can use the same ComputePath() function as described in the context of LPA* with a few changes:

◮ INCONS list adapted from ARA* ◮ Backpointers to make last line of algorithm less expensive.

New key function (takes into account v-values)

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 29 / 39

Algorithm 6 Anytime D* main function

1: function Main() 2:

g(sstart) = 0; v(sstart) = ∞; bp(sstart) = null;

3:

g(sgoal) = ∞; v(sgoal) = ∞; bp(sgoal) = null;

4:

OPEN = CLOSED = INCONS = ∅; ǫ = ǫ0;

5:

insert sstart into OPEN with key(sstart).

6:

while forever do

7:

ComputePath()

8:

publish current ǫ-suboptimal solution.

9:

If (ǫ = 1) wait for changes in edge costs;

10:

for all directed edges (u, v) with changed edge costs do

11:

update edge cost c(u, v) to model;

12:

fix bp(v) and g(v) and UpdateSetMembership(v);

13:

end for

14:

if significant edge cost changes observed then

15:

increase ǫ or re-plan from scratch

16:

else if ǫ > 1 then

17:

decrease ǫ

18:

end if

19:

Move states from INCONS to OPEN

20:

Update priorities of states in OPEN according to key

21:

CLOSED = ∅

22:

end while

23: end function

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 30 / 39

Theoretical properties of AD*

When ComputePath exits: for any state s for which

◮ sgoal can be reached from s ◮ v(s) ≥ g(s) ◮ key(s) ≤ mins′∈OPEN(key(s′))

it holds that g∗(s) ≤ g(s) ≤ ǫ ∗ g∗(s). No state is expanded more than twice during ComputePath(): at most nce as underconsistent and at most once as overconsistent. A state is expanded by ComputePath() only if it is inconsistent initially

• r its v-value is altered during execution.

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 31 / 39

Section 6 Example domains and results

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 32 / 39

Robotic arm manipulating an end-effector

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 33 / 39

Anytime behavior of AD*

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 34 / 39

Outdoor mobile robot navigation

Robots often operate in open, large and poorly modeled environments. Optimal trajectories must take into account robot’s momentum, angle

• f robot, constraints on rotational velocity.

Solution: 4-d state space:

◮ xy-position ◮ orientation of the robot ◮ translational velocity

Two-level planner: 2d planner (x,y) performs A*, results used as heuristic for 4d planner. Interleaving planning and execution: search from goal to agent’s current position

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 35 / 39

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 36 / 39

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 37 / 39

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 38 / 39

Conclusions

Anytime D* is a theoretically-sound solid option for planning, or even interleaving planning and execution in dynamic environments with limited deliberation time available. In the above scenarios, it often outperform strategies that are only anytime or incremental. Anytime D* is very reliant on the heuristic used: large inflation factor should substantially speed-up the planning process. If you’re interested in planning (e.g. self-driving cars) check out the

• paper. Even though lots of stuff is covered, the pace is manageable,

concepts are explained and motivated thoroughly, and most arising questions are answered.

Presented by: Eric Andrews Anytime search in dynamic graphs Thu 3, October 2013 39 / 39