Basic Motion Planning Algorithms Sohaib Younis Intelligent Robotics 7 January, 2013 1
Outline Introduction Environment Map Dijkstra's algorithm A* algorithm Summary 2
Introduction Definition: Motion Planning is aimed at enabling with capabilities of automatically deciding and executing a sequence of motion in order to achieve a task without collision with other objects in a given environment. 3
Introduction Motivation: Driver navigation assistance Unmanned Vehicles Games and Simulations 4
Environment Map Configuration Space: The Configuration Space here is assumed to be a grid, where empty cells represent free space and the filled cells are the obstacles. 5
Environment Map 6
Background: Developed by Edsger Wybe Dijkstra in 1956 It is a graph search algorithm for single source shortest path problem by producing a shortest path tree. 7
Tree Structure Open Nodes Expanded Nodes Node Structure g value (distance) coordinate 8
9
Open List: g (x,y) 0 (0,0) 10
Open List: g (x,y) 1 (0,1) 1 (1,0) 11
Open List: g (x,y) 2 (0,2) 2 (1,1) 12
Open List: g (x,y) 3 (0,3) 3 (1,2) 13
Open List: g (x,y) 3 (1,2) 4 (0,4) 4 (1,3) 14
Open List: g (x,y) 4 (0,4) 4 (1,3) 4 (2,2) 15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
Pseudo code: Initialize g = 0 at source While current node is not goal{ Select the node with lowest g value and expand Increment g value at each expansion} 50
Applications: Internet and telephone routing Traffic Planning 51
A* Algorithm Background: Developed by Hart, Nilsson and Raphael at Stanford Research Institute in 1968. It is an extension of Dijkstra a algorithm enhancing its performance by using heuristics. 52
A* Algorithm 53
A* Algorithm Heuristic Function: A function that estimates the distance to the goal (e.g Manhattan, Euclidean) A heuristic is admissible if it does not overestimates the distance from goal. 0 h(x,y) distance to goal at (x,y) 54
A* Algorithm Heuristic Function: 55
A* Algorithm Tree Structure Open Nodes Expanded Nodes Node Structure f value ( = g + h(x,y) ) coordinate 56
A* Algorithm Open List: f, g (x,y) 8, 0 (0,0) 57
A* Algorithm Open List: f, g (x,y) 8, 1 (0,1) 8, 1 (1,0) 58
A* Algorithm Open List: f, g (x,y) 8, 2 (1,1) 10, 2 (0,2) 59
A* Algorithm Open List: f, g (x,y) 10, 2 (0,2) 10, 3 (1,2) 60
A* Algorithm 61
A* Algorithm 62
A* Algorithm 63
A* Algorithm 64
A* Algorithm 65
A* Algorithm 66
A* Algorithm 67
A* Algorithm 68
A* Algorithm 69
A* Algorithm 70
A* Algorithm Pseudo code (1): Construct an admissible heuristic function Initialize g = 0 at source While current node is not goal{ Select the node with lowest f ( = g + h(x,y) ) value and expand Increment g value at each expansion} 71
A* Algorithm Pseudo code (2): Initialize g = 0 at source While current node is not goal{ Calculate distance to goal ( h(x,y) ) Select the node with lowest f ( = g + h(x,y) ) value and expand Increment g value at each expansion} 72
A* Algorithm Applications: Mobile Robotics Navigation Systems Network Routing Game Development 73
Summary Motion Planning using search algorithms Shortest path by Dijkstra s algorithm Efficient path planning using heuristics Applications of A* algorithm 74
Summary Literature: A note on two problems in connexion with graphs Dijkstra, E. W. (1959) A Formal Basis for the Heuristic Determination of Minimum Cost Paths Hart, P. E. & Nilsson, N. J. & Raphael, B. (1968) 75
Thank you for your attention! Any Questions? 76