and generate new nodes, typically sampling new nodes where there is
a lower density of milestones in a region of the configuration space.
Given that the space is sampled uniformly and that the configuration
space, or state-time space, in the case of the dynamic robot, still satisfies the property of expansiveness, kinodynamic planning has also
been shown to converge exponentially with the number of nodes [ 3].
To illustrate kinodynamic planning, consider the simple example
of a point robot moving in a plane, subject to an acceleration constraint. The state at time, t = 0, is the beginning, when the robot is at
the start position. From here, the control space is randomly sampled
for actions. These actions are integrated forward in time to create
nodes at time, t = 0+ dt. This process is continued until one of the
trees nodes reaches the goal region. The nodes are chosen based on
the density of samples in a given region of the state-time space, which
is parameterized by velocity, position, and time.
The planner will now be examined. It was implemented to plan for
a “skateboard robot” moving along complex terrain, consisting of
ramps, walls, and other obstacles. The kinodynamic planning method
shown above is the basis for the planner. The planner is implemented
for vehicle motion subject to dynamic terrain. The key difference
between the point robot and this skateboard robot is that the environment actively affects the dynamics of the robot, in the case of the
skateboard. For instance, if the skateboard is moving at a given velocity, that velocity changes when the skateboard goes up or down a ramp.
Exploitation of dynamics such as these is essential for path planning.
Planner for the Skateboard Robot
Figure 5: The model used to simulate a car’s motion. The turn
rate of the body is simply a function of the velocity, v, and
steering angle, .
The initial approach was to first implement a basic flat land planner for a vehicular robot moving about obstacles. To parameterize
the skateboard, a simple “bicycle” model was used for motion (see
Figure 5). Acceleration was also limited to the forward direction. The
root milestone was chosen and then expanded. Controls were chosen at random when creating new nodes. The process for creating a
new node was as follows:
1. Choose steering angle uniformly at random (+/– 35 deg ).
2. Choose a push force uniformly at random.
3. Choose an integration time uniformly at random.
Once these factors were chosen, the parent milestone was expanded
by integrating the board state forward, given the set of controls.
To ensure that the state time space was uniformly sampled, a grid-based approach, similar to the method presented in “Randomized
Kinodynamic Planning with Moving Obstacles” [ 3], was used.
The space was subdivided into a four-dimensional grid, [x, y, z, Θ].
Note that neither velocity nor time is taken into account. This means
that the sampling of the space is not completely uniform. It is just a fairly
good approximation. Next, a cell, or bucket, that contains at least one
milestone is chosen uniformly at random. Then, from that bucket, a
milestone is chosen, again, uniformly at random. This corresponds
roughly to sampling based on the density of milestones in a given region.
Using this basic method, the planner was able to find simple paths
through a limited number of obstacles. When there were dense sets
of obstacles that required careful maneuvering, the planner often
failed. Failure was defined as failing to complete the path in a set
threshold of milestones in the roadmap. Secondly, if the goal location
was extremely far from the start location, the planner would often fail
because the integration step when creating milestones was too small.
Increasing the average time step was not a sufficient answer, as the
robot would now not be able to carefully maneuver in tight spaces.
To combat these problems, several new heuristics were introduced,
which together are called complexity-based sampling.
The desired behavior of the robot is that in large free spaces there is a
large integration time and a larger average push force. In complex,
obstacle-cluttered regions, it is desirable to minimize acceleration
(pushing) and have a fairly small integration step, so that any given
action is fairly small. A simple way to approximate this behavior is to
base the integration time step and push force probability distribution
upon the complexity of the configuration space that is in the vicinity of
the current node being expanded. The method used to implement this
heuristic is to assign each grid bucket a complexity metric, which is
based upon the number of failed milestones within this bucket—the
idea being that buckets that have many failed milestones are probably
in a region with many obstacles. Unfortunately, this metric alone is not
sufficient to capture the intricacies of the problem. Take, for instance,
a very large free space that eventually leads to a very dense space of
obstacles. The problem in this case is that there will be a very high push
force in the area that is free, despite the fact that the robot will eventually hit an area with a high density of obstacles. The desired behavior of
the robot is that it sees that it will eventually hit obstacles from its current state and should slow down even before it reaches a dense region.
In order to do this, communication is introduced between parent and
child milestones and between a milestone and its bucket, which represents its course representation in space (see Figure 6).
The tree-shaped structure of the roadmap lends itself easily to this
model; a bi-directional tree is created where each node knows its
course representation in state space by storing a reference to its bucket.
Whenever a milestone is failed, the complexity is increased recursively,
going up the chain of nodes from child to parent. If a certain node fails,
the bucket of that node has its complexity metric increased. But, in
addition to this, the complexity metric of its parent node is also
increased. This continues up the chain with a decaying magnitude up
to a certain depth threshold. This method allows quick adaptations of
the selection distributions for pushing force and integration time for
vastly different environment types in a generalized fashion.