![Page 1: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/1.jpg)
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.
![Page 2: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/2.jpg)
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
![Page 3: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/3.jpg)
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
![Page 4: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/4.jpg)
IntroductionRobotic Systems
Sensing
Local Global
Agency
Single- Agent Multi-agent
Control
Centralized Distributed Hybrid
4/83
![Page 5: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/5.jpg)
IntroductionRobotic Systems
Sensing
Local Global
Agency
Single- Agent Multi-agent
Control
Centralized Distributed Hybrid
5/83
![Page 6: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/6.jpg)
IntroductionRobotic Systems
Sensing
Local Global
Agency
Single- Agent Multi-agent
Control
Centralized Distributed Hybrid
6/83
![Page 7: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/7.jpg)
IntroductionRobotic Systems
Sensing
Local Global
Agency
Single- Agent Multi-agent
Control
Centralized Distributed Hybrid
7/83
![Page 8: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/8.jpg)
IntroductionRobotic Systems
Sensing
Local Global
Agency
Single- Agent Multi-agent
Control
Centralized Distributed Hybrid
8/83
![Page 9: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/9.jpg)
IntroductionRobotic Systems
Sensing
Local Global
Agency
Single- Agent Multi-agent
Control
Centralized Distributed Hybrid
9/83
![Page 10: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/10.jpg)
IntroductionRobotic Systems
Sensing
Local Global
Agency
Single- Agent Multi-agent
Control
Centralized Distributed Hybrid
10/83
![Page 11: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/11.jpg)
Introduction
Robotic Systems
Sensing
Local Global
Agency
Single- Agent
Multi-agent
Control
Centralized
Distributed
Hybrid
11/83
![Page 12: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/12.jpg)
Introduction
Robotic Systems
Sensing
Local Global
Agency
Single- Agent
Multi-agent
Control
Centralized
Distributed
Hybrid
12/83
![Page 13: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/13.jpg)
Introduction
Robotic Systems
Sensing
Local Global
Agency
Single- Agent
Multi-agent
Control
Centralized
Distributed
Hybrid
13/83
![Page 14: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/14.jpg)
Introduction
Robotic Systems
Sensing
Local Global
Agency
Single- Agent
Multi-agent
Control
Centralized
Distributed
Hybrid
14/83
![Page 15: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/15.jpg)
Introduction
Robotic Systems
Sensing
Local Global
Agency
Single- Agent
Multi-agent
Control
Centralized
Distributed
Hybrid
15/83
![Page 16: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/16.jpg)
Introduction
Robotic Systems
Sensing
Local Global
Agency
Single- Agent
Multi-agent
Control
Centralized
Distributed
Hybrid
16/83
![Page 17: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/17.jpg)
Introduction
Soccer Playing Robots
Two main worldwide competitions/organizations:
17/83
![Page 18: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/18.jpg)
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
![Page 19: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/19.jpg)
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
![Page 20: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/20.jpg)
Introduction
Small Size League Robot Soccer System
20/83
![Page 21: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/21.jpg)
Introduction
Small Size Soccer League
21/83
![Page 22: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/22.jpg)
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
![Page 23: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/23.jpg)
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
![Page 24: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/24.jpg)
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
![Page 25: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/25.jpg)
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
![Page 26: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/26.jpg)
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
![Page 27: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/27.jpg)
Navigation System for SSL
Planning in a changing world • It is a key issue in a multi-agent systems
27/83
![Page 28: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/28.jpg)
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
![Page 29: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/29.jpg)
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
![Page 30: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/30.jpg)
Problem DefinitionA : agentq : robot configurationCfree : obstacle free configuration spaceT(s) : continuous function, mapping s ͼ [0,1] to a configuration in C.Rj(t) : area covered by all robots except jS’(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
![Page 31: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/31.jpg)
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
![Page 32: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/32.jpg)
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
![Page 33: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/33.jpg)
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
![Page 34: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/34.jpg)
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
![Page 35: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/35.jpg)
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
![Page 36: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/36.jpg)
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
![Page 37: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/37.jpg)
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
![Page 38: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/38.jpg)
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
![Page 39: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/39.jpg)
Planning Challenges
It is all about Time vs. Problem complexity
Dashed and straight curved lines indicates the corresponding hypothetical algorithm performance
39/83
![Page 40: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/40.jpg)
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
![Page 41: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/41.jpg)
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
![Page 42: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/42.jpg)
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
![Page 43: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/43.jpg)
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
![Page 44: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/44.jpg)
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
![Page 45: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/45.jpg)
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
![Page 46: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/46.jpg)
Randomized Approaches
Rapidly exploring random trees(RRT)
46/83
![Page 47: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/47.jpg)
Randomized Approaches
Rapidly exploring random trees(RRT)• Don't add extensions which would hit obstacles• Resulting tree contains only valid paths
47/83
![Page 48: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/48.jpg)
Randomized Approaches
Rapidly exploring random trees(RRT)• Search until goal is reached• Backtrack the tree
48/83
![Page 49: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/49.jpg)
Randomized Approaches
Rapidly exploring random trees(RRT)
49/83
![Page 50: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/50.jpg)
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
![Page 51: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/51.jpg)
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
![Page 52: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/52.jpg)
Randomized Approaches
Execution Extended RRT (ERRT)
52/83
![Page 53: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/53.jpg)
Randomized Approaches
Execution Extended RRT (ERRT)
The effect of waypoints. (waypoints = 50, P[waypoint] = 0.4)
53/83
![Page 54: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/54.jpg)
Randomized Approaches
Execution Extended RRT (ERRT)• Increase the nearest neighbor search via KD-tree.
54/83
![Page 55: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/55.jpg)
Randomized Approaches
Execution Extended RRT (ERRT)• Varying the waypoint cache sampling probability:
55/83
![Page 56: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/56.jpg)
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
![Page 57: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/57.jpg)
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
![Page 58: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/58.jpg)
Randomized Approaches
Bidirectional Multi-Bridge ERRT
The effect of multiple connection points in ERRT on plan length
58/83
![Page 59: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/59.jpg)
Randomized Approaches
ERRT further improvement
Path smoothing using the leader-follower approach
59/83
![Page 60: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/60.jpg)
Randomized Approaches
ERRT vs Visibility Graph in 2D
60/83
![Page 61: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/61.jpg)
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
![Page 62: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/62.jpg)
Randomized Approaches
Visibility GraphSimple Algorithm
62/83
![Page 63: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/63.jpg)
Randomized Approaches
ERRT vs VG Success Probability
Worst domain is above 97% success (Ring)
63/83
![Page 64: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/64.jpg)
Randomized Approaches
ERRT vs Visibility Graph in 2D
64/83
![Page 65: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/65.jpg)
Randomized Approaches
ERRT vs VG Success Probability
Worst domain is only 28% longer on average (Zigzag)
65/83
![Page 66: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/66.jpg)
Randomized Approaches
ERRT vs Visibility Graph in 2D
66/83
![Page 67: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/67.jpg)
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
![Page 68: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/68.jpg)
Randomized Approaches
ERRT vs Visibility Graph in 2D
68/83
![Page 69: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/69.jpg)
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
![Page 70: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/70.jpg)
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
![Page 71: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/71.jpg)
From Kinematic Planning to Dynamic Planning
Dynamic Window Method
71/83
![Page 72: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/72.jpg)
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
![Page 73: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/73.jpg)
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
![Page 74: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/74.jpg)
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
![Page 75: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/75.jpg)
From Kinematic Planning to Dynamic Planning
Dynamic Safety Search• Two robots in 2D• Trace velocity-time graph through algorithm
75/83
![Page 76: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/76.jpg)
From Kinematic Planning to Dynamic Planning
Dynamic Safety Search• Safety is preserved at each step, and thus maintained
overall
76/83
![Page 77: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/77.jpg)
From Kinematic Planning to Dynamic Planning
Dynamic Safety Search - Results
Left-right traversal task experiment with DSS and ERRT77/83
![Page 78: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/78.jpg)
From Kinematic Planning to Dynamic Planning
Dynamic Safety Search - Results
Left-right traversal task experiment with DSS and ERRT78/83
![Page 79: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/79.jpg)
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
![Page 80: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/80.jpg)
From Kinematic Planning to Dynamic Planning
Dynamic Safety Search - Results
80/83
![Page 81: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/81.jpg)
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
![Page 82: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/82.jpg)
Conclusion
82/83
![Page 83: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/83.jpg)
Conclusion
• ERRT motion planning algorithm– The waypoint cache– Multi-connect
• Dynamic Safety Search– Cooperative multi-robot safety
83/83
![Page 84: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/84.jpg)
Appendix
84/83
![Page 85: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/85.jpg)
Randomized Approaches
Rapidly exploring random trees(RRT)
Extend, Distance, and RandomState are domain-specific functions.
85/83
![Page 86: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/86.jpg)
Randomized Approaches
Rapidly exploring random trees(RRT)
86/83
![Page 87: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/87.jpg)
Randomized Approaches
Execution Extended RRT (ERRT)• The waypoint cache stores the previously obtained points by
replacing the already cached points randomly.
87/83
![Page 88: Real Time Motion Planning and Safe Navigation in Dynamic Environments*](https://reader035.vdocuments.mx/reader035/viewer/2022062501/56816737550346895ddbe84b/html5/thumbnails/88.jpg)
Randomized Approaches
ERRT vs Visibility Graph in 2D
88/83