Moving object removal and surface mesh mapping for path planning on 3D terrain

This paper proposes a method of moving object removal and map building for path planning on 3D terrain. Our method introduces a map representation named surface mesh maps, which is built from 3D LIDAR points using graph-based SLAM with moving object removal and polygon mesh reconstruction. We need moving object removal since moving objects, such as pedestrians, are included in raw maps built by SLAM in dynamic environments. For path planning, it is desirable that only stationary (static) objects remain in the maps. Occupancy voxel filtering can be used to remove moving objects. Stationary objects have high occupancy probability, and moving objects have low occupancy probability. However, due to shallow incidence of the LIDAR beams in 3D space, it is difficult to determine the correct probability of occupancy. Hence, we improve moving object removal using a new inverse measurement model with incident angles and a two-pass scheme to properly calculate the occupancy probability. In addition, we propose a path planning method using a graph search and the graph structures of the surface mesh maps. Path costs are given to the nodes and the arcs of the graph since path planning on 3D terrain requires an appropriate cost calculation. We introduce node costs and arc costs based on slopes, roughness, height differences, and travel distances. Path planning is performed by a graph search using the costs and graph pruning using collision detection and mesh boundary detection. The main contribution of our method is to combine SLAM with moving object removal and a graph search for path planning on large-scale 3D terrain. We conducted experiments by traveling over 5 km in outdoor dynamic environments. The results showed that the proposed method is capable of moving object removal and surface mesh mapping for path planning on 3D terrain.