Parallel OctoMapping: A Scalable Framework for Enhanced Path Planning in Autonomous Navigation

Yihui Mao; Tian Tan; Xuehui Shen; Warren E. Dixon; Rushikesh Kamalapurkar

cs.ROMar 23, 2026

Parallel OctoMapping: A Scalable Framework for Enhanced Path Planning in Autonomous Navigation

Authors: Yihui Mao, Tian Tan, Xuehui Shen, Warren E. Dixon, Rushikesh Kamalapurkar

Organizations: Department of Mechanical and Aerospace Engineering, University of Florida, Gainesville, FL 32611 USA · Department of Electrical and Systems Engineering, University of Pennsylvania, Philadelphia, PA 19104 USA

Abstract

Mapping is essential in robotics and autonomous systems because it provides the spatial foundation for path planning. Efficient mapping enables planning algorithms to generate reliable paths while ensuring safety and adapting in real time to complex environments. Fixed-resolution mapping methods often produce overly conservative obstacle representations that lead to suboptimal paths or planning failures in cluttered scenes. To address this issue, we introduce Parallel OctoMapping (POMP), an efficient OctoMap-based mapping technique that maximizes available free space and supports multi-threaded computation. To the best of our knowledge, POMP is the first method that, at a fixed occupancy-grid resolution, refines the representation of free space while preserving map fidelity and compatibility with existing search-based planners. It can therefore be integrated into existing planning pipelines, yielding higher pathfinding success rates and shorter path lengths, especially in cluttered environments, while substantially improving computational efficiency.

Figures & tables

Fig. 1: System overview of our proposed mapping framework

Category	Term	Definition	Sym.
OctoMap	node	A hierarchical spatial unit in the Octree corresponding to a cubic region of space.
	leaf size	The edge length of a leaf node.	$r$
	threshold	A user-defined range to classify points as unsafe, safe or clear for navigation.	thr
	occupied state	A state in which the OctoMap node contains point cloud.
	unoccupied state	A state where no point cloud is marked in an OctoMap node.
Region	region	A leaf node equally separated into 4 regions in 2D and 8 regions in 3D.

TABLE I: Terminology and notation.

Fig. 2: Parallel insertion of point clouds into the Octree with concurrent node construction. Each leaf node is subdivided into 4 regions in 2D or 8 regions in 3D. In the figure, “ ” represents occupied, “ ” represents unoccupied, and “ ” denotes the threshold bounding box. The bounding box is centered on a leaf node and used to determine the region state; by default, the edge length of the bounding box is set to half the leaf size. The regions are classified as clear “ ” (no points), safe “ ”, or unsafe “ ” (see Fig. 3 ).

Fig. 3: Illustration of the threshold setup and region classification: clear , safe , and unsafe . The left panel depicts the boundary of the area, where

h

denotes half the edge length of an OctoMap leaf node (equal to the map resolution r), and the threshold distance is

\mathrm{thr}=h\cdot\mathrm{ratio}

, with

\mathrm{ratio}

specified at initialization. Red points lie outside the thresholded boundary, while blue points lie inside; these points determine the region safe state label ( unsafe / safe / clear ).

Fig. 4: Illustration of the mapping configuration from Octree leaf nodes (left) to leaf regions and their projection onto the OGM (right). The occupancy grid is shown with a dash–dot outline, OctoMap nodes with a long-dashed outline, and region partitions with solid lines.

Fig. 5: Illustration of the diagonal examination pairs and rules for determining grid navigability. is the unsafe region in the leaf node, is the safe region in the leaf node, and is the clear region in the leaf node. The arrow represents the direction of examination. The floating square with a black border indicates that the corresponding cell of OGM should be marked as occupied after the examination.

Fig. 6: Illustration of the OGM produced by our method. This panel shows the bottom-right corner of the map in Fig. 4 . For clarity, the OGM cell boundaries are drawn with a black dash-dotted outline, and the leaf-node boundary is shown as a gold long-dashed box. The nine OGM cells are labeled 1–9, and nine representative points are shown, colored by whether they lie inside or outside the threshold bound. The occupancy of diagonal pairs of OGM cells is determined by using Fig. 5 . The arrows indicate the feasible navigations between grid cells, and the colors encode each cell’s feasible navigations. Colored crosses mark the point cloud blocked during the Diagonal Examination for the corresponding colored cells.

Region pair	Axis-dir. navigability	Diag-dir. navigability	Marking in cell	Notes
[unsafe unsafe]	unsafe: ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ unsafe: ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗	unsafe: ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ unsafe: ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗	Marking both diagonal cells as occupied	Axis-aligned: $\lVert p-c\rVert_{\infty}\geq h\cdot\textit{ratio}$ $\Rightarrow$ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ Diagonal: both regions contain points $\Rightarrow$ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗
[safe safe]	safe: ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ safe: ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓	safe: ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ safe: ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗	Mark one as occupied to block diagonal access while preserving axis-aligned navigation in the other region	Axis-aligned: $\lVert p-c\rVert_{\infty}<h\cdot\textit{ratio}$ $\Rightarrow$ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ Diagonal: both regions contain points $\Rightarrow$ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗
[clear clear]	clear: ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ clear: ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓	clear: ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ clear: ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓	Keep unoccupied in both diagonal cells	Both regions do not contain points $\Rightarrow$ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓
[unsafe safe]	unsafe: ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ safe: ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓	unsafe: ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ safe: ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗	Mark the unsafe cell as occupied . Allow axis-aligned navigation in the safe region.	Axis-aligned: $\lVert p-c\rVert_{\infty}\geq h\cdot\textit{ratio}$ $\Rightarrow$ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ $\lVert p-c\rVert_{\infty}<h\cdot\textit{ratio}$ $\Rightarrow$ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ Diagonal: both regions contain points $\Rightarrow$ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗
[unsafe clear]	unsafe: ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ clear: ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓	unsafe: ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ clear: ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓	Mark the unsafe cell as occupied . Allow axis-aligned and diagonal navigation in the clear region.	Axis-aligned: $\lVert p-c\rVert_{\infty}\geq h\cdot\textit{ratio}$ $\Rightarrow$ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ Diagonal: unsafe region is non-empty $\Rightarrow$ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ The clear region contains no points $\Rightarrow$ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓
[safe clear]	safe: ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ clear: ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓	safe: ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ clear: ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓	Mark the safe cell as occupied . Allow axis-aligned and diagonal navigation in the clear region.	Axis-aligned: $\lVert p-c\rVert_{\infty}<h\cdot\textit{ratio}$ $\Rightarrow$ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ Diagonal: safe region is non-empty $\Rightarrow$ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ ✗ The clear region contains no points $\Rightarrow$ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓ ✓

TABLE II: Navigability State Determination Table based on the Diagonal Examination

Fig. 7: Top row (POMP): Using the obstacle setup in Fig. 4 and the mapping rule in Fig. 5 , POMP assigns each region an unsafe/safe/clear label (top left), updates the corresponding occupancy grid cells and their valid navigations between cells (top middle), and produces the final occupancy map (top right). Compared to direct OGM, POMP preserves more valid navigations (blue). Bottom row (direct OGM): For the same point cloud map, a standard occupancy grid voxelizes space (bottom left), marks a cell as occupied if it contains any points and marks the valid navigations between cells (bottom middle), and yields the final occupancy map (bottom right).

Panel of Figure 10 — Fig. 8: Paths planned using POMP (red) and direct (traditional) OGM (blue) superimposed on the point cloud (a) and on the occupancy grid (b). The orange box (enlarged on the right) highlights the region responsible for the shorter path. Differences between our POMP and direct OGM are highlighted in (c) by cell (1)-(5). These cells are classified as unoccupied by POMP but occupied by the direct OGM. In particular, keeping cells (2) and (3) free in our method explains the shorter path found, as it preserves a traversable narrow passage (see middle figure).

Fig. 9: Two maps utilized in the randomized point-cloud experiments.

Panel of Figure 12 — Fig. 10: Experiment A-1: Runtime comparison with point clouds, showing from top to bottom: $500{,}000$ , $1{,}000{,}000$ , and $2{,}000{,}000$ points.

Fig. 11: Experiment A-2: Comparison of the total runtime of OctoMap construction and the subsequent projection of region safe states from leaf nodes onto OGM cell occupancy, against three baseline map representations (direct OGM, OctoMap-MTX and the original OctoMap). The y-axis is shown in log scale.

Fig. 12: Experiment A-3: Comparison of the navigable space ratio (NSR) across OGM cell resolutions.

Fig. 13: Experiment A-4: Planning runtime and path length are reported only when both approaches successfully found a path. Note that at a resolution of 2.0 m, across all 500 trial maps, no path was found in the direct OGM, but paths were still found in the POMP maps.

	Resolution (m)
	1.0	1.5	2.0	2.5	3.0	3.5	4.0
POMP 0.95	82.2%	68.8%	58.8%	50.0%	38.4%	23.0%	15.4%
POMP 0.75	82.2%	67.4%	57.7%	46.4%	37.6%	21.8%	14.4%
POMP 0.50	81.4%	66.8%	56.3%	43.0%	35.0%	19.6%	13.0%
POMP 0.25	81.4%	66.4%	55.8%	42.4%	34.0%	19.0%	12.4%
OGM	81.4%	66.4%	55.6%	42.0%	32.2%	16.2%	9.6%

TABLE III: Experiment A-5: Pathfinding Rate across 500 frames with varying threshold ratio

Fig. 14: Benchmark maps used in the application experiments include Cambridge_15, Cambridge_16, Cloud_7, Apartment_0, Apartment_1, and Apartment_2.

Dataset	Map dimensions ( $\text{m}^{3}$ )	Point number
Cambridge_15	400 $\times$ 400 $\times$ 60	119684095
Cambridge_16	400 $\times$ 400 $\times$ 35	113159239
Cloud_7	45 $\times$ 104 $\times$ 12	12915992
Apartment_0	9.4 $\times$ 14.8 $\times$ 5.3	4564613
Apartment_1	10.7 $\times$ 7.9 $\times$ 2.9	1909995
Apartment_2	9.4 $\times$ 10.2 $\times$ 2.8	2136963

TABLE IV: Details of Maps in Datasets for Benchmark Experiment

Dataset	Cambridge_15			Cambridge_16			Apartment_0			Apartment_1			Apartment_2
Resolution (m)	1.0	3.0	5.0	1.0	3.0	5.0	0.05	0.10	0.50	0.05	0.10	0.50	0.05	0.10	0.50
OctoMap	5089.00	4893.54	4878.26	4825.89	4626.40	4613.28	364.46	236.26	190.91	152.30	97.51	79.40	168.87	109.26	89.23
OctoMap-MTX	2695.07	2650.77	2511.81	2528.09	2446.34	2346.05	112.45	102.49	86.37	40.87	36.98	33.38	45.62	41.39	37.37
OGM	454.99	450.45	447.62	430.51	425.16	421.18	17.11	17.68	16.94	7.16	7.01	7.02	8.35	7.99	7.26
Ours	314.25	244.38	220.13	296.04	233.06	214.62	19.71	13.04	8.07	7.22	4.57	4.04	8.40	5.40	4.11

TABLE V: Experiment B-1(a): Construction Runtime (ms) of OctoMap, Occupancy Grid Map, and Ours under Varying Resolutions on Five Maps (20-thread CPU)

Dataset	Cambridge_15			Cambridge_16			Apartment_0			Apartment_1			Apartment_2
Resolution (m)	1.0	3.0	5.0	1.0	3.0	5.0	0.05	0.10	0.50	0.05	0.10	0.50	0.05	0.10	0.50
OctoMap	3336.56	3222.84	3209.94	3167.78	3047.52	3039.78	220.76	144.46	121.60	88.18	59.50	51.10	99.44	67.50	57.98
OctoMap-MTX	1873.78	1826.14	1721.92	1724.86	1677.24	1613.94	63.84	57.66	48.41	16.7	15.44	14.16	18.92	17.06	15.82
OGM	437.32	433.98	427.26	417.36	407.78	403.34	17.46	16.82	16.12	7.24	6.90	6.92	8.08	8.02	8.10
Ours	213.78	158.74	135.08	205.08	151.14	128.54	17.20	11.78	6.58	8.02	5.88	3.16	8.68	6.84	3.64

TABLE VI: Experiment B-1(b): Construction Runtime (ms) of OctoMap, Occupancy Grid Map, and Ours under Varying Resolutions on Five Maps (24-thread CPU)

	Cambridge_15				Cambridge_16				Cloud 7
Resolution (m)	2	6	10	14	2	6	10	14	0.5	1.0	1.5	2.0	2.5
OGM Pathfinding (%)	84.1	46.7	36.7	20.2	81.4	24.6	7.0	0.4	82.8	45.2	25.0	5.4	0.7
POMP Pathfinding (%)	91.5	68.8	50.2	45.1	82.7	29.6	7.5	6.3	92.1	68.4	44.9	33.6	16.4
OGM Path Length (m)	225.46	218.34	233.43	211.10	222.58	241.99	177.16	-	45.90	48.08	52.62	33.23	-
POMP Path Length (m)	225.42	218.30	233.40	210.90	222.42	237.70	176.64	-	45.84	47.70	50.94	31.22	-
A* on OGM (ms)	217.113	5.148	0.853	0.03	130.048	3.045	0.071	-	86.984	8.314	1.928	0.074	-

TABLE VII: Experiment B-2 and B-3: Pathfinding metrics across resolutions.

Fig. 15: Cloud_7 from WSF-19 in the Treescope dataset. The upper-left panel shows the raw point cloud map, the lower-left panel shows the octree constructed from the point cloud, and the right panel shows the planning result. The blue path is generated by POMP, while the purple path is obtained using the direct OGM.

Fig. 16: Visualization of the input point cloud VAT-0723U-03 and the resulting POMP Octree, from left to right with leaf sizes $r=0.5\mathrm{m}$ , $1.0\mathrm{m}$ , and $2.0\mathrm{m}$ .

Res. (m)	POMP	OctoMap-MTX	OctoMap
0.2	$\textbf{1.536}\!\pm\!\textbf{0.008}$	$5.817\!\pm\!0.096$	$6.513\!\pm\!0.098$
0.5	$\textbf{1.333}\!\pm\!\textbf{0.009}$	$4.592\!\pm\!0.062$	$3.433\!\pm\!0.051$
1.0	$\textbf{1.289}\!\pm\!\textbf{0.007}$	$4.158\!\pm\!0.048$	$2.601\!\pm\!0.043$
1.5	$\textbf{1.264}\!\pm\!\textbf{0.008}$	$3.995\!\pm\!0.031$	$2.428\!\pm\!0.030$
2.0	$\textbf{1.255}\!\pm\!\textbf{0.011}$	$3.890\!\pm\!0.053$	$2.291\!\pm\!0.046$
2.5	$\textbf{1.233}\!\pm\!\textbf{0.006}$	$3.845\!\pm\!0.024$	$2.194\!\pm\!0.050$

TABLE VIII: Experiment C-1: Per-sweep Octree construction time (ms).

	VAT-0723U-03		VAT-0723U-04
Res. (m)	POMP	OctoMap	POMP	OctoMap
0.2	$\textbf{11.803}\!\pm\!\textbf{0.448}$	$52.364\!\pm\!1.643$	$\textbf{13.245}\!\pm\!\textbf{0.426}$	$59.363\!\pm\!1.530$
0.5	$\textbf{2.735}\!\pm\!\textbf{0.229}$	$9.136\!\pm\!0.366$	$\textbf{2.929}\!\pm\!\textbf{0.284}$	$9.569\!\pm\!0.482$
1.0	$\textbf{0.429}\!\pm\!\textbf{0.071}$	$1.573\!\pm\!0.157$	$\textbf{0.465}\!\pm\!0.288$	$1.650\!\pm\!\textbf{0.191}$
1.5	$\textbf{0.192}\!\pm\!\textbf{0.069}$	$0.468\!\pm\!0.082$	$\textbf{0.189}\!\pm\!\textbf{0.076}$	$0.475\!\pm\!0.079$
2.0	$\textbf{0.127}\!\pm\!0.066$	$0.238\!\pm\!\textbf{0.059}$	$\textbf{0.136}\!\pm\!0.062$	$0.236\!\pm\!\textbf{0.058}$

TABLE IX: Experiment C-2: Per-sweep mapping time (ms).

	VAT-0723U-03								VAT-0723U-04
Res. (m)	Succ. (%) $\uparrow$		A* time (ms) $\downarrow$		JPS time (ms) $\downarrow$		Length (m) $\downarrow$		Succ. (%) $\uparrow$		A* time (ms) $\downarrow$		JPS time (ms) $\downarrow$		Length (m) $\downarrow$
	POMP	OGM	POMP	OGM	POMP	OGM	POMP	OGM	POMP	OGM	POMP	OGM	POMP	OGM	POMP	OGM
0.2	$\textbf{90.60}\%$	$88.80\%$	$52.833$	52.200	$43.766$	42.787	15.466	$15.469$	$\textbf{88.20}\%$	$86.00\%$	$51.005$	50.810	$41.391$	$41.391$	15.015	$15.019$
0.5	$\textbf{69.00}\%$	$65.00\%$	1.100	$1.405$	0.571	$0.585$	15.491	$15.507$	$\textbf{73.60}\%$	$69.60\%$	1.053	$1.355$	0.579	$0.591$	14.594	$14.637$
1.0	$\textbf{44.00}\%$	$38.40\%$	0.113	$0.153$	$0.069$	0.068	14.303	$14.389$	$\textbf{42.80}\%$	$37.00\%$	0.124	$0.161$	0.074	$0.076$	15.207	$15.262$
1.5	$\textbf{27.80}\%$	$24.20\%$	0.050	$0.067$	0.033	$0.034$	16.427	$16.740$	$\textbf{26.60}\%$	$20.20\%$	0.048	$0.053$	0.032	$0.033$	16.024	$16.247$

TABLE X: Experiment C-3: Planning metrics across resolutions.

	VAT-0723U-03		VAT-0723U-04
Res. (m)	POMP	OGM	POMP	OGM
0.2	$\textbf{94.17}\%$	$93.44\%$	$\textbf{94.11}\%$	$93.41\%$
0.5	$\textbf{84.20}\%$	$82.08\%$	$\textbf{84.02}\%$	$81.92\%$
1.0	$\textbf{66.38}\%$	$62.35\%$	$\textbf{65.23}\%$	$61.41\%$
1.5	$\textbf{50.42}\%$	$45.18\%$	$\textbf{49.13}\%$	$43.40\%$
2.0	$\textbf{35.01}\%$	$27.71\%$	$\textbf{32.94}\%$	$26.53\%$

TABLE XI: Experiment C-4: Per-sweep Navigable Space Ratio (%).

Occupancy Grid Mapping

See all 17 papers

Feb 24, 2026

Efficient Hierarchical Any-Angle Path Planning on Multi-Resolution 3D Grids

Hierarchical, multi-resolution volumetric mapping approaches are widely used to represent large and complex environments as they can efficiently capture their occupancy and connectivity information. Yet widely used path planning methods such as sampling and trajectory optimization do not exploit this explicit connectivity information, and search-based methods such as A* suffer from scalability issues in large-scale high-resolution maps. In many applications, Euclidean shortest paths form the underpinning of the navigation system. For such applications, any-angle planning methods, which find optimal paths by connecting corners of obstacles with straight-line segments, provide a simple and efficient solution. In this paper, we present a method that has the optimality and completeness properties of any-angle planners while overcoming computational tractability issues common to search-based methods by exploiting multi-resolution representations. Extensive experiments on real and synthetic environments demonstrate the proposed approach's solution quality and speed, outperforming even sampling-based methods. The framework is open-sourced to allow the robotics and planning community to build on our research.

Victor Reijgwart, Cesar Cadena, Roland Siegwart +1

Autonomous Systems Lab, ETH Zürich, Switzerland

Motion Planning

Ask PDF

Oct 14, 2026

Tiered-Resolution Volumetric Mapping for Real-Time Simultaneous Planning and Mapping on Natural Outdoor Environments

Real-time volumetric mapping for a planning robot is dominated by the cost of integrating and storing information at the finest resolution, yet only a small fraction of any map ever lies on or near a path the planner actually considers. We exploit this with a tiered-resolution mapping mechanism that integrates LiDAR into a single map structure at two resolutions simultaneously: fine inside a corridor around the robot's current plan, and coarse everywhere else. As opposed to traditional navigation pipelines where path planning uses the mapping result and provides no feedback, we propose a feedback loop where planning also informs mapping to dynamically integrate information at different leaf resolutions. Across two real-world teleoperated datasets, replayed and scored offline against an independent full-resolution map, the tiered maps are 13-15x smaller and require 6-8x less integration CPU time than a uniform fine map, while producing paths at a similar cost per meter as the ones generated from fine resolution maps on both scenarios. A uniform coarse map reaches the same size savings but its path quality collapses (+210% cost per meter) on the harder scenario, because it over-discretizes terrain the robot must traverse. Tiering approximates the coarse map's cheapness without its failure mode.

Afonso E. Carvalho, David Portugal, Paulo Peixoto +1

Institute of Systems and Robotics, University of Coimbra, Coimbra, Portugal · Robotics Institute, Carnegie Mellon University, Pittsburgh, PA, USA

3D Mapping Robot Navigation Simultaneous Localization and Mapping

Ask PDF

Mar 26, 2026

CTS-PLL: A Robust and Anytime Framework for Collaborative Task Sequencing and Multi-Agent Path Finding

The Collaborative Task Sequencing and Multi-Agent Path Finding (CTS-MAPF) problem requires agents to accomplish sequences of tasks while avoiding collisions, posing significant challenges due to its combinatorial complexity. This work introduces CTS-PLL, a hierarchical framework that extends the configuration-based CTS-MAPF planning paradigm with two key enhancements: a lock agents detection and release mechanism leveraging a complete planning method for local re-planning, and an anytime refinement procedure based on Large Neighborhood Search (LNS). These additions ensure robustness in dense environments and enable continuous improvement of solution quality. Extensive evaluations across sparse and dense benchmarks demonstrate that CTS-PLL achieves higher success rates and solution quality compared with existing methods, while maintaining competitive runtime efficiency. Real-world robot experiments further demonstrate the feasibility of the approach in practice.

Junkai Jiang, Yitao Xu, Ruochen Li +2

Multi-Agent Planning Multi-Agent Path Finding Robust MAPF

Ask PDF

Abstract

Occupancy Grid Mapping

Explore similar work

Efficient Hierarchical Any-Angle Path Planning on Multi-Resolution 3D Grids

Tiered-Resolution Volumetric Mapping for Real-Time Simultaneous Planning and Mapping on Natural Outdoor Environments

CTS-PLL: A Robust and Anytime Framework for Collaborative Task Sequencing and Multi-Agent Path Finding