WorksComputer Science
GPU-accelerated Incremental Euclidean Distance Transform for Online Motion Planning of Mobile Robots
Abstract
In this letter, we present a volumetric mapping system that effectively calculates Occupancy Grid Maps (OGMs) and Euclidean Distance Transforms (EDTs) with parallel computing. Unlike these mappers for high-precision structural reconstruction, our system incrementally constructs global EDT and outputs high-frequency local distance information for online robot motion planning. The proposed system constructs OGM with massive amount of data from multiple types of sensor inputs. Using GPU programming techniques, the system quickly computes EDT in parallel within local volume. The new observation is continuously integrated into the global EDT using the parallel wavefront algorithm while preserving the historical observations. Experiments with datasets have shown that our proposed approach outperforms existing state-of-the-art robot mapping systems and is particularly suitable for mapping unexplored areas. In its actual implementations on aerial and ground vehicles, the proposed system achieves real-time performance with limited onboard computational resources.
Introduction
An accurate representation of the local environment is critical to the effectiveness of mobile robot navigation [1, 2, 3, 4]. Especially, the distance information to nearby obstacles is a must in many motion planning algorithms [5, 6, 7]. To that end, OGMs and EDTs are often used to bridge environment representation and planning. Both the OGM and EDT are voxel maps. In an OGM, each voxel records the probability of being occupied by an obstacle. An EDT records the distance information from each voxel to the nearest obstacle. Typically, a local EDT is required to be updated at high frequency for real-time performance of the local planner, while a global EDT is queried less frequently by the global planner.
Creating these maps is challenging due to limited onboard computational resources and real-time requirements of motion planning. To solve this problem, many researchers (see, e.g., [8, 9, 10, 11]) choose to sacrifice precision for increased runtime speed. However, their wavefront-based methods can incur large errors in real-world scenarios because the distance values only propagate on observed voxels. Besides, computational resources are highly stretched when updating global EDT with a lightweight onboard computing unit. As the demand for a faster update procedure of global incremental EDT rises, it is natural to explore the possibility of computing OGM and EDT in a parallel fashion using GPU onboard. Unfortunately, most of the methods designed for CPUs reported in the literature have very limited parallelization capability.
We also note that various approaches to distance transformation have been studied in the pattern recognition society (see, e.g., [12, 13, 14]). These methods are mainly developed for fast distance transformation on a fully known binary map, usually a 2D image. However, in robotic applications, the environment map needs to be incremental as the robot moves, while the standard sensors of mobile robots have a limited sensing range. Moreover, the size of the map cannot be determined in advance for situations in unknown environments.
Motivated by the fast demand in real-time applications for small robots, such as small UAVs and UGVs, we propose in this letter an innovative and effective approach to increase the efficiency and accuracy of the mapping process through massive-scale parallelization. It is designed for the navigation purposes of a mobile robot and allows an incremental map to be constructed with dynamically adjustable map size. The proposed mapping system is versatile enough to construct OGMs from commonly used onboard sensors such as depth cameras and 2D- or 3D-LiDAR. It computes a fast and exact batch EDT with the parallel Euclidean distance transform. The batch EDT is then integrated into the global EDT through a parallel wavefront mechanism. With the utilization of parallel computing techniques, both the local and global EDT can be updated in real-time. The proposed system can be deployed on miniature computing devices with GPU, such as Nvidia Xavier NX, commonly used for small robots or unmanned systems. We will make the source code of our system available as an open-source project.1
The contributions of this work are: i) the development of an efficient approach that generates OGM and EDT through massive parallelization; ii) the design of an EDT integration method that allows the incremental construction of global EDT with dynamically adjustable map size; and lastly iii) the integration of the proposed algorithm in various ground/aerial mobile robots to demonstrate its effectiveness with onboard motion planning.
The rest of this letter is organized as follows: In Section II, we recall some background materials in fast EDT algorithms and state-of-the-art mappers. In Section III, we present the overview and framework of our proposed mapping system. The technical details of the proposed system are given in Section IV. The experimental and simulation results that demonstrate the effectiveness of our system are presented in Section V, while the result conducted on robotic platforms is given in Section VI. Finally, we draw some concluding remarks in Section VII.
Related Work and Background
In occupancy maps, such as OGM or Octomap, they are usually discretized into voxel grids that store occupancy information. Recently, some researchers (see, e.g., [15], [16]) use occupancy maps to perform surface reconstruction and motion planning at the same time. They have mainly focused on high-resolution space representation rather than fast onboard distance checking. Although the constructed maps can be used in collision checking in corridor-based planners, information on the distance to obstacles is generally not captured.
For many optimization-based motion planning algorithms, it is necessary to query the distance between the trajectory and its nearest obstacle. The demand of multiple queries in each planning iteration dramatically affects the overall efficiency of the planning algorithm. One common approach is to pre-compute an EDT map such that the query process can be executed in time. For robots, such as miniature aerial vehicles (MAVs), the onboard computers generally have limited computational power due to the very limited payload of the vehicle. The demand on real-time EDT generation has forced to exclude EDT mappers in the process (see, e.g., [8], [17]). Similarly, researchers in [5, 6] only maintain a local and memoryless EDT to represent the environment around the vehicle, whereas obstacles outside the local area are ignored. Also, the works in [10, 11] construct incremental EDT online by careful implementation of dedicated data structures. More specifically, the method, Voxblox in [10] builds EDTs out of the so-called truncated signed distance fields (TSDFs), which contains projected distance information within a truncation radius near the surface. They utilized the distance to propagate throughout the whole space in a quasi-Euclidean metric and finally obtain an EDT map. However, the resulting EDT map has two sources of errors, which would accumulate through the propagation. On the other hand, the approach, FIESTA in [11] uses double-linked lists (DLLs) to record distance information and improve both mapping precision and efficiency. Although their methods have been implemented on mobile robots with vision-based sensors, the computation cost is extremely high when maintaining a global EDT in large areas.
Many studies have been carried out in computational geometry and pattern recognition society in developing fast and exact distance transform algorithms. Saito and Toriwaki [12] devise an algorithm to produce EDT by independently scanning on each dimension. Meijster et al. [13] follow the concept of [12] to enhance the overall performance by additionally utilizing properties of parabola intersections. These decomposition-based distance transform algorithms break down the whole mapping procedure into strips or structuring functions, which is suitable for parallel computing. However, these methods compute EDT in a batch of fixed-size memory, which is often not the case in robotics applications. To address these issues, we employ the technique of voxel hashing [18] to store the mapping history and further utilize parallel wavefront to maintain the consistency between the new incoming data and the existing map. Finally, we would like to note that our previous work, i.e., the technique, GPU-LO in [19], only considers the limited observation problem during the mapping process. We propose in this letter more wavefronts in the system proposed to tackle problems associated with real-world mapping tasks.
Overview of the Proposed System Framework
In what follows, we present a framework, depicted in Figure 1, for generating OGM and EDT through massive parallelization, which can be divided into three stages: OGM construction, batch EDT, and global EDT integration.
Figure 1. Framework of the proposed system
In the OGM construction stage, the system intakes data from the sensors and builds the OGM. Position and attitude information are usually estimated by localization algorithms, while range measurement can be obtained from onboard range sensors. The range measurement is then integrated into the global OGM as occupancy probabilities by parallel implementations of ray casting and volumetric projection.
In the batch EDT stage, the parallel EDT algorithm transforms the OGM in the Euclidean distance metric. Domain knowledge, such as pre-built OGM or pre-defined forbidden zones, can also be incorporated into the EDT process. The algorithm scans each dimension in turn and calculates the distance value for each voxel.
Finally, in the global EDT integration stage, the EDT in the local area is integrated into the hash table that stores global EDT. After identifying the proper sources, the parallel wavefront is utilized to propagate actual distance values over both the local and global EDT. Hence the batch EDT is updated with global observation, and the history can be preserved in the hash table.
Algorithms and the Pipeline
We present in the following the detailed algorithms for constructing OGM, batch EDT and global EDT integration.
OGM Construction
Two GPU-accelerated algorithms for OGM construction are developed. The first one is a massively-parallel version of the traditional ray casting algorithm [20], while the second one is a parallel volumetric projection method utilizing sensor models [21]. Each voxel of OGM contains its occupancy probability and its visibility information.
In ray casting, given a pixel at entry of a depth image, the algorithm first transforms the pixel to a 3D voxel :
where is the transformation from the world frame to the sensor frame, is the intrinsic parameters of the sensor, and is the measurement at with depth . Rays are then cast from the sensor to measured obstacle points and update the occupancy probability of each voxel along the line. Denoting the number of points perceived as , the average travel distance as , and the voxel size as , the computational complexity of ray casting process is in the order . One most significant challenge to parallelize ray casting is that multiple rays might try to write to the same voxel with different data, leading to serious synchronization issues of possible memory conflict. We resolve this problem using CUDA-atomic operations, which forces I/O operations to be mutually exclusive.
On the other hand, we adopt the volumetric projection method of [21], which is developed to construct TSDF using optical triangulation scanners, to build OGMs with sensors such as 2D- or 3D-LiDAR. Instead of casting rays from pixels in the depth image to the 3D space, the projection computes the appropriate sensor value of each voxel in the local volume by simulating the acquisition process according to the sensor model. Assuming the range measurement data to be two-dimensional, the projection from the voxel to the sensor data entry can be formulated as:
where is the projection onto the sensor with the projected range . Thereafter, by comparing the projected range with the measured range , occupancy information can be updated. The detailed procedure can be found in our previous work [19]. In volumetric projection, the computational complexity is proportional to the volume size, i.e., , if there are voxels within the local volume. As is generally huge in many applications, such an approach is rarely used in CPU. It can be massively parallelized, however, in GPU without CUDA-atomic operations.
The choice among various OGM construction methods depends heavily on input data. For example, during the construction of point cloud, some sensors choose not to generate a point if the corresponding ray hits no object within the detection range. If a ray casting method is applied directly, the volume covered by such a ray will not be updated as known and obstacle-free. Besides, when the resolution of the range sensor is extremely high, a down-sampling procedure is required to speed up ray casting. As such, we give preference to volumetric projection if intrinsic parameters of the range sensor can be obtained.
Batch EDT
Unlike the existing EDT construction methods of [9, 10, 11], which propagate distance from newly observed voxels in an ordered fashion, our method first evaluates EDT in the local volume with a GPU-based scanning technique without introducing propagation error. The parallel batch EDT workflow is given in Algorithm 1, which consists of one initial scan and several auxiliary scans. We extend the scanning method of Meijster et al. [13], which performs exact EDT in linear time, to the 3D space. It decomposes memory into independent lines and can therefore be parallelized. In fact, any other EDT algorithm with similar attributes (see, e.g., [14]) can be applied in our framework. In addition to calculating distance values, our algorithm tracks the nearest obstacle of each voxel during scans, which is essential for subsequent wavefront propagation.
Algorithm 1: BATCHEDT
Input: OGM , dimension of OGM Output: Batch EDT in the local volume and nearest obstacles recording
;
;
;
while do
;
;
;
;
return
In the case of computing a 3D EDT, our objective is to find the nearest obstacle with respect to each voxel . In the initial scanning , distance values of all occupied voxels are set as , while those of empty voxels are set as . When generating EDT in 1D, each column is required to be swept forward and backward to compute each grid of its distance to the nearest obstacle within the same column. The sweep always starts with a value of at the obstacle and then increments in empty areas. This procedure can be described as:
where is the distance value of voxel when scanning the first dimension.
Figure 2 illustrates the auxiliary scan on the X-Y-plane. Every thread in the auxiliary scan on the X-Y-plane, every thread executes a sweeping process along X-axis while fixing the value on Y- and Z-axes. Assuming the nearest obstacle of voxel lies on the column , the distance function of its value can be written as
Figure 2. An illustration of distance functions in AUXSCAN(). When a voxel (the red dot) is inspected by the current thread, it will be assigned to one parabola segment given the lower envelope (pink curve). The blue dots denote the possible positions of the nearest obstacle of .
where is regarded as a constant since it is already calculated at INITSCAN(). The distance function of each voxel can be represented as a parabola shifted by its value (as shown in Figure 2). Essentially, the calculation of distance values is to find the lower envelope of the union of all parabolas, which can be formulated as
where denotes all columns of the map in the X-Y-plane. When traversing the row, parabolas are inserted one by one. If the incoming parabola intersects the current lower envelope, a new parabola segment will be created, and the lower envelope will be updated. We refer readers to [13] for details. After calculating distance values on the X-Y plane, the function AUXSCAN() can then be adopted to progress on Z-axis in a similar fashion. Finally, the nearest obstacles in the entire 3D space are captured.
Lastly, each scan is to be followed by a TRANSPOSE() function. We utilize a tensor transpose library, cuTT [22], to reorder the memory of the local volume so that coalescing memory access can be achieved. After all scans, each voxel of the EDT goes through validation CHECKVALID(). Note that during the OGM construction, voxels are divided into three categories: free, occupied, and unknown. A voxel will be further integrated into the global EDT only if it is ever observed by the robot.
Global EDT Integration
With the batch EDT process, the observable region in the local volume is filled with EDT at each timestamp. However, the mapping accuracy is greatly affected by dynamic obstacles, sensor noise, and more severe, limited observation, which can be illustrated in Figure 3. More specifically, Figure 3(a) depicts an MAV flying in 2D grids. When computing batch EDT in the local volume (green dotted-line box), the map only reveals the information inside the local volume and has no knowledge about sensed regions in history. In Figure 3(b), as the vehicle moves to the right, the obstacles at the left boundary are no longer inside the updating range and are ignored in the batch EDT process. In consequence, inconsistencies between the global EDT and the current local EDT occur. Besides, dynamic obstacles can also pop up (the light salmon grid at lower right) and disappear (the grid with red dashed line at upper right) in the field of view.
Figure 3. Limited observation and dynamic obstacle problems. Batch EDT is computed within green dotted line. The obstacles are painted in the dark red grids. The color shade in the observed space represents the distance value. The blue grids are unobserved area.
To tackle the above problems, we combine the batch EDT with our global integration algorithm. Essentially, this algorithm extract frontiers (Algorithm 2), and then perform parallel wavefront (Algorithm 3) separately. Afterwards, the updated EDT in the current local volume is stored in a GPU hash table [18], which aims to make the mapping spatial efficient. The updated portion of hash table on GPU is streamed to CPU; hence the global EDT on both CPU and GPU are continuously constructed in the run time.
The source extraction function GETSOURCE() of Algorithm 2 is explained in detail. Line 2 inspects each voxel that lies on the boundary of the batch EDT and its neighbor that lies outside the local volume. If the nearest obstacle is inside the local volume, but the distance value of the neighbor in GPU hash table DIST(GHash, ) can be raised by one obstacle inside the local volume, it implies that disappears in the robot’s field of view (Line 6). Otherwise, if an obstacle suddenly pops up in the new observation, it will lower the distance value of (Line 8). If the nearest obstacle , which is outside the local volume, can lower the distance value inside the local volume DIST(, ), then is appended to the frontier corresponding to the limited observation situation (Line 10).
Subsequently, the parallel wavefront function PARWAVE-FRONT() in Algorithm 3 propagates the accurate distance from the frontiers to both inside and outside the local volume. The essential problem of conducting propagation in parallel
Algorithm 2: GETSOURCE
Input: Current local EDT , global EDT stored in GPU hash table
Output: Initial frontiers of propagation
forall do
forall do
if then
else if then
if then
return
Algorithm 3: PARWAVEFRONT (raise)
Input: Initial frontiers , , current local EDT , and global EDT in GPU hash table
Output: Updated local EDT and global EDT
while do
forall do
forall do
if then
else if then
return
is that GPU implementations of wavefront algorithms [23], [24] are inherently computationally inefficient and can even be slower than traditional CPU-based algorithms. We tackle this problem by extending hierarchical frontiers [25], which originally solves single-source shortest path problems, to our wavefront context, where a voxel can be visited more than once in one or more iterations. Algorithm 3 shows the details of the parallel wavefront propagation to raise the distance value caused by vanished obstacles. In the function CHOOSELEVEL(), the frontier levels are changed in accordance with the number of elements of the frontier. If it exceeds the maximum size of a GPU thread-block, the frontier is stored on the global memory for multiple thread-blocks to access. Otherwise, they are placed in the shared memory of one thread-block, which is faster than on the global memory. In Line 4, each cell of the frontier is placed on a single thread. Starting at Line 5, we enumerate all the neighbors of whose distance value is not raised. If the nearest obstacle of is found disappeared inside the local volume, will be propagated by and be inserted back into . Otherwise, in Line 11, if the current voxel encounters a neighbor that has a closer obstacle to , the algorithm adds to another frontier for a lowering wavefront. The algorithm for lowering wavefront is similar to Algorithm 3 except that it propagates the minimum distance value to the neighborhood. Following the wavefront processing outside the local volume, the wavefront of begins to propagate global observation into the local volume. We omit this algorithm in the paper because of page limitation. Interested readers are referred to the detailed algorithm appended in the supplementary material.2 Finally, the propagation ends once all frontiers become empty.
Experimental Results
In this section, we conduct a series of thorough tests on three datasets: the UAV-pillar dataset, the UGV-Corridor dataset, and the Cow-and-Lady dataset. The UAV-Pillar dataset is synthesized in the Gazebo simulator, where an MAV is flying through pillars at a constant speed, gathering range information with an onboard 3D LiDAR. The UGV-Corridor dataset is recorded in a real-world environment with long and narrow corridors, where a UGV is equipped with a 16-line Velodyne LiDAR and drives through pedestrians. The Cow-and-Lady dataset is presented with the work Voxblox [10], and its data is collected using a depth camera in a small room.
Simulation in An Unexplored Environment
While exploring an unknown space, the robot updates its map with continuous sensor data stream. The degree of change in the sensed environment also affects the computational efficiency of the mapping process. We demonstrate that the speed of our method is stable while exploring an unknown environment.
To quantify the difference between two sensed environments, we define the change score as:
where is the set of all grids in the map, is the index of the grid, and are respectively the ground truth EDTs at two distinct timestamps over the global range. The unknown volume is treated as empty during ground truth EDT calculation. If there is no difference between the sensed environment at and , the score will be 0. The more the sensed environment changes, the higher the score is. The EDT updating speed versus the environment change score can be found in Figure 4, where the resolution is set as 0.2 m and the local update range is 20 m 20 m 6 m. The figure shows that the EDT construction time continues to increase as the environmental change score increases. However, the construction speed of our mapper remains almost constant. This is because the traditional mappers construct EDT purely with wave propagation and rely more on existing EDT.
Figure 4. EDT operation speed with different environmental change score.
Experiments on Real-world Datasets
We test each stage of our system separately to show how differences in techniques that affect performance. First, we show the necessity of transpose operation in the batch EDT stage. The coalescing pattern is realized by forcing each thread to access adjacent memory, and scanning is significantly accelerated. In this experiment, we test the UGV-Corridor dataset on an Xavier NX. Table I shows the difference between the approaches with and without the coalesced memory access. It is observed that the parallel EDT operation is much faster in the coalescing case.
| Method | Average Updating Time (ms) |
| Original | 46 |
| Coalescing | 19 |
Table I. Batch EDT time with and without coalesced memory access
Apart from the global EDT integration technique proposed in Section IV-C, we evaluate the methods on the Cow-and-Lady dataset with different graph traversal algorithms. The experiments are done on a PC with Intel i5-8300H CPU and a GTX1060 GPU, mapping m local range with m resolution. In our method, the GPU thread-block limit is set as 512, and 6-connectivity is used. The results of average updating time are shown in Table II. The baseline solution using dynamic brushfire on CPU with the bucket-heap data structure is adopted, which is similar to the implementation by Lau et al. [9]. This cache-oblivious data structure minimizes the I/O operations in theory. Following the work of [26], we have also implemented a GPU version3 of the bucket heap, aiming to speed up the computation. However, it turns out that this data structure is not suitable for GPU since its performance is even worse than that of the CPU bucket heap. In contrast, our proposed method is cache-aware and we have the freedom to decide the parameters catering to different computing platforms. Other techniques included for comparison with our works are the parallel Bellman-Ford method [27], the BFS (breadth-first search) method accelerated by prefix-sum4, and the algorithm with GPU priority queue5. The following experiments are conducted to show that our method outperforms the state-of-the-art EDT mapper, FIESTA
| Method | Average Updating Time (ms) |
| CPU bucket heap | 790 |
| GPU bucket heap | 2035 |
| GPU priority queue | 3020 |
| GPU Bellman-Ford | 250 |
| GPU Prefix Sum BFS | 22 |
| Ours | 10 |
Table II. Global EDT integration time with different propagation algorithms
[11]. All comparisons are done with the UGV-Corridor dataset, and the local volume is set to m. For results on the Cow-and-Lady dataset, we refer interested readers to the supplementary video.
First, we evaluate the performance of mappers on different computing platforms, i.e., a personal computer (with Intel i5-8300H CPU and a GTX1060 GPU) and an Nvidia Xavier NX. Since high-frequency local planning is usually limited within a local volume, the CPU-GPU stream is turned off in our method, while the accurate EDT inside the local volume is copied to CPU and broadcast by ROS (although the global EDT in our method can be directly accessed by a GPU-based motion planner [28]). In FIESTA, we simply disable the global_update option to update its EDT only within the local volume, which makes the condition work in its favor. The frequency of updating the EDT is 2 Hz, and the resolution is m. The elapsed time of constructing OGM and EDT are measured separately, and the results are plotted in bar charts as shown in Figure 5. Obviously, our method outperforms FIESTA in speed on both platforms. Furthermore, as discussed in § 5-A, the average EDT time of our method is expected to be less volatile.

Figure 5. The mapping results of different mappers on different platforms. The bold line in each center of the bar is the standard deviation of time throughout the run.
The second set of experiments is performed to investigate the extent to which hyperparameters, such as voxel size and connectivity, affect the accuracy of the EDT within the local volume. The results are shown in Figure 6. The mappers in this experiment take measurements from the UGV-Corridor dataset, construct their own OGM and EDT, and then calculate the RMS error of the EDT against ground truth. In the ground truth calculation, a K-D tree of the current OGM constructed by each mapper is first created. The nearest neighbor of each voxel is then found and the least distance to the obstacles is computed. Note that we only focus on the error induced by the EDT computation and do not include the error caused by the discretization of OGM. In Figure 6(a), all samples are tested with 6-connected in propagation. Since FIESTA is more accurate than Voxblox [10] according to Han et al. [11], we only compare our method with FIESTA. In both FIESTA and our method, global update is enabled while only the error in the local volume is taken into consideration. Also, the RMS error curve of the batch EDT version of our mapper is plotted to show the necessity of global EDT integration. We can see that the resolution and the RMS error are negatively correlated. The mapping accuracy with respect to connectivity is shown in Figure 6(b), where the resolution is fixed as 0.05 m. As the connectivity increases, the RMS error drops. From similar results in FIESTA, a likely explanation is that the error is mainly incurred by wavefront propagation. When the connectivity remains constant, as the number of voxels increases, error cases in the propagation has a larger chance to occur. On the other hand, as more neighbors are considered, the propagation error could be reduced. We refer interested readers to [29] for a more descriptive explanation. Compared to FIESTA, our mapper is more accurate in any situation because the exactness of our batch EDT method helps to reduce the error.

Figure 6. The RMS error in different parameter settings.
In addition to the experiments in the local volume, we compared the mapping performance with Voxblox [10], FIESTA [11], and GPU-LO [19] in terms of global EDT. From Figure 7(a), although maintaining the global EDT on both GPU and CPU causes extra overhead in our method, it is negligible compared to the drastically increasing load in FIESTA. Similarly, as shown in Figure 7(b), when measuring RMS error over the whole map, the newly proposed technique has outperformed FIESTA and GPU-LO. The results assure us that our system suffices to provide global EDT for online global motion planners.

Figure 7. The average speed and RMS error measured in the entire map.
Implementation on Actual Robotic Platforms
To verify that the entire mapping system can run on lightweight onboard computers, we conduct an experiment with an MAV navigating in an unexplored and cluttered indoor environment. The motion planner is taken directly from [30], which requires distance information from EDT. The range measurement comes from a 2D LiDAR mounted on the top of the MAV. An Intel RealSense T-265 tracking camera is used to estimate the current pose of the MAV. All computations
are done on an onboard Nvidia Xavier NX. In Figure 8, the MAV flies to an unmapped region while avoiding obstacles. The mapper updates EDT at 10 Hz within 20 m 20 m with 0.1 m voxel width, allowing the motion planner to obtain the required distance information in real-time.

Figure 8. The snapshots and the constructed maps when an MAV flying through obstacles. The movement is limited to a 2D plane since only a 2D LiDAR is used.
Lastly, Figure 9 depicts a global EDT map constructed using a UGV equipped with a 16-line LiDAR. The experiment was carried out in an indoor environment with narrow corridors. We set the resolution as 0.1 m while the local volume is 20 m 20 m 1.2 m. Using a computer with Intel i5 9300H CPU and Nvidia 1660Ti GPU, the vehicle navigates fully autonomously through walking pedestrians. The results show that our proposed technique is efficient and can be readily adopted for real applications.

Figure 9. OGM and EDT constructed by a UGV in narrow corridors.
Conclusion
In this letter, we have proposed a novel robotic volumetric mapping framework with parallel computing. The proposed system is capable of generating real-time OGMs and EDTs that can be used for rapid motion planning. The effectiveness and the efficiency of our proposed technique has been proved by various experiments in simulations, datasets, and the real world problems. With the deployment on the onboard computing devices of both MAV and UGV, we have demonstrated that our system can efficiently build incremental EDT for mobile robots to navigate in unexplored environments.
References
- [1]M. Lan, S. Lai, T. H. Lee, and B. M. Chen, “A survey of motion and task planning techniques for unmanned multicopter systems,” Unmanned Systems, vol. 9, no. 02, pp. 165–198, 2021.DOI
- [2]M. Mondal and S. Poslavskiy, “Offline navigation (homing) of aerial vehicles (quadcopters) in GPS denied environments,” Unmanned Systems, vol. 9, no. 02, pp. 119–127, 2021.DOI
- [3]S. Yuan, H. Wang, and L. Xie, “Survey on localization systems and algorithms for unmanned systems,” Unmanned Systems, vol. 9, no. 02, pp. 129–163, 2021.DOI
- [4]J. Q. Cui, S. K. Phang, K. Z. Ang, F. Wang, X. Dong, Y. Ke, S. Lai, K. Li, X. Li, J. Lin et al., “Search and rescue using multiple drones in post-disaster situation,” Unmanned Systems, vol. 4, no. 01, pp. 83–96, 2016.DOI
- [5]N. Ratliff, M. Zucker, J. A. Bagnell, and S. Srinivasa, “Chomp: Gradient optimization techniques for efficient motion planning,” in 2009 IEEE International Conference on Robotics and Automation. IEEE, 2009, pp. 489–494.DOI
- [6]B. Zhou, F. Gao, L. Wang, C. Liu, and S. Shen, “Robust and efficient quadrotor trajectory generation for fast autonomous flight,” IEEE Robotics and Automation Letters, vol. 4, no. 4, pp. 3529–3536, 2019.arxiv.org/abs/1907.01531
- [7]P. Arora and C. Papachristos, “Environment reconfiguration planning for autonomous robotic manipulation to overcome mobility constraints,” in 2021 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2021, pp. 2352–2358.DOI
- [8]R. Jarvis, “Distance transform based path planning for robot navigation,” in Recent trends in mobile robots. World Scientific, 1993, pp. 3–31.DOI
- [9]B. Lau, C. Sprunk, and W. Burgard, “Improved updating of Euclidean distance maps and Voronoi diagrams,” in 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2010, pp. 281–286.
- [10]H. Oleynikova, Z. Taylor, M. Fehr, R. Siegwart, and J. Nieto, “Voxblox: Incremental 3D Euclidean signed distance fields for on-board MAV planning,” in 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2017, pp. 1366–1373.DOI
- [11]L. Han, F. Gao, B. Zhou, and S. Shen, “Fiesta: Fast incremental euclidean distance fields for online motion planning of aerial robots,” in 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2019, pp. 4423–4430.
- [12]T. Saito and J.-I. Kawai, “New algorithms for Euclidean distance transformation of an n-dimensional digitized picture with applications,” Pattern recognition, vol. 27, no. 11, pp. 1551–1565, 1994.DOI
- [13]A. Meijster, J. B. Roerdink, and W. H. Hesselink, “A general algorithm for computing distance transforms in linear time,” in Mathematical Morphology and its applications to image and signal processing. Springer, 2002, pp. 331–340.
- [14]T. T. Cao, K. Tang, A. Mohamed, and T. S. Tan, “Parallel banding algorithm to compute exact distance transform with the GPU,” in Acm Siggraph Symposium on Interactive 3D Graphics and Games, 2010.
- [18]M. Nießner, M. Zollhöfer, S. Izadi, and M. Stamminger, “Real-time 3D reconstruction at scale using voxel hashing,” ACM Transactions on Graphics (ToG), vol. 32, no. 6, pp. 1–11, 2013.
- [19]Y. Chen, S. Lai, B. Wang, F. Lin, and B. M. Chen, “A GPU mapping system for real-time robot motion planning,” in 2021 IEEE International Conference on Real-time Computing and Robotics (RCAR). IEEE, 2021, pp. 762–768.DOI
- [20]J. Amanatides, A. Woo et al., “A fast voxel traversal algorithm for ray tracing,” in Eurographics, vol. 87, no. 3, 1987, pp. 3–10.DOI
- [21]B. Curless and M. Levoy, “A volumetric method for building complex models from range images,” 1996.
- [22]A.-P. Hynninen and D. I. Lyakh, “cuTT: A high-performance tensor transpose library for CUDA compatible GPUs,” arXiv preprint arXiv:1705.01598, 2017.arxiv.org/abs/1705.01598
- [23]P. Harish and P. J. Narayanan, “Accelerating large graph algorithms on the GPU using CUDA,” in International conference on high-performance computing. Springer, 2007, pp. 197–208.DOI
- [24]Y. Deng, B. D. Wang, and S. Mu, “Taming irregular eda applications on GPUs,” in Proceedings of the 2009 International Conference on Computer-Aided Design, 2009, pp. 539–546.DOI
- [25]L. Luo, M. Wong, and W.-m. Hwu, “An effective GPU implementation of breadth-first search,” in Design Automation Conference. IEEE, 2010, pp. 52–55.DOI
- [26]J. Iacono, B. Karsin, and N. Sitchinava, “A parallel priority queue with fast updates for GPU architectures,” arXiv preprint arXiv:1908.09378, 2019.arxiv.org/abs/1908.09378
- [27]F. Busato and N. Bombieri, “An efficient implementation of the Bellman-Ford algorithm for Kepler GPU architectures,” IEEE Transactions on Parallel and Distributed Systems, vol. 27, no. 8, pp. 2222–2233, 2015.DOI
- [28]H. Lu, Q. Zong, S. Lai, B. Tian, and L. Xie, “Flight with limited field of view: A parallel and gradient-free strategy for micro aerial vehicle,” IEEE Transactions on Industrial Electronics, 2021.DOI
- [29]O. Cuisenaire and B. Macq, “Fast Euclidean distance transformation by propagation using multiple neighborhoods,” Computer vision and Image understanding, vol. 76, no. 2, pp. 163–172, 1999.DOI
- [30]S. Lai, M. Lan, and B. M. Chen, “Model predictive local motion planning with boundary state constrained primitives,” IEEE Robotics and Automation Letters, vol. 4, no. 4, pp. 3577–3584, 2019.DOI



