Real-time Mapping and Positioning Based on Multi-sensor Fusion

1. Overview multi-sensor fusion

SLAM (Simultaneous Localization and Mapping) is a very crucial technology in autonomous driving. Real-time Mapping and Positioning can provide the basic technical framework, and localization technology is a key technology in the perception module. Most current applications are based on scenario development. It can be observed that the mapping and localization methods based on a single sensor cannot solve problems such as environmental degradation and low sampling frequency. Therefore, the accuracy and robustness of this mapping and localization scheme need to be improved. With the development of multi-sensor fusion methods, the complementary advantages of the performance of multiple sensors can be exploited, and the localization and mapping schemes using multiple sensors can better meet the requirements for high-precision localization in most scenarios, enhancing the algorithm’s robustness. These sensors include lidar, inertial measurement unit, and wheel odometer.

2. Applications

With the development of the intelligent driving industry, companies such as Baidu, Huawei, DJI Innovation, and Didi Chuxing have respectively released their own autonomous driving models. In recent years, autonomous driving technology has continuously extended to various industries, and major companies have also been constantly exploring specific scenario autonomous driving technology businesses, making autonomous driving technology applicable to various special working environments¹.

Companies such as Tage Idriver, WAYTOUS, and Yuexin Group, with rich research and development experience in challenging terrains like mines and mountainous areas, have designed unmanned mining vehicles capable of carrying tens or even hundreds of tons of goods, thereby enhancing the safety factor of mining operations. Meituan, Alibaba, ZELOS( ZELOS uses DAISCH’s IMU MS1A in their Delivery Vehicles), and JD have also launched their own brands of unmanned delivery vehicles in response to the low efficiency of traditional manual logistics. These vehicles are used to cope with the increased workload in the logistics industry. For scenarios like ports and science and technology parks with low-speed and specific operations, Gausium and Velobotics focus on developing autonomous vehicles for cleaning, passenger transportation, etc. For scenarios such as ships, RobotPlusPlus has a unique autonomous driving solution that realizes functions such as cleaning, rust removal, and wastewater recycling.

In order to address specific issues of autonomous vehicles in various scenarios, major companies are constantly updating and optimizing their respective autonomous driving solutions. Currently, the related technologies of intelligent vehicles mainly consist of technical modules such as environmental perception, planning and navigation, decision-making control, and information communication².

Robots are gradually penetrating into various fields such as transportation, aerospace, household, and catering. With the development of industries, different types of robots like welding type, cleaning type, logistics type, and household type have been developed. SLAM technology is a necessary technology for all robots³.

SLAM mobile robots have been applied in indoor scenarios. However, in situations such as bumpy roads, few feature points, similar corridors, strong lighting, etc., there are still problems with poor practicality, such as overlapping maps, low accuracy, and cumulative positioning errors, which cannot meet the working standards of specific scenarios. Therefore, it is important to design a SLAM algorithm that is suitable for a certain scenario and has strong adaptability.

3. Map construction/Mapping Algorithm

Simultaneous Localization and Mapping (SLAM) is defined as the process where, in an unknown environment, a robot uses sensor data to match the map in real time to determine its own position, and scans and builds a continuous map of the surrounding environment based on real-time data. The main task of SLAM is to construct an effective and suitable prior map and simultaneously obtain accurate positioning, and then study and optimize the state estimation problem. It has two representative algorithms:

3.1 Filtering methods

Kalman filtering and Particle filtering are the two most representative filtering algorithms.

In the Kalman filtering algorithm, there are parts that do not match the real system because the Gaussian noise in the motion equation and the observation equation is assumed to be linear. Therefore, it is only applicable to linear systems and has relatively good convergence for linear systems, but cannot be better applied to nonlinear systems. When dealing with the non-linear situation of the SLAM system, the non-linear filtering algorithm is more suitable for SLAM. In the non-linear filtering algorithms, the Extended Kalman Filter (EKF) algorithm has certain advantages. The Extended Kalman Filter uses the Taylor series to perform linear approximation expansion. The Mono-SLAM back-end optimization adopts the Extended Kalman Filter algorithm. In Mono-SLAM, the application of the Structure From Motion method is the first time, achieving real-time camera pose estimation and mapping ⁴. As the map area increases continuously, it leads to an increase in state variables, an increase in computational load, and a slowdown in running speed. The Unscented Kalman Filter (UKF) can perform lossless transformation of environmental information, solving the linearization problems of the above two algorithms. Compared with the other two, it has higher accuracy and better stability. However, the common problem is that all noise is linearly processed, resulting in the algorithm not being widely used in practical work.

Particle filtering is a recursive Bayesian filter based on the Monte Carlo principle and using non-parametric and approximate forms. It can solve problems of nonlinear systems and non-Gaussian noise. However, its computational complexity increases as the environmental range expands ⁵. Among them, Fast SLAM is one of the representative algorithms based on the particle filtering method. This algorithm discusses the positioning problem and the map construction problem with known poses separately. The algorithm requires a large number of particles to construct an accurate map, so the computational cost of this algorithm increases exponentially¹. At the same time, high-frequency resampling accelerates the degradation of particles and affects map construction. Later, the Gmapping algorithm was proposed. Gmapping is mainly applied to small scenes. The algorithm designs adaptive resampling to prevent particle degradation. The number of particles also increases as the environmental map expands, resulting in an increase in algorithm complexity and computational cost. Although this SLAM algorithm has achieved certain results, as the map size continues to increase, there are problems such as decreasing update efficiency and poor adaptability. It is difficult to meet the requirements of large-scale, multi-loop, and low feature information environments. Moreover, the pose calculation using the recursive method has deviations, which will affect subsequent estimations, and it is difficult to ensure the consistency and accuracy of the map. Therefore, the filter-based SLAM system is not suitable for large-scale scenarios⁶.

3.2 Graph optimization method

Compared with the filtering-based methods, the SLAM method based on graph optimization theory takes into account all the observations at all times for state estimation, discards the Markov assumption, and has higher accuracy per unit time. To minimize the cumulative error, the optimal estimation value of the state is solved using nonlinear optimization methods. According to the classification of laser radar and camera sensors, the graph-optimized SLAM method is divided into visual SLAM (VSLAM) and Lidar-SLAM. VSLAM is mainly based on camera sensors, and according to the imaging characteristics, there are monocular, stereo, and depth cameras; Lidar-SLAM, which is mainly based on Lidar sensors, is divided into 2D SLAM and 3D SLAM according to the number of beams ⁷.

VSLAM

In the analysis of the VSLAM algorithm, it was found that the back-end optimization adopted the bundle adjustment (BA) method of PTAM and proposed parallel threads for pose tracking and mapping to run simultaneously.

On this basis, a S-PTAM system based on stereo cameras and object detection modules was proposed. In the object detection aspect, deep learning technology was used, supporting online detection and outputting the 3D posture of objects in the image. This system achieved good target positioning results in real environments ⁸.

A SIFT feature-based system using GPU acceleration in the graph optimization framework was proposed to construct feature maps. Due to the map scale being limited by the features of the vehicle-mounted stereo camera, by fusing the features of the fisheye camera into the map, the limited viewpoint of the vehicle-mounted camera was extended. This method has high accuracy ⁹.

The core of the semi-dense monocular LSD-SLAM using direct method for image information matching is to minimize the photometric error, not relying on depth cameras and GPU, thus achieving semi-dense scene reconstruction. This method has high requirements for camera intrinsic parameters, photometric variation amplitude, etc., and the existing loop closure detection algorithms only rely on feature points.

Another representative visual SLAM is ORB-SLAM, which inherits the PTAM. On the basis of the original algorithm, real-time loop closure detection and repositioning functions were added. ORB SLAM divided the BA-based mapping thread into two threads: local mapping and loop closure detection. This method can be applied to poor environmental conditions.

Later, to expand the stereo camera and depth-camera, ORB-SLAM3 was proposed ¹⁰; when encountering scenes with few features such as glass and white walls, single visual SLAM algorithms cannot work. The VINS series of algorithms were proposed, integrating vision and IMU to improve positioning accuracy. IMU has the characteristic of high accuracy in a short time. To apply the visual algorithm to unmanned aerial vehicles, the GVINS algorithm was proposed based on the previous VINS series algorithms, tightly coupling GNSS raw measurements, vision, and inertial information, enabling the algorithm to be applied in various scales, indoor and outdoor scenarios ¹¹.

Lidar SLAM

With the development of LiDAR technology, compared to cameras, it is less affected by the intensity of external light. LiDAR can obtain accurate distance information. The Lidar SLAM algorithms are as follows:

In 2011, KartoSLAM was proposed. This algorithm optimizes the system based on the Sparse Pose Adjustment (SPA) method. In this method, the sparse system is decoupled using highly optimized and Cholesky methods. The front-end matching adopts the real-time correlation scanning method, which can save memory and reduce cache misses, thereby improving performance ¹². However, this algorithm has problems such as long time for constructing subgraphs and slow global matching performance in a large environment.

In 2016, Google proposed the Cartographer algorithm, which has a real-time loop closure detection function. The front-end scanning and matching relies on the Ceres library to make the algorithm more suitable for large scenes, enabling global pose optimization and enhancing the mapping accuracy ¹³.

For the 3D SLAM algorithm, after the LOAM algorithm was proposed in 2014, the LOAM algorithm was improved by Shan Tixiao in 2018 and the LEGO-LOAM algorithm was proposed. The main improvements included modifying the inter-frame feature constraint method, adopting a distribution optimization approach to reduce computational load, and also incorporating loop closure detection to minimize cumulative errors ¹⁴.

In 2020, Shan Tixiao proposed a Lidar and IMU tightly coupled Lidar odometer, a LIO-SAM algorithm with GPS factors added for closed-loop detection, and an algorithm optimized by factor graphs, which is applicable to various sensor fusion and global optimization ¹⁵. In 2022, in response to the problem of drift in map generation caused by existing algorithms, an Adp-LIO-SAM algorithm adapted to environmental space changes was proposed. The fuzzy algorithm was used to adapt different point cloud filtering thresholds for different sizes of spatial environments ¹⁶. To obtain more accurate detection results, the SC-F-LOAM algorithm was proposed. In this algorithm, F-LOAM is used as the Lidar odometer to improve the accuracy of attitude estimation. The closed-loop detection uses the Scan Context algorithm, continuing the optimization idea of LIO-SAM and using factor graphs for global optimization ¹⁷.

The main problem that the Lidar SLAM algorithm aims to solve is to improve the mapping accuracy. Compared with the single-sensor SLAM algorithm, it has certain limitations. During operation, it is prone to external interference, and in some specific scenarios, the system’s robustness and accuracy are relatively poor. At present, the main research focus is on multi-sensor fusion algorithms. Therefore, the SLAM system based on multiple sensors has become a current research hotspot ¹⁸.

In conclusion, the main reason why the positioning accuracy of the robot cannot be satisfying is that the algorithm has high requirements for hardware and the SLAM algorithm using a single sensor has large errors. By fusing information from multiple sensors (Lidar, IMU, wheel odometer), advantages can be complemented to improve the positioning accuracy. A robust and accurate SLAM algorithm based on graph optimization for positioning is proposed, which can establish a global map, analyze positioning errors, and provide reliable information for subsequent navigation and path planning ¹⁹.

4. Robot positioning algorithm

Accurate positioning information plays a crucial role in the application of robots, laying the foundation for better completion of subsequent related technologies. By matching sensor data with the map in real time to determine the robot’s location, and by perceiving the surrounding environmental information based on real-time data, the robot can better achieve autonomous navigation and positioning.

4.1 Location technology based on map matching

Map matching is a method that uses real-time road position information to match with pre-prepared high-precision maps to achieve robot positioning. Based on map matching, a starting point is generally required at the beginning, and the key is to study the matching algorithm. Therefore, it is usually combined with other positioning methods to achieve accurate positioning ²⁰.

4.2 Location technology based on landmark identification

A landmark refers to a general term for a type of object that contains specific positional information and enables robots to obtain the current specific location information through this information. Landmarks need to be set by humans, have a fixed usage range, and require robots to accurately and reliably identify the landmarks in order to obtain location information. Whether the landmarks can be accurately identified and the environmental location information can be quickly extracted are two important factors determining the accuracy of positioning ²¹.

The positioning methods using a single sensor are numerous and have obvious advantages in certain aspects. However, each method has its inevitable drawbacks and suffers from the problem of poor environmental generalization ability, failing to meet the standards for mobile robots. Based on the distinction between absolute positioning and relative positioning, various widely used positioning technologies in the market are classified and summarized as follows:

There is a clear complementarity among different independent positioning methods. The multi-sensor fusion technology can achieve the fusion between different sensors, thereby improving the positioning accuracy and reliability. When there is no GPS signal indoors, the IMU information and the wheel odometer information can be fused, and in the outdoors, when there are no obstructions such as tall buildings, tunnels, overpasses, or shady avenues, GPS can provide absolute positioning information, and combined with the high-frequency IMU, positioning can be carried out.

1. 任伟建, 高强, 康朝海, 等. 移动机器人同步定位与建图技术综述[J]. 计算机测量与控制, 2022, 30 (2): 1-10.
2. 汪涛. 智能行驶车辆定位技术研究[D].吉林大学,2017.
3. 范绪兵. 基于多传感器融合的复杂工况下自动驾驶定位与建图算法研究[D]. 吉林大学, 2022. DOI:10.27162/d.cnki.gjlin.2022.004319.
4. 周苏,李伟嘉,郭军华.基于激光雷达的停车场车辆定位算法[J].同济大学学报(自然科学版),2021,49(07):1029-1038.
5. Karimian A, Yang Z, Tron R. Statistical Outlier Identification in Multi-robot Visual SLAM using Exp ectation Maximization[J]. arXiv preprint arXiv:2002.02638, 2020.
6. Su Z, Zhou J, Dai J, et al. Optimization Design and Experimental Study of Gmapping Algorithm[C]// 2020 Chinese Control And Decision Conference (CCDC). IEEE, 2020: 4894-4898.
7. 刘镇扬,刘培,李育文,宋韬.基于 2D 激光和 TOF 相机的室内环境 3D 建图算法[J].工业控制计算 机,2022,35(09):24-26.
8. Pire T, Corti J, Grinblat G. Online object detection and localization on stereo visual SLAM system[J]. Journal of Intelligent & Robotic Systems, 2020, 98(2): 377-386.
9. Billings G, Camilli R, Johnson-Roberson M. Hybrid Visual SLAM for Underwater Vehicle Manipulat or Systems[J]. IEEE Robotics and Automation Letters, 2022, 7(3): 6798-6805.
10. Campos C, Elvira R, Rodríguez J J G, et al. Orb-slam3: An accurate open-source library for visual, vi sual–inertial, and multimap slam[J]. IEEE Transactions on Robotics, 2021, 37(6): 1874-1890.
11. Cao S, Lu X, Shen S. Gvins: Tightly coupled gnss–visual–inertial fusion for smooth and consistent st ate estimation[J]. IEEE Transactions on Robotics, 2022, 38(4): 2004-2021.
12. ] 黄瑞, 张轶. 高适应性激光雷达 SLAM[J]. 电子科技大学学报, 2021, 50(1): 52-58.
13. Ali W, Sheng L, Ahmed W. Robot Operating System-Based SLAM for a Gazebo-Simulated Turtlebot 2 in 2d Indoor Environment with Cartographer Algorithm[J]. International Journal of Mechanical and Mec hatronics Engineering, 2021, 15(3): 149-157.
14. Oelsch M, Karimi M, Steinbach E. R-LOAM: Improving LiDAR odometry and mapping with point-t o-mesh features of a known 3D reference object[J]. IEEE Robotics and Automation Letters, 2021, 6(2): 20 68-2075.
15. 周治国, 曹江微, 邸顺帆. 3D 激光雷达 SLAM 算法综述[J]. 仪器仪表学报, 2021, 42(9): 13-27.
16. Hu C. High-Precision Indoor Robot Dynamic Obstacle Detection with Laser and Camera[C]//Journal of Physics: Conference Series. IOP Publishing, 2021, 1952(2): 022006.
17. Liao L, Fu C, Feng B, et al. Optimized SC-F-LOAM: Optimized Fast LiDAR Odometry and Mappin g Using Scan Context[J]. arXiv preprint arXiv:2204.04932, 2022.
18. Hu M, Ao H, Jiang H. Experimental research on feature extraction of laser SLAM based on artificial l andmarks[C]//2019 Chinese Control And Decision Conference (CCDC). IEEE, 2019: 5495-5500.
19. Shen Y, Jiao Z. A Novel Self-Positioning Based on Feature Map Creation and Laser Location Method for RBPF-SLAM[J]. Journal of Robotics, 2021, 2021: 1-11.
20. Lee Y J, Suhr J K, Jung H G. Map Matching-Based Driving Lane Recognition for Low-Cost Precise Vehicle Positioning on Highways[J]. IEEE Access, 2021, 9: 42192-42205.
21. Koloushani M, Ozguven E E, Fatemi A, et al. Mobile mapping system-based methodology to perform automated road safety audits to improve horizontal curve safety on rural roadways[J]. Computational Res earch Progress in Applied Science & Engineering (CRPASE), 2020, 6(4): 263-275.