Figure 6: An illustration of a tree-based roadmap. Nodes are
placed in buckets, which correspond roughly to their location in
state-time space. This is a 4-D space, but for illustration purposes,
2-D is shown. Complexity-based sampling allows inter-node
communication. The failure of the last expanded node (red) causes
a traversal up the levels of the tree (arrows), causing each parent
to change its control selection strategy. The effect is decayed proportionally to the distance in the hierarchy from the failed node.
Goal Distance Heuristic
Another problem often faced is that the search tree grows randomly.
In complex spaces, this is not necessarily a bad thing, as a solution
often requires a large breadth to find a solution. Unfortunately, in
extremely simple situations such as a straight-line path in an open
environment, (unnecessarily) random paths may manifest because of
the stochastic nature of tree growth. In order to prevent this, a ranking heuristic was also implemented. Each grid bucket was given a
ranking. Then, the set of all buckets was placed in a queue (similar to
best-first search). The ranking was based on the distance to goal of
the average milestone within the bucket, and it was inversely proportional to the number of milestones in the bucket. This metric corresponded roughly to over-sampling regions leading towards the
goal, but decaying the rank with each additional expansion. Using
this technique, a good balance between expanding likely candidates
was achieved. Yet, it did not over sample a single region of the state-time space. In more difficult terrain, this technique alone was not
sufficient. Therefore, a probability, P, of choosing a random node
versus a shortest-path node was introduced instead. This probability
was changed based on the total number of failed milestones. This
made the planner act randomly in complex spaces. Yet, when in an
empty space, it planned straighter paths using the distance heuristic.
The last problem that was faced was that regions where there were
inclines in terrain were often improperly sampled or under-sampled.
One major problem is that generally the robot would not have the
huge push necessary to build enough speed to clear a ramp. The
preparation for a ramp has to be done well in advance (see Figure 7).
In regions where there is a ramp, the robot generally wants to be
moving at a fairly high velocity in order to clear the ramp.
Furthermore, the robot should be oriented in an angle such that the
robot will not veer or fall when on the ramp. To fix this, a similar
approach to complexity-based sampling mixed with goal distance
sampling was used. By examining changes in robot position in the
work space, which milestones correspond to the skateboard being on
ramps can be found. Using this information, the control-selection
parameters of milestones that eventually lead to a ramp can be
changed. In practice, the same approach as complexity-based sampling was used. Milestones that lead to ramps had their control selection parameters changed to favor a higher push force. The priority of
milestones that lead to ramps was also increased, meaning that such
milestones would be sampled more frequently. These heuristics led
to much better coverage of the roadmap on the ramps (see Table 1).
The planner was tested on various types of terrain (see Table 1). The
easiest map, Narrow Hall, consisted of an L-shaped bend along which
the robot had to navigate. The hardest map, Escape, required the skateboard to climb up a ramp and jump off in order to move from one
walled area to another. This required the skateboard to move away from
the goal in order to have enough room to build-up speed, then push
quickly to clear the wall, using the ramp. The combination of all three
heuristics found a solution much faster than no heuristic in all cases.
These results demonstrate that heuristic methods are quite useful in
many practical cases in problems where the search space is extremely
vast. However, in planning problems that involve multiple dynamic
maneuvers—jumping over walls or utilizing ramps—executed in
Figure 7: Left: The roadmap fails to climb up the ramp enough to reach the top. Speed needs to be built before getting to the ramp.
Right: Improved coverage of uphill areas in the roadmap by using new sampling heuristics. The gray lines represent edges on the
tree. The colored objects are failed milestones. Pink lines represent a link between milestones that reach a goal location.