1

Real Time Motion Planning and Safe Navigation in Dynamic Environments*

Kadir F. Uyanik

CENG585 Fundamentals of Autonomous Robotics14.01.2011

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

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

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

IntroductionRobotic Systems

Sensing

Local Global

Agency

Single- Agent Multi-agent

Control

Centralized Distributed Hybrid

4/83

IntroductionRobotic Systems

Sensing

Local Global

Agency

Single- Agent Multi-agent

Control

Centralized Distributed Hybrid

5/83

IntroductionRobotic Systems

Sensing

Local Global

Agency

Single- Agent Multi-agent

Control

Centralized Distributed Hybrid

6/83

IntroductionRobotic Systems

Sensing

Local Global

Agency

Single- Agent Multi-agent

Control

Centralized Distributed Hybrid

7/83

IntroductionRobotic Systems

Sensing

Local Global

Agency

Single- Agent Multi-agent

Control

Centralized Distributed Hybrid

8/83

IntroductionRobotic Systems

Sensing

Local Global

Agency

Single- Agent Multi-agent

Control

Centralized Distributed Hybrid

9/83

IntroductionRobotic Systems

Sensing

Local Global

Agency

Single- Agent Multi-agent

Control

Centralized Distributed Hybrid

10/83

Introduction

Robotic Systems

Sensing

Local Global

Agency

Single- Agent

Multi-agent

Control

Centralized

Distributed

Hybrid

11/83

Introduction

Robotic Systems

Sensing

Local Global

Agency

Single- Agent

Multi-agent

Control

Centralized

Distributed

Hybrid

12/83

Introduction

Robotic Systems

Sensing

Local Global

Agency

Single- Agent

Multi-agent

Control

Centralized

Distributed

Hybrid

13/83

Introduction

Robotic Systems

Sensing

Local Global

Agency

Single- Agent

Multi-agent

Control

Centralized

Distributed

Hybrid

14/83

Introduction

Robotic Systems

Sensing

Local Global

Agency

Single- Agent

Multi-agent

Control

Centralized

Distributed

Hybrid

15/83

Introduction

Robotic Systems

Sensing

Local Global

Agency

Single- Agent

Multi-agent

Control

Centralized

Distributed

Hybrid

16/83

Introduction

Soccer Playing Robots

Two main worldwide competitions/organizations:

17/83

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

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, juniorSoccer includes various leagues: humanoid, middle size, small size, standard platform, simulation

19/83

Introduction

Small Size League Robot Soccer System

20/83

Introduction

Small Size Soccer League

21/83

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

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

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

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

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

Navigation System for SSL

Planning in a changing world • It is a key issue in a multi-agent systems

27/83

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

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

Problem DefinitionA : agentq : robot configurationCfree : obstacle free configuration space

T(s) : continuous function, mapping s ͼ [0,1] to a configuration in C.Rj(t) : area covered by all robots except j

S’(t) : boolean safety function (true if no two robots overlap)

Given : A, Cfree , qinit , qgoal ;

Find : a path T(s) which is valid, feasible, and a solution.For a safe navigation:

30/83

Common Approaches• A generic motion (re)planning algorithm:

1. Map initial and goal locations to C-space representation2. Update environment model with new information3. Update C-space representation graph, or roadmap4. Search roadmap for a path between initial and goal locations5. Extract path vertices and edges as plan

31/83

Common Approaches• A generic motion (re)planning algorithm:

1. Map initial and goal locations to C-space representation2. Update environment model with new information3. Update C-space representation graph, or roadmap4. Search roadmap for a path between initial and goal locations5. Extract path vertices and edges as plan

32/83

Common Approaches• A generic motion (re)planning algorithm:

1. Map initial and goal locations to C-space representation2. Update environment model with new information3. Update C-space representation graph, or roadmap4. Search roadmap for a path between initial and goal locations5. Extract path vertices and edges as plan

33/83

Common Approaches• A generic motion (re)planning algorithm:

1. Map initial and goal locations to C-space representation2. Update environment model with new information3. Update C-space representation graph, or roadmap4. Search roadmap for a path between initial and goal locations5. Extract path vertices and edges as plan

34/83

Common Approaches• A generic motion (re)planning algorithm:

1. Map initial and goal locations to C-space representation2. Update environment model with new information3. Update C-space representation graph, or roadmap4. Search roadmap for a path between initial and goal locations5. Extract path vertices and edges as plan

35/83

Common Approaches• A generic motion (re)planning algorithm:

1. Map initial and goal locations to C-space representation2. Update environment model with new information3. Update C-space representation graph, or roadmap4. Search roadmap for a path between initial and goal locations5. Extract path vertices and edges as plan

36/83

Common Approaches• A generic motion (re)planning algorithm:

1. Map initial and goal locations to C-space representation2. Update environment model with new information3. Update C-space representation graph, or roadmap4. Search roadmap for a path between initial and goal locations5. 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

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 Cfree through collision checks• Sample environment randomly to model C-space• Search until tree reaches a goal• Non-optimal and probabilistically complete

38/83

Planning Challenges

It is all about Time vs. Problem complexity

Dashed and straight curved lines indicates the corresponding hypothetical algorithm performance

39/83

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

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

Randomized Approaches

Rapidly exploring random trees(RRT)1. Start with the initial state as the root of a tree2. Pick a random state in anywhere or in the direction of the target3. Find the closest node in the current tree4. Extend that node toward the target if possible

42/83

Randomized Approaches

Rapidly exploring random trees(RRT)1. Start with the initial state as the root of a tree2. Pick a random state in anywhere or in the direction of the target3. Find the closest node in the current tree4. Extend that node toward the target if possible

43/83

Randomized Approaches

Rapidly exploring random trees(RRT)1. Start with the initial state as the root of a tree2. Pick a random state in anywhere or in the direction of the target3. Find the closest node in the current tree4. Extend that node toward the target if possible

44/83

Randomized Approaches

Rapidly exploring random trees(RRT)1. Start with the initial state as the root of a tree2. Pick a random state in anywhere or in the direction of the target3. Find the closest node in the current tree4. Extend that node toward the target if possible

45/83

Randomized Approaches

Rapidly exploring random trees(RRT)

46/83

Randomized Approaches

Rapidly exploring random trees(RRT)• Don't add extensions which would hit obstacles• Resulting tree contains only valid paths

47/83

Randomized Approaches

Rapidly exploring random trees(RRT)• Search until goal is reached• Backtrack the tree

48/83

Randomized Approaches

Rapidly exploring random trees(RRT)

49/83

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

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

Randomized Approaches

Execution Extended RRT (ERRT)

52/83

Randomized Approaches

Execution Extended RRT (ERRT)

The effect of waypoints. (waypoints = 50, P[waypoint] = 0.4)

53/83

Randomized Approaches

Execution Extended RRT (ERRT)• Increase the nearest neighbor search via KD-tree.

54/83

Randomized Approaches

Execution Extended RRT (ERRT)• Varying the waypoint cache sampling probability:

55/83

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

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

Randomized Approaches

Bidirectional Multi-Bridge ERRT

The effect of multiple connection points in ERRT on plan length

58/83

Randomized Approaches

ERRT further improvement

Path smoothing using the leader-follower approach

59/83

Randomized Approaches

ERRT vs Visibility Graph in 2D

60/83

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

Randomized Approaches

Visibility GraphSimple Algorithm

62/83

Randomized Approaches

ERRT vs VG Success Probability

Worst domain is above 97% success (Ring)

63/83

Randomized Approaches

ERRT vs Visibility Graph in 2D

64/83

Randomized Approaches

ERRT vs VG Success Probability

Worst domain is only 28% longer on average (Zigzag)

65/83

Randomized Approaches

ERRT vs Visibility Graph in 2D

66/83

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

Randomized Approaches

ERRT vs Visibility Graph in 2D

68/83

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

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

From Kinematic Planning to Dynamic Planning

Dynamic Window Method

71/83

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 ellipse72/83

From Kinematic Planning to Dynamic Planning

Dynamic Safety Search1. Each robot is given a desired target action2. Each robot starts by thinking it will stop3. For each robot, test is done if a better action A can be

chosen, after which a robot can stop1. while avoiding static obstacles2. while avoiding the other robots based on their current action

73/83

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 positionCfree : obstacle free configuration spaceRj(t) : radius of the robotS(k,t0) : boolean safety function (true if no two robots overlap)

74/83

From Kinematic Planning to Dynamic Planning

Dynamic Safety Search• Two robots in 2D• Trace velocity-time graph through algorithm

75/83

From Kinematic Planning to Dynamic Planning

Dynamic Safety Search• Safety is preserved at each step, and thus maintained

overall

76/83

From Kinematic Planning to Dynamic Planning

Dynamic Safety Search - Results

Left-right traversal task experiment with DSS and ERRT77/83

From Kinematic Planning to Dynamic Planning

Dynamic Safety Search - Results

Left-right traversal task experiment with DSS and ERRT78/83

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

From Kinematic Planning to Dynamic Planning

Dynamic Safety Search - Results

80/83

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(n2) with the number of agents• Can be used to create an intuitive safe tele-operation system

81/83

Conclusion

82/83

Conclusion

• ERRT motion planning algorithm– The waypoint cache– Multi-connect

• Dynamic Safety Search– Cooperative multi-robot safety

83/83

Appendix

84/83

Randomized Approaches

Rapidly exploring random trees(RRT)

Extend, Distance, and RandomState are domain-specific functions.

85/83

Randomized Approaches

Rapidly exploring random trees(RRT)

86/83

Randomized Approaches

Execution Extended RRT (ERRT)• The waypoint cache stores the previously obtained points by

replacing the already cached points randomly.

87/83

Randomized Approaches

ERRT vs Visibility Graph in 2D

88/83