1. Real Time Motion Planning and Safe Navigation in Dynamic Environments* Kadir F. Uyanik CENG585 Fundamentals of Autonomous Robotics 14.01.2011 * Based on: Bruce J. R., Real-Time Motion Planning and Safe Navigation in Dynamic Multi-Robot Environments, PhD. Thesis, 2006 1 * Some of the slides are adapted from James Bruce’s PhD. Defense presentation.

2. Thesis Goal Enabling a multi agent system carry out navigation calculations within tight time constraints Making robots navigate robustly and operate safely without collisions 2/83

3. Outline Introduction –A classification in robotic systems –Robot soccer –Small Size League (SSL) system Navigation System for SSL Robots –Planning motions –Planning in a changing world Problem Definition Common Approaches –Grid Based –Visibility Graph –Randomized Sampling Based Planning Challenges Randomized Approaches –RRT, RRT-Connect –ERRT, ERRT-MultiConnect –ERRT vs Visibility Graphs Novelty up-to now From Kinematic Planning to Dynamic Planning –Dynamic Window Method –Dynamic Safety Search Conclusion 3/83

4. Introduction Robotic Systems Sensing LocalGlobal Agency Single- AgentMulti-agent Control CentralizedDistributedHybrid 4/83

5. Introduction Robotic Systems Sensing LocalGlobal Agency Single- AgentMulti-agent Control CentralizedDistributedHybrid 5/83

6. Introduction Robotic Systems Sensing LocalGlobal Agency Single- AgentMulti-agent Control CentralizedDistributedHybrid 6/83

7. Introduction Robotic Systems Sensing LocalGlobal Agency Single- AgentMulti-agent Control CentralizedDistributedHybrid 7/83

8. Introduction Robotic Systems Sensing LocalGlobal Agency Single- AgentMulti-agent Control CentralizedDistributedHybrid 8/83

9. Introduction Robotic Systems Sensing LocalGlobal Agency Single- AgentMulti-agent Control CentralizedDistributedHybrid 9/83

10. Introduction Robotic Systems Sensing LocalGlobal Agency Single- AgentMulti-agent Control CentralizedDistributedHybrid 10/83

11. Introduction Robotic Systems Sensing LocalGlobal Agency Single- AgentMulti-agent Control CentralizedDistributedHybrid 11/83

12. Introduction Robotic Systems Sensing LocalGlobal Agency Single- AgentMulti-agent Control CentralizedDistributedHybrid 12/83

13. Introduction Robotic Systems Sensing LocalGlobal Agency Single- AgentMulti-agent Control CentralizedDistributedHybrid 13/83

14. Introduction Robotic Systems Sensing LocalGlobal Agency Single- AgentMulti-agent Control CentralizedDistributedHybrid 14/83

15. Introduction Robotic Systems Sensing LocalGlobal Agency Single- AgentMulti-agent Control CentralizedDistributedHybrid 15/83

16. Introduction Robotic Systems Sensing LocalGlobal Agency Single- AgentMulti-agent Control CentralizedDistributedHybrid 16/83

17. Introduction Soccer Playing Robots Two main worldwide competitions/organizations: 17/83

18. Introduction Soccer Playing Robots Two main worldwide competitions/organizations: –FIRA Mirosot: Micro-Robot World Cup Soccer Tournament. Organized by Federation of International Robot-Soccer Association since 1996. 18/83

19. Introduction Soccer Playing Robots Two main worldwide competitions/organizations: –FIRA Mirosot: Micro-Robot World Cup Soccer Tournament. Organized by Federation of International Robot-Soccer Association since 1996. –Robocup: Robot World Cup, largest international robotics competition. Organized (officially) since 1997. This year in Istanbul/Turkey (June 4-10, 2011) Several categories: Soccer, rescue, @home, junior Soccer includes various leagues: humanoid, middle size, small size, standard platform, simulation 19/83

20. Introduction Small Size League Robot Soccer System 20/83

21. Introduction Small Size Soccer League 21/83

22. Navigation System for SSL Plan quickly before planned decisions become obsolete –Agents act parallel in multi-robot domains; unpredictable dynamics can arise, –Other team’s robots move very fast and world changes quickly. 22/83

23. Navigation System for SSL Plan quickly before planned decisions become obsolete –Agents act parallel in multi-robot domains; unpredictable dynamics can arise, –Other team’s robots move very fast and world changes quickly. Navigate robustly, don’t crash other robots, stay in the field 23/83

24. Navigation System for SSL Planning motions Motion Planning is about finding trajectories to satisfy a goal criteria starting from an initial-configuration to an end- configuration. 24/83

25. Navigation System for SSL Planning motions Motion Planning is about finding trajectories to satisfy a goal criteria starting from an initial-configuration to an end- configuration. Two main requirements are –Model of the environment or the world state is known to some degree –Model of the results of actions that create certain effect in the world 25/83

26. Navigation System for SSL Planning motions Motion Planning is about finding trajectories to satisfy a goal criteria starting from an initial-configuration to an end- configuration. Two main requirements are –Model of the environment or the world state is known to some degree –Model of the results of actions that create certain effect in the world This knowledge enables robot, in a way, to simulate it’s actions in mind and predict the output w/o actually executing them. 26/83

27. Navigation System for SSL Planning in a changing world It is a key issue in a multi-agent systems 27/83

28. Navigation System for SSL Planning in a changing world It is a key issue in a multi-agent systems Agent dynamics are the limitations due to the kinodynamic constraints of the robots 28/83

29. Navigation System for SSL Planning in a changing world It is a key issue in a multi-agent systems Agent dynamics are the limitations due to the kinodynamic constraints of the robots Domain dynamics includes environmental changes (due to other robots or physical laws) and changes in goal specification (due to higher level task oriented behaviors) 29/83

30. Problem Definition A: agent q: robot configuration C free: obstacle free configuration space T(s): continuous function, mapping s ͼ [0,1] to a configuration in C. R j (t): area covered by all robots except j S’(t): boolean safety function (true if no two robots overlap) Given: A, C free, q init, q goal ; Find: a path T(s) which is valid, feasible, and a solution. For a safe navigation: 30/83

31. Common Approaches A generic motion (re)planning algorithm: 1.Map initial and goal locations to C-space representation 2.Update environment model with new information 3.Update C-space representation graph, or roadmap 4.Search roadmap for a path between initial and goal locations 5.Extract path vertices and edges as plan 31/83

32. Common Approaches A generic motion (re)planning algorithm: 1.Map initial and goal locations to C-space representation 2.Update environment model with new information 3.Update C-space representation graph, or roadmap 4.Search roadmap for a path between initial and goal locations 5.Extract path vertices and edges as plan 32/83

33. Common Approaches A generic motion (re)planning algorithm: 1.Map initial and goal locations to C-space representation 2.Update environment model with new information 3.Update C-space representation graph, or roadmap 4.Search roadmap for a path between initial and goal locations 5.Extract path vertices and edges as plan 33/83

34. Common Approaches A generic motion (re)planning algorithm: 1.Map initial and goal locations to C-space representation 2.Update environment model with new information 3.Update C-space representation graph, or roadmap 4.Search roadmap for a path between initial and goal locations 5.Extract path vertices and edges as plan 34/83

35. Common Approaches A generic motion (re)planning algorithm: 1.Map initial and goal locations to C-space representation 2.Update environment model with new information 3.Update C-space representation graph, or roadmap 4.Search roadmap for a path between initial and goal locations 5.Extract path vertices and edges as plan 35/83

36. Common Approaches A generic motion (re)planning algorithm: 1.Map initial and goal locations to C-space representation 2.Update environment model with new information 3.Update C-space representation graph, or roadmap 4.Search roadmap for a path between initial and goal locations 5.Extract path vertices and edges as plan 36/83

37. Common Approaches A generic motion (re)planning algorithm: 1.Map initial and goal locations to C-space representation 2.Update environment model with new information 3.Update C-space representation graph, or roadmap 4.Search roadmap for a path between initial and goal locations 5.Extract path vertices and edges as plan Replanning can be done in two ways –Unconditional replanning: replan each time before deciding on an action –Conditional replanning: Plan once, monitor the environment and execution of plan to determine if it succeeds or fails. If fails, replan and continue execution. 37/83

38. Common Approaches The planners used in SSL-like domains are based on: –Grids Create a grid overlay of vertices covering the environment Connect grid neighbors with edges if free Search for shortest (least cost) path Non-optimal and complete –Visibility Graph Place vertices at critical points around each obstacle Add edges between every pair of critical points if free Optimal and complete –Randomized No need for grids or list of obstacle points; discover C free through collision checks Sample environment randomly to model C-space Search until tree reaches a goal Non-optimal and probabilistically complete 38/83

39. Planning Challenges It is all about Time vs. Problem complexity Dashed and straight curved lines indicates the corresponding hypothetical algorithm performance 39/83

40. Outline Introduction –A classification in robotic systems –Robot soccer –Small Size League (SSL) system Navigation System for SSL Robots –Planning motions –Planning in a changing world Problem Definition Common Approaches –Grid Based –Visibility Graph –Randomized Sampling Based Planning Challenges Randomized Approaches –RRT, RRT-Connect –ERRT, ERRT-MultiConnect –ERRT vs Visibility Graphs Novelty up-to now From Kinematic Planning to Dynamic Planning –Dynamic Window Method –Dynamic Safety Search Conclusion 40/83

41. Outline Introduction –A classification in robotic systems –Robot soccer –Small Size League (SSL) system Navigation System for SSL Robots –Planning motions –Planning in a changing world Problem Definition Common Approaches –Grid Based –Visibility Graph –Randomized Sampling Based Planning Challenges Randomized Approaches –RRT, RRT-Connect –ERRT, ERRT-MultiConnect –ERRT vs Visibility Graphs Novelty up-to now From Kinematic Planning to Dynamic Planning –Dynamic Window Method –Dynamic Safety Search Conclusion 41/83

42. Randomized Approaches Rapidly exploring random trees(RRT) 1.Start with the initial state as the root of a tree 2.Pick a random state in anywhere or in the direction of the target 3.Find the closest node in the current tree 4.Extend that node toward the target if possible 42/83

43. Randomized Approaches Rapidly exploring random trees(RRT) 1.Start with the initial state as the root of a tree 2.Pick a random state in anywhere or in the direction of the target 3.Find the closest node in the current tree 4.Extend that node toward the target if possible 43/83

44. Randomized Approaches Rapidly exploring random trees(RRT) 1.Start with the initial state as the root of a tree 2.Pick a random state in anywhere or in the direction of the target 3.Find the closest node in the current tree 4.Extend that node toward the target if possible 44/83

45. Randomized Approaches Rapidly exploring random trees(RRT) 1.Start with the initial state as the root of a tree 2.Pick a random state in anywhere or in the direction of the target 3.Find the closest node in the current tree 4.Extend that node toward the target if possible 45/83

46. Randomized Approaches Rapidly exploring random trees(RRT) 46/83

47. Randomized Approaches Rapidly exploring random trees(RRT) Don't add extensions which would hit obstacles Resulting tree contains only valid paths 47/83

48. Randomized Approaches Rapidly exploring random trees(RRT) Search until goal is reached Backtrack the tree 48/83

49. Randomized Approaches Rapidly exploring random trees(RRT) 49/83

50. Randomized Approaches Execution Extended RRT (ERRT) Waypoints –Idea: Previously successful plans can guide new search –Biases can be encoded in the target distribution The waypoint cache –Whenever a plan is found, store nodes in a fixed-size bin with random replacement –During random target point selection, sometimes choose a waypoint from the cache 50/83

51. Randomized Approaches Execution Extended RRT (ERRT) Waypoints –Idea: Previously successful plans can guide new search –Biases can be encoded in the target distribution The waypoint cache –Whenever a plan is found, store nodes in a fixed-size bin with random replacement –During random target point selection, sometimes choose a waypoint from the cache 51/83

52. Randomized Approaches Execution Extended RRT (ERRT) 52/83

53. Randomized Approaches Execution Extended RRT (ERRT) The effect of waypoints. (waypoints = 50, P[waypoint] = 0.4) 53/83

54. Randomized Approaches Execution Extended RRT (ERRT) Increase the nearest neighbor search via KD-tree. 54/83

55. Randomized Approaches Execution Extended RRT (ERRT) Varying the waypoint cache sampling probability: 55/83

56. Randomized Approaches RRT-Connect RRT-Connect (Kuffner and LaValle 2000) –A random target is chosen as with the RRT planner –Repeats the extension Until the target point is reached or The extension fails (obstacle hitting) –Grow two trees; from initial and goal configurations First, one tree extends toward the randomly sampled point q Then, second tree extends toward the same point q (CONNECT) Trees swaps roles (extending/connecting) at each iteration RRT grows in a fixed rate, but RRT-connect has much higher variance due to the repeated extensions 56/83

57. Randomized Approaches Bidirectional Multi-Bridge ERRT Similar to the RRT-connect algorithm. –Bidirectional –Both trees extends towards to the same point Connect multiple points between trees before planning is terminated (continue connecting although a path is already found). Use A* to find shortest path in the graph 57/83

58. Randomized Approaches Bidirectional Multi-Bridge ERRT The effect of multiple connection points in ERRT on plan length 58/83

59. Randomized Approaches ERRT further improvement Path smoothing using the leader-follower approach 59/83

60. Randomized Approaches ERRT vs Visibility Graph in 2D 60/83

61. Randomized Approaches Visibility Graph Introduced in the Shakey project at SRI in the late 60s. It can produce shortest paths in 2-D configuration spaces. 61/83

62. Randomized Approaches Visibility Graph Simple Algorithm 62/83

63. Randomized Approaches ERRT vs VG Success Probability Worst domain is above 97% success (Ring) 63/83

64. Randomized Approaches ERRT vs Visibility Graph in 2D 64/83

65. Randomized Approaches ERRT vs VG Success Probability Worst domain is only 28% longer on average (Zigzag) 65/83

66. Randomized Approaches ERRT vs Visibility Graph in 2D 66/83

67. Randomized Approaches ERRT vs VG Average Execution Time Worst cases for ERRT: 2.2ms, 12x slower than Vis. Graph (Ring) Worst cases for Vis. Graph: 12.2ms, 28x slower than ERRT (Square) 67/83

68. Randomized Approaches ERRT vs Visibility Graph in 2D 68/83

69. Novelty up-to now The waypoint cache –Extends RRT algorithm of LaValle and Kuffner [1] –Solution time decreases Multi-Connect –Improvement on RRT-Connect [2] –Decreases the path length [1] Steven M. LaValle and Jr. James J. Kuffner. Randomized kinodynamic planning. In International Journal of Robotics Research, Vol. 20, No. 5, pages 378–400, May 2001. [2] Jr. James J. Kuffner and Steven M. LaValle. RRT-Connect: An efficient approach to single-query path planning. In Proceedings of the IEEE International Conference on Robotics and Automation, 2000. 69/83

70. From Kinematic Planning to Dynamic Planning Dynamic Window Method Safe single agent navigation (Fox et al. [3]) –Checks if an action command is safe when applied for one control cycle In order to find a safe command: –Creates a grid over the acceleration window –Evaluates each grid cell with a combination of a safety test an evaluation metric –The safety test checks if a velocity command would hit an obstacle –If command is safe, evaluate action based on heuristics for reaching a desired target. Includes both safety and motion control in a single algorithm [3] Dieter Fox, Wolfram Burgard, and Sebastian Thrun. The dynamic window approach to collision avoidance. IEEE Robotics and Automation Magazine, 4, March 1997. 70/83

71. From Kinematic Planning to Dynamic Planning Dynamic Window Method 71/83

72. From Kinematic Planning to Dynamic Planning Dynamic Safety Search Trapezoidal velocity control is done based on the bounded velocity and acceleration profile Allowable velocities are obtained via traction circle/ellipse –D : max deceleration –F : max acceleration Traction circle and ellipse 72/83

73. From Kinematic Planning to Dynamic Planning Dynamic Safety Search 1.Each robot is given a desired target action 2.Each robot starts by thinking it will stop 3.For each robot, test is done if a better action A can be chosen, after which a robot can stop 1.while avoiding static obstacles 2.while avoiding the other robots based on their current action 73/83

74. From Kinematic Planning to Dynamic Planning Dynamic Safety Search Since it is always tested if a robot can stop after action A –It’s gonna be safe starting in the next control cycle –Safety will be maintained continuously (by induction) q: robot position C free: obstacle free configuration space R j (t): radius of the robot S(k,t 0 ): boolean safety function (true if no two robots overlap) 74/83

75. From Kinematic Planning to Dynamic Planning Dynamic Safety Search Two robots in 2D Trace velocity-time graph through algorithm 75/83

76. From Kinematic Planning to Dynamic Planning Dynamic Safety Search Safety is preserved at each step, and thus maintained overall 76/83

77. From Kinematic Planning to Dynamic Planning Dynamic Safety Search - Results Left-right traversal task experiment with DSS and ERRT 77/83

78. From Kinematic Planning to Dynamic Planning Dynamic Safety Search - Results Left-right traversal task experiment with DSS and ERRT 78/83

79. From Kinematic Planning to Dynamic Planning Dynamic Safety Search - Results Average execution time of safety search for each agent, as the total number of agents increases. For left-right traversal task. 79/83

80. From Kinematic Planning to Dynamic Planning Dynamic Safety Search - Results 80/83

81. From Kinematic Planning to Dynamic Planning Dynamic Safety Search - Conclusion Extends the Dynamic Window Approach to multiple cooperating agents Can guarantee safety for robots which operate without any error Does not guarantee completeness Allows agents to change goals every control cycle Scales at O(n 2 ) with the number of agents Can be used to create an intuitive safe tele-operation system 81/83

82. Conclusion 82/83

83. Conclusion ERRT motion planning algorithm –The waypoint cache –Multi-connect Dynamic Safety Search –Cooperative multi-robot safety 83/83

84. Appendix 84/83

85. Randomized Approaches Rapidly exploring random trees(RRT) Extend, Distance, and RandomState are domain-specific functions. 85/83

86. Randomized Approaches Rapidly exploring random trees(RRT) 86/83

87. Randomized Approaches Execution Extended RRT (ERRT) The waypoint cache stores the previously obtained points by replacing the already cached points randomly. 87/83

88. Randomized Approaches ERRT vs Visibility Graph in 2D 88/83