Voronoi Diagram Roadmap Cell decomp. Set of points equidistant from the closest Potential field two or more obstacle boundaries Maximizing the clearance between the points and obstacles Page 55
Voronoi Diagram Roadmap Cell decomp. Compute the Voronoi Diagram of C- Potential field space Page 56
Voronoi Diagram Roadmap Cell decomp. Compute shortest straightline path from Potential field start and Goal to closest point on Voronoi Diagram Page 57
Voronoi Diagram Roadmap Cell decomp. Compute shortest path from start to goal Potential field along Voronoi Diagram Page 58
Cell decomposition Roadmap Cell decomp. Decompose the free space into simple cells Potential field and represent the connectivity of the free space by the adjacency graph of these cells Page 59
Cell decomposition: Roadmap Exact decomposition (Trapezoidal) Cell decomp. Potential field Decompose the free space with vertical lines through the vertices without intersecting with the forbidden space Page 60
Cell decomposition: Roadmap Exact decomposition (Trapezoidal) Cell decomp. Potential field Add to the center of each segment and trapezoid a graph node Page 61
Cell decomposition: Roadmap Exact decomposition (Trapezoidal) Cell decomp. Potential field Find the shortest path through the obtained graph with a graph search algorithm Page 62
Cell decomposition: Roadmap Approximate decomposition Cell decomp. Potential field One of the most convenient way to make sampling-based planning algorithms is to define a grid over C and conduct a discrete search algorithm Neighborhoods: For each grid point q we need to define the set of nearby grid points for which an edge may be constructed Page 63
Cell decomposition: Roadmap Approximate decomposition Cell decomp. Potential field Once the grid and neighborhoods have been defined, a discrete planning problem is obtained Page 64
Cell decomposition: Roadmap Approximate decomposition Cell decomp. Potential field Decompose the C-Space with a start resolution to cell grid Each cell that intersect with obstacles is forbidden Is there some path existing? Page 65
Cell decomposition: Roadmap Approximate decomposition Cell decomp. Potential field If no Path is existing than refine the resolution until a solution is found Page 66
Potential field Roadmap Cell decomp. Define a potential function over the free Potential field space that has a global minimum at the goal and follow the steepest descent of the potential fun ction Page 67
Potential field Roadmap Cell decomp. – The goal location generates an attractive Potential field potential – pulling the robot towards the goal – The obstacles generate a repulsive potential – pushing the robot far away from the obstacles – The negative gradient of the total potential is treated as an artificial force applied to the robot Page 68
Potential field Roadmap Cell decomp. The sum of the forces control of the robot Potential field Page 69
Potential field Roadmap Cell decomp. Potential field Page 70
Potential field Pros Roadmap Cell decomp. Spatial paths are not preplanned and Potential field can be generated in real time Planning and controlling are merged into one function Cons Trapped in local minima in the potential field Because of this limitation, commonly used for local path planning Page 71
Checking Path Segment Path Segment Incremental Search SBPA Collision detection algorithms determine whether a configuration lies in C free RRT PRM Motion planning algorithms require that an entire path maps into C free The interface between the planner and collision detection usually involves validation of a path segment Page 72
Checking Path Segment Path Segment Incremental Search For a Path τ s : [0, 1] → C free a sampling for the SBPA interval [0, 1] is calculated RRT PRM The collision checker is called only on the samples Problem: How a Resolution can be found? How to guarantee that the places where the path is not sampled are collision-free? Page 73
Checking Path Segment Path Segment Incremental Search SBPA A fixed ∆ q > 0 is often chosen as the C-space RRT step size Points t 1 , t 2 ∈ [0,1] are chosen close enough PRM together to ensure that ρ ( τ (t 1 ), τ (t 2 )) ≤ ∆ q , ρ is a metric on C Page 74
Checking Path Segment Path Segment Incremental Search If ∆ q is too small, considerable time is wasted SBPA on collision checking (1) If ∆ q is too large, then there is a chance that RRT the robot could jump through a thin obstacle (2) PRM (2) (1) Page 75
Checking Path Segment Path Segment Incremental Search Suppose that for a configuration q (x t ,y t ,O) the SBPA collision detection algorithm indicates that RRT A (q) is at least d units away from collision PRM Suppose that the next candidate configuration to be checked along τ is q’ ( x’t,y’t,O’) If no point on A travels more than distance d when moving from q to q’ along τ , then q’ and all configurations between q and q’ must be collision-free Page 76
Checking Path Segment Path Segment Incremental Search The bounds d can generally be used to set a SBPA step size ∆ q for collision checking that RRT guarantees the intermediate points lie in C free PRM Page 77
Incremental Sampling and Searching Path Segment Incremental Search Most sample-based planning algorithms consisting of single-query model, witch means SBPA (q I , q G ) is given only once per robot and obstacle RRT set, following this template: PRM 1. Initialization 2. Vertex Selection Method (VSM) 3. Local Planning Method (LPM) 4. Insert an Edge in the Graph 5. Check for a Solution 6. Return to Step 2 Page 78
Incremental Sampling and Searching Path Segment Incremental Search 1. Initialization: SBPA Let G ( V,E ) represent an undirected search RRT graph, for which PRM V contains at least one vertex and E contains no edges. Typically, V contains q I , q G , or both. In general, other points in C free may be included 2. Vertex Selection Method: Choose a vertex q cur ∈ V for expansion Page 79
Incremental Sampling and Searching Path Segment Incremental Search 3. Local Planning Method (LPM): SBPA For some q new ∈ C free that may or may RRT not be represented by a vertex in V attempt to construct a path τ s : [0, 1] → PRM C free such that τ (0) = q cur and τ (1) = q new . Using the methods of Slides τ s must be checked to ensure that it does not cause a collision. If this step fails to produce a collision-free path segment, then go to step 2. Page 80
Incremental Sampling and Searching Path Segment Incremental Search 4. Insert an Edge in the Graph SBPA Insert τ s into E, as an edge from q cur to RRT q new . If q new is not already in V, then it is PRM inserted 5. Check for a Solution Determine whether G encodes a solution path. As in the discrete case, if there is a single search tree, then this is trivial; otherwise, it can become complicated and Page 81 expensive
Incremental Sampling and Searching Path Segment Incremental Search SBPA 6. Return to Step 2: Iterate unless a solution has been RRT found or some termination condition PRM is satisfied, in which case the algorithm reports failure Page 82
Sampling based Planning Algorithm: Path Segment Incremental Search There are several classes of algorithms based on the number of search trees: SBPA RRT Unidirectional (single-tree) methods PRM Bidirectional Methods Multi-directional (more than two trees) methods Page 83
Sampling based Planning Algorithm: Path Segment Unidirectional (single-tree) methods: A* (A-Star) Incremental Search Is a single-tree Method SBPA A* traverses the graph and follows the path RRT with the lowest cost PRM Keeps a sorted priority queue of alternate path segments along the way If by adding a new Point, a segment of the path get a higher cost than another stored path segment, the lower-cost path segment will be followed The process continues until the goal is reached Page 84
The start position is (1, 1) Path Segment The successive node (1, 2) Incremental Search There is no ambiguity, until the Robot reaches node (2, 4) SBPA The successor node can be determined by RRT evaluating the cost to the target from both the nodes (3,4) and (3,3) PRM Page 85
Sampling based Planning Algorithm: Path Segment Incremental Search Bidirectional Methods: By a Bug-Trap or a SBPA challenging region problem, we better use a bidirectional approach RRT PRM Page 86
Sampling based Planning Algorithm: Path Segment Incremental Search Multi-directional Methods: For a double bug- SBPA trap, multi-directional search may be needed RRT PRM Page 87
Rapidly Exploring Dense Trees Path Segment The idea is to incrementally construct a search Incremental Search tree that gradually improves the resolution but SBPA does not need to explicitly set any resolution RRT parameters PRM Instead of one long path, there are shorter paths that are organized into a tree If the sequence of samples is random, the resulting tree is called a rapidly exploring random tree (RRT), which indicate that a dense covering of the space is obtained Page 88
Rapidly Exploring Dense Trees Path Segment Basic RRT Algorithm: Incremental Search SBPA RRT 1. Initially, start with the initial configuration as root of tree PRM 2.Pick a random state in the configuration space 3. Find the closest node in the tree 4. Extend that node toward the state if possible 5. Goto (2) Page 89
Rapidly Exploring Dense Trees Path Segment Basic RRT Algorithm: Incremental Search SBPA RRT PRM 90
Rapidly Exploring Dense Trees Path Segment The algorithm for constructing RDTs Incremental Search (which includes RRTs) SBPA It requires the availability of a dense sequence, α , and iteratively connects from RRT α( i) to the nearest point among all those PRM reached by G Page 91
Rapidly Exploring Dense Trees Path Segment Incremental Search SBPA If the nearest point in S lies in an edge ( α ), then the edge is split into RRT two, and a new vertex is inserted into G PRM Page 92
Rapidly Exploring Dense Trees Path Segment Incremental Search Several main branches are first constructed SBPA as it rapidly reaches the far corners of the space RRT PRM More and more area is filled in by smaller branches The tree gradually improves the resolution as the iterations continue Page 93
Rapidly Exploring Dense Trees Path Segment Incremental Search This behavior turns out to be ideal for SBPA sampling-based motion planning RRT PRM Page 94
Rapidly Exploring Dense Trees Path Segment The RRT is dense in the limit (with Incremental Search probability one), which means that SBPA it gets arbitrarily to any point in the space RRT PRM Page 95
Probabilistic roadmaps (PRMs) Path Segment Incremental Search Separate planning into two stages: SBPA RRT • Learning Phase PRM – find random sample of free configurations (vertices) – attempt to connect pairs of nearby vertices with a local planner – if a valid plan is found, add an edge to the graph • Page 96
Probabilistic roadmaps (PRMs) Path Segment Incremental Search Learning Phase: SBPA RRT PRM Page 97
Probabilistic roadmaps (PRMs) Path Segment Incremental Search Separate planning into two stages: SBPA RRT • Query Phase PRM – find local connections to graph from initial and goal positions – search over roadmap graph using A* to find a plan Page 98
Probabilistic roadmaps (PRMs) Path Segment Query phase: Incremental Search SBPA RRT PRM Page 99
Introduction Motion Planning Configuration Space Sampling-Based Motion Planning Comparaison of related Algorithms
Recommend
More recommend
