Autonomous Mobile Manipulation

Planning: Graph-based, Dijkstra, A*

C. Papachristos

Robotic Workers (RoboWork) Lab

University of Nevada, Reno

CS-791

Graph-based Planning

Planning with Graphs !

​

• Commonly, we convert a planning problem to a discrete graph-based representation

​

• Leverage powerful graph algorithms�(e.g. BFS/DFS/IDS search) to facilitate objectives such as planning, topological analysis, etc.

CS791 C. Papachristos

Graph-based Planning

CS791 C. Papachristos

Graph-based Planning

Graph Search

​

• Search Tree:
• A tree through which we can search for optimal paths through the Graph-based representation of our planning problem

CS791 C. Papachristos

Graph-based Planning

Search Trees

​

• Start state at the root node
• Children correspond to successors
• A node corresponds to a unique plan from the start state to that state (follow Search Tree upwards from node)

​

Notes:

• For most problems, we want to avoid constructing the whole Search Tree for the entire Graph !
• We also seek to use search algorithms to efficiently traverse Search Tree

CS791 C. Papachristos

Graph-based Planning

Forward Search

​

• Begin from the start state, start growing a Search Tree until some solution is found (path to goal)
• Expanding a node refers to adding children to the Search Tree, pushing them onto the Open set
• Try to expand as few tree nodes as possible

​

• “Open” set refers to a list of frontier (unexpanded) plans
• Keeps track of nodes to expand next
• Often implemented as a Queue / Priority Queue data structure
• For each node in the Open set, we know of at least one path to it from the start state
• “Closed” set refers to a list of nodes that have been expanded
• For each node in the Closed set, we’ve already found the lowest-cost path to it from the start state

CS791 C. Papachristos

Graph-based Planning

Forward Search – Seminal Algorithm Structure

​

• Steven M. LaValle, “Planning Algorithms”, Cambridge University press, 2006

CS791 C. Papachristos

Graph-based Planning

Breadth-First Search

​

• BFS Q is FIFO Queue (and R is e.g. an Unordered Set):
• push (Q.Insert) into the end
• pop (Q.GetFirst) from the front

​

Open (Q )

Visited ( R )

{ B }

{ B }

Q.Insert ( B ) R.Insert( B )

front … end

CS791 C. Papachristos

Graph-based Planning

Breadth-First Search

​

• BFS Q is FIFO Queue (and R is e.g. an Unordered Set):
• push (Q.Insert) into the end
• pop (Q.GetFirst) from the front

​

Open (Q )

Visited ( R )

{ A, C, G }

{ B, A, C, G }

Q.Pop ( B )

Q.Insert ( A ) R.Insert( A )

Q.Insert ( C ) R.Insert( C )

Q.Insert ( G ) R.Insert( G )

front … end

CS791 C. Papachristos

Graph-based Planning

Breadth-First Search

​

• BFS Q is FIFO Queue (and R is e.g. an Unordered Set):
• push (Q.Insert) into the end
• pop (Q.GetFirst) from the front

​

Open (Q )

Visited ( R )

{ C, G, D }

{ B, A, C, G, D }

Q.Pop ( A )

Q.Insert ( B ) R.Insert( B )

Q.Insert ( C ) R.Insert( C )

Q.Insert ( D ) R.Insert( D )

front … end

CS791 C. Papachristos

Graph-based Planning

Breadth-First Search

​

• BFS Q is FIFO Queue (and R is e.g. an Unordered Set):
• push (Q.Insert) into the end
• pop (Q.GetFirst) from the front

​

Open (Q )

Visited ( R )

{ G, D, I }

{ B, A, C, G, D, I}

Q.Pop ( C )

Q.Insert ( A ) R.Insert( A )

Q.Insert ( B ) R.Insert( B )

Q.Insert ( G ) R.Insert( G )

Q.Insert ( D ) R.Insert( D )

Q.Insert ( I ) R.Insert( I )

front … end

CS791 C. Papachristos

Graph-based Planning

Breadth-First Search

​

• BFS Q is FIFO Queue (and R is e.g. an Unordered Set):
• push (Q.Insert) into the end
• pop (Q.GetFirst) from the front

​

Open (Q )

Visited ( R )

{ D, I }

{ B, A, C, G, D, I}

Q.Pop ( G )

Q.Insert ( B ) R.Insert( B )

Q.Insert ( C ) R.Insert( C )

Q.Insert ( I ) R.Insert( I )

front … end

CS791 C. Papachristos

Graph-based Planning

Breadth-First Search

​

• BFS Q is FIFO Queue (and R is e.g. an Unordered Set):
• push (Q.Insert) into the end
• pop (Q.GetFirst) from the front

​

Open (Q )

Visited ( R )

{ I, J }

{ B, A, C, G, D, I, J}

Q.Pop ( D )

Q.Insert ( A ) R.Insert( A )

Q.Insert ( C ) R.Insert( C )

Q.Insert ( I ) R.Insert( I )

Q.Insert ( J ) R.Insert( J )

front … end

CS791 C. Papachristos

Graph-based Planning

Breadth-First Search

​

• BFS Q is FIFO Queue (and R is e.g. an Unordered Set):
• push (Q.Insert) into the end
• pop (Q.GetFirst) from the front

​

Open (Q )

Visited ( R )

{ J }

{ B, A, C, G, D, I, J}

Q.Pop ( I )

Q.Insert ( C ) R.Insert( C )

Q.Insert ( D ) R.Insert( D )

Q.Insert ( G ) R.Insert( G )

Q.Insert ( J ) R.Insert( J )

front … end

CS791 C. Papachristos

Graph-based Planning

Breadth-First Search

​

• BFS Q is FIFO Queue (and R is e.g. an Unordered Set):
• push (Q.Insert) into the end
• pop (Q.GetFirst) from the front

​

Open (Q )

Visited ( R )

{ }

{ B, A, C, G, D, I, J}

Q.Pop ( J )

front … end

CS791 C. Papachristos

Graph-based Planning

CS791 C. Papachristos

Graph-based Planning

Depth-First Search

​

• DFS Q is LIFO Stack (and R is e.g. an Unordered Set):
• push (Q.Insert) into the front (/top)
• pop (Q.GetFirst) from the front (/top)

​

Open (Q )

Visited ( R )

{ B }

{ B }

Q.Insert ( B ) R.Insert( B )

top

CS791 C. Papachristos

Graph-based Planning

Depth-First Search

​

• DFS Q is LIFO Stack (and R is e.g. an Unordered Set):
• push (Q.Insert) into the front (/top)
• pop (Q.GetFirst) from the front (/top)

​

Open (Q )

Visited ( R )

{ A, C, G }

{ B, A, C, G }

Q.Pop ( B )

Q.Insert ( G ) R.Insert( G )

Q.Insert ( C ) R.Insert( C )

Q.Insert ( A ) R.Insert( A )

top

CS791 C. Papachristos

Graph-based Planning

Depth-First Search

​

• DFS Q is LIFO Stack (and R is e.g. an Unordered Set):
• push (Q.Insert) into the front (/top)
• pop (Q.GetFirst) from the front (/top)

​

Open (Q )

Visited ( R )

{ D, C, G }

{ B, A, C, G, D }

Q.Pop ( A )

Q.Insert ( D ) R.Insert( D )

Q.Insert ( C ) R.Insert( C )

Q.Insert ( B ) R.Insert( B )

top

CS791 C. Papachristos

Graph-based Planning

Depth-First Search

​

• DFS Q is LIFO Stack (and R is e.g. an Unordered Set):
• push (Q.Insert) into the front (/top)
• pop (Q.GetFirst) from the front (/top)

​

Open (Q )

Visited ( R )

{ I, J, C, G }

{ B, A, C, G, D, I, J }

Q.Pop ( D )

Q.Insert ( J ) R.Insert( J )

Q.Insert ( I ) R.Insert( I )

Q.Insert ( C ) R.Insert( C )

Q.Insert ( A ) R.Insert( A )

top

CS791 C. Papachristos

Graph-based Planning

Depth-First Search

​

• DFS Q is LIFO Stack (and R is e.g. an Unordered Set):
• push (Q.Insert) into the front (/top)
• pop (Q.GetFirst) from the front (/top)

​

Open (Q )

Visited ( R )

{ J, C, G }

{ B, A, C, G, D, I, J }

Q.Pop ( I )

Q.Insert ( J ) R.Insert( J )

Q.Insert ( G ) R.Insert( G )

Q.Insert ( D ) R.Insert( D )

Q.Insert ( C ) R.Insert( C )

top

CS791 C. Papachristos

Graph-based Planning

Depth-First Search

​

• DFS Q is LIFO Stack (and R is e.g. an Unordered Set):
• push (Q.Insert) into the front (/top)
• pop (Q.GetFirst) from the front (/top)

​

Open (Q )

Visited ( R )

{ C, G }

{ B, A, C, G, D, I, J }

Q.Pop ( J )

top

CS791 C. Papachristos

Graph-based Planning

Search Trees

​

• BFS

• DFS

CS791 C. Papachristos

Graph-based Planning

Depth-First Search

​

• Lower memory footprint than BFS when applied on highly-branching Graphs

​

• Not often used for path search, sometimes used to completely explore a Graph

​

• Both BFS and DFS are simple to implement, but are in most cases inefficient
• More complex algorithms exist that are faster, but more difficult to implement

​

• DFS not complete for infinite trees
• May explore a branch (that doesn’t include the goal) infinitely deep
• BFS is complete

​

• Combined approach called Iterative Deepening Search (IDS):
• Search promising paths Depth-wise while we can afford to
• Go back up to expand Breadth-wise if they haven’t worked out so far
• Deepen the allowed Depth-wise expansion and repeat …

​

CS791 C. Papachristos

Graph-based Planning

Non-uniform edge weights

​

• BFS, DFS, IDS Graph search algorithms deal with Graph Search
• For a Graph with uniform edges, we may equivalently consider a BFS solution as an optimal plan�(in the number of edges sense)

​

But:

​

• Graph-based path-planning does not necessarily consider uniform-edge representation:
• Grid-based vs Random Sampling-based planning

​

• Realistic motion planning problems for dynamical systems have significant cost differentiations:
• Dynamics constraints, effort penalization, etc.

CS791 C. Papachristos

Graph-based Planning

Dijkstra’s Algorithm

​

• Published by Edsger Dijkstra in 1959

​

• Basic idea: Expanding in order of closest to start (BFS with edge costs)

​

• One of the most commonly used routing algorithms in Graph Traversal problems
• Asymptotically the fastest known single-source shortest path algorithm for arbitrary directed graphs

​

• Open set is a Priority Queue, ordered w.r.t. to currently known best cost to arrive

CS791 C. Papachristos

Graph-based Planning

Dijkstra’s Algorithm

​

• Dijkstra Q is Priority Queue (and R is e.g. an Unordered Map):
• push (Q.Insert) ordered by lowest Cost
• pop (Q.GetFirst) from the front and add to Closed�

​

Open (Q )

Closed (R )

{ B(0) }

{ B(0) }

Q.Insert ( B(0) ) R.Insert( B(0) )

​

CS791 C. Papachristos

Graph-based Planning

Dijkstra’s Algorithm

​

• Dijkstra Q is Priority Queue (and R is e.g. an Unordered Map):
• push (Q.Insert) ordered by lowest Cost
• pop (Q.GetFirst) from the front and add to Closed�

​

Open (Q )

Closed (R )

{ C(2) ,

G(3) ,

A(5) }

{ B(0) ,

C(2) ,

G(3) ,

A(5) }

Q.Pop ( B(0) )

Q.Insert ( C(2) ) R.Insert( C(2) )

Q.Insert ( G(3) ) R.Insert( G(3) )

Q.Insert ( A(5) ) R.Insert( A(5) )

CS791 C. Papachristos

Graph-based Planning

Dijkstra’s Algorithm

​

• Dijkstra Q is Priority Queue (and R is e.g. an Unordered Map):
• push (Q.Insert) ordered by lowest Cost
• pop (Q.GetFirst) from the front and add to Closed�

​

Open (Q )

Closed (R )

{ G(3) ,

A(4) ,

D(4) ,

A(5) ,

I(7) }

{ B(0) ,

C(2) ,

G(3) ,

A(5) ,

D(4) ,

I(7) }

Q.Pop ( C(2) )

Q.Insert ( A(4) ) R.Insert( A(4) )

Q.Insert ( D(4) ) R.Insert( D(4) )

Q.Insert ( G(4) ) R.Insert( G(4) )

Q.Insert ( I(7) ) R.Insert( G(7) )

A(4)

CS791 C. Papachristos

Graph-based Planning

Dijkstra’s Algorithm

​

• Dijkstra Q is Priority Queue (and R is e.g. an Unordered Map):
• push (Q.Insert) ordered by lowest Cost
• pop (Q.GetFirst) from the front and add to Closed�

​

Open (Q )

Closed (R )

{ G(3) ,

A(4) ,

D(4) ,

A(5) ,

I(7) }

{ B(0) ,

C(2) ,

G(3) ,

A(5) ,

D(4) ,

I(7) }

Q.Pop ( C(2) )

Q.Insert ( A(4) ) R.Insert( A(4) )

Q.Insert ( D(4) ) R.Insert( D(4) )

Q.Insert ( G(4) ) R.Insert( G(4) )

Q.Insert ( I(7) ) R.Insert( G(7) )

s

u

v

u.d

v.d

w(u, v)

A(4)

CS791 C. Papachristos

Graph-based Planning

Dijkstra’s Algorithm

​

• Dijkstra Q is Priority Queue (and R is e.g. an Unordered Map):
• push (Q.Insert) ordered by lowest Cost
• pop (Q.GetFirst) from the front and add to Closed�

​

Open (Q )

Closed (R )

{ G(3) ,

A(4) ,

D(4) ,

A(5) ,

I(7) }

{ B(0) ,

C(2) ,

G(3) ,

A(4) ,

D(4) ,

I(7) }

Q.Pop ( C(2) )

Q.Insert ( A(4) ) R.Insert( A(4) )

Q.Insert ( D(4) ) R.Insert( D(4) )

Q.Insert ( G(4) ) R.Insert( G(4) )

Q.Insert ( I(7) ) R.Insert( G(7) )

CS791 C. Papachristos

Graph-based Planning

Dijkstra’s Algorithm

​

• Dijkstra Q is Priority Queue (and R is e.g. an Unordered Map):
• push (Q.Insert) ordered by lowest Cost
• pop (Q.GetFirst) from the front and add to Closed�

​

Open (Q )

Closed (R )

{ G(3),

A(4),

D(4),

A(5),

I(7) }

{ B(0),

C(2),

G(3),

A(4),

D(4),

I(7) }

Q.Pop ( C(2) )

Q.Insert ( A(4) ) R.Insert( A(4) )

Q.Insert ( D(4) ) R.Insert( D(4) )

Q.Insert ( G(4) ) R.Insert( G(4) )

Q.Insert ( I(7) ) R.Insert( I(7) )

CS791 C. Papachristos

Graph-based Planning

Dijkstra’s Algorithm

​

• Dijkstra Q is Priority Queue (and R is e.g. an Unordered Map):
• push (Q.Insert) ordered by lowest Cost
• pop (Q.GetFirst) from the front and add to Closed�

​

Open (Q )

Closed (R )

{ G(3) ,

A(4) ,

D(4) ,

A(5) ,

I(7) }

{ B(0) ,

C(2) ,

G(3) ,

A(4) ,

D(4) ,

I(7) }

Q.Pop ( C(2) )

Q.Insert ( A(4) ) R.Insert( A(4) )

Q.Insert ( D(4) ) R.Insert( D(4) )

Q.Insert ( G(4) ) R.Insert( G(4) )

Q.Insert ( I(7) ) R.Insert( I(7) )

CS791 C. Papachristos

Graph-based Planning

Dijkstra’s Algorithm

​

• Dijkstra Q is Priority Queue (and R is e.g. an Unordered Map):
• push (Q.Insert) ordered by lowest Cost
• pop (Q.GetFirst) from the front and add to Closed�

​

Open (Q )

Closed (R )

{ G(3) ,

A(4) ,

D(4) ,

A(5) ,

I(7) }

{ B(0) ,

C(2) ,

G(3) ,

A(4) ,

D(4) ,

I(7) }

Q.Pop ( C(2) )

Q.Insert ( A(4) ) R.Insert( A(4) )

Q.Insert ( D(4) ) R.Insert( D(4) )

Q.Insert ( G(4) ) R.Insert( G(4) )

Q.Insert ( I(7) ) R.Insert( I(7) )

CS791 C. Papachristos

Graph-based Planning

Dijkstra’s Algorithm

​

• Dijkstra Q is Priority Queue (and R is e.g. an Unordered Map):
• push (Q.Insert) ordered by lowest Cost
• pop (Q.GetFirst) from the front and add to Closed�

​

Open (Q )

Closed (R )

{ A(4) ,

D(4) ,

A(5) ,

I(7) }

{ B(0) ,

C(2) ,

G(3) ,

A(4) ,

D(4) ,

I(7) }

Q.Pop ( G(3) )

Q.Insert ( B(6) ) R.Insert( B(6) )

Q.Insert ( C(5) ) R.Insert( C(5) )

Q.Insert ( I(8) ) R.Insert( I(8) )

CS791 C. Papachristos

Graph-based Planning

Dijkstra’s Algorithm

​

• Dijkstra Q is Priority Queue (and R is e.g. an Unordered Map):
• push (Q.Insert) ordered by lowest Cost
• pop (Q.GetFirst) from the front and add to Closed�

​

Open (Q )

Closed (R )

{ D(4) ,

A(5) ,

I(7) }

{ B(0) ,

C(2) ,

G(3) ,

A(4) ,

D(4) ,

I(7) }

Q.Pop ( A(4) )

Q.Insert ( B(5) ) R.Insert( B(5) )

Q.Insert ( C(7) ) R.Insert( C(7) )

Q.Insert ( D(8) ) R.Insert( D(8) )

CS791 C. Papachristos

Graph-based Planning

Dijkstra’s Algorithm

​

• Dijkstra Q is Priority Queue (and R is e.g. an Unordered Map):
• push (Q.Insert) ordered by lowest Cost
• pop (Q.GetFirst) from the front and add to Closed�

​

Open (Q )

Closed (R )

{ A(5) ,

I(5) ,

J(6) ,

I(7) }

​

{ B(0) ,

C(2) ,

G(3) ,

A(4) ,

D(4) ,

I(7) ,

J(6) }

Q.Pop ( D(4) )

Q.Insert ( A(7) ) R.Insert( A(7) )

Q.Insert ( C(6) ) R.Insert( C(6) )

Q.Insert ( I(5) ) R.Insert( I(5) )

Q.Insert ( J(6) ) R.Insert( J(6) )

I(5)

CS791 C. Papachristos

Graph-based Planning

Dijkstra’s Algorithm

​

• Dijkstra Q is Priority Queue (and R is e.g. an Unordered Map):
• push (Q.Insert) ordered by lowest Cost
• pop (Q.GetFirst) from the front and add to Closed�

​

Open (Q )

Closed (R )

{ I(5) ,

J(6) ,

I(7) }

​

{ B(0) ,

C(2) ,

G(3) ,

A(4) ,

D(4) ,

I(5) ,

J(6) }

Q.Pop ( A(5) )

Continue

CS791 C. Papachristos

Graph-based Planning

Dijkstra’s Algorithm

​

• Dijkstra Q is Priority Queue (and R is e.g. an Unordered Map):
• push (Q.Insert) ordered by lowest Cost
• pop (Q.GetFirst) from the front and add to Closed�

​

Open (Q )

Closed (R )

{ J(6) ,

I(7) }

{ B(0) ,

C(2) ,

G(3) ,

A(4) ,

D(4) ,

I(5) ,

J(6) }

Q.Pop ( I(5) )

Q.Insert ( C(10) ) R.Insert( C(10) )

Q.Insert ( D(6) ) R.Insert( D(6) )

Q.Insert ( G(10) ) R.Insert( G(10) )

Q.Insert ( J(8) ) R.Insert( J(8) )

CS791 C. Papachristos

Graph-based Planning

Dijkstra’s Algorithm

​

• Dijkstra Q is Priority Queue (and R is e.g. an Unordered Map):
• push (Q.Insert) ordered by lowest Cost
• pop (Q.GetFirst) from the front and add to Closed�

​

Open (Q )

Closed (R )

{ I(7) }

{ B(0) ,

C(2) ,

G(3) ,

A(4) ,

D(4) ,

I(5) ,

J(6) }

Path B-C-D-J with cost 6

Q.Pop ( J(6) )

CS791 C. Papachristos

Graph-based Planning

Dijkstra’s Algorithm

​

• At the end, we can recover the lowest-cost route from the start to any node
• Or, if we early-terminate at a goal, we can recover the lowest-cost route to�any node with cost < goal

​

• Easy to implement, only implementation detail is the Priority Queue

​

But

• Doesn’t actually know the goal exists until it is found

​

• Can we guide the search to expand nodes that are closer to the goal earlier?
• Can this be achieved without breaking the condition that a node is only accepted�with its lowest cost of arrival?

CS791 C. Papachristos

Graph-based Planning

Cost-to-arrive

Heuristic Cost-to-goal

CS791 C. Papachristos

Graph-based Planning

CS791 C. Papachristos

Graph-based Planning

A* Algorithm

​

• Use Dijkstra’s Priority Queue:
• push (Q.Insert) ordered by lowest Cost + Heuristic
• pop (Q.GetFirst) from the front and add to Closed�

​

Open (Q )

Closed (R )

{ B(0) }

{ B(0) }

1

CS791 C. Papachristos

Graph-based Planning

A* Algorithm

​

• Use Dijkstra’s Priority Queue:
• push (Q.Insert) ordered by lowest Cost + Heuristic
• pop (Q.GetFirst) from the front and add to Closed�

​

Open (Q )

Closed (R )

{ C(2+3=5),

G(3+5=8),

A(5+5=10) }

{ B(0),

C(5),

G(8),

A(10) }

1

A(10)

C(8)

G(8)

CS791 C. Papachristos

5

3

5

Graph-based Planning

A* Algorithm

​

• Use Dijkstra’s Priority Queue:
• push (Q.Insert) ordered by lowest Cost + Heuristic
• pop (Q.GetFirst) from the front and add to Closed�

​

Open (Q )

Closed (R )

{ G(8),

A(4+5=9),

A(10) }

{ B(0),

C(5),

G(8),

A(10) }

1

A(9)

A(9)

CS791 C. Papachristos

5

Graph-based Planning

A* Algorithm

​

• Use Dijkstra’s Priority Queue:
• push (Q.Insert) ordered by lowest Cost + Heuristic
• pop (Q.GetFirst) from the front and add to Closed�

​

Open (Q )

Closed (R )

{ D(4+2=6),

G(8),

A(9),

A(10) }

​

{ B(0),

C(5),

G(8),

A(9),

D(6) }

1

D(6)

CS791 C. Papachristos

2

Graph-based Planning

A* Algorithm

​

• Use Dijkstra’s Priority Queue:
• push (Q.Insert) ordered by lowest Cost + Heuristic
• pop (Q.GetFirst) from the front and add to Closed�

​

Open (Q )

Closed (R )

{ D(6),

G(8),

A(9),

A(10) }

​

{ B(0),

C(5),

G(8),

A(9),

D(6) }

1

G(9)

CS791 C. Papachristos

5

Graph-based Planning

A* Algorithm

​

• Use Dijkstra’s Priority Queue:
• push (Q.Insert) ordered by lowest Cost + Heuristic
• pop (Q.GetFirst) from the front and add to Closed�

​

Open (Q )

Closed (R )

{ D(6),

I(8),

G(8),

A(9),

A(10) }

​

{ B(0),

C(5),

G(8),

A(9),

D(6),

I(8) }

1

I(7)

CS791 C. Papachristos

1

Graph-based Planning

A* Algorithm

​

• Use Dijkstra’s Priority Queue:
• push (Q.Insert) ordered by lowest Cost + Heuristic
• pop (Q.GetFirst) from the front and add to Closed�

​

Open (Q )

Closed (R )

{ I(8),

G(8),

A(9),

A(10) }

​

{ B(0),

C(5),

G(8),

A(9),

D(6),

I(8) }

1

A(12)

CS791 C. Papachristos

5

Graph-based Planning

A* Algorithm

​

• Use Dijkstra’s Priority Queue:
• push (Q.Insert) ordered by lowest Cost + Heuristic
• pop (Q.GetFirst) from the front and add to Closed�

​

Open (Q )

Closed (R )

{ I(6),

I(8),

G(8),

A(9),

A(10) }

​

{ B(0),

C(5),

G(8),

A(9),

D(6),

I(6) }

1

I(6)

CS791 C. Papachristos

1

Graph-based Planning

A* Algorithm

​

• Use Dijkstra’s Priority Queue:
• push (Q.Insert) ordered by lowest Cost + Heuristic
• pop (Q.GetFirst) from the front and add to Closed�

​

Open (Q )

Closed (R )

{ J(6),

I(6),

I(8),

G(8),

A(9),

A(10) }

{ B(0),

C(5),

G(8),

A(9),

D(6),

I(6),

J(6) }

1

J(6)

CS791 C. Papachristos

Graph-based Planning

A* Algorithm

​

• Use Dijkstra’s Priority Queue:
• push (Q.Insert) ordered by lowest Cost + Heuristic
• pop (Q.GetFirst) from the front and add to Closed�

​

Open (Q )

Closed (R )

{ I(6),

I(8),

G(8),

A(9),

A(10) }

{ B(0),

C(5),

G(8),

A(9),

D(6),

I(6),

J(6) }

Path B-C-D-J with cost 6

1

CS791 C. Papachristos

Graph-based Planning

A* Algorithm

​

• A* is an extension of Dijkstra’s algorithm, and achieves faster performance by using a heuristic

​

Best-First Search:

• A* traverses a graph following a path of lowest expected total cost or distance

​

• The cost function is a sum of two functions:
• Cost-to-go, known cost from the start node to current node
• Cost-to-arrive, estimate of distance from current node to goal

CS791 C. Papachristos

Graph-based Planning

In Motion Planning:

• A* is commonly used in robot planning,�especially for low-dimensional State-Spaces

​

Limitations:

​

• Need to construct Graph.

​

• Sometimes an admissible heuristic function�is difficult to find (as hard as the problem)

​

• Naïve grid-discretization may not be a good�representation of the problem

https://qiao.github.io/PathFinding.js/visual/

CS791 C. Papachristos

Dynamic Environment Planning

Dynamic Environments

​

• Inherent Uncertainty and/or dynamic Changes in the environment can affect the planning problem structure

​

• Required to adapt in real-time !
• Instead of naïvely updating Graph weights and recomputing the entire solution from scratch

​

• Relevant approaches :

​

• Potential Field-based hybrids
• Receding Horizon hybrids

​

• Lifelong Planning A* (LPA*) - https://en.wikipedia.org/wiki/Lifelong_Planning_A*
• Dynamic A* (D*) - https://en.wikipedia.org/wiki/D*

CS791 C. Papachristos

Dynamic Environment Planning

Potential Fields

​

• A function that relates a virtual “elevation” / “cost” with some kind of distance to the goal�(a Potential function, as in the context of potential energy)

​

• Taking control actions that direct the robot in the direction of maximum gradient (down),�the robot should “fall” towards the goal (minimum energy state)

​

• The global potential function should “attract” the robot towards the goal, from any valid state

CS791 C. Papachristos

Dynamic Environment Planning

Global Potential Function

​

• A smooth, differentiable function so that it is easy to calculate the target vector

Conic potential farther (slower growth)

CS791 C. Papachristos

Dynamic Environment Planning

Global Potential Function

​

• A smooth, differentiable function so that it is easy to calculate the target vector

Grad: Vector field whose value at a point x is the vector whose components are the partial derivatives of U

CS791 C. Papachristos

Dynamic Environment Planning

Obstacle Potential

​

• To avoid obstacles, so we add an additional component that “repels” from obstacles

CS791 C. Papachristos

Dynamic Environment Planning

Obstacle Potential

​

• To avoid obstacles, so we add an additional component that “repels” from obstacles

CS791 C. Papachristos

Dynamic Environment Planning

Potential Fields

​

• Sample pure Potential Field result:

CS791 C. Papachristos

Dynamic Environment Planning

Potential Fields

​

• Relatively simple to implement !

​

But

• Pure Potential Field navigation has issues with stationary�points or local minima
• Modifying the potential functions can allow these�conditions to be avoided (e.g. harmonic potentials)
• Dealing with higher-order state spaces (arms etc.) can be difficult to grasp/visualize
• To attempt constraint satisfaction, these have to be recasted into a Potential Field component

​

• Applicable for real-time high-rate reactive control !
• Can be combined with an expensive Graph-based planning solution to handle small deviations required (e.g. for obstacle avoidance) around a nominal reference trajectory …

CS791 C. Papachristos

Time for Questions !

CS-791

CS791 C. Papachristos