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.
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.
References:
Archives