SLAM Based on Multi-Sensor

Abstract

Simultaneous localization and mapping (SLAM) is the key technology for mobile robots to achieve complete autonomy and multi-function. Mobile robots mainly carry sensors such as camera and LiDAR to obtain measurement of the environment. However, due to the lack of accuracy and robustness as well as the complexity of the scene, the SLAM using a single sensor will cause degradation issues and fail to meet the requirements. The applications of multi-sensor fusion can make up for the limitations and shortcomings of a single sensor and adapt to complex environmental changes. For example, the high-frequency output of IMU can handle rapid motion, while the feature tracking of camera can overcome the drift of IMU, and LiDAR point cloud can provide high-precision and long-distance depth information. In order to establish an information-rich environment map and achieve accurate positioning, the advantage of multi-sensor data complementarity can be used to realize multi-sensor fusion SLAM, so as to improve the robustness and adaptability of the robot in unknown dynamic and complex environment.

Appendices

Further Reading

Understand the characteristics and differences between tightly coupled and loosely coupled multiple sensors and the LIO-SAM and Fast-LIO tightly coupled methods.

A loosely coupled system provides flexibility to the combination of sensors with lower requirement of timestamp synchronization and typically requires lower computational cost. Taking LIO as an example, loosely coupled LiDAR-inertial Odometry (LIO) methods typically process the LiDAR and IMU measurements separately and fuse their results later. A common procedure of the loosely coupled approach is obtaining a pose measurement by registering a new scan and then fusing the pose measurement with IMU measurements. The separation between scan registration and data fusion reduces the computation load. However, it ignores the correlation between the system’s other states (e.g., velocity) and the pose of the new scan. Moreover, in the case of featureless environments, the scan registration could degenerate in certain directions and causes unreliable fusion in later stages.

Unlike the loosely coupled methods, tightly coupled LiDAR-inertial odometry methods typically fuse the raw feature points (instead of scan registration results) of LiDAR with IMU data. It needs to put the LiDAR point cloud features into the feature vector, so the dimension of the whole system state vector will be very high, which requires a high amount of calculation. Tightly coupled LIO will put the original observation of LiDAR and the original observation of IMU together for joint processing, considering the internal relationship and mutual influence. That is, a tightly coupled system jointly optimizes over both inertial and LiDAR sensor measurements, which provides higher accuracy in both mapping and tracking. The general idea is that IMU data is first used for the distortion of LiDAR observation, and then LiDAR observation and IMU observation will be thrown together into some form of state estimation model, and the purpose is to minimize the overall error of LiDAR observation and IMU observation. The final state quantities such as Position, Velocity, Orientation, Bias, and Gravity are estimated. There are two main approaches to tightly coupled LIO: optimization based and filter based.

LIO-SAM is a factor graph tightly coupled LiDAR inertial odometry via smoothing and mapping system¹, which adds IMU pre-integration and GPS information and removes the inter-frame matching module. The inputs are IMU, point cloud, and GPS. The LIO-SAM method obtains the IMU pre-integration factor through IMU measurement between adjacent key frames. Then, the laser odometer factor is obtained by matching the key frame with the local map. When a new pose node is inserted into the factor graph, the GPS factor is added to the pose node. Finally, the loop factor is obtained by matching the key frame and the candidate loop key frame, and the overall map is optimized and constructed based on the optimization of the factor graph.

The imu_preintegration node in the LIO-SAM specifically performs pre-integration on the IMU. The pre-integration result is given to the scan to local map part at the back end to remove distortion and initial registration values. The registration result will in turn return to the imu_preintegration node. In this node, a sliding window optimization problem of no more than N frames is constructed based on gtsam (Georgia Tech Smoothing and Mapping) factor graph optimization lib², which specifically optimizes the current bias of the IMU. The new bias will be used for IMU pre-integration at subsequent times. At the same time, the IMU pre-integration results are also used as IMU pre-integration factors, which are input into the back-end factor graph optimization problem, to participate in the optimization of the entire trajectory, as shown in Fig 6.15.

Fig. 6.15

System structure of LIO-SAM

Fast-LIO and Fast-LIO2 fuse LiDAR feature points with IMU data using a tightly coupled iterated extended Kalman filter to allow robust navigation in fast-motion, noisy, or cluttered environments where degeneration occurs³. The LiDAR inputs are fed into the feature extraction module to obtain planar and edge features. Then the extracted features and IMU measurements are fed into the state estimation module for state estimation at 10Hz−50Hz. The estimated pose then registers the feature points to the global frame and merges them with the feature points map built so far. The updated map is finally used to register further new points in the next step.

The framework overview of Fast-LIO is shown in Fig 6.16. Fast-LIO is compatible with rotary mechanical LiDAR and solid-state LiDAR. Firstly, feature extraction is performed on the point cloud, and the feature points are distorted by IMU observation. Then, the LiDAR observation and IMU observation are placed in Iterated Kalman Filter to estimate the position, velocity, orientation, bias, and gravity state quantities at the current moment, all of which are located in the world coordinate system.

Fig. 6.16

Framework overview of FAST-LIO

Exercises

• 1. Calibrate an IMU sensor according to the IMU calibration method described.

• 2. Analyze the process of LiDAR and IMU extrinsic parameter calibration.

• 3. Kalman Filter is a Gaussian filter method while Particle Filter is a nonparametric filter method. What are the advantages and disadvantages, and application scenarios of these two methods?

• 4. What are the current improvements to address the limitations of the Kalman Filter method?

• 5. Explain the difference between filter-based SLAM and graph optimization-based SLAM.

• 6. What method does the Cartographer algorithm use to reduce computational resource consumption and ensure real time and accuracy in loop closure detection?

• 7. Refer to the method of environmental mapping under 2D data sets using the Cartographer algorithm and perform mapping under 3D data sets.