13.3 Sampling-based path planning

Algorithms that guarantee a solution are pretty slow, so instead lets try something that probabilistically guarantees a solution.

RRT

Let $X$ be a d-dimensional state space representing all possible poses of the robot. Let $G\subset X$ be the a d-dimensional sphere of states that represent the goal.

$t$ is the allowed time $k$ is the maximum number of vertices $goal\_bais$ is the percentage of time the algorithm should try to connect to a goal state

Tree = Init(X, G, start, max_dist, t, k, goal_bais)
iteration=0
WHILE(ElapsedTime() < t AND iteration < k AND NoGoalFound(Tree,G)):
	iteration += 1
	IF RandomPercentage() < goal_bias:
		q_rand = SampleRandomGoal(G)
	ELSE
		q_rand = SampleRandomState(X)
	q_nearest = NearestVertex(q_rand)
	q_new = Extend(q_nearest, q_rand, max_dist)
	edge = CreatePath(q_nearest, q_new)
	IF IsAllowablePath(edge):
		Tree.addVertex(q_new)
		Tree.addEdge(edge)
return Tree

Some things worth talking more in depth about:

1. determining the next point q_rand isn’t truly random, its selecting from points within a radius of points that are already in the tree to ensure that it can be connected. You can do it the other way (truly random) and that also works - but then you have to do some cleanup every time you add a node to see if you can more efficiently connect nodes.
2. Connecting points requires iterating through all current nodes and checking the distance, a time consuming and inefficient process, although theres no way around it. Extend(max_dist) replaces the node with a node max_dist away if the dist is over max_dist
3. Collision checking is really hard! it takes up 90% of the runtime, but you can be lazy and only do it once you’ve found a solution and then search within a certain range of that path for a collision free-path.
4. Path smoothing is an important part of the algorithm since a bunch of random points will not be a smooth path.