Conservative Hypercube Volumes for Compact, Certifiably Continuous Roadmaps

1. Introduction

Motion planning is a foundational technique in robotics, and configuration space (${\mathcal{C}}_{\mathrm{space}}$) [1] representations are broadly applied, particularly in motion planning for manipulators. The ${\mathcal{C}}_{\mathrm{space}}$ for robot manipulators, however, are typically specified implicitly based on workspace (2D or 3D) obstacles, the manipulator’s link geometry and its forward kinematics. Implicitly specified ${\mathcal{C}}_{\mathrm{space}}$ poses certain challenges. First, though off-the-shelf libraries can find distances and free volumes in 3D workspaces [2,3,4], identifying corresponding free volumes in ${\mathcal{C}}_{\mathrm{space}}$ is more difficult. Instead, planning approaches often consider only individual configurations (rather than continuous sets or volumes of configurations). Evaluating configurations one-by-one may yield large roadmaps and leads to the second challenge: strictly guaranteeing collision-freedom on continuous plans. Checking validity only for configurations at regular intervals along a plan assume the validity between checked configurations and may require a large number of such checks down to a specified resolution. We address the challenges of implicit ${\mathcal{C}}_{\mathrm{space}}$ by deriving an explicit representation of free space volumes that are fast to compute and guarantees collision-freedom for all configurations within the volume.

We derive an explicit, conservation representation for volumes of free configurations, and we incorporate these free volumes into sampling-based planning to strengthen efficiency and correctness guarantees. We perform experiments for serial-link manipulators with 6-7 DOF (degrees of freedom) in single and multi-query settings, showing that our approach offers time, memory, and correctness improvements over PRM roadmaps (see 6).

2. Related Work

Sampling-based motion planners grow trees or graphs through a ${\mathcal{C}}_{\mathrm{space}}$ and often provide both theoretically robust convergence and efficient performance in practice [5,6,7,8,9,10,11,12,13,14,15,16]. We discuss related approaches for ${\mathcal{C}}_{\mathrm{space}}$ representation based on cell decomposition, learning, and optimization.

Cell decomposition methods, sometimes integrated with sampling-based methods [17,18], can offer resolution-completeness due to the underlying cell decomposition. However, decomposing the entire configuration space poses scalability challenges in higher dimensions. Other approaches deterministically sample configurations using the harmonic function to create a hierarchical cell decomposition [19] or define a resolution parameter to generate simplicial polytopes in a goal-biased direction to cover the ${\mathcal{C}}_{\mathrm{space}}$ [20]. The approach of [21] uses hyperspheres to approximate free regions for explicit ${\mathcal{C}}_{\mathrm{space}}$ or produces inexact approximations of implicit ${\mathcal{C}}_{\mathrm{space}}$. Generally, determining cells or volumes for implicitly defined ${\mathcal{C}}_{\mathrm{space}}$ is a key challenge that this work addresses.

Several approaches have applied learning and optimization to represent the ${\mathcal{C}}_{\mathrm{space}}$. Learned approximations of ${\mathcal{C}}_{\mathrm{space}}$ support fast planning [22,23]. Learning ${\mathcal{C}}_{\mathrm{space}}$ connectivity has enabled proofs of plan non-existence [24] and fast convergence when planning through narrow passages [25]. Optimization-based free space approximations using convex formulations offer strict validity certificates [26,27] or using nonlinear programming supports fast convergence [28]. General challenges of such learning and optimization approaches include inexactness or error of the representation, convergence guarantees, and overhead to construct the representation. Our work addresses these challenges with a representation that is conservative (we never misrepresent an obstacle configuration as free), robustly computable for serial manipulators in a 3D workspace, and efficient to construct on-the-fly during sampling-based planning.

Some previous studies utilized resolution-independent collision checking methods [29,30,31,32,33] to develop efficient Bounding Volumes (BVs) for robotic manipulator arms. This approach ensures that there are no missing parts of the robot’s rigid body during collision checking. In contrast, other methods [21,34,35,36,37,38] create conservative variants of bubbles, such as diamonds, burrs, hypercubes, or hyperspheres, of the ${\mathcal{C}}_{\mathrm{free}}$ to validate line segments. These methods utilize heuristic functions, like A*, for safety assessments, aiming to establish a continuous, collision-free path that connects the centers of intersecting volumes. In this work, we use the similar approach to study and compare the efficiency of hypercube volumes for serial manipulators in high-dimensional space.

3. Problem Definition

We address geometric motion planning for serial manipulators with revolute joints operating in a 3D workspace.

A geometric motion planning problem consists of a configuration space ${\mathcal{C}}_{\mathrm{space}}$ composed of disjoint free space ${\mathcal{C}}_{\mathrm{free}}$ and obstacle region ${\mathcal{C}}_{\mathrm{obs}}$, a start configuration ${\mathit{q}}_{\mathrm{start}}\in {\mathcal{C}}_{\mathrm{free}}$, and goal configuration ${\mathit{q}}_{\mathrm{goal}}\in {\mathcal{C}}_{\mathrm{free}}$ [39]. A feasible plan is a path $\sigma$, such that $\sigma [0,1]\in {\mathcal{C}}_{\mathrm{free}}$, $\sigma \left[0\right]={\mathit{q}}_{\mathrm{start}}$, and $\sigma \left[1\right]\in {\mathit{q}}_{\mathrm{goal}}$.

Following common practice for manipulator motion planning, we start from an implicitly defined ${\mathcal{C}}_{\mathrm{space}}$ in terms of a 3D workspace ${\mathcal{W}}_{\mathrm{space}}$ composed of disjoint free ${\mathcal{W}}_{\mathrm{free}}$ and obstacle ${\mathcal{W}}_{\mathrm{obs}}$ regions coupled with the manipulator forward kinematics, $\mathtt{fk}{\mathcal{C}}_{\mathrm{space}}\mapsto {\mathcal{W}}_{\mathrm{space}}$, to determine all manipulator workspace points at a particular configuration. We then define ${\mathcal{C}}_{\mathrm{space}}$ components as ${\mathcal{C}}_{\mathrm{free}}\triangleq \mathit{q}\mathtt{fk}\left(\mathit{q}\right)\subseteq {\mathcal{W}}_{\mathrm{free}}$ and ${\mathcal{C}}_{\mathrm{obs}}\triangleq \mathit{q}\mathtt{fk}\left(\mathit{q}\right)\neg \subseteq {\mathcal{W}}_{\mathrm{free}}$.

Our analysis assumes that the maximum workspace displacement occurs at the manipulator’s end-effector point. Generally, this assumption holds for the typical case of serial link manipulators with thin links along straight lines between joints, though manipulators with differently-shaped link geometries would require additional considerations.

4. Free Space Volumes

We find volumes of free space to support efficient and correct motion planning. While direct methods exist to find separation and penetration distances in ${\mathcal{W}}_{\mathrm{space}}$ [2,3], the typical implicit ${\mathcal{C}}_{\mathrm{space}}$ representations present challenges to finding similar exact distances in ${\mathcal{C}}_{\mathrm{space}}$. Instead, we find bounds on ${\mathcal{C}}_{\mathrm{space}}$ distances relative to ${\mathcal{W}}_{\mathrm{space}}$ displacement. These bounds determine ${\mathcal{C}}_{\mathrm{space}}$ volumes to enable the construction of compact planning graphs with guaranteed continuity.

We identify configuration volumes from an upper bound on ${\mathcal{W}}_{\mathrm{space}}$ displacement and lower bound on separation. Specifically, two conditions determine the volume around a configuration $\mathit{q}$ (see Section 4.1). First, for all configurations in the volume, the actual ${\mathcal{W}}_{\mathrm{space}}$ displacement from $\mathit{q}$ is no more than the bound (4.2). Second, the bound is less than the workspace separation at $\mathit{q}$ (4.3). When these two conditions hold, all points in the volume are free.

4.1. Preliminary Definitions and Bound Condition

We first define separation/penetration in terms of metric and signed distance functions [40]. Then, we will precisely state the bound condition for ${\mathcal{C}}_{\mathrm{space}}$ volumes.

We write workspace metric functions as ${\lambda }_W$ and ${\mathcal{C}}_{\mathrm{space}}$ metric functions as ${\lambda }_C$, where a metric function $\lambda$ is non-negative, symmetric, satisfies the triangle inequality, and $\lambda (x,x)=0$.

4.1.1. Signed Distance Functions

In general, a signed distance function determines a level of separation or penetration based on distance to a boundary $\partial \mathcal{X}$.

Definition 1.

A Signed Distance Function (SDF) $\varsigma :{\mathcal{C}}_{\mathrm{space}}\mapsto \Re$ is,

$$\varsigma \left(\mathit{q}\right)=\left\{\begin{array}{cc}\lambda (\mathit{q},\partial \mathcal{X}),\hfill & \mathit{q}\in {\mathcal{C}}_{\mathrm{free}}\hfill \\ -\lambda (\mathit{q},\partial \mathcal{X}),\hfill & \mathit{q}\notin {\mathcal{C}}_{\mathrm{free}}\hfill \end{array}\right.,$$

where $\lambda (\mathit{q},\partial \mathcal{X})$ is a distance to free space boundary $\partial \mathcal{X}$.

SDFs may incorporate metric functions operating in either ${\mathcal{C}}_{\mathrm{space}}$ or ${\mathcal{W}}_{\mathrm{space}}$.

Definition 2.

A Workspace SDF ${\varsigma }_{\mathcal{W}}$: $\mathit{q}\mapsto \Re$ finds separation or penetration using a workspace metric,

$${\varsigma }_{\mathcal{W}}\left(\mathit{q}\right)=\left\{\begin{array}{cc}\underset{{\overrightarrow{p}}_{\mathrm{robot}},{\overrightarrow{p}}_{\mathrm{obs}}}{min}{\lambda }_W({\overrightarrow{p}}_{\mathrm{robot}},{\overrightarrow{p}}_{\mathrm{obs}}),\hfill & \mathit{q}\in {\mathcal{C}}_{\mathrm{free}}\hfill \\ -\underset{{\overrightarrow{p}}_{\mathrm{col}}}{max}\underset{{\overrightarrow{p}}_{\mathrm{surf}}}{min}{\lambda }_W({\overrightarrow{p}}_{\mathrm{col}},{\overrightarrow{p}}_{\mathrm{surf}}),\hfill & \mathit{q}\notin {\mathcal{C}}_{\mathrm{free}}\hfill \end{array}\right.,$$

where ${\overrightarrow{p}}_{\mathrm{robot}},{\overrightarrow{p}}_{\mathrm{col}}\in {\mathcal{W}}_{\mathrm{space}}$ are points on the robot at $\mathit{q}$ that are respectively free or colliding, ${\overrightarrow{p}}_{\mathrm{obs}}\in {\mathcal{W}}_{\mathrm{space}}$ is an obstacle point, and ${\overrightarrow{p}}_{\mathrm{surf}}\in {\mathcal{W}}_{\mathrm{space}}$ is on the obstacle surface.

Definition 3.

A C_space SDF ${\varsigma }_{\mathcal{C}}:{\mathcal{C}}_{\mathrm{space}}\mapsto \Re$ finds separation or penetration using a ${\mathcal{C}}_{\mathrm{space}}$ metric,

$${\varsigma }_{\mathcal{C}}\left(\mathit{q}\right)=\left\{\begin{array}{cc}\underset{{\mathit{q}}_b}{min}{\lambda }_C(\mathit{q},{\mathit{q}}_b),\hfill & \mathit{q}\in {\mathcal{C}}_{\mathrm{free}}\hfill \\ -\underset{{\mathit{q}}_b}{min}{\lambda }_C(\mathit{q},{\mathit{q}}_b),\hfill & \mathit{q}\notin {\mathcal{C}}_{\mathrm{free}}\hfill \end{array}\right.,$$

where ${\mathit{q}}_b$ is a configuration on the free space boundary.

Direct computation of workspace SDFs under $L^2$-norm metrics is described in prior work and implemented through open source libraries [2,3,4,40,41]. However, computing ${\mathcal{C}}_{\mathrm{space}}$ SDFs poses additional challenges due to the typically implicit representation of ${\mathcal{C}}_{\mathrm{space}}$ obstacles.

4.1.2. Bound Condition

We address SDFs for implicit ${\mathcal{C}}_{\mathrm{space}}$ of serial manipulators by identifying bounds to ensure separation. The idea is to satisfy two properties between a free configuration $\mathit{q}$ and another configuration ${\mathit{q}}^{\prime }$. First, we need an upper bound on workspace displacements going from $\mathit{q}$ to ${\mathit{q}}^{\prime }$. Second, we need a lower bound on separation distance at $\mathit{q}$. These properties form the following inequality,

$${\lambda }_W(p_{\mathit{q}},p_{{\mathit{q}}^{\prime }})\le {\lambda }_C(\mathit{q},{\mathit{q}}^{\prime })\le {\varsigma }_{\mathcal{W}}\left(\mathit{q}\right),$$

(1)

where $p_{\mathit{q}},p_{{\mathit{q}}^{\prime }}\in {\mathcal{W}}_{\mathrm{space}}$ are corresponding robot points at $\mathit{q}$,${\mathit{q}}^{\prime }$ with the greatest displacement. The first part of the inequality bounds the maximum workspace displacement; from $\mathit{q}$ to ${\mathit{q}}^{\prime }$, where robot points move by at most ${\lambda }_C(\mathit{q},{\mathit{q}}^{\prime })$. The second part of the inequality is the lower bound on separation, i.e., distance from $\mathit{q}$ to ${\mathit{q}}^{\prime }$ is less than separation at $\mathit{q}$. When this inequality holds, ${\mathit{q}}^{\prime }$ must also be free.

4.2. Workspace Displacement Bound

We now determine the upper bounds on workspace displacement for serial manipulators with revolute joints, addressing the first part of relation (1). Our analysis considers the end-effector, assuming it is the point of maximum displacement, which holds for typical cases of manipulators with thin links. Several prior research [29,36,37,38] used arc length at each joint to define ${\mathcal{C}}_{\mathrm{space}}$ displacement within convex volumes of ${\mathcal{C}}_{\mathrm{free}}$,

$$d_{arc}=\sum _{i=1}^n\left({∥{\theta }_i-{\theta }_i^{\prime }∥}_1*\sum _{j=i}^n{L}_j\right)$$

(2)

where ${\sum }_{j=i}^n{L}_j$ represents the radius of the cylinder whose axis is collocated with the axis of the $i^{th}$ joint and that encloses all the links starting from $i^{th}$ joint to the end-effector (see Figure 1a). The expression for $d_{arc}$ is a conservative upper bound on the workspace displacement ${\lambda }_W(p_{\mathit{q}},p_{{\mathit{q}}^{\prime }})$ of any point on the manipulator when the configuration changes from $\mathit{q}$ to ${\mathit{q}}^{\prime }$. Moving the robot from configuration $\mathit{q}$ to an arbitrary configuration ${\mathit{q}}^{\prime }$ within a lower bound (${\varsigma }_{\mathcal{W}}\left(\mathit{q}\right)$) means that no point on the kinematic chain will move more than separation (in case of free space displacement) and thus no collision will occur.

We use the chord length (see Figure 1b) between $B_1$ and $B_2$ to define tighter bound on our volumes,

$${\lambda }_C(\mathit{q},{\mathit{q}}^{\prime })=2*\sum _{i=1}^n\left({∥sin\left({\displaystyle \frac{{\theta }_i-{\theta }_i^{\prime }}{2}}\right)∥}_1*\sum _{j=i}^n{L}_j\right).$$

(3)

Hence, the inequality can be restated as,

$${\lambda }_W(\mathit{q},{\mathit{q}}^{\prime })\le {\lambda }_C(\mathit{q},{\mathit{q}}^{\prime })\le d_{arc}\le {\varsigma }_{\mathcal{W}}\left(\mathit{q}\right)$$

(4)

4.3. Separation Bound

Next, we use ${\mathcal{C}}_{\mathrm{space}}$ distances that lower-bound separation to identify free space volumes, addressing the second part of relation in 1. First, displacements from a free configuration that are less than separation must also yield another free configuration (Lemma 1). Then, we determine free volumes based on the separation distance (Theorem 1).

Lemma 1.

Given configurations $\mathit{q},{\mathit{q}}^{\prime }\in {\mathcal{C}}_{\mathrm{space}}$, when $\mathit{q}\in {\mathcal{C}}_{\mathrm{free}}$ and relation ${\lambda }_C(\mathit{q},{\mathit{q}}^{\prime })\le {\varsigma }_{\mathcal{W}}\left(\mathit{q}\right)$ holds, then ${\mathit{q}}^{\prime }\in {\mathcal{C}}_{\mathrm{free}}$ also.

Proof. 

From Figure 1b, the maximum workspace displacement from $O{B}_1(\mathit{q}$) to $O{B}_2\left({\mathit{q}}^{\prime }\right)$ cannot exceed ${\lambda }_C(\mathit{q},{\mathit{q}}^{\prime })$. When this displacement is less than workspace separation ${\varsigma }_{\mathcal{W}}\left(\mathit{q}\right)$, the robot has not moved far enough to collide with any obstacle, so ${\mathit{q}}^{\prime }$ must also be free.    □

Theorem 1.

Let $\mathcal{H}\left(\mathit{q}\right)$ be the hypercube centered at $\mathit{q}$ with radial length ${\lambda }_C(\mathit{q},{\mathit{q}}^{\prime })\le {\varsigma }_{\mathcal{W}}\left(\mathit{q}\right)$ in ${\mathcal{C}}_{\mathrm{space}}$. Hypercube $\mathcal{H}\left(\mathit{q}\right)$ is fully in the free space, $\mathcal{H}\left(\mathit{q}\right)\subseteq {\mathcal{C}}_{\mathrm{free}}$ if $\mathit{q}\in {\mathcal{C}}_{\mathrm{free}}$. Similarly, for $\mathcal{H}\left(\mathit{q}\right)\subseteq {\mathcal{C}}_{\mathrm{obs}}$ if $\mathit{q}\in {\mathcal{C}}_{\mathrm{obs}}$.

Proof. 

We show using Equations 3 and 4 that every configuration ${\mathit{q}}^{\prime }$ within hypercube $\mathcal{H}\left(\mathit{q}\right)$ has insufficient displacement to reach an obstacle. Hypercube $\mathcal{H}\left(\mathit{q}\right)$ consists of points ${\mathit{q}}^{\prime }{∥\mathit{q}-{\mathit{q}}^{\prime }∥}_1\le {\lambda }_C(\mathit{q},{\mathit{q}}^{\prime })$. Every ${\mathit{q}}^{\prime }\in \mathcal{H}\left(\mathit{q}\right)$ therefore satisfies relation ${\lambda }_C(\mathit{q},{\mathit{q}}^{\prime })\le {\varsigma }_{\mathcal{W}}\left(\mathit{q}\right)$, so ${\mathit{q}}^{\prime }\in {\mathcal{C}}_{\mathrm{free}}$ according to Equation 4. Since every ${\mathit{q}}^{\prime }\in \mathcal{H}\left(\mathit{q}\right)$ is in ${\mathcal{C}}_{\mathrm{free}}$, we know $\mathcal{H}\left(\mathit{q}\right)\subseteq {\mathcal{C}}_{\mathrm{free}}$.    □

Using Theorem 1, we construct hypercubes with circumscribed radius ${\lambda }_C(\mathit{q},{\mathit{q}}^{\prime })$ where $\mathit{q}$ is the center. Given the circumradius ${\lambda }_C(\mathit{q},{\mathit{q}}^{\prime })$ of a hypercube, we get the side length s in n-dimension, as

$$s={\displaystyle \frac{2*{\lambda }_C(\mathit{q},{\mathit{q}}^{\prime })}{\sqrt{n}}}.$$

(5)

Our hypercubes at any configuration $\mathit{q}$ is axis-aligned and the bounding box along each joint axis can be represented by set of linear inequalities.

$$\mathcal{H}\left(\mathit{q}\right)=\{x\in {\mathbb{R}}^n|l_i<x_i<u_i\forall i=1,\dots ,n\}$$

(6)

Here, $l_i$ refers to lower limit and $u_i$ is upper limit of an edge (1D) at each $i^{th}$ dimension. We can determine the lower and upper limit along each joint axis of hypercube for known centroid configuration $\mathit{q}$ and its side length s by computing $l_i=q_i-{\displaystyle \frac{s}{2}}$ and $u_i=q_i+{\displaystyle \frac{s}{2}}$.

The result of Theorem 1 is an explicit representation of ${\mathcal{C}}_{\mathrm{free}}$ that requires only an implicit ${\mathcal{C}}_{\mathrm{space}}$ specification in terms of workspace obstacles and robot kinematics. Specifically, we have derived a conservative under-approximation of the free configuration space. That is, every configuration within an identified free hypercube $\mathcal{H}\left(\mathit{q}\right)$ must necessarily be free, while some configurations just beyond the boundaries of $\mathcal{H}\left(\mathit{q}\right)$ could—and typically will—also be free. Further, the free hypercubes $\mathcal{H}\left(\mathit{q}\right)$ can be efficiently computed at any free configuration $\mathit{q}$ just by finding the workspace separation distance at $\mathit{q}$ (computable using off-the-shelf libraries [2,3]) and computing hypercube circumradius from the robot link lengths. Despite the conservatism of the approximation, such efficiently computable free space volumes offer a new capability to reason about sets of free configurations. In the next section, we will apply the free space volumes of Theorem 1 to strengthen efficiency and correctness guarantees of sampling-based planners.

5. Free Volume Graphs

We construct compact and certifiably continuous roadmaps as Free Volume Graphs (FVGs) where nodes are free space volumes and edges represent valid plans between nodes.

5.1. Graph Construction

Our approach to constructing FVGs is similar to PRM construction [8], with two key properties to support sparsity and guarantee the continuity of edges. First, each FVG node represents a volume or set of configurations. In contrast, nodes in baseline planning approaches commonly represent a single configuration. We further promote sparsity by rejecting new configurations enclosed by existing free volumes. Second, we ensure FVG edge continuity by only adding edges between free volumes that intersect. In contrast, baseline planners commonly check validity at discrete configurations along regular intervals between nodes. These two properties result in FVGs that are compact roadmaps in which graph paths guarantee the existence of continuous, collision-free motion plans.

5.1.1. Nodes

We add a new node to the FVG in a similar manner as PRMs by sampling a configuration and attempting to connect the existing graph to the new configuration (see Figure 1). However, there are two important differences in adding FVG nodes. First, when a newly sampled node is enclosed by a volume already in the FVG, we reject the sample (see Figure 2). Second, we must store the volume information for each node. In the case of hypercube nodes $\mathcal{H}\left(\mathit{q}\right)$, we store the center configuration $\mathit{q}$ and the edge length as described in Theorem 1. The additional volume information means that each FVG node will require more memory than a PRM node. However, our experimental results in Section 6 show that vastly fewer nodes are required for an FVG compared to a PRM, resulting in overall memory savings, even though each FVG node takes more space. Finally, our implementation in Section 6 is multi-directional and grows FVGs from a start and goal, similar to RRT-Connect [6], for faster convergence.

5.1.2. Edges

We add edges to the FVG between intersecting volume nodes (see Algorithm 2 and Figure 3). When adding a new node $v^{\prime }$, we create an edge to any existing node in the FVG that intersects $v^{\prime }$. Compared to validity checking at regular intervals, adding edges based on intersection guarantees the existence of continuous, collision-free plans between nodes.

We find an intersection/overlap between any two hypercubes ($H_i$ and $H_j$) using the edge bounding condition, such that the maxima of the lower limit are strictly less than or equal to the minima of the upper limit of the hypercubes.

$$max(l_1^i,l_2^i)\le min(u_1^i,u_2^i).$$

(7)

The edges are formed between the centers of hypercubes by interpolating through the intersecting regions between them.

5.1.3. Overall Graph

Finally, Algorithm 3 describes overall FVG construction. We progressively grow the graph until a start and goal node are in the same connected component.

5.2. Analysis

An FVG is an explicit, conservative representation of free space and valid motion plans. We provide a brief proof that an FVG path certifies the existence of a continuous motion plan, offering a stronger correctness guarantee than checking the validity of only individual configurations.

Proposition 1.

Let $G=VE$ be an FVG and $p=v_1\dots v_k$ be path through FVG (i.e., each $v_i\in V$ and each $v_i{v}_{i+1}\in E$). For a motion planning problem from ${\mathit{q}}_{\mathrm{start}}$ to ${\mathit{q}}_{\mathrm{goal}}$, where ${\mathit{q}}_{\mathrm{start}}\in v_1$ and ${\mathit{q}}_{\mathrm{goal}}\in v_k$, path p guarantees the existence of a continuous, collision-free motion plan from ${\mathit{q}}_{\mathrm{start}}$ to ${\mathit{q}}_{\mathrm{goal}}$.

Proof. 

We prove this by induction. For the basis, ${\mathit{q}}_{\mathrm{start}}\in v_1$ means path p contains a free configuration for the start. For the inductive step, $v_i{v}_{i+1}\in E$ means $v_i$ and $v_{i+1}$ are intersecting volumes, so there exists a collision-free plan from every ${\mathit{q}}_i\in v_i$ to every ${\mathit{q}}_{i+1}\in v_{i+1}$. By induction, there exists a continuous, collision-free plan from ${\mathit{q}}_{\mathrm{start}}\in v_1$ to ${\mathit{q}}_{\mathrm{goal}}\in v_k$. □

Proposition 1 shows that FVGs offer strict correctness guarantees by connecting nodes with intersecting volumes to provide a certificate of continuous motion plan existence.

6. Experimental Results

We evaluate the efficiency and correctness of our algorithm with a baseline PRM [8] from OMPL [42] to analyze for sampling density, multi-query, and uniform-randomness criteria during roadmap construction, collision checking and planning.

6.1. Experimental Setup

We run tests for the three manipulators and environments shown in Figure 4. The environments cover pick and place, kitchen, and industrial workcell tasks, and the robots are 6 or 7 DOF UR5, Franka, and Schunk LWA4D manipulators. We implement our planner through OMPL [42], model robot kinematics using Amino [43], and check collisions using the Collision Library [3]. We run our experiments on AMD ${\mathit{Ryzen}}^{TM}$ 9 7950X CPU with 16 cores at 4.5GHz.

6.2. Bound Consistency and Tightness

We compare the consistency of our upper bound (${\lambda }_C(p_{\mathit{q}},p_{{\mathit{q}}^{\prime }})$) on workspace displacement with the SNG bound [35], which employs the Euclidean distance metric in Equation 2. In contrast, our approach uses the Manhattan distance metric. Despite this difference in metric, we observe that both methods ensure a tight upper bound on workspace displacement, as illustrated in Figure 5.

Interestingly, the choice of distance metric can influence how tight the upper bound on workspace displacement is, allowing for a potentially more precise estimate in ${\mathcal{C}}_{\mathrm{space}}$.

6.3. Time and Memory Use

We construct roadmaps using the baseline planner and our approach (see Table 1). We varied robot and obstacle locations within the scenes to create different ${\mathcal{C}}_{\mathrm{space}}$ and then constructed roadmaps covering several goal configurations. Each table row shows averages plus-minus standard deviation error for 50 trials of one roadmap construction problem.

6.3.1. Overall Time and Memory

We compare the overall time and memory use of our method and the baseline PRM to determine speedup and memory reduction. We compute speedup as ${\displaystyle \frac{T_{\mathrm{P}RM}}{T_{\mathrm{F}VG}}}$, where T is time use, and we compute memory reduction as ${\displaystyle \frac{M_{\mathrm{P}RM}}{M_{\mathrm{F}VG}}}$, where M is memory use. We note that each FVG node requires one additional value to store the hypercube circumradius compared to PRM nodes that store only the configuration.

Overall, the results show FVGs require $1.3-23\times$ (times) less memory than PRMs to cover the same configurations in tested scenes. Time use improves up to $2\times$ for some scenes, but is slightly slower ($0.8\times$) in others, which we describe next through profiling results.

6.3.2. Collision Checking Time

We profiled the baseline and our approach to determine bottlenecks and subroutine costs (see Table 2). We used the gperftools [44] statistical profiler and ran 100 trials in each environment setting to collect sufficient statistics to identify bottlenecks and costs.

The bottlenecks for both our algorithm and the baseline are collision and distance checking. We compute average time T for a subroutine as $T=t{\displaystyle \frac{p}{n}}$, where t is the total running time, p is the percent of time spent in the subroutine (from profiling), and n is the number of subroutines calls (from a counter in the program). The FVGs required fewer collision-related calls than PRMs largely due to the fewer FVG nodes. However, distance checks are more expensive than collision checks, leading to overall similar running times.

7. Discussion and Future Work

This work introduces an approach to constructing conservative approximations of free space for serial manipulators and presents the Free Volume Graph (FVG) planning framework. The FVG enables the generation of sparse roadmaps using hypercubes to determine certification of continuous collision-free paths. Experimental comparisons demonstrate that, relative to traditional Probabilistic Roadmap Methods (PRM), the FVG approach achieves comparable construction times, up to 23× lower memory usage, and provides stronger correctness guarantees due to its conservative volumetric representation.

While the core ideas build upon earlier efforts in conservative motion planning and volumetric approximations [e.g., [29,36,37,38]], the FVG method was developed with extensibility in mind. In particular, it opens new possibilities for reasoning not only about the free space but also about the configuration space (${\mathcal{C}}_{\mathrm{space}}$) regions that are in collision or out-of-bounds. This dual characterization could be highly beneficial for improving rejection sampling efficiency by explicitly modeling invalid regions, thereby reducing the sampling burden in complex or constrained environments.

A key avenue for future work involves extending the FVG framework to approximate the boundary and interior of the invalid configuration space. If tight and enclosed intersections of high-dimensional hypercubes can be accurately computed, this would allow the construction of conservative collision regions, complementing the current free-space approximations. Such capability could significantly advance the development of tools for proving motion plan infeasibility, a critical but underexplored aspect of motion planning.

Additionally, we further plan to investigate the application of FVG in real-time planning scenarios, particularly for manipulators operating in high-dimensional and cluttered workspaces. Enhancements in incremental graph construction, parallelism, and data structure optimization [45,46,47,48,49] could further improve FVG’s practical deployment in robotics systems.