image
Hathaway Zhu
Industrial Robots, Paper Reading

Hierarchical Dynamic Roadmap

[1]Yang, Yiming, Wolfgang Merkt, Vladimir Ivan, Zhibin Li, and Sethu Vijayakumar. “HDRM: A resolution complete dynamic roadmap for real-time motion planning in complex scenes.” IEEE Robotics and Automation Letters 3, no. 1 (2017): 551-558.

[2]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.

Recap of DRM

Features of DRM in paper[2]:

  • A variant of PRM (see my post on PRM)
  • DRM is dynamic in the sense that the graph [latexpage]$G$ can be dynamically updated in real-time to account for changes in the environment, thus facilitating real-time planning. While for PRM, it has a static roadmap that can be reused when a different goal position is selected (Thus PRM is called a multi-query planning algorithm).
  • The key feature of DRM: A configuration-to-workspace mapping. The occupation information for DRM is stored per each workspace voxel. Each voxel stores a list of edges that sweep through this voxel. When the environment changes,
    1. find all the voxels that are occupied by the obstacles in the environment.
    2. Iterate through the occupation lists of these invalid voxels and find all the invalid edges
    3. Remove all the invalid edges for each voxel

The main observed limitation of DRM in paper[2]: low success rate.

The underlying reason for this low success rate and planning failture of DRM in paper[2]: The edge is invalidated if one or more voxels in the swept volume are in collision. However, there will be many sub-edges still valid in the cases where only very few of the voxels are in collision. Yet, the whole edge is considered invalid as these sub-edges are not stored in the roadmap.

Introduction to Hierarchical DRM

Steps for Hierarchical DRM

  1. Hierarchical Configuration Structure
  2. Removal of Swept Volumes
  3. Hierarchical Occupation Lists
  4. Motion Planning Using HDRM

Hierarchical Configuration Structure: How to create a hierarchical structure to efficiently store tens of millions of configurations

The data structure stores [latexpage]$M=K_1K_2\cdots K_i\cdots K_N$ vertices to represent all the possible configurations(i.e. each joint is assigned with a value, not cartesian workspace) for the robot arm to be investigated, with $K_1$ denotes the number of evenly discretized values within a range for the 1st joint of the robot arm. $K_i$ denotes the number of discrete values for the ith joint of the robot arm. And the total number of joints of the robot arm is $N$. So for each joint of the robot arm, $K_1$, $K_2$ can be different.

Let say for joint one, the range is between [latexpage]$10, 30$ degress. And there are 100 discrete values. Let $$

In this paper, the authors do not specify the value for each joint of the robot arm in order to represent a specific configuration. They only specify the first $m$ joints. Let say each of the first $m$ joint is assigned with a value $v_1$, $v_2$, $\cdots v_m$, and let denote this $m\times 1$ vector $v=[v_1, v_2, \cdots, v_m]$. And $q(v)$

Removal of Swept Volumes: why and how to remove the swept volumes and edges

Hierarchical Occupation Lists: process and stores the occupation lists of all vertices

When the roadmap contains tens or potentially hundreds of millions of vertices, their occupation lists are too expensive to store using classical methods. Next, we discuss how to exploit the hierarchical
structure to resolve this problem.

  • Generate a set of workspace(Cartesian) voxels [latexpage] $\mathbb{V}$, given the size of the workspace and grid resolution $s$.
  • Associate each voxel $v\in \mathbb{V}$ with an empty occupation list $\mathcal{O}_v$
  • Calculate $K_1, K_2, …., K_N$ (N joints in total) according to equation 6-9.
  • Calculate the case when n=1, i.e. level 1. Then for the first joints all the possible configurations is $1, 2, \cdots, K_1$, then $k(n)=k(1)$ has $K_1$ discrete possible values, we denote it as $\mathcal{H}(n=1,i= 1), \mathcal{H}(n=1,i= 2), \cdots, \mathcal{H}(n=1,i= K_1)$.
    • Check the 1st configuration: $i=1$
      • find the occupied voxels for $\mathcal{H}(n=1, i=1)$
      • update $\mathcal{O}_v$ for each voxel that is in collision with this configuration. i. e. add the tuple (1, 1) (the vertices in HDRM, Not Cartesian space) to each voxel that is in collision. Then one can know that this voxel is in collision with the 1st configuration of level 1.
    • Check the 2nd configuration: $i=2$.
      • find the occupied voxels for $\mathcal{H}(n=1, i=2)$
      • update $\mathcal{O}_v$ for each voxel that is in collision with this configuration. i. e. add the tuple (1,2) to each voxel that is in collision. Then one can know that this voxel is in collision with the 2nd configuration of level 2.
    • $\cdots$
    • Check the $K_1$-th configuration: $i=K_1$, do the same as before.
    • Check self-collision
  • Calculate the case when n=2, i.e. level 2. Then for the first 2 joints all the possible configurations is $1, 2, \cdots, K_1*K_2$, then $k(n)=k(2)$ has $K_1*K_2$ discrete possible values, we denote it as $\mathcal{H}(n=2,i= 1), \mathcal{H}(n=2,i= 2), \cdots, \mathcal{H}(n=2,i= K_1*K_2)$.
    • Check the first 2 configurations: $i=1$, and do the same as before
  • Given the voxel of start and goal position in Cartesian, Find the closest vertices (in the HDRM, not Cartesian), and treat them as $q_{start}$ and $q_{goal}$.
  • find a valid path from start vertex to goal vertex

Leave a Comment