Serialized Red-Green-Gray: Quicker Heuristic Validation of Edges in Dynamic Roadmap Graphs
Authors: Yulie Arad, Stav Ashur, Marta Markowicz, James D. Motes, Marco Morales, Nancy M. Amato
Organizations: Siebel School of Computing and Data Science, University of Illinois, 201 N. Goodwin Avenue, Urbana, IL 61801, USA · Department of Computer Science, Instituto Tecnológico Autónomo de México (ITAM), Mexico City, 01080, México
Motion planning in dynamic environments, such as robotic warehouses, requires fast adaptation to frequent changes in obstacle poses. Traditional roadmap-based methods struggle in such settings, relying on inefficient reconstruction of a roadmap or expensive collision detection to update the existing roadmap. To address these challenges we introduce the Red-Green-Gray (RGG) framework, a method that builds on SPITE to quickly classify roadmap edges as invalid (red), valid (green), or uncertain (gray) using conservative geometric approximations. Serial RGG provides a high-performance variant leveraging batch serialization and vectorization to enable efficient GPU acceleration. Empirical results demonstrate that while RGG effectively reduces the number of unknown edges requiring full validation, SerRGG achieves a 2-9x speedup compared to the sequential implementation. This combination of geometric precision and computational speed makes SerRGG highly effective for time-critical robotic applications.
Figures & tables
Figure 1: Examples of outer- and inner-approximations of swept volumes corresponding to roadmap edges. A manipulator link and a mobile robot are over-approximated by oriented bounding boxes (pink) and under-approximated by splines (dark blue). Obstacles are over-approximated by OBBs (gray) and under-approximated by oval shapes (light blue). In (a), o1 yields a gray edge (invalid) and o2 yields a green edge. In (b), o1 yields a gray edge (valid) and o2 yields a red edge.
Figure 2: Gray objects denote static obstacles while light blue objects denote dynamic obstacles. (a) Initial roadmap validation with the obstacle in its starting position. (b) Over-approximations of the edges represented as OBBs in dark blue. (c) Under-approximations of the edges represented as splines in light blue.
Figure 3: Light blue objects denote dynamic obstacles. (a) The edges corresponding to gray OBBs will be processed by SerRGG. (b) Red splines correspond to edges that are invalid, whereas gray splines are unknown (c) Final edge classification produced by SerRGG, where red edges are invalid, gray edges remain unknown, and green edges are valid.
Figure 4: Gray objects denote static obstacles while light blue objects denote dynamic obstacles. (a) The region of the spatial grid affected by the obstacle’s motion is highlighted in orange. (b) Roadmap edges associated with these grid cells are highlighted in orange and processed by SerRGG. (c) Final edge classification produced by SerRGG, where ref edges are invalid, gray edges remain unknown, and green edges are valid.
Nodes
10
100
1000
5000
10000
10000
20000
40000
60000
Edges
79
1204
11624
57148
115216
115069
230436
461054
691736
Preprocessing Time (s)
1.05
6.36
24.33
150.08
129.31
259.54
589.13
1087.74
1672.04
Table 1: Preprocessing Time as a Function of Roadmap Size
Nodes
Edges
Obstacle Size
SerRGG Time ( μ s)
RGG Time ( μ s)
SerRGG OA Time ( μ s)
SPITE Time ( μ s)
Unknown Edges (RGG)
Validate RGG Time ( μ s)
Unknown Edges (OA)
Validate OA Time ( μ s)
10
79
2×2×2
818.0
13.8
558.2
10.2
3.17
605.2
3.62
625.5
100
1204
2×2×2
756.9
45.3
610.8
24.9
10.36
1366.9
11.65
1370.2
1000
11624
2×2×2
731.0
178.3
571.3
108.0
78.22
5378.2
86.77
5339.2
10000
115216
2×2×2
825.4
1252.5
659.7
605.9
563.19
25056.7
634.22
24810.0
10
79
10×2×2
768.2
22.5
583.0
17.6
9.49
1591.9
13.88
1545.4
100
1204
10×2×2
829.1
124.5
664.7
122.3
46.88
5756.5
59.21
5704.8
Table 2: Comparing SPITE, RGG and SerRGG for Environment Size 20×20×20
Nodes
Edges
Obstacle Size
SerRGG Time ( μ s)
RGG Time ( μ s)
Speedup
Brute Force Time ( μ s)
Unknown Fraction
Validate Remaining Time ( μ s)
Environment
Factor
5000
57148
10×1×1
954.5
2396.1
2.5103
13502471.7
0.0198
80383.6
5×20×20
1
10000
115069
10×1×1
1277.4
5423.8
4.2460
23635076.6
0.0179
125486.8
10×20×20
2
20000
230436
10×1×1
1491.8
7220.9
4.8404
42407585.8
0.0105
142604.1
20×20×20
2
40000
461054
10×1×1
1788.9
7207.1
4.0288
83371215.6
0.0030
87439.3
40×20×20
2
60000
691736
10×1×1
2042.5
7315.1
3.5814
121769499
0.0034
166644.5
60×20×20
1.5
Table 3: Roadmap Size Test Results (Obstacle Size 10×1×1 )
Nodes
Edges
# Obstacles
Obstacle Size
SerRGG Time ( μ s)
RGG Time ( μ s)
Speedup
Brute Force Time ( μ s)
Unknown Fraction
Validate Remaining Time ( μ s)
1000
11624
3
2×2×2
942
547
0.5807
8618093.6
0.0249
71081
10000
115216
3
2×2×2
1617.9
4284.7
2.6483
40917146
0.0177
203030
1000
11624
3
10×1×1
1020.7
1317
1.2903
8738171
0.0450
131055.6
10000
115216
3
10×1×1
2874.3
11920.3
4.1472
41164237.6
0.0311
364856.6
1000
11624
3
10×2×2
1069.3
2218
2.0743
8781372.3
0.0670
211223.7
10000
115216
3
10×2×2
3646.2
30734.4
8.4292
42120244.7
0.0497
599736.5
Table 4: Number of Obstacles Test Results (Environment 20×20×20 )
We present the dynamic rotation-stacked visibility graph (dRVG), an online motion planner that guides polygonal robots to specified goals in initially unknown, static environ- ments. It merges local roadmaps from successive observations to plan collision-free translations and rotations without a uniform position grid. A spatial quadtree schedules sensing goals across regions to reduce repeated visits while retaining all orientation configurations for routing. Under exact sensing and geometric computation and star-shaped robot and envelope assumptions, dRVG with center scans is resolution-complete relative to full- map RVG at the same angular resolution. In experiments using footprint scans, dRVG solves all 140 cases across 20 difficult maps and seven angular resolutions within a 20 s planning budget, with a median planning time of 1.18 s at 360 orientation layers. Six microMVP demonstrations illustrate the complete online planning loop on a physical robot.
Duo Zhang, Hechen Zhang, Junshan Huang +1
Department of Computer Science, Rutgers, the State University of New Jersey, Piscataway, NJ, USA
This paper develops the Rapidly-iterating kinoDynamic Grid (RDG) algorithm, an asymptotically near-optimal kinodynamic motion planning algorithm that produces high quality solutions through rapid iteration. The algorithm leverages a state space grid decomposition to perform node selection, dynamics propagation, and graph revision in constant time complexity with respect to the number of nodes in the trajectory tree. Through a covering ball sequence induction proof, the algorithm is shown to be asymptotically near-optimal and probabilistically complete. Different subsystems of the algorithm are evaluated against common nearest-neighbor search-based methods at generating exploration bias. The RDG algorithm is evaluated through simulated trials in complex, kinodynamic motion planning problem environments up to 10 DOF relative to similar sparse, kinodynamic planning algorithms with optimality guarantees, SST and DIRT. The RDG algorithm outperforms both SST and DIRT in mean final solution quality by up to 104% and 40% respectively. Additionally, RDG maintained a 100% success rate, even on a 10-DOF test case where both SST and DIRT did not.
Image-goal navigation requires a global planner that connects visually specified endpoints while providing observations that a local policy can follow. Three-dimensional Gaussian Splatting (3DGS) provides map-registered RGB, opacity, and depth renderings, but does not explicitly encode navigable topology. We introduce GS-PRM, a Gaussian visual roadmap whose milestones, local connections, and visual references are constructed directly through 3DGS rendering without deriving an occupancy grid or signed distance field. We define render-based collision-free (RC-free) configurations and paths. Panoramic depth supplies conservative clearance for milestone validation and adaptive coverage, while edge-aligned orthographic rendering tests the robot's swept volume. We establish conditional soundness and sufficient conditions for deterministic query completeness and probabilistic construction completeness of GS-PRM. To validate the performance of GS-PRM, we conduct experiments in both simulated and real-world environments, demonstrating long-horizon navigation using goal-conditioned policies guided by GS-PRM. Compared with a state-of-the-art 3DGS-based navigation system, our GS-PRM-based navigation system achieves up to a twice higher average success rate and reduces travel time by up to 57.2% when both systems complete the navigation task.