Loop Closure Detection Based on Differentiable Manifold

Abstract

Loop closure detection is an important part of SLAM (simultaneous location and mapping), which can effectively reduce the cumulative error of the system after long period of exploration. The existing loop closure detection methods are mainly to evenly distribute the accumulated error in the robot trajectory, but the motion error of the actual robot is also related to its motion speed and rotation angle, while the corrected motion trajectory of the robot is difficult to match the real trajectory. Based on the analysis of the mechanism of robot motion error, this paper proposes a novel loop closure detection method based on differentiable manifold, which mainly includes real-time pose based on manifold tangent space and smooth motion trajectory model of robot based on differential geometry. Firstly, we introduce the Frenet framework structure and establish the corresponding manifold tangent space theory for the keyframe pose nodes. The real-time problem of robot motion is equivalent to the problem of finding the optimal angle tangent vector. Secondly, the motion speed between keyframes is used to determine the characteristics of the robot motion trajectory. We calculate the curvature and torsion of the curve composed of several nodes based on the manifold tangent space and then combine the curve interpolation and fitting of the keyframe nodes to achieve the approximation of the robot motion trajectory, and the smooth curve of the robot trajectory is obtained. Finally, the experiment verifies that the method in this paper can effectively ensure the continuity and smoothness of the robot’s trajectory, thereby reducing the cumulative error of the system and improving the accuracy of loop closure detection.