ASL

Autonomous Systems Lab

Motion Planning | Graph Search

Autonomous Mobile Robots

Martin Rufli – IBM Research GmbH

Margarita Chli, Paul Furgale, Marco Hutter, Davide Scaramuzza, Roland Siegwart

Autonomous Mobile Robots

Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Graph Search | 1
 ASL
Autonomous Systems Lab

Introduction | the see – think – act cycle

knowledge, mission
data base commands
Localization “position“ Cognition
Map Building global map Path Planning

environment model
path
local map

Information Path

Motion Control
Extraction Execution
Perception

see-think-act actuator
raw data
commands

Sensing Acting

Real World
Environment
Autonomous Mobile Robots
Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Introduction to Optimization Techniques | 2
 ASL
Autonomous Systems Lab

Introduction | the motion planning problem

Goal

Autonomous Mobile Robots

Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Introduction to Optimization Techniques | 3
 ASL
Autonomous Systems Lab

Graph construction | overview

A graph G ( N , E ) is characterized by

a set of nodes N

edges E connecting pairs of nodes

Graphs for motion planning are commonly constructed
from map or sensor data

Autonomous Mobile Robots

Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Graph Search | 4
 ASL
Autonomous Systems Lab

Graph construction | Grid and Lattice graphs

Lattice graphs are largely independent of the workspace representation

They overlay a repetitive discretization on the workspace

Autonomous Mobile Robots

Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Graph Search | 5
 ASL
Autonomous Systems Lab

Motion Planning | Graph Search

Autonomous Mobile Robots

Martin Rufli – IBM Research GmbH

Margarita Chli, Paul Furgale, Marco Hutter, Davide Scaramuzza, Roland Siegwart

Autonomous Mobile Robots

Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Graph Search | 6
 ASL
Autonomous Systems Lab

Deterministic graph search | overview

Encompasses deterministic optimization algorithms operating on

graph structures G ( N , E )

The methods find a (globally lowest-cost) connection between a pair of nodes

Autonomous Mobile Robots

Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Graph Search | 7
 ASL
Autonomous Systems Lab

Breadth-first search | working principle

The method expands nodes according to a FIFO queue and a Closed list

It backtracks the solution from the goal state backwards in a greedy way

0 BF(Graph G, Node Start, Node Goal) FIFO Closed

1 Queue.init(FIFO) f=
B
2 Queue.push(Start)
3 while Queue is not empty: 1 1

4 Node curr = Queue.pop() f = 1

A D
5 if curr is Goal return 1 f=
1
E F
6 Closed.push(curr) 1

7 Nodes next = expand(curr) f =

C
8 for all next not in Closed:
9 Queue.push(next)

Autonomous Mobile Robots

Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Graph Search | 8
 ASL
Autonomous Systems Lab

Breadth-first search | working principle

The method expands nodes according to a FIFO queue and a Closed list

It backtracks the solution from the goal state backwards in a greedy way

0 BF(Graph G, Node Start, Node Goal) FIFO Closed

1 Queue.init(FIFO) f=
B
2 Queue.push(Start)
3 while Queue is not empty: 1 3

4 Node curr = Queue.pop() f = 1

A D
5 if curr is Goal return 1 f=
1
E F
6 Closed.push(curr) 1

7 Nodes next = expand(curr) f =

C
8 for all next not in Closed:
9 Queue.push(next)

Autonomous Mobile Robots

Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Graph Search | 9
 ASL
Autonomous Systems Lab

Breadth-first search | properties

The trajectory to the first goal state encountered is optimal iff all edge
costs on the graph are identical and positive

Optimality of the solution is retained for arbitrary positive edge costs, if
search is continued until queue is empty

Breadth-first search has a time complexity of O (V + E )

Autonomous Mobile Robots

Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Graph Search | 10
 ASL
Autonomous Systems Lab

Dijkstra‘s search | working principle

Dijkstra‘s search expands nodes according to a HEAP and a Closed list

It backtracks the solution from the goal state backwards in a greedy way

0 Min_Bin_Heap_Push(Node up)
1 insert up at end of heap
3 while up < parent(up):
2
4 swap(up, parent(up))

4 7

5 9

Autonomous Mobile Robots

Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Graph Search | 11
 ASL
Autonomous Systems Lab

Dijkstra‘s search | working principle

Dijkstra‘s search expands nodes according to a HEAP and a Closed list

It backtracks the solution from the goal state backwards in a greedy way

0 Min_Bin_Heap_Push(Node up)
1 insert up at end of heap
3 while up < parent(up):
2
4 swap(up, parent(up))

0 Min_Bin_Heap_Pop() 4 7 4 3
1 return top element of heap
2 move bottom element to top as down
3 while down > min(child(down)):
5 9 5 9 7
4 swap(down, min(child(down)))

Autonomous Mobile Robots

Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Graph Search | 12
 ASL
Autonomous Systems Lab

Dijkstra‘s search | working principle

Dijkstra‘s search expands nodes according to a HEAP and a Closed list

It backtracks the solution from the goal state backwards in a greedy way

0 Dijkstra(Graph G, Node Start, Node Goal) HEAP Closed

1 Queue.init(BIN_MIN_HEAP) f =
B
2 Queue.push(Start)
3 while Queue is not empty: 1 1 .5

4 Node curr = Queue.pop() 1

A f =
5 if curr is Goal return D
1 1
E
6 Closed.push(curr)
7 Nodes next = expand(curr) f=
C
8 for all next not in Closed:
9 Queue.push(next)

Autonomous Mobile Robots

Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Graph Search | 13
 ASL
Autonomous Systems Lab

Dijkstra‘s search | properties & requirements

The sequence to the first goal state encountered is optimal

Edge costs must be strictly positive; otherwise, employ Bellman-Ford

Dijkstra‘s search has a time complexity of O (V log V + E )

Autonomous Mobile Robots

Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Graph Search | 14
 ASL
Autonomous Systems Lab

The A* algorithm | working principle

A* expands nodes according to a HEAP and a Closed list

It makes use of a heuristic function to guide search

It backtracks the solution from the goal state backwards in a greedy way
0 A_Star(Graph G, Heur H, Node Start, Goal) HEAP Closed
1 Queue.init(BIN_MIN_HEAP, H) B hf==2
2 Queue.push(Start)
3 while Queue is not empty: 1 1 .5

4 Node curr = Queue.pop() 2

A
5 if curr is Goal return
1 1
D hf==1 E
6 Closed.push(curr)
7 Nodes next = expand(curr)
C hf==2
8 for all next not in Closed:
9 Queue.push(next)

Autonomous Mobile Robots

Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Graph Search | 15
 ASL
Autonomous Systems Lab

The A* algorithm | working principle

A* expands nodes according to a HEAP and a Closed list

It makes use of a heuristic function to guide search

It backtracks the solution from the goal state backwards in a greedy way
0 A_Star(Graph G, Heur H, Node Start, Goal) HEAP Closed
1 Queue.init(BIN_MIN_HEAP, H) B hf==0
2 Queue.push(Start)
3 while Queue is not empty: 1 1 .5

4 Node curr = Queue.pop() 2

A
5 if curr is Goal return
1 1
D hf==1 E
6 Closed.push(curr)
7 Nodes next = expand(curr)
C hf==3
8 for all next not in Closed:
9 Queue.push(next)

Autonomous Mobile Robots

Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Graph Search | 16
 ASL
Autonomous Systems Lab

The A* algorithm | properties & requirements

The trajectory to the first goal state encountered is optimal

Edge costs must be strictly positive

For optimality to hold heuristic must be consistent

Autonomous Mobile Robots

Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Graph Search | 17
 ASL
Autonomous Systems Lab

Randomized graph search | overview

Encompasses optimization algorithms operating according to a randomized

node expansion step

The associated graph is thus usually constructed online during search

Randomization is appropriate for high-dimensional search spaces

Autonomous Mobile Robots

Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Graph Search | 18
 ASL
Autonomous Systems Lab

The RRT algorithm | working principle

RRT grows a randomized tree during search

It terminates once a state close to the goal state is expanded

Autonomous Mobile Robots

Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Graph Search | 19
 ASL
Autonomous Systems Lab

The RRT algorithm | example

RRT grows a randomized tree during search

It terminates once a state close to the goal state is expanded

0 RRT(Node Start, Node Goal, System Sys,

Goal
Environment Env)
1 Graph.init(Start)
2 while Graph.size() is less than threshold
3 Node rand = rand()
4 Node near = Graph.nearest(rand)
5 try
6 Node new = Sys.propagate(near, rand)
7 Graph.addNode(new) Start
8 Graph.addEdge(near,new)

Autonomous Mobile Robots

Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Graph Search | 20
 ASL
Autonomous Systems Lab

The RRT algorithm | properties

Solutions are almost surely sub-optimal

RRT is probabilistically complete

Autonomous Mobile Robots

Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Graph Search | 21
 ASL
Autonomous Systems Lab

Graph search| further reading

Any-angle search

D. Ferguson and A. T. Stentz. “Field D*: An Interpolation-based Path Planner and Replanner”.
In Proceedings of the International Symposium on Robotics Research (ISRR), 2005.

The D* algorithm

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
(ICRA), 2002.

The RRT* algorithm

S. Karaman and E. Frazzoli. “Sampling-based Algorithms for Optimal Motion Planning”.
International Journal of Robotics Research, 30(7): 846–894, 2011.

Autonomous Mobile Robots

Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Graph Search | 22