\u4ee3\u7801\uff1aTixiaoShan\/LIO-SAM<\/a><\/p>\n We propose a framework for tightly-coupled lidar inertial odometry via smoothing and mapping, LIO-SAM, that achieves highly accurate, real-time mobile robot trajectory es- timation and map-building. LIO-SAM formulates lidar-inertial odometry atop a factor graph, allowing a multitude of relative and absolute measurements, including loop closures, to be incorporated from different sources as factors into the system. The estimated motion from inertial measurement unit (IMU) pre-integration de-skews point clouds and produces an initial guess for lidar odometry optimization. The obtained lidar odometry solution is used to estimate the bias of the IMU. To ensure high performance in real-time, we marginalize old lidar scans for pose optimization, rather than matching lidar scans to a global map. Scan-matching at a local scale instead of a global scale significantly improves the real-time performance of the system, as does the selective introduction of keyframes, and an efficient sliding window approach that registers a new keyframe to a fixed-size set of prior \u201csub-keyframes.\u201d The proposed method is extensively evaluated on datasets gathered from three platforms over various scales and environments.<\/p>\n \u672c\u8bba\u6587\u63d0\u51fa\u4e00\u79cd\u7d27\u8026\u5408\u7684<\/span>\u5e73\u6ed1\u5efa\u56fe\u6fc0\u5149\u60ef\u5bfc\u91cc\u7a0b\u8ba1\u6846\u67b6<\/span><\/span>\uff0cLIO-SAM\uff0c\u5b8c\u6210\u9ad8\u51c6\u786e\u5ea6\u3001\u5b9e\u65f6\u7684\u79fb\u52a8\u673a\u5668\u4eba\u8f68\u8ff9\u4f30\u8ba1\u548c\u5730\u56fe\u6784\u5efa\u3002LIO-SAM\u5728\u56e0\u5b50\u56fe\u4e0a\u5236\u5b9a\u4e86\u4e00\u4e2a\u6fc0\u5149\u60ef\u5bfc\u91cc\u7a0b\u8ba1\uff0c\u5141\u8bb8\u8bb8\u591a\u76f8\u5bf9\u548c\u7edd\u5bf9\u6d4b\u91cf\uff0c\u5305\u62ec\u95ed\u73af\uff0c\u4ece\u4e0d\u540c\u7684\u6765\u6e90\u4f5c\u4e3a\u56e0\u5b50\u56fe\u7684\u8f93\u5165\u7eb3\u5165\u7cfb\u7edf\u4e2d\u3002IMU\u9884\u79ef\u5206<\/span>\u7684\u8fd0\u52a8\u4f30\u8ba1\u5bf9\u70b9\u4e91<\/span>\u8fdb\u884c\u53bb\u7578\u53d8<\/span>\u5e76\u4ea7\u751f\u6fc0\u5149\u91cc\u7a0b\u8ba1\u4f18\u5316\u7684\u521d\u59cb\u4f30\u8ba1\u3002\u6fc0\u5149\u96f7\u8fbe\u91cc\u7a0b\u8ba1<\/span>\u65b9\u6848\u7528\u4e8e\u4f30\u8ba1IMU\u7684\u96f6\u504f<\/span>\u3002\u4e3a\u4e86\u786e\u4fdd\u5728\u5b9e\u65f6\u73af\u5883\u4e0b\u5f97\u5230\u8f83\u597d\u7684\u6027\u80fd\uff0c\u5728\u59ff\u6001\u4f18\u5316\u7684\u65f6\u5019\u8fb9\u7f18\u5316\u6389\u65e7\u7684\u6fc0\u5149\u96f7\u8fbe\u5e27<\/span><\/span>\uff0c\u800c\u4e0d\u662f\u5c06\u6fc0\u5149\u96f7\u8fbe\u70b9\u4e91\u5e27\u5339\u914d\u5230\u5168\u5c40\u5730\u56fe\u4e2d\u3002\u5c40\u90e8\u5c3a\u5ea6<\/span><\/span>\u800c\u4e0d\u662f\u5168\u5c40\u5c3a\u5ea6\u4e0a\u7684\u626b\u63cf\u5339\u914d<\/span>\u53ef\u4ee5\u663e\u8457\u9ad8\u7cfb\u7edf\u7684\u5b9e\u65f6\u6027\u80fd<\/span>\uff0c\u9009\u62e9\u6027\u5f15\u5165\u5173\u952e\u5e27<\/span>\u4ee5\u53ca\u6709\u6548\u7684\u6ed1\u52a8\u7a97\u53e3<\/span>\u65b9\u6cd5\uff0c\u5c06\u65b0\u7684\u5173\u952e\u5e27\u6ce8\u518c\u5230\u56fa\u5b9a\u5927\u5c0f\u7684\u201c\u5b50\u5173\u952e\u5e27\u201d\u4e2d<\/span>\u3002\u63d0\u51fa\u7684\u65b9\u6cd5\uff0c\u5728\u4e0d\u540c\u5c3a\u5ea6\u548c\u73af\u5883\u4e0b\uff0c\u4ece\u4e09\u4e2a\u5e73\u53f0\u641c\u96c6\u7684\u6570\u636e\u96c6\u4e0a\uff0c\u8fdb\u884c\u4e86\u5e7f\u6cdb\u7684\u8bc4\u4f30\u3002<\/p>\n State estimation, localization and mapping are fundamental prerequisites for a successful intelligent mobile robot, required for feedback control, obstacle avoidance, and planning, among many other capabilities. Using vision-based and lidar-based sensing, great efforts have been devoted to achieving high-performance real-time simultaneous lo- calization and mapping (SLAM) that can support a mobile robot\u2019s six degree-of-freedom state estimation. Vision-based methods typically use a monocular or stereo camera and triangulate features across successive images to determine the camera motion. Although vision-based methods are especially suitable for place recognition, their sensitivity to initialization, illumination, and range make them unreliable when they alone are used to support an autonomous navigation system. On the other hand, lidar-based methods are largely invariant to illumination change. Especially with the recent availability of long-range, high-resolution 3D lidar, such as the Velodyne VLS-128 and Ouster OS1-128, lidar becomes more suitable to directly capture the fine details of an environment in 3D space. Therefore, this paper focuses on lidar-based state estimation and mapping methods.<\/p>\n \u72b6\u6001\u4f30\u8ba1\u3001\u5b9a\u4f4d\u4e0e\u5efa\u56fe\u662f\u4e00\u4e2a\u6210\u529f\u7684\u667a\u80fd\u79fb\u52a8\u673a\u5668\u4eba\u7684\u57fa\u7840\u524d\u63d0\uff0c\u8fd9\u662f\u53cd\u9988\u63a7\u5236\u3001\u907f\u969c\u548c\u89c4\u5212\u7b49\u8bb8\u591a\u5176\u4ed6\u529f\u80fd\u6240\u5fc5\u9700\u7684\u3002\u5229\u7528\u57fa\u4e8e\u89c6\u89c9\u548c\u57fa\u4e8e\u6fc0\u5149\u96f7\u8fbe\u7684\u611f\u77e5\uff0c\u4eba\u4eec\u5df2\u7ecf\u4ed8\u51fa\u4e86\u5de8\u5927\u7684\u52aa\u529b\u81f4\u529b\u4e8e\u5b9e\u73b0\u79fb\u52a8\u673a\u5668\u4eba\u7684\u9ad8\u6027\u80fd\u5b9e\u65f6\u540c\u6b65\u5b9a\u4f4d\u4e0e\u5efa\u56fe\uff0c\u5b83\u53ef\u4ee5\u652f\u6301\u79fb\u52a8\u7684\u516d\u81ea\u7531\u5ea6\u7684\u72b6\u6001\u4f30\u8ba1\u3002\u57fa\u4e8e\u89c6\u89c9\u7684\u65b9\u6cd5\u901a\u5e38\u91c7\u7528\u5355\u76ee\u6216\u8005\u6df1\u5ea6\u76f8\u673a\uff0c\u5e76\u901a\u8fc7\u8fde\u7eed\u56fe\u50cf\u7684\u4e09\u89d2\u5316\u7279\u5f81\u6765\u786e\u5b9a\u76f8\u673a\u8fd0\u52a8\u3002\u5c3d\u7ba1\u57fa\u4e8e\u89c6\u89c9\u7684\u65b9\u6cd5\u7279\u522b\u9002\u5408\u4f4d\u7f6e\u8bc6\u522b\uff0c\u4f46\u662f\u89c6\u89c9\u65b9\u6cd5\u5bf9\u521d\u59cb\u5316\u3001\u5149\u7167\u3001\u8ddd\u79bb\u7684\u654f\u611f\u6027\uff0c\u4f7f\u5f97\u5b83\u4eec\u5728\u5355\u72ec\u7528\u4e8e\u652f\u6301\u81ea\u52a8\u5bfc\u822a\u7cfb\u7edf\u65f6\u4e0d\u662f\u5f88\u53ef\u9760\u3002\u53e6\u4e00\u65b9\u9762\uff0c\u57fa\u4e8e\u6fc0\u5149\u96f7\u8fbe\u7684\u65b9\u6cd5\u5728\u5f88\u5927\u7a0b\u5ea6\u4e0a\u4e0d\u53d7\u5149\u7167\u53d8\u5316\u7684\u5f71\u54cd\u3002\u7279\u522b\u662f\u968f\u7740\u6700\u8fd1\u63d0\u51fa\u7684\u8fdc\u8ddd\u79bb\u3001\u9ad8\u5206\u8fa8\u7387\u4e09\u7ef4\u6fc0\u5149\u96f7\u8fbe\uff0c\u6bd4\u5982Velodyne128\u7ebf\u4ee5\u53caOuster128\u7ebf\uff0c\u6fc0\u5149\u96f7\u8fbe\u53d8\u5f97\u66f4\u9002\u5408\u76f4\u63a5\u6355\u6349\u4e09\u7ef4\u7a7a\u95f4\u73af\u5883\u4e2d\u7684\u7ec6\u8282\u3002\u56e0\u6b64\uff0c\u672c\u6587\u7740\u91cd\u7814\u7a76\u57fa\u4e8e\u6fc0\u5149\u96f7\u8fbe\u7684\u72b6\u6001\u4f30\u8ba1\u548c\u5efa\u56fe\u65b9\u6cd5\u3002<\/p>\n Many lidar-based state estimation and mapping methods have been proposed in the last two decades. Among them, the lidar odometry and mapping (LOAM) method proposed in [1] for low-drift and real-time state estimation and mapping is among the most widely used. LOAM, which uses a lidar and an inertial measurement unit (IMU), achieves state-of- the-art performance and has been ranked as the top lidar- based method since its release on the KITTI odometry benchmark site [2]. Despite its success, LOAM presents some limitations – by saving its data in a global voxel map, it is often difficult to perform loop closure detection and incorporate other absolute measurements, e.g., GPS, for pose correction. Its online optimization process becomes less efficient when this voxel map becomes dense in a feature-rich environment. LOAM also suffers from drift in large-scale tests, as it is a scan-matching based method at its core.<\/p>\n \u6700\u8fd1\u4e8c\u5341\u5e74\uff0c\u51fa\u73b0\u4e86\u8bb8\u591a\u57fa\u4e8e\u6fc0\u5149\u96f7\u8fbe\u7684\u72b6\u6001\u4f30\u8ba1\u548c\u5efa\u56fe\u65b9\u6cd5\u3002\u5176\u4e2d\uff0cLOAM\u65b9\u6cd5\uff0c\u5728\u4f4e\u504f\u79fb\u548c\u5b9e\u65f6\u72b6\u6001\u4f30\u8ba1\u548c\u5efa\u56fe\u4e2d\uff0c\u662f\u5e94\u7528\u6700\u5e7f\u6cdb\u7684\u3002LOAM\u91c7\u7528\u6fc0\u5149\u96f7\u8fbe\u548cIMU\u8fbe\u5230\u4e86\u6700\u597d\u7684\u6027\u80fd\uff0c\u5e76\u81ea\u4eceKITTI\u91cc\u7a0b\u8ba1Benchmark\u699c\u5355\u53d1\u5e03\u4ee5\u6765\u4e00\u76f4\u88ab\u5217\u4e3a\u6700\u597d\u7684\u6fc0\u5149\u96f7\u8fbe\u65b9\u6cd5\u3002\u5c3d\u7ba1LOAM\u53d6\u5f97\u4e86\u6210\u529f\uff0c\u4f46\u8fd8\u662f\u5b58\u5728\u4e00\u4e9b\u9650\u5236\uff1a\u901a\u8fc7\u5c06\u6570\u636e\u4fdd\u5b58\u5728\u5168\u5c40\u4f53\u7d20\u5730\u56fe\u4e2d\uff0c\u901a\u5e38\u5f88\u96be\u6267\u884c\u56de\u73af\u68c0\u6d4b\u4ee5\u53ca\u7ed3\u5408\u5176\u4ed6\uff0c\u6bd4\u5982GPS\uff0c\u7edd\u5bf9\u6d4b\u91cf\u503c\u7528\u4e8e\u59ff\u6001\u6821\u6b63\u3002\u5f53\u4f53\u7d20\u5730\u56fe\u5728\u7279\u5f81\u4e30\u5bcc\u7684\u73af\u5883\u4e2d\u53d8\u5f97\u7a20\u5bc6<\/span>\u65f6\uff0cLOAM\u7684\u5728\u7ebf\u4f18\u5316\u8fc7\u7a0b\u5c31\u4f1a\u53d8\u5f97\u6548\u7387\u5f88<\/span>\u4f4e<\/span>\u3002LOAM\u5728\u5927\u89c4\u6a21\u6d4b\u8bd5\u4e2d\u4e5f\u5b58\u5728\u6f02\u79fb<\/span>\u95ee\u9898\uff0c\u56e0\u4e3a\u5b83\u7684\u6838\u5fc3\u662f\u4e00\u79cd\u57fa\u4e8escan-match<\/span>\u626b\u63cf\u5339\u914d\u7684\u65b9\u6cd5\u3002<\/p>\n In this paper, we propose a framework for tightly-coupled lidar inertial odometry via smoothing and mapping, LIO- SAM, to address the aforementioned problems. We assume a nonlinear motion model for point cloud de-skew, estimating the sensor motion during a lidar scan using raw IMU measurements. In addition to de-skewing point clouds, the estimated motion serves as an initial guess for lidar odometry optimization. The obtained lidar odometry solution is then used to estimate the bias of the IMU in the factor graph. By introducing a global factor graph for robot trajectory estimation, we can efficiently perform sensor fusion using lidar and IMU measurements, incorporate place recognition among robot poses, and introduce absolute measurements, such as GPS positioning and compass heading, when they are available. This collection of factors from various sources is used for joint optimization of the graph. Additionally, we marginalize old lidar scans for pose optimization, rather than matching scans to a global map like LOAM. Scan-matching at a local scale instead of a global scale significantly im- proves the real-time performance of the system, as does the selective introduction of keyframes, and an efficient sliding window approach that registers a new keyframe to a fixed- size set of prior \u201csub-keyframes.\u201d<\/p>\n \u672c\u6587\u4e2d\uff0c\u63d0\u51fa\u4e00\u4e2a\u901a\u8fc7\u5e73\u6ed1\u548c\u5efa\u56fe\u7684\u7d27\u8026\u5408\u6fc0\u5149\u60ef\u5bfc\u91cc\u7a0b\u8ba1\u6846\u67b6\uff0cLIO-SAM\uff0c\u7528\u6765\u89e3\u51b3\u4e0a\u9762\u63d0\u5230\u7684\u95ee\u9898\u3002\u6211\u4eec\u5047\u8bbe\u4e00\u4e2a\u70b9\u4e91\u53bb\u7578\u53d8\u540e\u7684\u975e\u7ebf\u6027\u8fd0\u52a8\u6a21\u578b\uff0c\u4f7f\u7528\u539f\u59cbIMU\u6d4b\u91cf\u6570\u636e\u4f30\u8ba1\u6fc0\u5149\u96f7\u8fbe\u626b\u63cf\u8fc7\u7a0b\u4e2d\u7684\u4f20\u611f\u5668\u8fd0\u52a8\u3002\u9664\u4e86\u70b9\u4e91\u53bb\u7578\u53d8\uff0c\u8fd0\u52a8\u4f30\u8ba1\u4f5c\u4e3a\u6fc0\u5149\u96f7\u8fbe\u91cc\u7a0b\u8ba1\u4f18\u5316\u7684\u521d\u59cb\u4f30\u8ba1\u3002\u5f97\u5230\u7684\u6fc0\u5149\u96f7\u8fbe\u91cc\u7a0b\u8ba1\u89e3\u51b3\u65b9\u6848\u7528\u4e8e\u4f30\u8ba1\u56e0\u5b50\u56fe\u4e2dIMU\u96f6\u504f<\/strong>\u3002\u901a\u8fc7\u5f15\u5165\u673a\u5668\u4eba\u8f68\u8ff9\u4f30\u8ba1\u7684\u5168\u5c40\u56e0\u5b50\u56fe\uff0c\u6211\u4eec\u5229\u7528\u6fc0\u5149\u96f7\u8fbe\u548cIMU\u6d4b\u91cf\u6709\u6548\u5730\u8fdb\u884c\u4f20\u611f\u5668\u878d\u5408\uff0c\u7ed3\u5408\u673a\u5668\u4eba\u59ff\u6001\u4e4b\u95f4\u7684\u4f4d\u7f6e\u8bc6\u522b\uff0c\u5e76\u5728\u53ef\u7528\u7684\u65f6\u5019<\/span>\uff0c\u5f15\u5165GPS<\/span>\u5b9a\u4f4d\u6216\u8005\u6307\u5357\u9488\u822a\u5411<\/span>\u7b49\u7edd\u5bf9\u6d4b\u91cf\u3002\u4e0d\u540c\u6765\u6e90\u56e0\u5b50\u7684\u96c6\u5408\u7528\u4e8e\u56e0\u5b50\u56fe<\/span>\u7684\u8054\u5408\u4f18\u5316\u3002\u6b64\u5916\uff0c\u6211\u4eec\u8fb9\u7f18\u5316\u65e7\u7684\u6fc0\u5149\u96f7\u8fbe\u626b\u63cf\u6765\u4f18\u5316\u59ff\u6001\uff0c\u800c\u4e0d\u662f\u7c7b\u4f3cLOAM\u90a3\u6837\u5c06\u626b\u63cf\u5339\u914d\u5230\u5168\u5c40\u5730\u56fe\u4e2d\u3002\u5c40\u90e8\u5c3a\u5ea6\u800c\u4e0d\u662f\u5168\u5c40\u5c3a\u5ea6\u4e0a\u7684\u626b\u63cf\u5339\u914dscan-match\u53ef\u4ee5\u663e\u8457\u63d0\u9ad8\u7cfb\u7edf\u7684\u5b9e\u65f6\u6027\u80fd\uff0c\u4ee5\u53ca\u9009\u62e9\u6027\u5730\u5f15\u5165\u5173\u952e\u5e27\u548c\u4e00\u79cd\u6709\u6548\u7684\u6ed1\u52a8\u7a97\u53e3\u65b9\u6cd5\uff0c\u5c06\u65b0\u7684\u5173\u952e\u5e27\u6ce8\u518c\u5230\u4e4b\u524d\u7684\u201c\u5b50\u5173\u952e\u5e27\u201d\u96c6\u5408\u4e2d\u3002<\/p>\n The main contributions of our work can be summarized as follows:<\/p>\n \u672c\u6587\u5de5\u4f5c\u7684\u4e3b\u8981\u8d21\u732e\u603b\u7ed3\u5982\u4e0b\uff1a<\/p>\n Lidar odometry is typically performed by finding the relative transformation between two consecutive frames us- ing scan-matching methods such as ICP [3] and GICP [4]. Instead of matching a full point cloud, feature-based matching methods have become a popular alternative due to their computational efficiency. For example, in [5], a plane- based registration approach is proposed for real-time lidar odometry. Assuming operations in a structured environment, it extracts planes from the point clouds and matches them by solving a least-squares problem. A collar line-based method is proposed in [6] for odometry estimation. In this method, line segments are randomly generated from the original point cloud and used later for registration. However, a scan\u2019s point cloud is often skewed because of the rotation mechanism of modern 3D lidar, and sensor motion. Solely using lidar for pose estimation is not ideal since registration using skewed point clouds or features will eventually cause large drift.<\/p>\n \u6fc0\u5149\u96f7\u8fbe\u91cc\u7a0b\u8ba1\u901a\u5e38\u662f\u901a\u8fc7ICP\u30103\u3011\u6216\u8005GICP\u30104\u3011\u7b49scan-match\u626b\u63cf\u5339\u914d\u7b97\u6cd5\u67e5\u627e\u4e24\u4e2a\u8fde\u7eed\u5e27\u4e4b\u95f4\u7684\u76f8\u5bf9\u8f6c\u6362\u6765\u8fdb\u884c\u7684\u3002\u9664\u4e86\u5339\u914d\u4e00\u6574\u7247\u70b9\u4e91\uff0c\u57fa\u4e8e\u7279\u5f81\u7684\u5339\u914d<\/span>\u65b9\u6cd5\u5df2\u7ecf\u6210\u4e3a\u4e00\u79cd\u6d41\u884c\u7684\u66ff\u4ee3\u65b9\u6cd5\u3002\u4f8b\u5982\uff0c\u6587\u732e\u30105\u3011\u4e2d\u63d0\u51fa\u7684\u57fa\u4e8e\u5e73\u9762\u7684\u5b9e\u65f6\u6fc0\u5149\u96f7\u8fbe\u5339\u914d\u65b9\u6cd5\u3002\u5047\u8bbe\u5728\u7ed3\u6784\u5316\u73af\u5883\u4e2d\u8fdb\u884c\u64cd\u4f5c\uff0c\u5b83\u4ece\u70b9\u4e91\u4e2d\u63d0\u53d6\u5e73\u9762<\/span>\uff0c\u5e76\u901a\u8fc7\u89e3\u51b3\u6700\u5c0f\u4e8c\u4e58\u95ee\u9898\u5bf9\u5b83\u4eec\u8fdb\u884c\u5339\u914d<\/span>\u3002\u6587\u732e\u30106\u3011\u4e2d\u63d0\u51fa\u4e00\u79cd\u57fa\u4e8e\u9879\u5708\u7ebf\u7684\u91cc\u7a0b\u8ba1\u4f30\u8ba1\u65b9\u6cd5\u3002\u5728\u8be5\u65b9\u6cd5\u4e2d\uff0c\u4ece\u539f\u59cb\u70b9\u4e91\u4e2d\u968f\u673a\u751f\u6210\u7ebf\u6bb5\uff0c\u5e76\u7528\u4e8e\u4e4b\u540e\u7684\u70b9\u4e91\u914d\u51c6\u3002\u7136\u800c\uff0c\u7531\u4e8e\u73b0\u4ee3\u4e09\u7ef4\u6fc0\u5149\u96f7\u8fbe\u7684\u65cb\u8f6c\u673a\u5236\u548c\u4f20\u611f\u5668\u7684\u8fd0\u52a8\uff0c\u626b\u63cf\u7684\u70b9\u4e91\u901a\u5e38\u90fd\u662f\u503e\u659c\u7684\u3002\u4ec5\u4ec5\u4f7f\u7528\u6fc0\u5149\u96f7\u8fbe\u8fdb\u884c\u59ff\u6001\u4f30\u8ba1\u5e76\u4e0d\u662f\u7406\u60f3\u7684\uff0c\u56e0\u4e3a\u4f7f\u7528\u70b9\u4e91\u6216\u7279\u5f81\u8fdb\u884c\u914d\u51c6\u6700\u7ec8\u4f1a\u5bfc\u81f4\u5927\u7684\u6f02\u79fb\u3002<\/p>\n Therefore, lidar is typically used in conjunction with other sensors, such as IMU and GPS, for state estimation and mapping. Such a design scheme, utilizing sensor fusion, can typically be grouped into two categories: loosely-coupled fusion and tightly-coupled fusion. In LOAM [1], IMU is introduced to de-skew the lidar scan and give a motion prior for scan-matching. However, the IMU is not involved in the optimization process of the algorithm. Thus LOAM can be classified as a loosely-coupled method. A lightweight and ground-optimized lidar odometry and mapping (LeGO-LOAM) method is proposed in [7] for ground vehicle map- ping tasks [8]. Its fusion of IMU measurements is the same as LOAM. A more popular approach for loosely-coupled fusion is using extended Kalman filters (EKF). For example, [9]- [13] integrate measurements from lidar, IMU, and optionally GPS using an EKF in the optimization stage for robot state estimation.<\/p>\n \u56e0\u6b64\uff0c\u6fc0\u5149\u96f7\u8fbe\u901a\u5e38\u4e0e\u5176\u4ed6\u4f20\u611f\u5668\u8054\u5408\u4f7f\u7528\uff0c\u6bd4\u5982IMU\u6216\u8005GPS\uff0c\u7528\u4e8e\u59ff\u6001\u4f30\u8ba1<\/span>\u4ee5\u53ca\u5efa\u56fe\u3002\u8fd9\u79cd\u5229\u7528\u4f20\u611f\u5668\u878d\u5408\u7684\u8bbe\u8ba1\u65b9\u6848\uff0c\u901a\u5e38\u53ef\u4ee5\u5206\u4e3a\u4e24\u7c7b\uff1a\u677e\u8026\u5408\u878d\u5408\u4ee5\u53ca\u7d27\u8026\u5408\u878d\u5408\u3002\u5728LOAM\u4e2d\uff0c\u5f15\u5165IMU\u7528\u6765\u6fc0\u5149\u96f7\u8fbe\u626b\u63cf\u6570\u636e\u7684\u53bb\u7578\u53d8<\/span>\uff0c\u5e76\u7ed9\u51fa\u626b\u63cf\u5339\u914d\u7684\u8fd0\u52a8\u5148\u9a8c\u4fe1\u606f\u3002\u7136\u800c\uff0cIMU\u5e76\u6ca1\u6709\u6d89\u53ca\u5230\u7b97\u6cd5\u7684\u4f18\u5316\u8fc7\u7a0b\u3002\u56e0\u6b64LOAM\u53ef\u4ee5\u5f52\u7c7b\u4e3a\u677e\u8026\u5408<\/span>\u65b9\u6cd5\u3002\u6587\u732e\u30107\u3011\u4e2d\u63d0\u51fa\u4e00\u79cd\u7528\u4e8e\u5730\u9762\u8f66\u8f86\u5efa\u56fe\u4efb\u52a1\u7684\u8f7b\u91cf\u7ea7\u5730\u9762\u4f18\u5316\u6fc0\u5149\u96f7\u8fbe\u91cc\u7a0b\u8ba1\u4e0e\u5efa\u56fe\u7b97\u6cd5LeGO-LOAM\u3002\u5176\u5bf9IMU\u6570\u636e\u7684\u878d\u5408\u8fc7\u7a0b\u4e0eLOAM\u4e2d\u4e00\u81f4\u3002EKF\u6269\u5c55\u5361\u5c14\u66fc\u6ee4\u6ce2\u662f\u53e6\u4e00\u79cd\u66f4\u52a0\u6d41\u884c\u7684\u677e\u8026\u5408\u878d\u5408\u65b9\u6cd5\u3002\u4f8b\u5982\uff0c\u6587\u732e\u30109\u3011\u5230\u6587\u732e\u301013\u3011\u5728\u4f18\u5316\u9636\u6bb5\u4f7f\u7528EKF\u7ed3\u5408\u6fc0\u5149\u96f7\u8fbe\u548cIMU\/GPS\u89c2\u6d4b\uff0c\u6765\u7528\u4e8e\u673a\u5668\u4eba\u72b6\u6001\u4f30\u8ba1\u3002<\/p>\n Tightly-coupled systems usually offer improved accuracy and are presently a major focus of ongoing research [14]. In [15], preintegrated IMU measurements are exploited for de-skewing point clouds. A robocentric lidar-inertial state estimator, R-LINS, is presented in [16]. R-LINS uses an error-state Kalman filter to correct a robot\u2019s state estimate recursively in a tightly-coupled manner. Due to the lack of other available sensors for state estimation, it suffers from drift during long-during navigation. A tightly-coupled lidar inertial odometry and mapping framework, LIOM, is introduced in [17]. LIOM, which is the abbreviation for LIO-mapping, jointly optimizes measurements from lidar and IMU and achieves similar or better accuracy when compared with LOAM. Since LIOM is designed to process all the sensor measurements, real-time performance is not achieved – it runs at about 0.6\u00d7real-time in our tests.<\/p>\n \u7d27\u8026\u5408<\/span>\u7cfb\u7edf\u901a\u5e38\u63d0\u4f9b\u66f4\u9ad8\u7684\u7cbe\u5ea6\uff0c\u5e76\u4e14\u662f\u76ee\u524d\u6b63\u5728\u7814\u7a76\u7684\u4e3b\u8981\u70ed\u70b9\u3002\u6587\u732e\u301015\u3011\u4e2d\uff0cIMU\u9884\u79ef\u5206<\/span>\u88ab\u7528\u5230\u70b9\u4e91\u53bb\u7578\u53d8\u4e2d\u3002\u5728\u6587\u732e\u301016\u3011\u4e2d\u63d0\u51fa\u4e00\u79cd\u673a\u5668\u4eba\u6fc0\u5149\u60ef\u5bfc\u72b6\u6001\u4f30\u8ba1\u5668RLINS\u3002\u5b83\u4ee5\u4e00\u79cd\u7d27\u8026\u5408\u7684\u65b9\u5f0f\u9012\u5f52\u5730\u91c7\u7528\u9519\u8bef\u72b6\u6001\u5361\u5c14\u66fc\u6ee4\u6ce2\u5668<\/span>\u53bb\u77eb\u6b63\u673a\u5668\u4eba\u7684\u72b6\u6001\u4f30\u8ba1\u3002\u7531\u4e8e\u7f3a\u4e4f\u5176\u4ed6\u7528\u4e8e\u72b6\u6001\u4f30\u8ba1\u7684\u4f20\u611f\u5668\uff0c\u5b83\u5728\u957f\u8ddd\u79bb<\/span>\u5bfc\u822a\u8fc7\u7a0b\u4e2d\u5bb9\u6613\u51fa\u73b0\u6f02\u79fb<\/span>\u3002\u6587\u732e\u301017\u3011\u4e2d\u5f15\u5165\u4e00\u79cd\u7d27\u8026\u5408\u7684\u6fc0\u5149\u60ef\u5bfc\u91cc\u7a0b\u8ba1\u4e0e\u5efa\u56fe\u6846\u67b6\uff0cLIOM\u3002\u5176\u662fLIO-Mapping\u7684\u7f29\u5199\uff0c\u8054\u5408\u4f18\u5316\u4e86\u6fc0\u5149\u96f7\u8fbe\u548cIMU\u7684\u6d4b\u91cf\u7ed3\u679c\uff0c\u4e0eLOAM\u76f8\u6bd4\u83b7\u5f97\u4e86\u5dee\u4e0d\u591a\u751a\u81f3\u66f4\u597d\u7684\u7cbe\u5ea6\u3002\u56e0\u4e3aLIOM\u65b9\u6cd5\u88ab\u8bbe\u8ba1\u7528\u6765\u5904\u7406\u6240\u6709\u7684\u4f20\u611f\u5668\u6d4b\u91cf\u503c\uff0c\u56e0\u6b64\u4e0d\u80fd\u8fbe\u5230\u5b9e\u65f6\u6027–\u5728\u6211\u4eec\u7684\u6d4b\u8bd5\u4e2d\u5927\u7ea6\u8dd1\u4e86\u516d\u6210\u7684\u5b9e\u65f6\u901f\u5ea6\u3002<\/p>\n We first define frames and notation that we use throughout the paper. We denote the world frame as W and the robot body frame as B. We also assume the IMU frame coincides with the robot body frame for convenience. The robot state x can be written as:<\/p>\n \u9996\u5148\u6211\u4eec\u5b9a\u4e49\u6574\u4e2a\u8bba\u6587\u4e2d\u4f7f\u7528\u7684\u5750\u6807\u7cfb\u4e0e\u53d8\u91cf\u7b26\u53f7\u3002\u6211\u4eec\u5c06\u4e16\u754c\u5750\u6807\u7cfb<\/span>\u5b9a\u4e49\u4e3aW<\/span>\uff0c\u673a\u5668\u4eba\u5750\u6807\u7cfb<\/span>\u4e3aB<\/span>\u3002\u4e3a\u4e86\u65b9\u4fbf\u8d77\u89c1\uff0c\u6211\u4eec\u8fd8\u5047\u8bbe\u201cIMU\u5750\u6807\u7cfb\u4e0e\u673a\u5668\u4eba\u672c\u4f53\u5750\u6807\u7cfb\u91cd\u5408\u201d\u3002\u673a\u5668\u4eba\u7684\u72b6\u6001x\u53ef\u4ee5\u5199\u4e3a\uff1a<\/p>\n where R \u2208SO(3) is the rotation matrix, p \u2208R3 <\/span>is the position vector, v is the speed, and b is the IMU bias. The transformation T \u2208SE(3) from B to W is represented as T = [R |p].<\/p>\n \u5176\u4e2d R \u2208SO(3)<\/span>\u00a0\u662f\u5b9a\u4e49\u5728\u7279\u6b8a\u6b63\u4ea4\u7fa4\u4e0a\u7684\u65cb\u8f6c\u77e9\u9635\uff0cp \u2208R3<\/span>\u00a0<\/span> \u662f\u5e73\u79fb\u5411\u91cf\uff0cv \u662f\u901f\u5ea6\uff0cb\u662fIMU\u7684\u96f6\u504f\u3002\u53d8\u6362\u77e9\u9635 T \u2208SE(3)\u00a0<\/span> \u8868\u793a\u4ece\u673a\u5668\u4eba\u673a\u5750\u6807\u7cfb\u00a0<\/span> \u5230\u4e16\u754c\u5750\u6807\u7cfb\u7684\u7279\u6b8a\u6b27\u5f0f\u7fa4\uff0c\u8868\u793a\u4e3a T = [R |p]\u3002<\/p>\n \u56fe1. LIO-SAM\u7684\u7cfb\u7edf\u7ed3\u6784\u3002\u63a5\u65363D\u6fc0\u5149\u96f7\u8fbe\u3001IMU\u4ee5\u53caGPS\u4f5c\u4e3a\u8f93\u5165\u3002\u5f15\u5165\u56db\u79cd\u56e0\u5b50\u53bb\u6784\u5efa\u56e0\u5b50\u56fe\u3002a)IMU\u9884\u79ef\u5206\u56e0\u5b50\uff0cb) \u6fc0\u5149\u91cc\u7a0b\u8ba1\u56e0\u5b50\uff0cc) GPS\u56e0\u5b50\uff0cd) \u95ed\u73af\u56e0\u5b50\u3002\u8fd9\u4e9b\u56e0\u5b50\u5982\u4f55\u4ea7\u751f\u63cf\u8ff0\u5728\u7b2c3\u8282\u4e2d\u3002<\/p>\n An overview of the proposed system is shown in Figure 1. The system receives sensor data from a 3D lidar, an IMU and optionally a GPS. We seek to estimate the state of the robot and its trajectory using the observations of these sensors. This state estimation problem can be formulated as a maximum a posteriori (MAP) problem. We use a factor graph to model this problem, as it is better suited to perform inference when compared with Bayes nets. With the assumption of a Gaussian noise model, the MAP inference for our problem is equivalent to solving a nonlinear least-squares problem [18]. Note that without loss of generality, the proposed system can also incorporate measurements from other sensors, such as elevation from an altimeter or heading from a compass.<\/p>\n \u56fe 1 \u663e\u793a\u4e86\u6240\u63d0\u51fa\u7684\u7cfb\u7edf\u7684\u6982\u89c8\u3002\u8be5\u7cfb\u7edf\u4ece\u6fc0\u5149\u96f7\u8fbe\u3001IMU\u4ee5\u53ca\u53ef\u9009\u7684GPS<\/span>\u4e2d\u63a5\u6536\u4f20\u611f\u5668\u6570\u636e\u3002\u6211\u4eec\u8bd5\u56fe\u5229\u7528\u8fd9\u4e9b\u4f20\u611f\u5668\u7684\u89c2\u6d4b\u6765\u4f30\u8ba1\u673a\u5668\u4eba\u7684\u72b6\u6001\u53ca\u5176\u8f68\u8ff9\u3002\u72b6\u6001\u4f30\u8ba1\u95ee\u9898<\/span>\u53ef\u4ee5\u8868\u793a\u4e3a\u4e00\u4e2a\u6700\u5927\u540e\u9a8cMAP\u95ee\u9898<\/span>\u3002\u6211\u4eec\u4f7f\u7528\u56e0\u5b50\u56fe<\/span>\u53bb\u5efa\u6a21\u8fd9\u4e2a\u95ee\u9898\uff0c\u56e0\u4e3a\u4e0e\u8d1d\u53f6\u65af\u7f51\u7edc\u76f8\u6bd4\uff0c\u56e0\u5b50\u56fe\u66f4\u9002\u5408\u6267\u884c\u63a8\u7406<\/span>\u4efb\u52a1\u3002\u5728\u9ad8\u65af\u566a\u58f0\u6a21\u578b\u7684\u5047\u8bbe\u4e0b\uff0c\u6211\u4eec\u95ee\u9898\u7684\u6700\u5927\u540e\u9a8c\u4f30\u8ba1\u7b49\u4ef7\u4e8e\u6c42\u89e3\u4e00\u4e2a\u975e\u7ebf\u6027\u6700\u5c0f\u4e8c\u4e58<\/span>\u95ee\u9898\u3002\u6ce8\u610f\uff0c\u5982\u679c\u4e0d\u5931\u4e00\u822c\u6027\uff0c\u6211\u4eec\u63d0\u51fa\u7684\u7cfb\u7edf\u8fd8\u53ef\u4ee5\u5305\u542b\u6765\u81ea\u5176\u4ed6\u4f20\u611f\u5668\u7684\u6d4b\u91cf\uff0c\u6bd4\u5982\u9ad8\u5ea6\u8ba1\u7684\u6d77\u62d4\u9ad8\u5ea6\u4ee5\u53ca\u6307\u5357\u9488\u7684\u822a\u5411\u7b49\u3002<\/p>\n We introduce four types of f actors along with one variable type for factor graph construction. This variable, representing the robot\u2019s state at a specific time, is attributed to the nodes of the graph. The four types of factors are:<\/p>\n A new robot state node x is added to the graph when the change in robot pose exceeds a user-defined threshold. The factor graph is optimized upon the insertion of a new node using incremental smoothing and mapping with the Bayes tree (iSAM2) [19]. The process for genera<\/p>\n \u6211\u4eec\u4ecb\u7ecd\u56db\u79cd<\/span>\u7c7b\u578b\u7684\u56e0\u5b50<\/span>factors\uff0c\u4ee5\u53ca\u4e00\u79cd\u56e0\u5b50\u56fe\u6784\u9020\u7684\u53d8\u91cf\u7c7b\u578bvariable\u3002\u8be5\u53d8\u91cf\u8868\u793a\u673a\u5668\u4eba\u72b6\u6001\u5728\u7279\u5b9a\u65f6\u95f4\u7684\u72b6\u6001\u5f52\u5c5e\u4e8e\u56e0\u5b50\u56fe\u4e2d\u7684\u8282\u70b9nodes\u3002\u8fd9\u56db\u79cd\u56e0\u5b50\u5206\u522b\u662f\uff1a<\/p>\n \u5f53\u673a\u5668\u4eba\u59ff\u6001\u7684\u53d8\u5316\u8d85\u8fc7\u7528\u6237\u5b9a\u4e49\u7684\u9608\u503c\u65f6\uff0c\u56fe\u4e2d\u4f1a\u6dfb\u52a0\u4e00\u4e2a\u65b0\u7684\u673a\u5668\u4eba\u72b6\u6001\u8282\u70b9\u00a0<\/span>\u00a0\u3002\u5728\u4f7f\u7528\u8d1d\u53f6\u65af\u6811\uff08iSAM2\uff09\u589e\u91cf\u5f0f\u5e73\u6ed1\u548c\u5efa\u56fe\u8fc7\u7a0b\u63d2\u5165\u4e00\u4e2a\u65b0\u7684\u8282\u70b9\u65f6\uff0c\u56e0\u5b50\u56fe\u4f1a\u968f\u4e4b\u8fdb\u884c\u4f18\u5316\u3002<\/p>\n The measurements of angular velocity and acceleration from an IMU are defined using Eqs. 2 and 3:<\/p>\n IMU\u89d2\u901f\u5ea6\u548c\u52a0\u901f\u5ea6\u7684\u89c2\u6d4b\u516c\u5f0f\u5b9a\u4e49\u5982\u4e0b\uff1a<\/p>\n where\u02c6\u03c9t and \u02c6at are the raw IMU measurements in B at time t. \u02c6\u03c9t and \u02c6at are affected by a slowly varying bias bt and white noise nt. RBWt is the rotation matrix from W to B. g is the constant gravity vector in W.<\/p>\n \u5176\u4e2d \u02c6\u03c9t <\/span>\u548c \u02c6at <\/span>\u662f\u673a\u4f53\u5750\u6807\u7cfb\u4e0b t<\/span> \u65f6\u523b\u7684IMU\u89c2\u6d4b\u6e90\u6570\u636e\u3002\u02c6\u03c9t <\/span>\u548c \u02c6at \u5747\u53d7\u5230\u7f13\u6162\u53d8\u5316\u7684IMU\u96f6\u504f bt<\/span>\u00a0\u4ee5\u53ca\u767d\u566a\u58f0 nt<\/span> \u7684\u5f71\u54cd\u3002 RBWt \u662f\u4ece\u4e16\u754c\u5750\u6807\u7cfb\u5230\u673a\u4f53\u5750\u6807\u7cfb\u7684\u65cb\u8f6c\u77e9\u9635\u3002t\u00a0<\/span>\u00a0\u662f\u4e16\u754c\u5750\u6807\u7cfb\u00a0<\/span> W \u4e0b\u7684\u91cd\u529b\u5e38\u6570\u5411\u91cf\u3002<\/p>\n We can now use the measurements from the IMU to infer the motion of the robot. The velocity, position and rotation of the robot at time t + \u2206t can be computed as follows:<\/p>\n \u73b0\u5728\u6211\u4eec\u53ef\u4ee5\u91c7\u7528IMU\u6d4b\u91cf\u53bb\u63a8\u65ad\u51fa\u673a\u5668\u4eba\u7684\u8fd0\u52a8\u3002\u673a\u5668\u4eba\u5728 t + \u2206t<\/span>\u00a0\u65f6\u523b\u7684\u901f\u5ea6\u3001\u4f4d\u7f6e\u4ee5\u53ca\u65cb\u8f6c\u8ba1\u7b97\u5982\u4e0b\uff1a<\/p>\n where Rt = RWB t = RtBW T. Here we assume that the t angular velocity and the acceleration of B remain constant during the above integration.<\/p>\n \u5176\u4e2d\u00a0<\/span> Rt = RWB t = R t BW T\u3002\u8fd9\u91cc\u6211\u4eec\u5047\u8bbe\u673a\u5668\u4eba\u89d2\u901f\u5ea6\u548c\u52a0\u901f\u5ea6\u5728\u79ef\u5206\u8fc7\u7a0b\u4e2d\u4fdd\u6301\u5300\u901f\u4e0d\u53d8\u3002<\/p>\n We then apply the IMU preintegration method proposed in [20] to obtain the relative body motion between two timesteps. The preintegrated measurements \u2206vij , \u2206pij , and \u2206Rij between time i and j can be computed using:<\/p>\n \u7136\u540e\u6211\u4eec\u91c7\u7528\u6587\u732e\u301020\u3011\u63d0\u51fa\u7684IMU\u9884\u79ef\u5206<\/span>\u53bb\u83b7\u53d6<\/span>\u4e24\u4e2a\u65f6\u95f4\u6233\u4e4b\u95f4\u7684\u76f8\u5bf9\u8fd0\u52a8<\/span>\u3002\u9884\u79ef\u5206\u5f97\u5230\u7684\u7b2c i<\/span> \u548c\u7b2c j<\/span>\u00a0\u65f6\u523b\u4e4b\u95f4\u7684\u901f\u5ea6\u6d4b\u91cf \u2206vij<\/span>\u3001\u4f4d\u7f6e\u6d4b\u91cf \u2206pij <\/span>\u4ee5\u53ca\u65cb\u8f6c\u6d4b\u91cf \u2206Rij <\/span>\u8ba1\u7b97\u5982\u4e0b\uff1a<\/p>\n Due to space limitations, we refer the reader to the de- scription from [20] for the detailed derivation of Eqs. 7 – 9. Besides its efficiency, applying IMU preintegration also naturally gives us one type of constraint for the factor graph – IMU preintegration factors. The IMU bias is jointly optimized alongside the lidar odometry factors in the graph.<\/p>\n \u7531\u4e8e\u7bc7\u5e45\u9650\u5236\uff0c\u6211\u4eec\u8ba9\u8bfb\u8005\u53c2\u8003\u6765\u81ea\u6587\u732e\u301020\u3011\u7684\u63cf\u8ff0\uff0c\u6765\u5f97\u5230\u516c\u5f0f\u30102-9\u3011\u7684\u8be6\u7ec6\u63a8\u5bfc\u3002\u9664\u4e86IMU\u9884\u79ef\u5206\u7684\u6548\u7387<\/span>\u4e4b\u5916\uff0c\u5e94\u7528IMU\u9884\u79ef\u5206\u4e5f\u53ef\u4ee5\u5f88\u81ea\u7136\u5730\u7ed9\u51faIMU\u9884\u79ef\u5206\u56e0\u5b50\u5728\u56e0\u5b50\u56fe\u4e2d\u7684\u7ea6\u675f\u6761\u4ef6<\/span>\u3002IMU\u96f6\u504f\u56e0\u5b50<\/span>\u4e0e\u6fc0\u5149\u96f7\u8fbe\u91cc\u7a0b\u8ba1\u56e0\u5b50<\/span>\u5728\u56e0\u5b50\u56fe<\/span>\u4e2d\u4e00\u8d77\u8fdb\u884c\u8054\u5408\u4f18\u5316<\/span>\u3002<\/p>\n When a new lidar scan arrives, we first perform fea- ture extraction. Edge and planar features are extracted by evaluating the roughness of points over a local region. Points with a large roughness value are classified as edge features. Similarly, a planar feature is categorized by a small roughness value. We denote the extracted edge and planar features from a lidar scan at time i as Fei and Fpi respectively. All the features extracted at time i compose a lidar frame Fi, where Fi = {Fei , Fpi }. Note that a lidar frame F is represented in B. A more detailed description of the feature extraction process can be found in [1], or [7] if a range image is used.<\/p>\n \u5f53\u65b0\u7684\u4e00\u5708\u6fc0\u5149\u96f7\u8fbe\u626b\u63cf\u5230\u6765\u65f6\uff0c\u9996\u5148\u6267\u884c\u7279\u5f81\u63d0\u53d6<\/span>\u3002\u901a\u8fc7\u8bc4\u4f30\u5c40\u90e8\u533a\u57df<\/span>\u4e0a\u70b9\u7684\u7c97\u7cd9\u5ea6\/\u66f2\u7387<\/span>\u6765\u63d0\u53d6\u8fb9\u7f18\u548c\u5e73\u9762\u7279\u5f81<\/span>\u3002\u7c97\u7cd9\u5ea6\/\u66f2\u7387\u8f83\u5927\u7684\u70b9<\/span>\u88ab\u5212\u5206\u4e3a\u8fb9\u7f18\u7279\u5f81<\/span>\u3002\u7c7b\u4f3c\u5730\uff0c\u4e00\u4e2a\u5e73\u9762\u7279\u5f81\u4e5f\u53ef\u4ee5\u6309\u4e00\u4e2a\u5c0f\u7684\u7c97\u7cd9\u5ea6\/\u66f2\u7387\u8fdb\u884c\u5206\u7c7b\u3002 \u6211\u4eec\u5c06\u4ece\u6fc0\u5149\u96f7\u8fbe\u5728 t<\/span>\u00a0\u65f6\u523b\u626b\u63cf\u70b9\u4e91\u4e2d\u63d0\u53d6\u7684\u8fb9\u7f18\u7279\u5f81\u4ee5\u53ca\u5e73\u9762\u7279\u5f81\u5206\u522b\u5b9a\u4e49\u4e3a Fei<\/span> \u4ee5\u53ca\u00a0Fpi\u3002\u5728 t<\/span>\u00a0\u65f6\u523b\u63d0\u53d6\u7684\u6240\u6709\u7279\u5f81\u6784\u6210\u4e86\u4e00\u4e2a\u6fc0\u5149\u96f7\u8fbe\u70b9\u4e91\u5e27\u00a0<\/span> Fi\uff0c\u5176\u4e2d Fi = {Fei , Fpi }<\/span> \u3002\u6ce8\u610f\u4e00\u4e2a\u6fc0\u5149\u96f7\u8fbe\u70b9\u4e91\u5e27 F<\/span>\u00a0\u662f\u8868\u793a\u5728\u673a\u5668\u4eba\u5750\u6807\u7cfb\u4e0b\u7684\u3002\u7279\u5f81\u63d0\u53d6\u6b65\u9aa4\u7684\u66f4\u591a\u8be6\u7ec6\u63cf\u8ff0\u53ef\u4ee5\u5728\u6587\u732e\u30101\u3011\u6216\u5982\u679c\u7528\u5230\u8ddd\u79bb\u56fe\u50cf\u4e5f\u53ef\u4ee5\u53c2\u8003\u6587\u732e\u30107\u3011\u3002<\/p>\n Using every lidar frame for computing and adding factors to the graph is computationally intractable, so we adopt the concept of keyframe selection, which is widely used in the visual SLAM field. Using a simple but effective heuristic approach, we select a lidar frame Fi+1 as a keyframe when the change in robot pose exceeds a user-defined threshold when compared with the previous state xi. The newly saved keyframe, Fi+1, is associated with a new robot state node, xi+1, in the factor graph. The lidar frames between two keyframes are discarded. Adding keyframes in this way not only achieves a balance between map density and memory consumption but also helps maintain a relatively sparse factor graph, which is suitable for real-time nonlinear optimization. In our work, the position and rotation change thresholds for adding a new keyframe are chosen to be 1m and 10\u25e6 .<\/p>\n \u5982\u679c\u5c06\u6bcf\u4e00\u5e27\u6fc0\u5149\u96f7\u8fbe\u70b9\u4e91\u90fd\u62ff\u6765\u8ba1\u7b97\u56e0\u5b50\u5e76\u6dfb\u52a0\u5230\u56e0\u5b50\u56fe\u4e2d\u662f\u96be\u4ee5\u8ba1\u7b97\u7684<\/span>\uff0c\u56e0\u6b64\u6211\u4eec\u91c7\u7528\u4e86\u5173\u952e\u5e27\u9009\u62e9<\/span>\u7684\u6982\u5ff5\uff0c\u5176\u88ab\u5e7f\u6cdb\u5e94\u7528\u4e8e\u89c6\u89c9SLAM\u9886\u57df\u3002\u4f7f\u7528\u4e00\u79cd\u7b80\u5355\u4f46\u6709\u6548\u7684\u542f\u53d1\u5f0f\u65b9\u6cd5\uff0c\u5f53\u673a\u5668\u4eba\u59ff\u6001<\/span>\u7684\u53d8\u5316<\/span>\u4e0e\u5148\u524d\u7684\u72b6\u6001\u76f8\u6bd4\uff0c\u5982\u679c\u8d85\u8fc7<\/span>\u4e86\u7528\u6237\u5b9a\u4e49\u7684\u9608\u503c<\/span>\u65f6\uff0c\u6211\u4eec\u9009\u62e9\u6b64\u65f6\u7684\u6fc0\u5149\u96f7\u8fbe\u5e27\u4f5c\u4e3a\u5173\u952e\u5e27<\/span>\u3002\u65b0\u4fdd\u5b58\u7684\u5173\u952e\u5e27 \u4e0e\u56e0\u5b50\u56fe\u7684\u4e2d\u65b0\u7684\u673a\u5668\u4eba\u72b6\u6001\u8282\u70b9\u76f8\u5173\u8054<\/span>\u3002\u7136\u540e\u4e22\u5f03<\/span>\u6389\u4e24\u4e2a\u5173\u952e\u5e27\u4e4b\u95f4<\/span>\u7684\u5176\u4ed6\u6fc0\u5149\u96f7\u8fbe\u5e27\u3002\u8fd9\u6837\u6dfb\u52a0\u7684\u5173\u952e\u5e27\u4e0d\u4ec5\u53ef\u4ee5\u5b9e\u73b0\u5730\u56fe\u5bc6\u5ea6<\/span>\u548c\u5185\u5b58\u6d88\u8017<\/span>\u4e4b\u95f4\u7684\u5e73\u8861\uff0c\u800c\u4e14\u6709\u52a9\u4e8e\u4fdd\u6301\u4e00\u4e2a\u76f8\u5bf9\u7a00\u758f<\/span>\u7684\u56e0\u5b50\u56fe\u7ed3\u6784<\/span>\uff0c\u9002\u7528\u4e8e\u5b9e\u65f6<\/span>\u7684\u975e\u7ebf\u6027\u4f18\u5316<\/span>\u3002\u5728\u6211\u4eec\u7684\u5de5\u4f5c\u4e2d\uff0c\u6dfb\u52a0\u65b0\u5173\u952e\u5e27\u7684\u5e73\u79fb\u548c\u65cb\u8f6c\u7684\u53d8\u5316\u9608\u503c\u9009\u62e9\u4e3a1\u7c73\u548c10\u5ea6\u3002<\/p>\n Let us assume we wish to add a new state node xi+1 to the factor graph. The lidar keyframe that is associated with this state is Fi+1. The generation of a lidar odometry factor is described in the following steps:<\/p>\n \u5047\u8bbe\u6211\u4eec\u5e0c\u671b\u5728\u56e0\u5b50\u56fe\u4e2d\u6dfb\u52a0\u4e00\u4e2a\u65b0\u7684\u72b6\u6001\u8282\u70b9\u00a0<\/span> xi+1\u3002\u90a3\u4e48\u4e0e\u8be5\u72b6\u6001\u76f8\u5173\u8054\u7684\u6fc0\u5149\u96f7\u8fbe\u5173\u952e\u5e27\u4e3a\u00a0<\/span> Fi+1\u3002\u6fc0\u5149\u96f7\u8fbe\u91cc\u7a0b\u8ba1\u56e0\u5b50\u7684\u751f\u6210\u8fc7\u7a0b\u63cf\u8ff0\u5982\u4e0b\uff1a<\/p>\n We implement a sliding window approach to create a point cloud map containing a fixed number of recent lidar scans. Instead of optimizing the transformation between two consecutive lidar scans, we extract the n most recent keyframes, which we call the sub-keyframes, for estimation. The set of sub-keyframes {Fi\u2212n, …, Fi}is then transformed into frame W using the transformations {Ti\u2212n, …, Ti}associated with them. The transformed sub-keyframes are merged together into a voxel map Mi. Since we extract two types of features in the previous feature extraction step, Mi is composed of two sub- voxel maps that are denoted Mei , the edge feature voxel map, and Mpi , the planar feature voxel map. The lidar frames and voxel maps are related to each other as follows:<\/p>\n \u6211\u4eec\u5b9e\u73b0\u4e86\u4e00\u79cd\u6ed1\u52a8\u7a97\u53e3\u65b9\u6cd5<\/span>\u53bb\u6784\u5efa\u70b9\u4e91\u5730\u56fe\uff0c\u5176\u4e2d\u5305\u542b\u4e86\u56fa\u5b9a\u6570\u76ee\u7684\u6700\u8fd1\u6fc0\u5149\u96f7\u8fbe\u7684\u626b\u63cf\u70b9\u4e91\u3002\u6211\u4eec\u6ca1\u6709\u4f18\u5316\u4e24\u4e2a\u8fde\u7eed\u7684\u6fc0\u5149\u96f7\u8fbe\u626b\u63cf\u70b9\u4e91\u5e27\u4e4b\u95f4\u7684\u53d8\u6362\u5173\u7cfb\uff0c\u800c\u662f\u63d0\u53d6\u4e86n\u4e2a\u6700\u8fd1\u7684\u5173\u952e\u5e27\uff0csub-keyframes\u5b50\u5173\u952e\u5e27<\/span>\uff0c\u6765\u8fdb\u884c\u4f30\u8ba1\u3002\u91c7\u7528\u53d8\u6362\u5173\u7cfb{Ti\u2212n, …, Ti} <\/span>\u5c06\u5173\u8054\u7684\u5b50\u5173\u952e\u5e27\u96c6\u5408{Fi\u2212n, …, Fi}<\/span> \u8f6c\u6362\u5230\u4e16\u754c\u5750\u6807\u7cfb<\/span>\u4e2d\u3002\u8f6c\u6362\u540e\u7684\u5b50\u5173\u952e\u5e27\u4f1a\u88ab\u5408\u5e76\u6210\u4e00\u4e2a\u4f53\u7d20\u5730\u56feMi<\/span><\/span>\u3002\u7531\u4e8e\u6211\u4eec\u5728\u4e4b\u524d\u7684\u7279\u5f81\u63d0\u53d6\u6b65\u9aa4\u4e2d\u63d0\u53d6\u4e86\u4e24\u79cd\u7279\u5f81\uff1a\u8fb9\u7f18\u7279\u5f81\u4ee5\u53ca\u5e73\u9762\u7279\u5f81\uff0c\u56e0\u6b64\u4f53\u7d20\u5730\u56feMi\u00a0<\/span>\u7531\u4e24\u4e2a\u5b50\u4f53\u7d20\u5730\u56fe\u6784\u6210\uff0c\u5b9a\u4e49\u8fb9\u7f18\u7279\u5f81\u4f53\u7d20\u5730\u56fe\u4e3aMei\uff0c\u5e73\u9762\u7279\u5f81\u4f53\u7d20\u5730\u56fe\u4e3aMpi\u3002\u6fc0\u5149\u96f7\u8fbe\u5e27\u4ee5\u53ca\u4f53\u7d20\u5730\u56fe\u4e4b\u95f4\u7684\u5173\u7cfb\u5982\u4e0b\uff1a<\/p>\n \u2032Fei and \u2032Fpi are the transformed edge and planar features in W. Mei and Mpi are then downsampled to eliminate the duplicated features that fall in the same voxel cell. In this paper, n is chosen to be 25. The downsample resolutions for Me i and Mp i are 0.2m and 0.4m, respectively.<\/p>\n \u800c \u2032Fei<\/span>\u00a0\u548c \u2032Fpi\u00a0<\/span>\u00a0\u662f\u8f6c\u6362\u5230\u4e16\u754c\u5750\u6807\u7cfb\u4e0b\u7684\u8fb9\u7f18\u7279\u5f81\u548c\u5e73\u9762\u7279\u5f81\u3002\u7136\u540e\u5bf9\u5b50\u4f53\u7d20\u5730\u56fe Mei<\/span>\u00a0\u548c Mpi<\/span> \u8fdb\u884c\u4e0b\u91c7\u6837\uff0c\u6d88\u9664\u843d\u5728\u540c\u4e00\u4e2a\u4f53\u7d20\u5355\u5143\u4e2d\u7684\u91cd\u590d\u5197\u4f59\u7279\u5f81\u3002\u672c\u6587\u4e2d\uff0cn \u9009\u62e925\u3002Mei<\/span> \u548c Mpi \u4e0b\u91c7\u6837\u7684\u5206\u8fa8\u7387\u5206\u522b\u8bbe\u4e3a0.2m\u548c0.4m\u3002<\/p>\n We match a newly obtained lidar frame Fi+1, which is also {Fei+1, Fpi+1}, to Mi via scan- matching. Various scan-matching methods, such as [3] and [4], can be utilized for this purpose. Here we opt to use the method proposed in [1] due to its computational efficiency and robustness in various challenging environments.<\/p>\n \u6211\u4eec\u5c06\u65b0\u83b7\u5f97\u7684\u6fc0\u5149\u96f7\u8fbe\u5173\u952e\u5e27 Fi+1 = {Fei+1, Fpi+1}<\/span>\u00a0\u901a\u8fc7\u626b\u63cf\u5339\u914d\u5230\u4e16\u754c\u5750\u6807\u7cfb\u4e0b\u7684\u4f53\u7d20\u5730\u56fe Mi<\/span>\u00a0\u4e2d\u3002\u4e3a\u6b64\uff0c\u53ef\u4ee5\u4f7f\u7528\u5404\u79cd\u626b\u63cf\u5339\u914dscan-match\u65b9\u6cd5\uff0c\u6bd4\u5982ICP\u30103\u3011\u548cGICP\u30104\u3011\u3002\u8fd9\u91cc\uff0c\u6211\u4eec\u9009\u62e9\u4f7f\u7528\u6587\u732e\u30101\u3011\u4e2d\u63d0\u51fa\u7684\u65b9\u6cd5\uff0c\u56e0\u4e3a\u5b83\u5177\u5907\u51fa\u8272\u8ba1\u7b97\u6548\u7387\u548c\u5728\u5404\u79cd\u6709\u6311\u6218\u6027\u7684\u73af\u5883\u4e2d\u6709\u5f88\u597d\u7684\u9c81\u68d2\u6027\u3002<\/p>\n We first transform {Fe i+1, Fp i+1}from B to W and obtain {\u2032Fe \u2032Fp i+1, i+1}. This initial transformation is obtained by using the predicted robot motion,\u02dc Ti+1, from the IMU. For each feature in \u2032Fe i+1 or \u2032Fp i+1, we then find its edge or planar correspondence in Me i or Mp i . For the sake of brevity, the detailed procedures for finding these correspondences are omitted here, but are described thoroughly in [1].<\/p>\n \u6211\u4eec\u9996\u5148\u5c06\u6fc0\u5149\u96f7\u8fbe\u5173\u952e\u5e27 {Fe i+1, Fp i+1}<\/span>\u00a0\u4ece\u673a\u5668\u4eba\u5750\u6807\u7cfb\u8f6c\u6362\u5230\u4e16\u754c\u5750\u6807\u7cfb\u4e2d\uff0c\u5f97\u5230\u8f6c\u6362\u540e\u7684\u5173\u952e\u5e27\u96c6\u5408\u4e3a\u00a0<\/span> {\u2032Fe \u2032Fp i+1, i+1}\u3002\u521d\u59cb\u7684\u53d8\u6362\u5173\u7cfb\u662f\u901a\u8fc7\u4eceIMU\u9884\u6d4b\u7684\u673a\u5668\u4eba\u8fd0\u52a8 \u02dc Ti+1<\/span> \u4e2d\u5f97\u5230\u7684\u3002\u5bf9\u4e8e\u6bcf\u5e27 \u2032Fe i+1 \u6216 \u2032Fp i+1<\/span> \u4e2d\u7684\u8fb9\u7f18\u7279\u5f81\u548c\u5e73\u9762\u7279\u5f81\uff0c\u6211\u4eec\u7136\u540e\u627e\u5230\u5b83\u4eec\u5bf9\u5e94\u7684\u5b50\u4f53\u7d20\u5730\u56fe Mei \u548c Mpi\u00a0\u4e2d\u7684\u8fb9\u7f18\u548c\u5e73\u9762\u3002\u4e3a\u4e86\u7b80\u6d01\u8d77\u89c1\uff0c\u627e\u5230\u5bf9\u5e94\u5173\u7cfb\u7684\u8be6\u7ec6\u8fc7\u7a0b\u5728\u8fd9\u91cc\u7701\u7565\uff0c\u6587\u732e\u30101\u3011\u4e2d\u7ed9\u51fa\u4e86\u8be6\u7ec6\u7684\u63cf\u8ff0\u3002<\/p>\n The distance between a fea- ture and its edge or planar patch correspondence can be computed using the following equations:<\/p>\n \u8fb9\u7f18\u7279\u5f81\u4e0e\u5e73\u9762\u7279\u5f81\u548c\u5b83\u4eec\u5bf9\u5e94\u7684\u8fb9\u7f18\u6216\u5e73\u9762\u5757\u4e4b\u95f4\u7684\u8ddd\u79bb<\/span>\u53ef\u4ee5\u901a\u8fc7\u5982\u4e0b\u7684\u516c\u5f0f\u8fdb\u884c\u8ba1\u7b97\uff1a<\/p>\n where k, u, v, and w are the feature indices in their corresponding sets. For an edge feature pei+1,k in \u2032Fei+1, pei,u and pei,v are the points that form the corresponding edge line in Mei . For a planar feature ppi+1,k in \u2032Fpi+1, ppi,u, ppi,v , and ppi,w form the corresponding planar patch in Mpi . The GaussNewton method is then used to solve for the optimal transformation by minimizing:<\/p>\n \u5176\u4e2d\uff0ck, u, v, w <\/span>\u00a0\u662f\u5728\u8fb9\u7f18\u7279\u5f81\u6216\u5e73\u9762\u7279\u5f81\u5bf9\u5e94\u96c6\u5408\u4e2d\u7684\u7d22\u5f15\u53f7\u3002\u5bf9\u4e8e\u5728\u8f6c\u6362\u540e\u7684\u8fb9\u7f18\u5173\u952e\u5e27 \u2032Fe i+1<\/span>\u00a0\u4e2d\u7684\u8fb9\u7f18\u7279\u5f81 pe i+1,k <\/span>\u00a0\u6765\u8bf4\uff0cpei,u \u548c pei,v \u00a0\u662f\u5f62\u6210\u8fb9\u7f18\u5b50\u4f53\u7d20\u5730\u56fe Mei<\/span> \u4e2d\u76f8\u5bf9\u5e94\u7684\u8fb9\u7f18\u7ebf\u4e0a\u7684\u70b9\u3002\u800c\u5bf9\u4e8e\u5728\u8f6c\u6362\u540e\u7684\u5e73\u9762\u5173\u952e\u5e27 \u2032Fpi+1 \u4e2d\u7684\u5e73\u9762\u7279\u5f81 ppi+1,k <\/span>\u6765\u8bf4\uff0cppi,u, ppi,v \u4ee5\u53ca ppi,w\u00a0\u5f62\u6210\u4e86\u5e73\u9762\u5b50\u4f53\u7d20\u5730\u56fe Mpi<\/span>\u00a0\u4e2d\u5bf9\u5e94\u7684\u5e73\u9762\u5757\u3002\u7136\u540e\u91c7\u7528\u9ad8\u65af\u725b\u987f\u6cd5\u6765\u6c42\u89e3\u6700\u4f18\u7684\u53d8\u6362\u77e9\u9635\u5173\u7cfb\uff0c\u901a\u8fc7\u6700\u5c0f\u5316\u4e0b\u8ff0\u516c\u5f0f\uff1a<\/p>\n At last, we can obtain the relative transformation \u2206Ti,i+1 between xi and xi+1, which is the lidar odometry factor linking these two poses:<\/p>\n \u6700\u540e\uff0c\u6211\u4eec\u53ef\u4ee5\u5f97\u5230\u72b6\u6001\u8282\u70b9 xi<\/span>\u00a0\u4e0e\u72b6\u6001\u8282\u70b9 xi+1<\/span>\u00a0\u4e4b\u95f4\u7684\u76f8\u5bf9\u53d8\u6362\u5173\u7cfb \u2206Ti,i+1\uff0c\u5b83\u662f\u8fde\u63a5\u8fd9\u4e24\u79cd\u4f4d\u59ff\u7684\u6fc0\u5149\u91cc\u7a0b\u8ba1\u56e0\u5b50\uff1a<\/p>\n We note that an alternative approach to obtain \u2206Ti,i+1 is to transform sub-keyframes into the frame of xi. In other words, we match Fi+1 to the voxel map that is represented in the frame of xi. In this way, the real relative transformation \u2206Ti,i+1 can be directly obtained. Because the transformed features \u2032Fei and \u2032Fpi can be reused multiple times, we instead opt to use the approach described in Sec. III-C.1 for its computational efficiency.<\/p>\n \u6211\u4eec\u6ce8\u610f\u5230\uff0c\u83b7\u5f97 \u2206Ti,i+1<\/span>\u00a0\u7684\u53e6\u4e00\u79cd\u65b9\u6cd5\u662f\u5c06\u5b50\u5173\u952e\u5e27\u8f6c\u6362\u5230 xi<\/span>\u00a0\u72b6\u6001\u8282\u70b9\u7684\u5750\u6807\u7cfb\u4e0b\u3002\u6362\u53e5\u8bdd\u8bf4\uff0c\u5c31\u662f\u5c06 Fi+1<\/span>\u00a0\u4e0e\u5728 xi<\/span>\u00a0\u72b6\u6001\u8282\u70b9\u5750\u6807\u7cfb\u4e0b\u8868\u793a\u7684\u4f53\u7d20\u5730\u56fe\u8fdb\u884c\u5339\u914d\u3002\u8fd9\u79cd\u65b9\u5f0f\u4e0b\uff0c\u53ef\u4ee5\u76f4\u63a5\u5f97\u5230\u5b9e\u65bd\u76f8\u5bf9\u53d8\u6362\u5173\u7cfb\u00a0<\/span> \u2206Ti,i+1\u3002\u7531\u4e8e\u8f6c\u6362\u540e\u7684\u7279\u5f81 \u2032Fei \u548c \u2032Fpi \u00a0\u53ef\u4ee5\u591a\u6b21\u91cd\u590d\u4f7f\u7528\uff0c\u6240\u4ee5\u6211\u4eec\u9009\u62e9\u91c7\u7528\u7b2cIII\u8282-C.1\u4e2d\u63cf\u8ff0\u7684\u65b9\u6cd5\uff0c\u56e0\u4e3a\u5b83\u7684\u8ba1\u7b97\u6548\u7387\u5f88\u9ad8\u3002<\/p>\n Though we can obtain reliable state estimation and map- ping by utilizing only IMU preintegration and lidar odometry factors, the system still suffers from drift during long- duration navigation tasks. To solve this problem, we can introduce sensors that offer absolute measurements for elim- inating drift. Such sensors include an altimeter, compass, and GPS. For the purposes of illustration here, we discuss GPS, as it is widely used in real-world navigation systems.<\/p>\n \u867d\u7136\u4ec5\u4ec5\u5229\u7528IMU\u9884\u79ef\u5206\u548c\u6fc0\u5149\u91cc\u7a0b\u8ba1\u56e0\u5b50\u5c31\u53ef\u4ee5\u83b7\u5f97\u53ef\u9760\u7684\u72b6\u6001\u4f30\u8ba1\u4ee5\u53ca\u5efa\u56fe\uff0c\u4f46\u662f\u8fd9\u4e2a\u7cfb\u7edf\u5728\u957f\u671f\u7684\u5bfc\u822a\u4efb\u52a1\u4e2d\uff0c\u4ecd\u7136\u5b58\u5728\u6f02\u79fb\u95ee\u9898<\/span>\u3002\u4e3a\u4e86\u89e3\u51b3\u8fd9\u4e2a\u95ee\u9898\uff0c\u6211\u4eec\u53ef\u4ee5\u5f15\u5165\u63d0\u4f9b\u6d88\u9664\u6f02\u79fb\u7684\u7edd\u5bf9\u6d4b\u91cf\u503c\u7684\u4f20\u611f\u5668<\/span>\u3002\u8fd9\u4e9b\u4f20\u611f\u5668\u5305\u62ec\u9ad8\u5ea6\u8ba1\u3001\u6307\u5357\u9488\u7f57\u76d8\u4ee5\u53caGPS\u5168\u7403\u5b9a\u4f4d\u7cfb\u7edf\u3002\u8fd9\u91cc\u4e3a\u4e86\u9610\u8ff0\u6e05\u695a\uff0c\u6211\u4eec\u8ba8\u8bba\u4e86\u5176\u4e2dGPS\uff0c\u56e0\u4e3a\u5b83\u5728\u73b0\u5b9e\u5bfc\u822a\u7cfb\u7edf\u4e2d\u88ab\u5e7f\u6cdb\u4f7f\u7528\u3002<\/p>\n When we receive GPS measurements, we first transform them to the local Cartesian coordinate frame using the method proposed in [21]. Upon the addition of a new node to the factor graph, we then associate a new GPS factor with this node. If the GPS signal is not hardware-synchronized with the lidar frame, we interpolate among GPS measurements linearly based on the timestamp of the lidar frame.<\/p>\n \u5f53\u6211\u4eec\u63a5\u6536GPS\u6d4b\u91cf\u65f6\uff0c\u9996\u5148\u4f7f\u7528\u6587\u732e\u301021\u3011\u4e2d\u63d0\u5230\u7684\u65b9\u6cd5\u5c06GPS\u5750\u6807\u8f6c\u6362\u5230\u5c40\u90e8\u7b1b\u5361\u5c14\u5750\u6807\u7cfb\u4e0b<\/span>\u3002\u5728\u56e0\u5b50\u56fe\u4e2d\u6dfb\u52a0\u4e00\u4e2a\u65b0\u7684\u8282\u70b9\u540e\uff0c\u6211\u4eec\u5c06\u65b0\u7684GPS\u56e0\u5b50\u548c\u8fd9\u4e2a\u65b0\u8282\u70b9\u5173\u8054\u8d77\u6765\u3002\u5982\u679cGPS\u4fe1\u53f7\u4e0e\u6fc0\u5149\u96f7\u8fbe\u6ca1\u6709\u505a\u786c\u4ef6\u540c\u6b65\uff0c\u90a3\u4e48\u6211\u4eec\u6839\u636e\u6fc0\u5149\u96f7\u8fbe\u7684\u65f6\u95f4\u6233\u5728GPS\u4e2d\u8fdb\u884c\u7ebf\u6027\u63d2\u503c<\/span>\u3002<\/b><\/p>\n We note that adding GPS factors constantly when GPS reception is available is not necessary because the drift of lidar inertial odometry grows very slowly. In practice, we only add a GPS factor when the estimated position covariance is larger than the received GPS position covariance.<\/p>\n \u6211\u4eec\u6ce8\u610f\u5230\uff0c\u5f53GPS\u63a5\u6536\u53ef\u7528\u65f6\uff0c\u4e0d\u65ad\u6dfb\u52a0GPS\u56e0\u5b50\u662f\u6ca1\u6709\u5fc5\u8981\u7684\uff0c\u56e0\u4e3a\u6fc0\u5149\u60ef\u5bfc\u91cc\u7a0b\u8ba1\u6f02\u79fb\u589e\u52a0\u662f\u975e\u5e38\u7f13\u6162\u7684<\/span>\u3002\u5728\u5b9e\u8df5\u4e2d\uff0c\u6211\u4eec\u53ea\u9700\u8981\u5728\u4f30\u8ba1\u7684\u4f4d\u7f6e\u534f\u65b9\u5dee\u5927\u4e8e\u63a5\u6536\u5230\u7684GPS\u4f4d\u7f6e\u534f\u65b9\u5dee\u7684\u65f6\u5019\uff0c\u624d\u53bb\u589e\u52a0\u4e00\u4e2aGPS\u56e0\u5b50\u3002<\/p>\n Thanks to the utilization of a factor graph, loop closures can also be seamlessly incorporated into the proposed system, as opposed to LOAM and LIOM. For the purposes of illustration, we describe and implement a naive but effective Euclidean distance-based loop closure detection approach. We also note that our proposed framework is compatible with other methods for loop closure detection, for example, [22] and [23], which generate a point cloud descriptor and use it for place recognition.<\/p>\n \u7531\u4e8e\u5229\u7528\u4e86\u56e0\u5b50\u56fe\uff0c\u95ed\u73af\u4e5f\u53ef\u4ee5\u65e0\u7f1d\u5730\u63d2\u5165\u5230\u63d0\u51fa\u7684\u7cfb\u7edf\u4e2d\uff0c\u800c\u4e0d\u662f\u8ddfLOAM\u548cLIO Mapping\u90a3\u6837\u3002\u4e3a\u4e86\u8bf4\u660e\uff0c\u6211\u4eec\u63cf\u8ff0\u5e76\u5b9e\u73b0\u4e86\u4e00\u79cd\u6734\u7d20\u4f46\u6709\u6548\u7684<\/span>\u57fa\u4e8e\u6b27\u6c0f\u8ddd\u79bb\u7684\u95ed\u73af\u68c0\u6d4b\u65b9\u6cd5<\/span><\/span>\u3002\u6211\u4eec\u8fd8\u6ce8\u610f\u5230\uff0c\u6211\u4eec\u63d0\u51fa\u7684\u6846\u67b6\u4e0e\u5176\u4ed6\u7684\u95ed\u73af\u68c0\u6d4b\u65b9\u6cd5\u662f\u517c\u5bb9\u7684\uff0c\u4f8b\u5982\u6587\u732e\u301022\u3011\u301023\u3011\u63d0\u5230\u7684\u65b9\u6cd5\uff0c\u4ed6\u4eec\u751f\u4ea7\u4e86\u4e00\u4e2a\u70b9\u4e91\u63cf\u8ff0\u7b26\u5e76\u5c06\u5b83\u7528\u4e8e\u4f4d\u7f6e\u8bc6\u522b\u3002<\/p>\n When a new state xi+1 is added to the factor graph, we first search the graph and find the prior states that are close to xi+1 in Euclidean space. As is shown in Fig. 1, for example, x3 is one of the returned candidates. We then try to match Fi+1 to the sub-keyframes {F3\u2212m, …, F3, …, F3+m}using scan-matching. Note that Fi+1 and the past sub-keyframes are first transformed into W before scan-matching. We obtain the relative transformation \u2206T3,i+1 and add it as a loop closure factor to the graph. Throughout this paper, we choose the index m to be 12, and the search distance for loop closures is set to be 15m from a new state xi+1.<\/p>\n \u5f53\u4e00\u4e2a\u65b0\u7684\u72b6\u6001 xi+1<\/span>\u00a0\u88ab\u52a0\u5165\u5230\u56e0\u5b50\u56fe\u4e2d\uff0c\u6211\u4eec\u9996\u5148\u5728\u56e0\u5b50\u56fe\u4e2d\u8fdb\u884c\u641c\u7d22\uff0c\u627e\u5230\u6b27\u5f0f\u7a7a\u95f4\u4e2d\u63a5\u8fd1\u72b6\u6001 xi+1<\/span>\u00a0\u7684\u5148\u9a8c\u72b6\u6001\u3002\u5982\u56fe1\u6240\u793a\uff0c\u72b6\u6001\u8282\u70b9 x3<\/span>\u00a0\u662f\u5176\u4e2d\u4e00\u4e2a\u8fd4\u56de\u7684\u5019\u9009\u5bf9\u8c61\u3002\u7136\u540e\u6211\u4eec\u5c1d\u8bd5\u7528\u626b\u63cf\u5339\u914dscan-match\u65b9\u5f0f\u5c06\u5173\u952e\u5e27 Fi+1<\/span>\u00a0\u4e0e\u5b50\u5173\u952e\u5e27\u96c6 {F3\u2212m, …, F3, …, F3+m}<\/span>\u00a0\u8fdb\u884c\u5339\u914d\u3002\u6ce8\u610f\uff0c\u5173\u952e\u5e27 Fi+1<\/span>\u00a0\u548c\u8fc7\u53bb\u7684\u5b50\u5173\u952e\u5e27\u5728\u626b\u63cf\u5339\u914dscan-match\u4e4b\u524d\u5df2\u7ecf\u5148\u88ab\u8f6c\u6362\u5230\u4e16\u754c\u5750\u6807\u7cfbW\u4e2d \u3002\u7136\u540e\u6211\u4eec\u5c31\u5f97\u5230\u4e86\u76f8\u5bf9\u53d8\u6362 \u2206T3,i+1<\/span>\u00a0\uff0c\u5e76\u5c06\u5176\u4f5c\u4e3a\u4e00\u4e2a\u95ed\u73af\u56e0\u5b50\u6dfb\u52a0\u5230\u56e0\u5b50\u56fe\u4e2d\u3002\u5728\u672c\u6587\u4e2d\uff0c\u6211\u4eec\u9009\u62e9\u7d22\u5f15\u6570m\u4e3a12\uff0c\u95ed\u73af\u4e0e\u65b0\u72b6\u6001 xi+1<\/span>\u00a0\u7684\u641c\u7d22\u8303\u56f4\u8bbe\u4e3a15m\u3002<\/p>\n In practice, we find adding loop closure factors is espe- cially useful for correcting the drift in a robot\u2019s altitude, when GPS is the only absolute sensor available. This is because the elevation measurement from GPS is very inaccurate – giving rise to altitude errors approaching 100m in our tests, in the absence of loop closures.<\/p>\n \u5b9e\u9645\u4e2d\uff0c\u6211\u4eec\u53d1\u73b0\uff0c\u5f53GPS\u662f\u552f\u4e00\u53ef\u7528\u7684\u7edd\u5bf9\u4f4d\u7f6e\u6d4b\u91cf\u4f20\u611f\u5668\u7684\u65f6\u5019\uff0c\u6dfb\u52a0\u95ed\u73af\u56e0\u5b50\u5bf9\u4e8e\u4fee\u6b63\u673a\u5668\u4eba\u9ad8\u7a0b\u4e0a\u7684\u6f02\u79fb\u7279\u522b\u6709\u7528<\/span>\u3002\u8fd9\u662f\u7531\u4e8eGPS\u7684\u9ad8\u7a0b\u6d4b\u91cf\u662f\u975e\u5e38\u4e0d\u51c6\u786e\u7684<\/span>\uff0c\u5728\u6211\u4eec\u7684\u6d4b\u8bd5\u4e2d\uff0c\u6ca1\u6709\u95ed\u73af\u60c5\u51b5\u4e0b\uff0c\u4f1a\u5bfc\u81f4\u63a5\u8fd1100\u7c73\u7684\u9ad8\u7a0b\u8bef\u5dee\u3002<\/p>\n We now describe a series of experiments to qualitatively and quantitatively analyze our proposed framework. The sensor suite used in this paper includes a Velodyne VLP- 16 lidar, a MicroStrain 3DM-GX5-25 IMU, and a Reach M GPS. For validation, we collected 5 different datasets across various scales, platforms and environments. These datasets are referred to as Rotation, Walking, Campus, Park and Amsterdam, respectively. The sensor mounting platforms are shown in Fig. 2. The first three datasets were collected using a custom-built handheld device on the MIT campus. The Park dataset was collected in a park covered by vegetation, using an unmanned ground vehicle (UGV) – the Clearpath Jackal. The last dataset, Amsterdam, was collected by mounting the sensors on a boat and cruising in the canals of Amsterdam. The details of these datasets are shown in Table I.<\/p>\n \u6211\u4eec\u73b0\u5728\u63cf\u8ff0\u4e00\u7cfb\u5217\u7684\u5b9e\u9a8c\u6765\u5b9a\u6027\u548c\u5b9a\u91cf\u5730\u5206\u6790\u6211\u4eec\u63d0\u51fa\u7684\u6846\u67b6\u3002\u672c\u6587\u91c7\u7528\u7684\u4f20\u611f\u5668\u5957\u4ef6\u5305\u62ec\u4e00\u4e2a<\/b>Velodyne16\u7ebf\u6fc0\u5149\u96f7\u8fbe<\/span>\uff0c\u4e00\u4e2aMicroStrain<\/span> 3DM-GX5-25\u7684IMU<\/span>\u4ee5\u53ca\u4e00\u4e2aReach M\u7684GPS<\/span>\u3002\u4e3a\u4e86\u8fdb\u884c\u5b9e\u9a8c\u9a8c\u8bc1\uff0c\u6211\u4eec\u5728\u4e0d\u540c\u5c3a\u5ea6\u3001\u5e73\u53f0\u548c\u73af\u5883\u4e2d\u641c\u96c6\u4e865\u4e2a\u4e0d\u540c\u7684\u6570\u636e\u96c6\u3002\u8fd9\u4e9b\u6570\u636e\u96c6\u5206\u522b\u4e3aRotation\uff0cWalking\uff0cCampus\uff0cPark\uff0cAmsterdam\u3002\u4f20\u611f\u5668\u5b89\u88c5\u5e73\u53f0\u5982\u56fe2\u6240\u793a\u3002<\/p>\n We compare the proposed LIO-SAM framework with LOAM and LIOM. In all the experiments, LOAM and LIO- SAM are forced to run in real-time. LIOM, on the other hand, is given infinite time to process every sensor measurement. All the methods are implemented in C++ and executed on a laptop equipped with an Intel i7-10710U CPU using the robot operating system (ROS) [24] in Ubuntu Linux. We note that only the CPU is used for computation, without parallel computing enabled. Our implementation of LIO-SAM is freely available on Github1. Supplementary details of the experiments performed, including complete visualizations of all tests, can be found at the link below2 .<\/p>\n \u6211\u4eec\u5c06\u63d0\u51fa\u7684LIO-SAM\u6846\u67b6\u4e0eLOAM\u548cLIO Mapping\u8fdb\u884c\u6bd4\u8f83\u3002\u5728\u6240\u6709\u5b9e\u9a8c\u4e2d\uff0cLOAM\u548cLIO-SAM\u5b9e\u65f6\u7684\u8fd0\u884c\u4e86\u3002\u800cLIO-Mapping\u76f8\u53cd\uff0c\u7ed9\u4e86\u65e0\u9650\u591a\u7684\u65f6\u95f4\u6765\u5904\u7406\u6bcf\u4e2a\u4f20\u611f\u5668\u7684\u89c2\u6d4b\u503c\u3002\u6240\u6709\u7684\u65b9\u6cd5\u90fd\u5728C++\u4e2d\u5b9e\u73b0\uff0c\u5e76\u5728\u914d\u5907\u4e86Intel i7-10710U CPU\u7684\u7b14\u8bb0\u672c\u4e0a\u8fd0\u884c\uff0c\u4f7f\u7528\u7684\u5e73\u53f0\u662f\u662fUbuntu\u4e2d\u7684ROS\u673a\u5668\u4eba\u64cd\u4f5c\u7cfb\u7edf\u3002\u6211\u4eec\u6ce8\u610f\u5230\uff0c\u53ea\u6709CPU\u9002\u7528\u4e8e\u8ba1\u7b97\uff0c\u6ca1\u6709\u542f\u7528\u5e76\u884c\u8ba1\u7b97\u3002\u6211\u4eec\u7684LIO-SAM\u7684\u5b9e\u73b0\u53ef\u4ee5\u5728\u6211\u4eec\u7684Github<\/a>\u4e2d\u8fdb\u884c\u4e0b\u8f7d\u3002\u6267\u884c\u5b9e\u9a8c\u7684\u8865\u5145\u7ec6\u8282\uff0c\u5305\u62ec\u6240\u6709\u6d4b\u8bd5\u7684\u5b8c\u6574\u89c6\u9891\uff0c\u53ef\u4ee5\u5728\u6211\u4eec\u7684YouTube\u9891\u9053<\/a>\u4e2d\u627e\u5230<\/p>\n In this test, we focus on evaluating the robustness of our framework when only IMU preintegration and lidar odometry factors are added to the factor graph. The Rotation dataset is collected by a user holding the sensor suite and performing a series of aggressive rotational maneuvers while standing still. The maximum rotational speed encountered in this test is 133.7 \u25e6\/s. The test environment, which is populated with structures, is shown in Fig. 3(a). The maps obtained from LOAM and LIO-SAM are shown in Figs. 3(b) and (c) respec- tively.<\/p>\n \u5728\u8fd9\u4e2a\u6d4b\u8bd5\u4e2d\uff0c\u5f53\u53ea\u4f7f\u7528IMU\u9884\u79ef\u5206\u4ee5\u53ca\u6fc0\u5149\u91cc\u7a0b\u8ba1\u56e0\u5b50\u88ab\u6dfb\u52a0\u5230\u56e0\u5b50\u56fe\u4e2d\u65f6\uff0c\u91cd\u70b9\u8bc4\u4f30\u6211\u4eec\u63d0\u51fa\u7684LIO-SAM\u6846\u67b6\u7684\u9c81\u68d2\u6027\u3002Rotation\u6570\u636e\u96c6\u662f\u4eba\u624b\u6301\u4f20\u611f\u5668\u5957\u4ef6\u91c7\u96c6\u7684\uff0c\u5728\u539f\u5730\u9759\u6b62\u4e0d\u79fb\u52a8<\/span>\u6761\u4ef6\u4e0b\uff0c\u53ea\u8fdb\u884c\u7eaf\u65cb\u8f6c\u64cd\u4f5c<\/span>\u3002\u672c\u5b9e\u9a8c\u4e2d\u6700\u5927\u7684\u65cb\u8f6c\u901f\u5ea6\u4e3a133.7\u5ea6\u6bcf\u79d2<\/span>\u3002\u6d4b\u8bd5\u73af\u5883\u5982\u56fe3(a)\u6240\u793a\u3002LOAM\u548cLIO-SAM\u4e2d\u751f\u6210\u7684\u5730\u56fe\u5982\u56fe3(b)\uff0c\u56fe3(c)\u6240\u793a\u3002<\/p>\n![\"\"](\"http:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/11\/lio-sam-graph-scaled.jpg\")
<\/a><\/p>\n\u4e00\u3001\u6458\u8981 Abstract<\/h1>\n
\u4e8c\u3001\u5f15\u8a00 Introduction<\/h1>\n
\n
\n
\u4e09\u3001\u76f8\u5173\u5de5\u4f5c Related Work<\/h1>\n
\u56db\u3001\u57fa\u4e8e\u5e73\u6ed1\u548c\u5efa\u56fe\u7684\u6fc0\u5149\u60ef\u5bfc\u91cc\u7a0b\u8ba1 Lidar Inertial Odometry Via Smoothing and Mapping<\/h1>\n
1. \u7cfb\u7edf\u603b\u89c8 System Overview<\/h3>\n
![\"\"](\"http:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/robot-satte-x-300x52.png\")
<\/a><\/p>\n<\/a><\/p>\n\n
\n
2. IMU\u9884\u79ef\u5206\u56e0\u5b50 IMU Preintegration Factor<\/h3>\n
![\"\"](\"http:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/eqs2-3-300x65.png\")
<\/a><\/p>\n![\"\"](\"http:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/eqs4-6-300x123.png\")
<\/a><\/p>\n![\"\"](\"http:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/eqs7-9-300x87.png\")
<\/a><\/p>\n3.\u6fc0\u5149\u96f7\u8fbe\u91cc\u7a0b\u8ba1\u56e0\u5b50 Lidar Odometry Factor<\/h3>\n
a\uff09\u5efa\u7acb\u5b50\u5173\u952e\u5e27\u7684\u4f53\u7d20\u5730\u56fe Sub-keyframes for voxel map<\/b><\/h5>\n
![\"\"](\"http:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/eqsxx-300x80.png\")
<\/a><\/p>\nb\uff09\u626b\u63cf\u5339\u914d Scan-matching<\/b><\/h5>\n
c\uff09\u76f8\u5bf9\u53d8\u6362\u5173\u7cfb Relative transformation<\/b><\/h5>\n
![\"\"](\"http:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/eqs10-11-300x153.png\")
<\/a><\/p>\n![\"\"](\"http:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/eqs11--300x61.png\")
<\/a><\/p>\n![\"\"](\"http:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/eqs12-300x60.png\")
<\/a><\/p>\n4. GPS \u56e0\u5b50 GPS Factor<\/b><\/h3>\n
5. \u95ed\u73af\u56e0\u5b50 Loop Closure Factor<\/h3>\n
\u4e94\u3001\u5b9e\u9a8c Experiments<\/h1>\n
\n\n
\n \u6570\u636e\u96c6<\/th>\n \u626b\u63cf\u6570<\/th>\n \u9ad8\u7a0b\u53d8\u5316(\u7c73)<\/th>\n \u8f68\u8ff9\u957f\u5ea6(\u7c73)<\/th>\n \u6700\u5927\u65cb\u8f6c\u901f\u5ea6(\u5ea6\/\u79d2)<\/th>\n<\/tr>\n \n Rotation<\/td>\n 582<\/td>\n 0<\/td>\n 0<\/td>\n 213.9<\/td>\n<\/tr>\n \n Walking<\/td>\n 6502<\/td>\n 0.3<\/td>\n 801<\/td>\n 133.7<\/td>\n<\/tr>\n \n Campus<\/td>\n 9865<\/td>\n 1.0<\/td>\n 1437<\/td>\n 124.8<\/td>\n<\/tr>\n \n Park<\/td>\n 24691<\/td>\n 19.0<\/td>\n 2898<\/td>\n 217.4<\/td>\n<\/tr>\n \n Amsterdam<\/td>\n 107656<\/td>\n 0<\/td>\n 19065<\/td>\n 17.2<\/td>\n<\/tr>\n<\/tbody>\n<\/table>\n 1. Rotation\u6570\u636e\u96c6<\/b><\/h3>\n
![\"\"](\"http:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/09\/experiment-.jpeg\")
<\/a><\/p>\n