Basic Motion Planning Algorithms

Transcription

1 Basic Motion Planning Algorithms Sohaib Younis Intelligent Robotics 7 January,

2 Outline Introduction Environment Map Dijkstra's algorithm A* algorithm Summary 2

3 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

4 Introduction Motivation: Driver navigation assistance Unmanned Vehicles Games and Simulations 4

5 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

6 Environment Map 6

7 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

8 Tree Structure Open Nodes Expanded Nodes Node Structure g value (distance) coordinate 8

9 9

10 Open List: g (x,y) 0 (0,0) 10

11 Open List: g (x,y) 1 (0,1) 1 (1,0) 11

12 Open List: g (x,y) 2 (0,2) 2 (1,1) 12

13 Open List: g (x,y) 3 (0,3) 3 (1,2) 13

14 Open List: g (x,y) 3 (1,2) 4 (0,4) 4 (1,3) 14

15 Open List: g (x,y) 4 (0,4) 4 (1,3) 4 (2,2) 15

16 16

17 17

18 18

19 19

20 20

21 21

22 22

23 23

24 24

25 25

26 26

27 27

28 28

29 29

30 30

31 31

32 32

33 33

34 34

35 35

36 36

37 37

38 38

39 39

40 40

41 41

42 42

43 43

44 44

45 45

46 46

47 47

48 48

49 49

50 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

51 Applications: Internet and telephone routing Traffic Planning 51

52 A* Algorithm Background: Developed by Hart, Nilsson and Raphael at Stanford Research Institute in It is an extension of Dijkstra a algorithm enhancing its performance by using heuristics. 52

53 A* Algorithm 53

54 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

55 A* Algorithm Heuristic Function: 55

56 A* Algorithm Tree Structure Open Nodes Expanded Nodes Node Structure f value ( = g + h(x,y) ) coordinate 56

57 A* Algorithm Open List: f, g (x,y) 8, 0 (0,0) 57

58 A* Algorithm Open List: f, g (x,y) 8, 1 (0,1) 8, 1 (1,0) 58

59 A* Algorithm Open List: f, g (x,y) 8, 2 (1,1) 10, 2 (0,2) 59

60 A* Algorithm Open List: f, g (x,y) 10, 2 (0,2) 10, 3 (1,2) 60

61 A* Algorithm 61

62 A* Algorithm 62

63 A* Algorithm 63

64 A* Algorithm 64

65 A* Algorithm 65

66 A* Algorithm 66

67 A* Algorithm 67

68 A* Algorithm 68

69 A* Algorithm 69

70 A* Algorithm 70

71 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

72 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

73 A* Algorithm Applications: Mobile Robotics Navigation Systems Network Routing Game Development 73

74 Summary Motion Planning using search algorithms Shortest path by Dijkstra s algorithm Efficient path planning using heuristics Applications of A* algorithm 74

75 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

76 Thank you for your attention! Any Questions? 76