ME5751�Robotics Motion Planning

Yizhe Chang chang@cpp.edu

Lecture Note Set #18-19

​

Outline

• Single Query PRM Review
• Random Exploring Dense Tree
• Using RDT trees for planning
• Multiple Query Probabilistic Roadmap
• Supplement: sampling technique

​

SQ PRM Algorithm overview

• Add start configuration c_start to roadmap R( N, E )
• Loop
• Randomly select an existing node c to expand
• Randomly generate a new node c ’ from c
• If edge e from c to c ’ is collision-free, add (c’, e) to R
• If c ’ belongs to endgame region, return path(R)
• Return if stopping criteria is met

​

• To get the path, simply search a path on the roadmap R from last c’ to the c_start

Sampling strategy: node selection/generation

• Step 4: Randomly Generate new Node c ’ from c
• One could pick the next node for expansion by picking from all nodes in the roadmap with equal probability.
• This is easy to implement, but leads to poor expansion -> Clustering

Outline

• Single Query PRM Review
• Random Exploring Dense Tree
• RDT generation
• RDT generation (original version)
• KD tree
• Multiple Query Probabilistic Roadmap
• Supplement: sampling technique

​

Rapid exploring random tree

• Allow quickly generate a “roadmap”, or quickly “pave road”
• Initially by Steven M. LaValle and James J. Kuffner Jr.

Rapid exploring dense tree

• Rapid exploring dense tree (RDT) is a topological graph G(V, E)
• It is generated using the following pseudo-code: S(G), free space in the configuration space. In this case, assume all config are free

​

​

​

​

• Referred as “dense” because all the configuration on the edge can serve as a node

​

Rapid exploring dense tree

• If the nearest configuration is a node

​

​

​

​

• If the nearest configuration is on the edge

Rapid exploring dense tree

• If we have an obstacle in the configuration space

​

​

​

​

​

​

​

Let’s make an RDT

Random exploring dense tree

• An example of RDT with obstacle

​

From: http://aamirhatim.com/v6/projects/article.php?project=rrt

Rapid Exploring Dense Tree (original version)

• Previously we discussed a “modified” version
• Many tutorials or paper (including the wiki) introduced the “original version” that fixes the step size

RDT considering wheel kinematics

• We can consider the wheel kinematics in the roadmap
• q is the configuration. If we search in the velocity configuration?
• Implemented on the “original” version

​

Outline

• Single Query PRM Review
• Random Exploring Dense Tree
• RDT generation
• RDT generation (original version)
• KD tree
• Multi-query PRM

​

Binary search tree

• Binary search tree: use tree to represent an ordered list
• Left child -> small
• Right child -> big

https://www.geeksforgeeks.org/binary-search-tree-data-structure/

K-d tree

• Each “layer” alternate the dimension
• Layer 1: horizontal, layer 2: vertical, layer 3: horizontal, layer 4: vertical …
• Construct a K-d tree with k-dimension, n points: O(kn log n)
• Search in a K-d tree to find the nearest: O(log n)
• Tutorial from G2G: https://www.geeksforgeeks.org/k-dimensional-tree/

​

​

​

​

​

Can you spot anything wrong?

Finding the nearest configuration

• We still need some approximation

​

​

​

• We firstly break up a “too long” edge into several nodes
• Then, find the nearest node

​

​

​

Outline

• Single Query PRM Review
• Random Exploring Dense Tree
• RDT generation
• RDT generation (original version)
• KD tree
• Using trees for planning
• Multiple Query Probabilistic Roadmap
• Supplement: sampling technique

​

​

Single tree planning

• The initial configuration q₀ is the “start configuration” q_i
• We can randomly, say 1/100, to set the α(i) to the goal config q_g. This will let the RRT to “grow” towards the goal

​

​

RRT single tree for planning

• Add start configuration q_i to tree G( N, E )
• Loop
• dn <- Toss a 100-number (0-99) dice
• If dn < 99
• select a random configuration α(i)
• Else if dn==99
• select a configuration α(i)=q_g
• Adding α(i) or q_s (see previous slide) to G (You need to search “nearest”)
• If α(i) belongs to endgame region, return path(G)
• Return if stopping criteria is met

Balanced-bidirectional search

• The initial configuration q₀ is the “start configuration” q_i
• We can randomly, say 1/100, to set the α(i) to the goal config q_g. This will let the RRT to “grow” towards the goal

​

​

Balanced-bidirectional search

• Balanced, bidirectional search
• Start from q_I and q_g
• Build two trees T_a and T_b
• Swap T_a and T_b

Bi-directional

q_n

q’_n

alpha(i)

q_s

q’_s

goal

T_a

T_b

start

Bi-directional: found!

q’_n

q_n

​

start

T_a

T_b

q’_n

​

alpha(i)

q_s, q’_s

​

Bi-directional: node was not added

q_n

q’_n

q’_s

start

T_a

T_b

alpha(i)

q_s

Bi-directional: extreme situation

• We want the balanced-growth, that is why we “swap”
• If we do not swap, we will keep expanding T_a

start

alpha(i)

goal

T_a

T_b

Outline

• Single Query PRM Review
• Random Exploring Dense Tree
• Using RDT trees for planning
• Multiple Query Probabilistic Roadmap
• Supplement: sampling technique

​

Drawback of RDT/RRT

• The RDT/RRT is generated for one set of start-goal configuration
• If we change the start-goal configuration, for the same map, the RRT must be regenerated
• We use probabilistic roadmaps (PRM) to build a roadmap for the future use
• Strictly speaking, the methods we previously introduced should not be called “probabilistic roadmap” methods. They are just “sample-based method”

Multiple queries

• Two phases in PRM for motion planning:
• Preprocessing phase: build a roadmap

Effort is invested to build G in a way that is useful for quickly answering future queries. For this reason, it is called a roadmap, which in some sense should be accessible from every part of C_free.

• Query phase: find the path on a roadmap

For each given initial and goal configuration (q_i and q_g), configurations must be connected easily to G using a local planner. Following this, a discrete search is performed using any of the algorithms in search-based methods to obtain a sequence of edges that forms a path from q_I to q_G.

​

​

Build a roadmap

• A basic roadmap generation algorithm: we have a graph with a loop!

Build a roadmap

• Select neighboring samples
• Nearest K (K=15 e.g.)
• Component K (usually K=1), meaning a new node with only expand to 1 existing node
• Radius: Take all points within a ball of radius r centered at α(i)
• Visibility: “worthwhile” to connect

​

​

​

Multiple queries: Query

• Query phase
• Happen after N times of iteration
• q_I and q_G are connected
• This is an assumption
• Find start->q_I and goal->q_G
• Then? Dijisktra, or simply breadth/depth first search (A* is okay but not necessary)

​

​

Visibility Roadmaps

• A special type of roadmap
• When generate neighbors, there are more computations but worth-while
• Definition:
• Guard: To become a guard, a vertex, q must not be able to see other guards. Thus, the visibility region, V (q), must be empty of other guards.
• Connector: To become a connector, a vertex, q, must see at least two guards. Thus, there exist guards q1 and q2, such that q ∈ V (q1) ∩ V (q2).

​

Visibility roadmaps

• When expand a new sample α(i)
• The new sample, α(i), is not able to connect to any guards. In this case, the new sample α(i) becomes a guard to insert to the graph G
• The new sample, α(i), can see two guards that were not connected. In this case, the new sample becomes a connector
• If neither of above hold, the new sample α(i) is not useful and should be discarded.

​

• Advantage:
• Simplified the roadmap, improved the roadmap efficiency
• Disadvantage:
• The position of guards are not optimized, and cannot update

Heuristic for improving roadmaps

• Most heuristics are used to reduce the number of samples
• Vertex Enhancement
• Assign probability on p(V) to decide where is the new nodes
• Sampling on the free space boundaries
• Reduce the samples on the large free space (no need)
• Gaussian Sampling
• Obtain the boundary points by Gaussian distribution
• Increase the distance between boundary samples
• Bridge-test Sampling
• Find narrow corridors between obstacles

​

​

Outline

• Single Query PRM Review
• Random Exploring Dense Tree
• Using RDT trees for planning
• Multiple Query Probabilistic Roadmap
• Supplement: sampling technique

​

Sampling Sequence

• Our goal of sampling: through samples we wish to get unbiased estimation of the whole configuration space
• As we get sample continuously, we get sample sequences
• In most of cases, we want the sample sequences to be as “even” as possible
• For example, make 10 random samples between number 0 - 9
• Desired sampling sequence: 1, 6, 3, 5, 8, 4, 7, 9, 6, 8
• Undesired sampling sequence: 1, 1, 1, 6, 3, 6, 9, 4, 2, 2

Sampling Sequences

• Can we get samples “evenly” by uniform sampling?
• You can play a game on https://www.random.org/, random 0 ~ 6 for 10 times?
• The generated sequence is called “naïve sequence”. It is usually include many repetition
• How do we solve it?

​

​

van der Corput Sequence

• A pre-determined sequence
• You can start anywhere
• Every number is just far from the previous
• Looks random enough to confuse people, especially if you set the resolution to be high

Pseudorandom

Pseudorandom

• You can try this multiple times and see if you get the same result or not.
• In most computers, the seed is set to be the current system time (so the seed is different every time you have)

​

Low-dispersion sampling

• Our sample dimension is N-d
• We want to place samples to make the largest uncovered area be as small as possible. (Put samples to the regions where it was uncovered!)
• Depends on how we define the “uncovered area”
• Regardless how do we define the dispersion, we want to minimize the uncovered area (yellow area)

​

​

Low-dispersion sampling

• The resolution determines the dispersion
• We can prove the dispersion for Sukharev grid is:
• k: number of samples, d-dimension

​

​

• Instead, we can have “lattice grid”, a grid that shows where samples cover

​

​

Low-discrepancy Sampling

• Discrepancy measures whether the right number of points fall into boxes.
• We can quantify that how many samples fall into a box
• E.g. our sampling goal:
• Box is ¼ of the sampling space
• We wish out of 200 samples, ~50 samples were in this box
• Halton/Hammersley sampling
• (t,s)-sequences and (t,m,s)-nets
• Lattices

​