Dynamic Roadmap

[1]Leven, Peter, and Seth Hutchinson. “A framework for real-time path planning in changing environments.” The International Journal of Robotics Research 21, no. 12 (2002): 999-1030.

In sampling-based algorithms, collision checking is usually the most expensive operation and reportedly consumes up to 90–95% of the planning time [1]. The Dynamic Roadmap (DRM) , an extension to the probabilistic roadmap (HRM) algorithmically reduces the collision checking time by encoding configuration-to-workspace occupation information.

The basic idea of DRM

Our approach is closely related to recent probabilistic roadmap approaches. These planners use preprocessing and query stages, and are aimed at planning many times in the same environment. In contrast, our preprocessing stage creates a representation of the configuration space that can be easily modified in real time to account for changes in the environment, thus facilitating real-time planning.

We begin by

constructing a graph that represents a roadmap in the configuration space, but we do not construct this graph for a specific workspace. Instead, we construct the graph for an obstacle-free workspace, and
encode the mapping from workspace cells to nodes and arcs in the graph.
When the environment changes, this mapping is used to make the appropriate modifications to the graph, and plans can be generated by searching the modified graph.

The dynamic roadmap (DRM) is a variation of the PRM proposed by Leven and Hutchinson [1]. The DRM is dynamic in the sense that the graph G can be dynamically updated in different environments.

The key feature of DRM is a configuration-to-workspace mapping. The main observed limitation of the existing DRM method is its low success rate. Although the success rate can be improved for particular tasks by carefully selecting the samples, those approaches can not solve generic problems, therefore they are not resolution complete[2].

step 1: Sampling the configuration

Uniform sampling is unlikely to place samples in narrow passages, and reflects a complete absence of prior knowledge about the environment.

Thus we use an importance sampling scheme that is based on the manipulability measure associated with the manipulator Jacobian matrix.

Manipulability is a quantitative measure of the ability of a robot to position and orient its end-effector. The formula for the manipulability is [latex]$\omega = \sigma_1, \sigma_2, \cdots, \sigma_n$ in which $\sigma_i$ are the singular values of the manipulator Jacobian. One shortcoming of the manipulability measure for our purposes is that it does not reflect joint limits. When the robot is near a joint limit, its movement is restricted. In an effort to include samples near joint limits we adopt the following convention: at configurations in which some joint is near a limit, the manipulability is defined to be zero. The nearness of a joint to its limit is a parameter of our sampling algorithm.

High manipulability(means robot has great dexterity): fewer samples;
Low manipulability: sample densely.

Steps:

Sample the C-space of the robot uniformly at random and compute the manipulability for each configuration. We exclude from this computation configurations in which the robot collides with itself(self-collision: collision between distinct links of the robot).
Create a histogram of the manipulability values that we have computed.
we normalize the number in each bucket of the histogram and create the cumulative distribution function from these normalized values. The bucket size of the histogram and the number of samples to take are parameters.

step 2: Connect the nodes (local planner)

The local planner determines

how the nodes of the roadmap are connected (In the paper [1], simple straight-line planner is used.)
whether the connection is feasible given the presence of obstacles (i.e. collision check, most computational expensive part for traditional PRM ).

The roadmap for DRM is built without obstacles in the workspace. It should be reasonably fast because of no need for collision check as opposed to traditional PRM. In addtion, the local planner consider self-collisions of the robot when determining whether two nodes can be connected.

Distance function: provides a measure of the difficulty the local planner would have connecting two configurations. An ideal distance function would be the swept volume in the workspace of the trajectory connecting two configurations. But this distance function is very expensive to compute; Thus most PRM methods use approximations based solely on the two configurations that are to be connected.

[latexpage]$q_i$ refers to the configuration of the i-th joint(i.e. the angle, theta).

Workspace to Configuration Space mapping

step 1: Computation of [latexpage]$\phi_n^{-1}$ and $\phi_a^{-1}$

The workspace, we use a uniform, rectangular decomposition, which we denote by $W$. We denote the C-space by $C$, and define the mapping φ from workspace to configuration space as:

φ(w) is exactly the C-space obstacle region. We do not explicitly represent the C-space, but instead use the roadmap $G$.

Two additional mappings:

From the workspace to the nodes in the roadmap $G$;
From the workspace to the arcs(edges) in the roadmap $G$. They represents nodes and edges that collide with obstacles.

In terms of the implementation, it is much easier to compute the inverse maps [latexpage]$\phi_n^{-1}$ and $\phi_a^{-1}$. The inverse map $\phi_n^{-1}$ corresponds to the set of cells in $W$ that intersect with the robot at configuration $q$, and the map $\phi_a^{-1}$ corresponds to the cells in $W$ that intersect with the swept volume of the robot as it follows the path $\gamma$ in the roadmap.

When the inverse maps are computed, the forward maps can easily be represented, for example, by using doubly linked pointers.

The set of cells in $\phi_n^{-1}$ is computed by a voxelization algorithm inspired by the voxelization algorithm for polyhedra described in Kaufman and Shimony (1986).

The computation of $\phi_a^{-1}$ involves computing the swept volume of the robot as it traverses a path computed by the local planner between two configurations. Computing the swept volume for a robot trajectory is not a trivial problem. There is a simple solution for convex polyhedra that only translate, but the general case for motion with rotation is much more complex. Because of this complexity, methods for approximating the swept volume have been proposed.