Point Cloud Mapping in Dynamic Environments using Static Probability based NDT matching for Autonomous Vehicles

Abstract

Point cloud map in autonomous vehicle is used in various applications such as localization and path planning. Point cloud map can be matched with the sensor information acquired in real time to find the current location of the vehicle and can be integrated with information that recognizes other vehicles to generate a route of the ego vehicle. Graph based SLAM is widely used to construct such point cloud maps. Pose of the vehicle in each time is expressed as node, the odometry between the two different nodes as edge, and sensor information can be utilized as constraints in nodes and edges. Gaining the accurate pose of each node is possible when all graphs are optimized to minimize the error of constraint. One of the various ways to produce edge constraint when generating graphs that is based on LiDAR point cloud is scan-matching. This method compares two LiDAR point clouds in continuous nodes and extracts the relative position between two nodes. However, various moving objects in real driving situations become an obstacle in scan-matching. Therefore, this research proposes a scan-matching based edge constraint generation method for graph-based SLAM that can solve the mapping problem in dynamic environment. This research calculates the static probability of each point and produces the edge constraint of graph through the scan-matching that can reflects the calculated probability. The static probability means the mathematical likelihood of a point being measured from a static object and calculated reflecting LiDAR characteristics such as multi-echo and beam divergence. Also, this paper proposes ‘Weighted NDT’, which is a modified NDT algorithm to weight the static probability and reinforcing the authority of points with higher probability. The results were utilized in graph construction to build a robust point cloud map even in dynamic environments. Finally, the proposed graph edge generation algorithm was compared with the functionality of the existing scan-matching algorithm using dynamic object rejection algorithm. As a result, it was confirmed that the proposed algorithm has a significantly lower matching error than the dynamic object removal method, and that more accurate point cloud map generation is possible.