SG-ISBP: Orchard Robots Localization and Mapping with Ground Optimization and Loop Closure Detection Integration

Orchard robots tasks rely on highly accurate, real-time trajectory estimation and map-building. In this paper, a novel tightly-coupled LIDAR inertial simultaneous localization and mapping (SLAM) system, SG-ISBP-SLAM, is proposed. The algorithm involves both ground optimization and loop closure detection. Aiming at the uneven orchard ground, this paper designs a ground segmentation method from a global perspective. It takes the nearest neighbor seed plane as the base-line and iteratively grows a global plane based on principal component analysis (PCA). The LIDAR scan is divided into 3D concentric zone representations to assign an appropriate density of cloud points among bins. Based on the partition strategy, the improved spatial binary pattern (ISBP) is encoded for lower time-consuming loop closure detection. To validate the performance of the proposed algorithm, qualitative and quantitative experiments have been conducted. Experimental results indicate that SG-ISBP-SLAM provides low-time consumption and reliable non-flat ground segmentation capabilities. Moreover, the loop module can efficiently correct robot localization trajectory drift.