ME5751�Robotics Motion Planning

Yizhe Chang chang@cpp.edu

Lecture Note Set #12-13

​

Outline

• Search-based Planning
• Dijisktra Algorithm
• Flood Fill Algorithm
• Supplement: Priority Queue
• Supplement: Dictionary
• A* path planning
• Heuristics
• Search with potential/cost map, parameter tuning

Review

• Breadth first search
• Depth first search
• Binary tree

BFS/DFS pseudocode

• Using stack for?

• Using queue for?

BFS/DFS pseudocode

• Using stack for?

• Using queue for?

Path Planning

• Given a known map, how can we get from position A to B?
• For grid-based planning, what is our configuration space?

Valid and not valid path

From https://en.wikipedia.org/wiki/Motion_planning

Many ways, which one is the shortest?

• You see this example everywhere… Thanks Google and other online mapping providers

Problem definition

• How can we find the shortest path between 2 vertices in a graph
• Graph and be both directed or undirected
• All edges have a non-negative weight
• Graph must be connected, or we shall be able to say we cannot find a path

42^nd St

43^rd St

44^th St

8^th Ave

7^th Ave

6^th Ave

5^th Ave

400

400

400

80

80

200

Pomona, California

PABT

Grand Central

Mission statement

• Find shortest path from A -> B:

​

All images below, unless otherwise noted, are from: (https://brilliant.org/wiki/dijkstras-short-path-finder/)

​

So Dijisktra pop up a solution

• Edsger W. Dijkstra
• Dutch computer scientist, Turning award winner
• Professor of Eindhoven University of Technology and UT Austin
• 1959 “A note on two problems in connexion with graphs”
• Propose algorithm to find shortest path in a graph “Dijisktra’s Algorithm”
• “Simplicity is prerequisite for reliability”

​

​

Edsger W. Dijkstra, 1930-2002

Image from Wikipedia.org

Some notation

• It is a graph G
• Every edge has a weight, noted as distance or w(i, j), (e.g. w(α, β)=1)
• Every vertex(node) has a value, noted as d(i) (e.g. d(α)=3)

​

​

α

β

γ

δ

ε

ζ

η

Shortest path first algorithm

• Pseudocode first

Dijisktra

• Expand A, remove the smallest (A here)
• \alpha = 3, \beta = 7, \ gamma = 5

A

 α=3

 β=7

γ=5

search path

source(previous) path

α

β

γ

δ

ε

ζ

η

List of all nodes Q

Dijisktra’s Algorithm

• Expand the current smallest (\alpha), remove it afterward:
• \delta = 10, \beta = 4

A

 α=3

 β=7

γ=5

 δ=10

 β=4

α

β

γ

δ

ε

ζ

η

List of all nodes Q

Dijisktra’s Algorithm

• Expand the current smallest (\alpha), remove it afterward:
• \delta = 10, \beta = 4

A

 α=3

 β=7

γ=5

 δ=10

 β=4

α

β

γ

δ

ε

ζ

η

List of all nodes Q

Dijisktra’s Algorithm

• Expand the current smallest (\beta), remove it afterward:
• \gamma = 7, \epsilon = 5, \zeta =8

A

 α=3

 β=7

γ=5

 δ=10

 β=4

α

β

γ

δ

ε

ζ

η

  γ =7

 ε =5

ζ

=8

List of all nodes Q

Dijisktra’s Algorithm

• Expand the current smallest (\gamma), remove it afterward:
• \zeta=7, where is \beta?

A

 α=3

γ=5

 δ=10

 β=4

α

β

γ

δ

ε

ζ

η

 ε =5

ζ

=8

ζ

=7

List of all nodes Q

Dijisktra’s Algorithm

• Expand the current smallest (\epsilon), remove it afterward:
• \zeta=8, \delta = 7, \ita=8, B=7

A

 α=3

γ=5

 δ=10

 β=4

α

β

γ

δ

ε

ζ

η

 ε =5

ζ

=7

δ=7

ζ

=8

η=8

B=7

List of all nodes Q

Dijisktra’s Algorithm

• Expand the current smallest (who???), remove it afterward:
• We find B is among the smallest, we are done!

A

 α=3

γ=5

 δ=10

 β=4

α

β

γ

δ

ε

ζ

η

 ε =5

ζ

=7

δ=7

ζ

=8

η=8

B=7

List of all nodes Q

Dijisktra’s Algorithm

• How we find path from A to B then?
• We trace back from B

A

 α=3

γ=5

 β=4

α

β

γ

δ

ε

ζ

η

 ε =5

ζ

=7

δ=7

η=8

B=7

Dijisktra’s Algorithm: Reflection

• Value on each node:
• Shortest known distance to the start node

A

 α=3

γ=5

 β=4

α

β

γ

δ

ε

ζ

η

 ε =5

ζ

=7

δ=7

η=8

B=7

Dijisktra’s Algorithm: Reflection

• We almost expand all nodes

​

A

 α=3

γ=5

 β=4

α

β

γ

δ

ε

ζ

η

 ε =5

ζ

=7

δ=7

η=8

B=7

Dijisktra’s Algorithm: Reflection

• Originally we have a graph
• We made it a tree
• The search on tree is neither DFS nor BFS, but�“best first”

A

 α=3

γ=5

 β=4

α

β

γ

δ

ε

ζ

η

 ε =5

ζ

=7

δ=7

η=8

B=7

Shortest path first algorithm

• Pseudocode revisit

Why Dijisktra works? intuition

• Lemma 1: Optimal Substructure
• The subpath of any shortest path is itself a shortest path ! �
• Lemma 2: Triangle inequality
• If d(u,v) is the shortest path length between u and v, d(u,v) ≤ d(u,x) + d(x,v)

from http://math.mit.edu/~rothvoss/18.304.3PM/Presentations/1-Melissa.pdf

Dijisktra’s Algorithm: where is \beta?

• Expand the smallest on Q ensured that no possibility of undiscovered shorter path

A

 α=3

γ=5

 δ=10

 β=4

α

β

γ

δ

ε

ζ

η

 ε =5

ζ

=8

ζ

=7

List of all nodes Q

Flood fill algorithm

• From start to goal?

start

goal

Flood fill algorithm

• Basically Dijisktra’s Algorithm on Grid map

0

1

1

2

2

3

3

3

4

4

4

4

4

1

1

1

1

1

1

Flood fill algorithm

• And we know the potential map…

0

100

100

200

200

300

250

300

450

500

400

300

275

100

50

25

600�500

500�550

Conclusion

• Dijisktra’s algorithm
• Complete and correct
• “Best-first”, but pretty like breadth first
• Most of SPF search are based on Dijisktra’s alg
• Flood fill:
• Dijistkra’s algorithm on grid map
• If no potential (cost) is applied on each cell,�Flood fill is basically brushfire

Dijisktra’s algorithm’s application

• Good people use it for motion planning
• Evil people use it for financing!
• Is there an arbitrage opportunity?
• Ex. $1 -> CHF1.3941 CHF -> € 0.9308 -> $1.00084

​

From: http://math.mit.edu/~rothvoss/18.304.3PM/Presentations/1-Melissa.pdf

Outline

• Search-based Planning
• Dijisktra Algorithm
• Flood Fill Algorithm
• Supplement: Priority Queue
• Supplement: Dictionary, unordered_map
• A* path planning
• Heuristics
• Search with potential/cost map, parameter tuning

Question

• On best first search, we always want to expand the node with “smallest cost”.
• Among all nodes we discovered, how can we find the “smallest cost” node?

Priority Queue

• There is “max” priority queue, and “min” priority queue
• A queue, with two operations:
• Push: push into the queue
• Pop: only pop the max (max priority q)/ min (min priority q)

https://www.geeksforgeeks.org/priority-queue-set-1-introduction/

Priority Queue

• There is a library for min priority queue

​

Priority Queue

• How this library is implemented?
• Various methods, e.g. heap.
• Most common: binary search tree, or min heap

​

Binary search tree

Min heap: parents are always smaller than the child

Outline

• Search-based Planning
• Dijisktra Algorithm
• Flood Fill Algorithm
• Supplement: Priority Queue
• Supplement: Dictionary, unordered_map
• A* path planning
• Heuristics
• Search with potential/cost map, parameter tuning

Dictionary

• Dictionary holds key:value pair. Key-Value is provided in the dictionary to make it more optimized.
• You can consider a dictionary an array, but the array key is indexed by your defined value (numbers, string, etc.)
• Dictionary is implemented through hash function (almost a hash table).
• Further reading: https://www.w3schools.com/python/python_dictionaries.asp
• https://www.geeksforgeeks.org/python-dictionary/
• Try this

Priority Queue with Dictionary

• It is possible to implement priority queue with dictionary or tuple
• Check this post:
• https://stackoverflow.com/questions/40205223/priority-queue-with-tuples-and-dicts

​

Outline

• Search-based Planning
• Dijisktra Algorithm
• Flood Fill Algorithm
• Supplement: Priority Queue
• Supplement: Dictionary
• A* path planning
• Heuristics
• Search with potential/cost map, parameter tuning

Dijisktra’s Algorithm: re-visit

• What is wrong with Dijisktra?
• It is perfect! – (this is the problem)
• We see it is, er, more like a breadth first search

​

A

 α=3

γ=5

 δ=10

 β=4

  γ =7

 ε =5

ζ

=8

α

β

γ

δ

ε

ζ

η

from Wikipedia.org

 δ=6

Dijisktra’s Algorithm: re-visit

• In Dijisktra, which node we needs to expand?
• The node value we are going to expand is a proved shortest distance from start
• The node value on nodes we do not expand yet has the shortest known distance from start

α

β

γ

δ

ε

ζ

η

A

 α=3

γ=5

 δ=10

 β=4

  γ =7

 ε =5

ζ

=8

 δ=6

Dijisktra’s Algorithm: re-visit

• Why we keeps thinking on the start
• In the above case, if distance to \delta is 0.1, we will expand \delta next, which is far from B
• We want to go to node B!

α

β

γ

δ

ε

ζ

η

A

 α=3

γ=5

 δ=10

 β=4

  γ =7

 ε =5

ζ

=8

 δ=6 4.1

0.1

4.1

Heuristic

• Heuristic technique
• (Ancient Greek: εὑρίσκω, "find" or "discover"), often called simply a heuristic,
• approach that is not guaranteed to be optimal, perfect or rational;
• but instead sufficient for reaching an immediate goal;
• We make all decisions using heuristic
• Which restaurant to eat for tonight?
• Shall I call Uber or take a bus back home?
• We, internally, set a value to “optimize”, regardless whether this value is correct.

Heuristic function

• When we are at point \beta, can we estimate how far we are from node B?
• We can “fabricate” a function, that is our heuristic function
• On grid map, it is intuitive to set the Heuristic H(i, j) = distance to goal
• Of course can be Manhattan distance

​

3.61

5

Greedy Best First Search

• We always expand the node that has smaller Heuristic
• E.g. if we define Euclidean distance as our�Heuristic function H

​

​

​

​

​

​

• Who shall I expand next?

​

a5

5

b4

4.47

b5

4.24

Greedy best first search

• So we expand the node with smallest Heuristic

4.27

4.47

3.61

a5

5

b4

4.47

b5

4.24

c5

3.61

Greedy best first search

• So we get our answer

4.27

4.47

3.61

2.82

3.16

2.24

2.24

1.41

2

1

1

Greedy best first search

• https://www.redblobgames.com/pathfinding/a-star/introduction.html
• And we can show Greedy is not always a good thing, that is why it is called “greedy”…

A* algorithm

• Can we make a compromise between Dijisktra and “greedy best first?”
• Dijisktra – make decision solely on known
• Greedy – make decision solely on Heuristic

​

This quote is here because it optimizes my heuristic on your (understanding on heuristic, and on combining heuristic with known fact)

A* algorithm

• Node value d(α):
• d(α) = distance from start (known) + heuristic to goal (estimated)
• When we expand a node, we expand the smallest d(α)

​

a5

5

b4

5.57

b5

5.24

c5

5.61

A* Algorithm

a5

5

b4

5.57

b5

5.24

c5

5.61

a3

6.12

3.16+3 =6.16

2.82+3 =5.83

2.24+4 =6.24

2.24+4 =6.24

4+3 �=7

A* Algorithm

• Further reading:

https://en.wikipedia.org/wiki/A*_search_algorithm

​

https://www.redblobgames.com/pathfinding/a-star/introduction.html

​

Dijisktra Algorithm A* Algorithm (from Wikipedia)

A* Algorithm

• We see
• the node value may increase

(children may have a larger value�than the parents)

3.16+3 =6.16

2.82+3 =5.83

2.24+4 =6.24

2.24+4 =6.24

4+3 �=7

A* Algorithm: potential field

• With potential field, every move has a “cost”: c(i,j)
• For a node α value:

d(i,j ) = c(i,j) + H(i,j)

​

• How we should set Heuristics?
• Maybe:

A* Algorithm: potential field

• With potential field, every move has a “cost”: c(i,j)
• For a node α value:

d(i,j) = c(i,j) + H(i,j)

​

• How we should set Heuristics?
• If we say our every move has a cost of “1”, we basically numerically let c(i,j) dominate

A* Algorithm: potential field

A* Algorithm: parameter tuning

A* Algorithm: parameter tuning

Valid and not valid path

From https://en.wikipedia.org/wiki/Motion_planning

tuning a bike ⬄

tuning parameter

Conclusion

Further Development

Dijisktra (1956)

A* (1968)

D* (1995)

Focused D* (1995)

Life long planning A* (2004)

D* lite (1995)