Discrete motion planning

TSFS12: Autonomous Vehicles —planning, control, and learning

systems
Lecture 2: Erik Frisk <[email protected]>
 2

Course announcements (1/2)

¥ If youÕre not yet registered for the course; you need to do that for full
access to Lisam functionality, e.g., submissions
¥ Submissions: hand in by deadline, possible for an iteration
(but donÕt hand in unfinished exercises!)
¥ There is a ÒFind lab partnerÓ functionality on the Lisam page
¥ Link to hand-in 1 introduction video available at the git repo
¥ Link to ÒRobotics HandbookÓ from git-repo was broken; now fixed
 3

Course announcements (2/2)

¥ TA session on Thursday 15:15
¥ At the exercise we have a reduced teacher presence, since typically too
early for a lot of questions.
¥ To help those new to Python development, I have a set of pre-recoded
videos available at the git-repo to get you started in an IDE.

If you are experienced in python, there will probably be no news for you.
 4

Motion planning, from discrete problems …

 5

… to continuous, nonholonomic, systems with inertia

NASA/Lockheed Martin X-33 Re-entry trajectory

From “Planning Algorithms”, S. LaValle, 2006.
 6

Motion planning and discrete graph search

¥ Graph search algorithms are very useful for planning motion and
trajectories for autonomous vehicles
¥ But, robots do not move on a graph?
¥ Discretize (spatial and temporal)
¥ Use graph search as a component in a continuous planner, for
example in so called lattice planners (Lecture 4)
¥ This lecture will focus on the graph search problem and introduce
fundamental algorithms
¥ These algorithms are the topic of hand-in 1, and will be used also in
hand-in 2.
 7

Scope of this lecture

¥ Formalization of a planning problem as a graph search problem
¥ Main algorithms for graph search
¥ DijkstraÕs algorithm
¥ A*
¥ Properties of heuristics in A* to ensure optimality and efficiency
¥ Introduction to any-time planning using A* Ñ ARA*
Graphs and discrete planning problems
 9

Solving problems by searching in a graph

Nodes/vertices

Start

Edges

1891
Edges could have
weights/costs/lengths

Goal
 10

A node can represent any state in a search space

1 4 3
7 6

5 8 2

1 3

7 4 6
5 8 2
1 4 3

7 6 2

5 8
 11

Formulating a planning problem as a graph search

¥ Problem solving is sometimes well formulated as graph search problems
¥ Formulation of graph search problem requires
¥ State-space 𝒳
¥ For each state x ∈ 𝒳 there is an action space 𝒰(x)
¥ A state-transition function, i.e., description of the next state x′ if
action u is used in state x
x′ = f(x, u) ∈ 𝒳 for each u ∈ 𝒰(x)
¥ Initial state xI and goal state xG
 12

Definition of a graph search problem

5 10 15 20 25

X = {1, . . . , 25}
4 9 14 19 24
U (x) ⊆ {Up, Down, Left, Right} G
xI = 1, xG = 14
3 8 13 18 23
<latexit sha1_base64="SbJcnYSNz01U35KGQVDSSUm+bZQ=">AAACa3icbVFda9RAFJ2kftRV62ofFPVhcGmpUJZk3dL2QSgqqOBDFdMubJYwmb3ZHTqZpDM3dZeQF3+ib/4DX/wPTrJh8evAwLnnfnImzqUw6HnfHXfj2vUbNzdvdW7fubt1r3v/wZnJCs0h4JnM9ChmBqRQEKBACaNcA0tjCefxxes6f34F2ohMfcZlDpOUzZRIBGdopaj7NUwZzjmT5aiiuy9pWPr7NJxmaPbp4CCswrAT7C2e093QFLEBhEtbEiIssAzyypY29E32Ra2DD5DgOvgkZnOsmjmL6H29oZ5/WbApXURvqQ2HUbfn9b0G9F/it6RHWpxG3W/2QF6koJBLZszY93KclEyj4BKqTlgYyBm/YDMYW6pYCmZSNl5VdMcqU5pk2j6FtFF/7yhZaswyjW1l7Yz5O1eL/8uNC0yOJqVQeYGg+GpRUkiKGa2Np1OhgaNcWsK4FvZWyudMM472ezqNCccN6IocDlty7K9NOBv0/Rf9wcdh7+RVa8cmeUKekT3ik0NyQt6RUxIQTn44W85D55Hz0912H7tPV6Wu0/Zskz/g7vwCNl23jg==</latexit>

0
x = f (x, u) ∈ X
<latexit sha1_base64="gf2DqGDZaF+P8gvFl/WoAI84rhw=">AAACBHicbZDLSsNAFIYn9VbrLeqym8EiVpCS1ELtQii6cVnBXqAJZTKdtEMnkzAzkZbQhRtfxY0LRdz6EO58G5M0iLcfBj7+cw7nzO8EjEplGB9abml5ZXUtv17Y2Nza3tF39zrSDwUmbewzX/QcJAmjnLQVVYz0AkGQ5zDSdSaXSb17S4SkPr9Rs4DYHhpx6lKMVGwN9OL0CJ5Dtzw9geExtCi3PKTGGLGoNx/oJaNipIJ/wcygBDK1Bvq7NfRx6BGuMENS9k0jUHaEhKKYkXnBCiUJEJ6gEenHyJFHpB2ln5jDw9gZQtcX8eMKpu73iQh5Us48J+5MTpS/a4n5X60fKvfMjigPQkU4XixyQwaVD5NE4JAKghWbxYCwoPGtEI+RQFjFuRXSEBqp4ALqtQwa5lcInWrFPK1Ur2ul5kUWRx4UwQEoAxPUQRNcgRZoAwzuwAN4As/avfaovWivi9acls3sgx/S3j4Bi1GXMg==</latexit>

2 7 12 17 22
U (8) = {Left, Down}
<latexit sha1_base64="Qvg02Rg2u6y6qrXJFHFZa52nm6g=">AAACDHicbVDLSgMxFM34rPVVdekmWAQFKTMq6EYQdeHCRQWrhU4pmfRODWYyQ3JHLcN8gBt/xY0LRdz6Ae78G9PHQq0HAifnnEtyT5BIYdB1v5yx8YnJqenCTHF2bn5hsbS0fGniVHOo8VjGuh4wA1IoqKFACfVEA4sCCVfBzXHPv7oFbUSsLrCbQDNiHSVCwRlaqVUq1zb2N+kB9TMf4R6zMwgx36KDy0l8p3I/tym34vZBR4k3JGUyRLVV+vTbMU8jUMglM6bhuQk2M6ZRcAl50U8NJIzfsA40LFUsAtPM+svkdN0qbRrG2h6FtK/+nMhYZEw3CmwyYnht/no98T+vkWK438yESlIExQcPhamkGNNeM7QtNHCUXUsY18L+lfJrphlH21/RluD9XXmUXG5XvJ3K9vlu+fBoWEeBrJI1skE8skcOySmpkhrh5IE8kRfy6jw6z86b8z6IjjnDmRXyC87HN46Nmrk=</latexit>


18
 u = Up 1 6 11 16 21
f (17, u) = 22 u = Right S

16 u = Down

<latexit sha1_base64="XXLiyZOZlwP/lytqTZPm2RlvEk4=">AAACUHicbZHPb9MwFMdfOn5s4ccKO3KxqIaGhKqkTHQ7TJoGB44D0W1SU1WO+5Jac5zIfgGqqH/iLrvt7+DCYWhz04Ao7CtZ/urznp+tr+NCSUtBcOW11u7df/BwfcN/9PjJ0832s+cnNi+NwIHIVW7OYm5RSY0DkqTwrDDIs1jhaXz+flE//YrGylx/oVmBo4ynWiZScHJo3E6TnbD/hpWv2QHzoxhTqSvh5tm5H+6xV6x0PCL8TtWgmEeR3+utwM8yndKCh+9W+If8m577EepJM23c7gTdoBb734SN6UCj43H7MprkosxQk1Dc2mEYFDSquCEpFLrZpcWCi3Oe4tBZzTO0o6oOZM62HZmwJDduaWI1/ftExTNrZ1nsOjNOU/tvbQHvqg1LSvZGldRFSajF8qKkVIxytkiXTaRBQWrmDBdGurcyMeWGC3J/4Nch7NdiS9Pfbcx++CeEk143fNvtfdrtHB41cazDC3gJOxBCHw7hIxzDAARcwA+4hl/epffTu2l5y9bfO2zBilr+Lap9sCo=</latexit>
 13

State-space and search tree

1 4 3

¥ Na•ve solution; exhaustive 7 6

5 8 2
search
Up Right
¥ Build search tree and explore
Left
until solution is found Down

¥ Different ways to traverse 1 3 1 4 3 1 4 3 1 4 3

the tree 7 4 6 7 6 7 8 6 7 6

5 8 2 5 8 2 5 2 5 8 2
¥ depth first
Left Right Up Down Left Right Up Down
¥ breadth first
¥ É 4 3 1 3 4 3 1 4 3 1 4 3 1 4 3 1 4 1 4 3

1 7 6 7 4 6 1 7 6 5 7 6 7 8 6 7 8 6 7 6 3 7 6 2

5 8 2 5 8 2 5 8 2 8 2 5 2 5 2 5 8 2 5 8
 14

Queues
¥ A queue is a data structure where you can
¥ Push (or insert) elements on the queue
¥ Pop (or remove) elements from the queue
¥ Very useful for describing and implementing search algorithms
¥ Three different queues will be used
¥ FIFO - First In First Out
¥ LIFO - Last In First Out
¥ Priority Queue - assign priority to each element
≈ efficiently keep the queue always sorted (not exactly true but É)
Will return to this queue later.
 15

General forward search (and keep track of visited nodes)

¥ During the search, define a 1 function ForwardSe arch :
mapping 2 Q. i n s e r t ( xI )
3
Type of queue
x′ = previous(x) 4 while Q 6= ; decides depth first
5 x = Q. pop ( ) or breadth first
¥ Keeps track of paths, node x 6 i f x = xG
is predecessor of node x′ 7 return SUCCESS
8
¥ Keeps track of which nodes 9 f o r u 2 U (x)
that are visited 10 x0 = f (x, u)
11 i f no p r e v i o u s ( x0 )
¥ Depth first - LIFO queue 12 p r e v i o u s ( x0 ) = x
¥ Breadth First - FIFO queue 13 Q. i n s e r t ( x0 )
14
15 return FAILURE
 16

Breadth First search - FIFO queue Node Previous

1 2
5 10 15 20 25 2 1
3 2
4 3
5 4
6 1
4 9 14 19 24
7 2
8 3
G 9
10
3 8 13 18 23 11 6
12
13
14 19
15
2 7 12 17 22 16 11
17 16
18 17
19 18
20 19
1 6 11 16 21 21 16
22 17
S 23 18
24 19
25
 17

Depth First search - LIFO queue

5 10 15 20 25

4 9 14 19 24

G
3 8 13 18 23

2 7 12 17 22

1 6 11 16 21

S
 18

Backtrack to obtain path Node Previous

1 -
5 10 15 20 25 2 1
3 2
4 3
5 4
6 1
4 9 14 19 24
7 2
8 3
G 9
10
3 8 13 18 23 11 6
12
13
14 19
15
2 7 12 17 22 16 11
17 16
18 17
19 18
20 19
1 6 11 16 21 21 16
22 17
S 23 18
24 19
25
 19

Generate path from visited mapping

¥ The mapping

x′ = previous(x)
1 function B a c k t r a c k ( v i s i t e d , s o u r c e , g o a l ) :
2 i f found
defines the path. 3 p = ;
4 u = goal
¥ Node x is the predecessor 5 while p r e v i o u s [ u ] 6= s t a r t
6 i n s e r t u at the beginning of p
of node x′ 7 u = previous [ u ]
8 i n s e r t u at the beginning of p
¥ Backtracking from goal to
start then gives the path
 20

What about quality of plan?

1 A 2 Depth First/LIFO Breadth First/FIFO
1 I I
I C G
1 1 A A B
B C C

G G

¥ Clearly neither depth-first nor breadth-first finds the shortest path in the
graph
¥ Not surprising since there is no notion of distance/stage-cost in the search
¥ Next step is to find shortest paths É
Dijkstra’s algorithm - finding shortest path
 22

Dijkstra’s algorithm
¥ Well known algorithm, first published in the 1950Õs
¥ Computes, not only the shortest path between two nodes
¥ Not only that, it can compute the shortest path between a source node
and all other nodes; shortest path tree
(hint: useful in extra assignment for HI1)
¥ Idea:
¥ keep track of cost-to-come for each visited node, and
¥ explore the tree search prioritized by cost-to-come
¥ Use Priority Queues instead of FIFO/LIFO
 23

Priority Queue
¥ You can insert and pop (element, priority) pairs
¥ Here priority is typically path cost (length/time)
¥ Operations (for min-priority queue)
¥ insert(element, priority) - insert pair into the queue
¥ pop() - returns element and priority corresponding to the lowest priority
¥ decrease_key(element, priority) - change priority for an element
¥ In general, you can decrease an elements priority by pushing it again
¥ This is not strictly needed, I will come back to this; lazy delete
¥ Insert and pop are no longer constant time operations, typically 𝒪(log n)
for implementations based on a data-structure called heap
 24

Dijkstra’s algorithm
1 function D i j k s t r a :
¥ d(x, x′) cost (∼ length) to go from 2 C(xI ) = 0
3 Q. i n s e r t ( xI , C(xI ) )
node x to x′ 4

¥ During search, update additional 5 while Q 6= ;

6 x = Q. pop ( )
mapping 7 i f x = xG
C(x) 8 return SUCCESS
9
that keeps track of cost-to-come to 10 f o r u 2 U (x)
node x. 11 x0 = f (x, u)
12 i f no p r e v i o u s ( x0 ) o r
¥ Use a priority queue, with cost-to- 13 C(x0 ) > C(x) + d(x, x0 )
come as priority to explore the 14 p r e v i o u s ( x0 ) = x
shortest paths first 15 C(x0 ) = C(x) + d(x, x0 )
16 Q. i n s e r t ( x0 , C(x0 ) )
¥ Modify the search to rewire in 17

case a cheaper path is found 18 return FAILURE

 25

Dijkstra on the small graph

Start: I
1 A 2 Prio 0 Previous(x)
Pop I: A B I -
1 Prio 1 1 A I
I C G Pop A: B C
B I
C A B
Prio 1 3
1 1 G C
Pop B: C alt. C C
B Prio 2 2 3
Pop C: G alt G C
Prio 3 3 3
Breadth First/FIFO Pop G: Goal
I

A B

C C? Here path to C
is rewired
G
 26

Dijkstra’s algorithm
Start: 1 Pop 5: 17 21
5 10 15 20 25 Prio 0 Prio 4 4
Pop 1: 2 6 Pop 17: 21 18 22
Prio 1 1 Prio 4 5 5
Pop 2: 6 3 7 Pop 21: 18 22
4 9 14 19 24 Prio 1 2 2 Prio 5 5
Pop 6: 3 7 11 Pop 18: 22 19 23
G Prio 2 2 2 Prio 5 6 6
Pop 3: 7 11 4 8 Pop 22: 19 23
3 8 13 18 23 Prio 2 2 3 3 Prio 6 6
Pop 7: 11 4 8 Pop 19: 23 14 20 24
Prio 2 3 3 Prio 6 7 7 7
Pop 11: 4 8 16 Pop 23: 14 20 24
2 7 12 17 22 Prio 3 3 3 Prio 7 7 7
Pop 4: 8 16 5 Pop 14: Goal
Prio 3 3 4
Pop 8: 16 5
1 6 11 16 21 Prio 3 4

S Pop 16:
Prio
5 17 21
4 4 4
 Node Previous Cost 27

Example, backtracking path 1

2
-1
1
0
1
3 2 2
4 3 3
¥ During search, two mappings are 5 4 4
6 1 1
updated 7 2 2
8 3 3
¥ Previous(x) - keep track of parent 9

node 10
11 6 2
12
¥ Cost(x) - current cost to come 13
14 19 7
¥ Using Previous(x), backtracking gives 15
16 11 3
the resulting path 17 16 4
18 17 5
¥ 14 - 19 - 18 - 17 - 16 - 11 - 6 - 1 19 18 6
20 19 7
¥ Note that you get minimum length 21 16 4

path from start to all nodes 22

23
17
18
5
6
24 19 7
25
 28

Sketch of proof of optimality

¥ Typically a proof by induction
Unvisited nodes
¥ Assume C(x) is minimal for all visited nodes
¥ Take an edge to an unvisited node u with xG
Visited nodes
the lowest C(x) (corresponds to the pop- w
operation in the priority queue) u
¥ Then, C(u) = C(v) + d(v, u) is minimal
v
¥ If there was a shorter path
¥ via visited nodes, that edge would have
been chosen
¥ Via unvisited nodes, that edge would xI
have been explored before
 29

Properties of Dijkstra’s algorithms

¥ Once you pop the goal node (on line 6), 1 function D i j k s t r a :
you are sure youÕve found the optimal path 2 C(xI ) = 0
3 Q. i n s e r t ( xI , C(xI ) )
¥ Complexity properties ≈ edges gives 4

insertions and nodes pops: 5 while Q 6= ;

6 x = Q. pop ( )
𝒪( | E | Tinsert + | V | Tpop) 7 i f x = xG
8 return SUCCESS
¥ With balanced binary heap, both 9

operations are 𝒪( | V | log | V | ) and 10 f o r u 2 U (x)

x0 = f (x, u)
| E | = 𝒪( | V |2 ) so resulting
11
12 i f no p r e v i o u s ( x0 ) o r
2
𝒪( | V | log | V | ) 13 C(x0 ) > C(x) + d(x, x0 )
14 p r e v i o u s ( x0 ) = x
¥ Worst-case bounds for fully connected 15 C(x0 ) = C(x) + d(x, x0 )
graphs maybe not that relevant; nodes are16 Q. i n s e r t ( x0 , C(x0 ) )
typically only connected to a few nodes. 17
18 return FAILURE
 30

Dijkstra optimal in length, but not in performance

A*
5.38 km path
6594 nodes visited

Depth First
26 km path
7570 nodes visited
Dijkstra
5.38 km path
11626 nodes visited
A* - efficiently finding an optimal path
 32

Keep optimality but reduce the number of visited nodes

<

Strategy:
1. Prioritize nodes according to estimated final length
2. Explore nodes in the search that have high chance to be in optimal path.
 33

Estimated final length

¥ Let C(x) be the cost to come as before
¥ Let h(x) ≥ 0 be an estimate of cost to go
to the goal; called a heuristic function
¥ The estimated total length is then

C(x) + h(x)

used in the priority queue

<
¥ Means; explore nodes that have a low
estimated final length
¥ With a proper heuristic, we will find an
optimal solution without exploring too many
nodes
 34

Estimated final length — heuristics

¥ Let C(x) be the cost to come as before
¥ Let h(x) be an estimate of cost
to go to the goal; called a heuristic
function
¥ The estimated length is then

C(x) + h(x)

used in the priority queue

¥ In the example here, the
Euclidean distance to the goal
is used as heuristic
 35

Dijkstra vs A* — very similar just a change of priority

1 function D i j k s t r a : 1 function A s t a r :
2 C(xI ) = 0 2 C(xI ) = 0
3 Q. i n s e r t ( xI , C(xI ) ) 3 Q. i n s e r t ( xI , C(xI ) + h(xI ) )
4 4
5 while Q 6= ; 5 while Q6= ;
6 x = Q. pop ( ) 6 x = Q. pop ( )
7 i f x = xG 7 i f x = xG
8 return SUCCESS 8 return SUCCESS
9 9
10 f o r u 2 U (x) 10 f o r u 2 U (x)
11 x0 = f (x, u) 11 x0 = f (x, u)
12 i f no p r e v i o u s ( x0 ) o r 12 i f no p r e v i o u s ( x0 ) o r
13 C(x0 ) > C(x) + d(x, x0 ) 13 C(x0 ) > C(x) + d(x, x0 )
14 p r e v i o u s ( x0 ) = x 14 p r e v i o u s ( x0 ) = x
15 C(x0 ) = C(x) + d(x, x0 ) 15 C(x0 ) = C(x) + d(x, x0 )
16 Q. i n s e r t ( x0 , C(x0 ) ) 16 Q. i n s e r t ( x0 , C(x0 ) + h(x0 ) )
17 17
18 return FAILURE 18 return FAILURE
 36

Does A* find the optimal path?

¥ Efficiency of A* depends on the heuristic; the better estimate of cost-to-go,
the more efficient search
¥ The heuristic helps us prioritize; do not prioritize nodes that probably is not
part of the solution
¥ The priority in Dijkstra is C(x) and C(x) + h(x) in A*
¥ Clearly, for h(x) = 0 both algorithms give the same result and explore
exactly the same search space
¥ The higher the value of cost-to-go for a node, the lower priority in the
search.

Note that no node is excluded from the search, it is just put way back in the
queue if the expected cost to go through that node is high!
 37

Does A* find the optimal path?

¥ Edge costs in black, heuristic in red
¥ A* will find path SÑG
¥ What went wrong here? Found 1 A 3
sub-optimal path.
h=6
¥ Heuristic seems to be the reason! S G

h=7 5 h=0
 38

Does A* find the optimal path?

¥ Let h*(x) be the (unknown) true cost-to-go function
¥ A heuristic that satisfies
h(x) ≤ h*(x)
is called admissible
¥ With an admissible heuristic, once the goal node is popped the optimal
solution is found.
¥ Optimality can be proven with a similar argument as for Dijkstra, not
covered now.
 39

With admissible heuristic, optimality is preserved

<

Explore nodes in the search that have high chance to be in optimal path; here
means explore nodes that, with underestimated cost-to-go, is cheaper than others
 40

A* search with manhattan heuristic

3 5 10 1 15 2 20 3 25 Start: 1 Pop 5: 16
Prio 5 Prio 7
Pop 1: 2 6 Pop 16: 17 21
Prio 5 5 Prio 7 9
2 4 9 0 14 1 19 2 24 Pop 2: 3 6 7 Pop 17: 18 21 22
Prio 5 5 5 Prio 7 9 9
G Pop 3: 4 6 7 8 Pop 18: 19 21 22 23
Prio 5 5 5 5 Prio 7 9 9 9
3 3 2 8 13 2 18 3 23 Pop 4: 6 7 8 5 Pop 19: 14 20 21 22 23 24
Prio 5 5 5 7 Prio 7 9 9 9 9 9
Pop 6: 7 8 11 5 Pop 14: Goal!
Prio 5 5 5 7
4 2 3 7 12 3 17 4 22
Pop 7: 8 11 5
Prio 5 5 7
Pop 8: 11 5
Prio 5 7
5 1 4 6 3 11 4 16 5 21
Pop 11: 5 16
Prio 7 7
S
 Node Previous Cost 41

Resulting path 1
2
1
1
0
1
¥ During A* search, two functions are 3 2 2
4 3 3
updated 5 4 4
6 1 1
¥ Previous(x) - keep track of parent 7 2 2

node 8
9
3 3

10
¥ Cost(x) - current cost to come 11 6 2
12
¥ Using Previous(x), backtracking gives 13
the resulting path 14
15
19 7

¥ 14 - 19 - 18 - 17 - 16 - 11 - 6 - 1 16
17
11
16
3
5
18 17 5
19 18 6
20 19 7
21 16 5
22 17 5
23 18 6
24 19 7
25
A closer look at heuristics —
consistent and admissible heuristics
 43

Estimated final length - example heuristic

¥ Clearly the heuristic is
important to gain efficiency
in the search
¥ In complex search problems,
this can be really difficult
¥ In a simple path planning
example, e.g., use the
Euclidean distance
h(x) = | x − xG |
¥ Heuristic trivially admissible
h(x) ≤ h*(x)
 44

Heuristics, not always so simple

¥ Euclidean distance as heuristic can
be a good choice for path planning
¥ Maze or cube-like problems
¥ Nonholonomic vehicles, e.g.,
parking maneuver of a car
¥ High-degree of freedom
problems, e.g., positioning
of a robotic arm

Figure from: “Optimisation based path planning for car parking in narrow
environments”, P. Zips et.al., Robotics and Autonomous Systems, 2016
 45

What happens here?

¥ Heuristic is admissible, so A* will find the optimal path
S-A-C-G 1 A 1
¥ Perform A*, you will see that node C
h=4
returns to the priority queue S C
3
G
during search 1
h=2 2 h=1 h=0
¥ Consistent heuristic B

h(x) − h(x′) ≤ d(x, x′) h=1

¥ Consistency means that the estimate, i.e., heuristic, becomes better and
better along the path to the goal, h(x′) ≥ h(x) + d(x, x′). Here, the
problem is the poor heuristic at C
h(C) ≤ h(A) + d(A, C)
 46

Consistency has to do with efficiency, not optimality

¥ The search will find an optimal solution, regardless if the heuristic is
consistent or not
¥ Inconsistency might lead to inefficiency, in a worst case exponential
increase in node expansions

Martelli, Alberto. "On the complexity of admissible search algorithms"

Artificial Intelligence 8.1 (1977): 1-13.
 47

Properties of consistent heuristics

¥ The Euclidean heuristic is consistent
h(x) = |x − xG | = |(x − x0 ) − (xG − x0 )| ≤ |x − x0 | + |x0 − xG | ≤
<latexit sha1_base64="Jv6KUqRTCYNdt1OvRsemqnOVnxw=">AAACVHicbZFJSwMxFMczo3WpW9Wjl2CxnaKWGS1UD4LoQY8V7AKdUjJppg3NLCYZaWn9kHoQ/CRePJhZELcHIb/3f+8l4R8nZFRI03zT9IXF3NLyymp+bX1jc6uwvdMSQcQxaeKABbzjIEEY9UlTUslIJ+QEeQ4jbWd8Hdfbj4QLGvj3chqSnoeGPnUpRlJJ/cJ4ZEwqsHQB55PjSf9mDhUZCsuVY0PlMcyhzchD0lCew0MF5bQ1lm07X4r3r6o6r1xJJwbG5AjGWab2C0WzaiYB/4KVQRFk0egXXuxBgCOP+BIzJETXMkPZmyEuKWbkKW9HgoQIj9GQdBX6yCOiN0tMeYIHShlAN+Bq+RIm6veJGfKEmHqO6vSQHInftVj8r9aNpHvWm1E/jCTxcXqRGzEoAxg7DAeUEyzZVAHCnKq3QjxCHGGp/iGfmHCeBEyhXsvg3PoyoXVStU6rJ3e14uVVZscK2AP7wAAWqINLcAsaoAkweAbvGtA07VX70Bf0XNqqa9nMLvgR+uYnAMKr1g==</latexit>
≤ |x − x0 | + h(x0 ) ≤ d(x, x0 ) + h(x0 )
¥ Consistent heuristic implies admissible (triangle equality is
necessary and sufficient): Pearl, Judea. "Heuristics: intelligent
search strategies for computer problem solving." (1984).
¥ Proof sketch: Let the path be the optimal path from node x to xG; xG = x0
then the induction step is given by: x1

h(xn ) ≤ h(xn−1 ) + d(xn , xn−1 ) ≤

<latexit sha1_base64="wJ+dMOc3vRAq7Gpq/EQgK/wQwJ0=">AAACQ3icfZDLSgMxGIUz9VbrrerSTbAIrZcyUwXdCEU3LivYC3bGkknTNjSTGZOMWIa+mxtfwJ0v4MaFIm4FM+0gasUfAh/nnJ8kxw0Ylco0H43U1PTM7Fx6PrOwuLS8kl1dq0k/FJhUsc980XCRJIxyUlVUMdIIBEGey0jd7Z/Gfv2GCEl9fqEGAXE81OW0QzFSWmplL3v52xYvQJuRaxhzxPesYQHuwHZs7MIvZZSw7Uzvavu/2DEcB3ihlc2ZRXM0cBKsBHIgmUor+2C3fRx6hCvMkJRNywyUEyGhKGZkmLFDSQKE+6hLmho58oh0olEHQ7illTbs+EIfruBI/b4RIU/KgefqpIdUT/72YvEvrxmqzpETUR6EinA8vqgTMqh8GBcK21QQrNhAA8KC6rdC3EMCYaVrz+gSrN9fnoRaqWjtF0vnB7nySVJHGmyATZAHFjgEZXAGKqAKMLgDT+AFvBr3xrPxZryPoykj2VkHP8b4+ATVLat6</latexit>
h∗ (xn−1 ) + d(xn , xn−1 ) = h∗ (xn ) xn−1
x = xn
 48

Short summary on heuristics

¥ Heuristic function h(x) estimates distance from goal state
¥ Two properties
¥ h(x) ≤ h*(x) Ñ admissibility, implies optimality of solution
¥ h(x) − h(x′) ≤ d(x, x′) Ñ consistency, efficiency
(nodes doesnÕt re-appear in search after popped)
¥ The closer h(x) is to the true distance h*(x), the better.
¥ A* the trivial heuristic h(x) = 0 becomes Dijkstra
¥ Consistency implies admissibility
¥ Heuristic that fulfills triangle inequality, e.g., Euclidean distance is
consistent
 49

Heuristics not always so easy

(x) (xg)

h(x) = | x − xg |

(x, y, θ)

h(x) = ?

Need to solve problem to get accurate heuristic

(xg, yg, θg)

 50

Best First Search

¥ Assume your heuristic is 1 function B e s t F i r s t :
very good, i.e. , close to the 2 Q. i n s e r t ( xI , h(xI ) )
3
real cost-to-go. 4 while Q6= ;
¥ Then it makes sense to 5 x = Q. pop ( )
6 i f x = xG
expand node x with lowest 7 return SUCCESS
h(x), i.e., use heuristic h(x) 8

as priority in the queue. 9 f o r u 2 U (x)

10 x0 = f (x, u)
¥ Direct forward search using 11 i f no p r e v i o u s ( x0 )
a priority queue with the 12 p r e v i o u s ( x0 ) = x
heuristic as priority. 13 Q. i n s e r t ( x0 , h(x0 ) )
14
¥ Hint: Related to the extra 15 return FAILURE
assignment for HI1
Any-time planning
 52

What about a non-admissible heuristic?

¥ What happens with a non-admissible heuristic, i.e., doesnÕt satisfy
h(x) ≤ h*(x)
¥ With a non-admissible heuristic, a solution will be found but may not be
optimal.
¥ The solution may be found faster though!
 53

Visited nodes for different inflation factors


  
hc(x) = c h(x)
  
 
 

 
   

 


   



 
 
 

  
 

  

                   
                    
                    

 54

Effects of non-admissible heuristics in map routing

L LE L L

hc(x) = c h(x)

7L
 55

ARA* - Anytime A*, basic principle

¥ Basic principle
1. Find a solution with an inflated heuristic
2. Lower inflation factor
3. Reuse previous computations and compute a new solution
4. Finish if satisfied with solution (or out of time), else go to 2
¥ Likhachev et.al. "ARA*: Anytime A* with provable bounds on sub-
optimality."
Advances in neural information processing systems, 2004.
¥ Connects to receding horizon control and replanning; this will be
returned to later in the course
Some concluding comments
 57

Forward-, backward-, and bi-drectional search

¥ Sometimes it is better to
search in a particular
direction
qI qG
¥ Backward search
¥ Forward search
¥ Bi-directional search
 58

Reading instructions
¥ ÒPlanning AlgorithmsÓ, Chapter 2 (mainly sections 2.1-2.3), S. LaValle.
¥ Want to dig a little deeper? HereÕs some extra reading for the interested É
¥ ÒARA*: Anytime A* with provable bounds on sub-optimality.Ó,
Likhachev et al. Advances in neural information processing systems,
2004.
¥ ÒPriority queues and Dijkstra's algorithmÓ, Chen, Mo, et al.. Computer
Science Department, University of Texas at Austin, 2007.
¥ Connections to dynamic optimization, optimal control, and
reinforcement learning
Bertsekas, D. (2019). ÒReinforcement learning and optimal controlÓ.
Athena Scientific.
https://www.mit.edu/~dimitrib/RLbook.html
 59

Some take-home messages

¥ How to formulate path planning as a Hand-in 1
search on a graph Discrete planning in a structured road
network
¥ Basic search algorithms for motion
planning in discrete graphs, in
particular A*
¥ The heuristic function used in A*,
and how it affects search efficiency
¥ Discrete graph search algorithms
will be directly useful for motion
planning with motion models D )
)
D
¥ There are many extensions to the D
basic A* )
 60

Graph planning with motion models

(x, y, θ) = (0,0,0)

¥ Revisit graph search

algorithms in
¥ Lecture 4 - Planning under
differential-constraints
¥ Hand-in 2 (x, y, θ) = (20,20,π)
www.liu.se