Synchronous positioning and mapping method and system for multi-source information fusion

Abstract

The invention provides a multi-source information fusion SLAM (Simultaneous Localization and Mapping) algorithm and system. Vision, laser radar and IMU (Inertial Measurement Unit) data are deeply fused. A system framework comprises data preprocessing, pose estimation, back-end optimization and a hardware platform. The data module preprocesses the laser point cloud; an improved ICP laser odometer and a visual odometer (combining optical flow tracking and angular point detection) are adopted for pose estimation, and error state Kalman filtering fusion is carried out. And back-end optimization is based on laser point cloud and visual word bag model design loopback detection, accumulative errors are reduced, and a color point cloud map is generated. A hardware platform is carried on an unmanned aerial vehicle and an unmanned vehicle, a multi-sensor scanner is constructed, sensor calibration is carried out, and remote networking and ROS distributed configuration are adopted. The method is high in positioning precision, high in robustness and centimeter-level in mapping precision in a complex environment, information of multiple sensors is effectively fused, the generated color RGB point cloud map is close to a real space, construction of a digital twinborn model is facilitated, and AR / VR technology development is promoted.

Description

Technical Field

[0001] This invention relates to the field of Simultaneous Localization and Mapping (SLAM) technology, specifically to a multi-source information fusion SLAM algorithm and system for complex environments. Background Technology

[0002] In today's era of rapid technological advancement, simultaneous localization and mapping (SLAM) technology plays a crucial role in numerous fields, such as autonomous driving, robot navigation, and virtual reality. However, in complex scenarios with limited satellite signals, poor lighting conditions, and sparse environmental textures, traditional single-sensor SLAM technology faces many severe challenges, including decreased positioning accuracy and easy loss of tracking.

[0003] While visual SLAM technology can achieve localization and mapping to a certain extent, it is highly susceptible to changes in lighting conditions. In environments with drastic changes in light intensity or insufficient lighting, the performance of visual sensors deteriorates significantly, leading to reduced localization accuracy and potentially even loss of target tracking. LiDAR SLAM technology is also prone to degradation in certain extreme environments. For example, in environments with smoke or dust, LiDAR point cloud data can be interfered with, affecting the accuracy of its localization and mapping.

[0004] Therefore, in order to meet the challenges posed by these complex environments, it is particularly important to develop a SLAM algorithm that can integrate the advantages of multiple sensors and adapt to complex environments. Summary of the Invention

[0005] This invention aims to propose a multi-source information fusion SLAM algorithm and system for complex environments, which achieves high-precision and robust localization and mapping by deeply fusing data from vision, lidar, and inertial measurement unit (IMU).

[0006] The system framework mainly includes data reading and processing, pose estimation and front-end mapping, and back-end optimization.

[0007] Data reading and processing:

[0008] The raw laser point cloud data undergoes preprocessing, including outlier removal. Statistical filtering, radius filtering, and selection based on laser point characteristics, point cloud intensity, and incident angle are used to remove outliers that may be caused by environmental interference or sensor errors, providing a more accurate data foundation for subsequent processing.

[0009] Feature extraction is performed on the distortion-corrected point cloud. Local smoothness is calculated based on the curvature of the laser points to extract planar and edge feature points. These feature points better reflect the geometry of the environment, providing crucial information for subsequent pose estimation and map construction.

[0010] Pose estimation and pre-mapping:

[0011] For the laser odometry section: A lightweight, feature-based improved Iterative Closest Point (ICP) scanning matching algorithm is designed. By extracting inlier set sampling (ISS) keypoints and calculating fast point feature histogram (FPFH) features, coarse registration is performed using Teaser++ followed by fine ICP registration to estimate the pose transformation relationship of point clouds in adjacent frames.

[0012] The visual odometry section: Lucas-Kanade (LK) optical flow tracking and corner detection are used to perform feature tracking on the incoming image frames, while simultaneously detecting new corner features. Triangulation is used to determine the 3D positions of feature points, and the reprojection error is minimized through a perspective-n-point solution to estimate the system pose.

[0013] Fusion section: Error State Kalman Filtering (ESKF) is used to fuse the laser odometry and visual odometry. ESKF state prediction is performed using an IMU, and state updates are conducted via camera and radar, while the IMU is also monitored.

[0014] The 3D positions of feature points are determined by triangulation, and the reprojection error is minimized by PnP (Perspective-n-Point) to estimate the pose of the system.

[0015] Backend optimization:

[0016] A loop closure detection method is designed based on scanning context (SC) description information from LiDAR point clouds and a visual bag-of-words model. SC description information is extracted from the LiDAR point clouds as a global descriptor, and a keyframe strategy is employed to reduce computational load. For the visual part, a bag-of-words model is used to establish similarity relationships between images to detect loop closures.

[0017] Adding loopback frames as constraints to the backend optimization effectively reduces cumulative error and corrects global pose.

[0018] By assigning color to point cloud maps using image RGB information, color attribute information is added, providing better visualization effects and generating 3D maps that are closer to reality.

[0019] Hardware platform construction:

[0020] A multi-sensor fusion scanner was built, equipped with Livox lidar, cameras, built-in IMU, and a computing platform. Land and air datasets were collected using drones and unmanned vehicles, adapting to the data acquisition needs of different scenarios. Sensor extrinsic parameter calibration was performed. By selecting scenes with distinct edge features, open-source calibration algorithms were used to convert data from each sensor into a unified coordinate system, ensuring accurate fusion of data from different sensors.

[0021] By adopting a geographically dispersed network and ROS distributed network configuration mode, remote data acquisition and local real-time data processing are achieved.

[0022] A computer device includes: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the above-described multi-source information fusion SLAM method for complex environments.

[0023] A computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the aforementioned multi-source information fusion SLAM method for complex environments. Attached Figure Description Figure 1 Multi-source information fusion SLAM system framework; Figure 2 Allan variance method flowchart; Figure 3 Point cloud data preprocessing flowchart; Figure 4 Visual odometry calculation flowchart; Figure 5 Multi-sensor fusion scanner; Figure 6 Scanner casing design model; Figure 7 ROS node architecture of the software system; Figure 8 ROS Distributed Network Communication and Remote Networking Diagram. Detailed Implementation

[0024] Hardware platform construction:

[0025] The process involves building a multi-sensor fusion scanner, including selecting appropriate LiDAR, cameras, IMUs, computing platforms, and other equipment, and integrating them with various vehicles to meet the data acquisition needs of different scenarios. Sensor extrinsic parameter calibration is performed by selecting scenes with distinct edge features and using advanced calibration algorithms to convert data from each sensor into a unified coordinate system.

[0026] Software implementation:

[0027] The system uses Ubuntu 20.04 and the Robot Operating System (ROS) for data communication and processing. It employs C++ for programming, bash for script execution, and VS Code for development. The cross-platform compiler CMake compiles the code into executable files corresponding to the ROS nodes.

[0028] Based on advanced optimization algorithms, laser odometry factors, visual odometry factors, and IMU factors are established to optimize the final output of laser vision. A loop closure detection algorithm integrating laser vision is designed. The loop closure detection module establishes the relationship between the current frame and historical frames, and uses loop closure frames as constraints to correct the global pose and feature points.

[0029] Performance testing:

[0030] Positioning accuracy is evaluated on both public and private datasets, and the overall and local accuracy of the system is quantitatively assessed by calculating the absolute trajectory error (ATE) and relative pose error (RPE).

[0031] The robustness of the algorithm is evaluated in challenging scenarios, including laser degradation and loss of visual texture, to verify the system performance.

[0032] The system's processing time for one frame of data is analyzed and compared with other multi-sensor fusion frameworks to evaluate computational efficiency.

[0033] Map-based relocalization and high-quality color point cloud analysis:

[0034] The NDT algorithm is used to relocalize an existing map, validating the map's effectiveness. The generated color point cloud map is analyzed to evaluate its stability and execution efficiency.

[0035] The multi-source information fusion SLAM algorithm and system proposed in this invention have the following beneficial effects:

[0036] It exhibits higher positioning accuracy and robustness in complex environments, with mapping accuracy reaching the centimeter level.

[0037] It effectively integrates information from multiple sensors, including laser, vision, and IMU, achieving complementary advantages of data and overcoming the limitations of a single sensor.

[0038] The designed loop closure detection algorithm that integrates laser vision can effectively reduce the cumulative error of the system and improve the accuracy of localization and mapping.

[0039] By visually rendering point cloud maps, the generated colored RGB point cloud maps have detailed color textures and are closer to real space, which is conducive to building digital twin models and promoting the development of AR / VR.

Claims

1. A multi-source information fusion SLAM system for complex environments, characterized in that, The application relates to a multi-sensor fusion scanning system and a method thereof. a. A data reading and processing module is used for pre-processing laser raw point cloud data, including removing abnormal points, screening and removing abnormal points from the aspects of laser point characteristics, point cloud intensity and incident angle, extracting features from the point cloud after distortion removal, and extracting plane and edge feature points according to the local smoothness calculated based on the curvature of the laser point; b. A pose estimation and front mapping module comprises a laser odometry part, a visual odometry part and a fusion part, wherein the laser odometry part designs a lightweight and feature-based improved iterative closest point (ICP) scan matching algorithm, extracts an inlier set sampling (ISS) key point, calculates a fast point feature histogram (FPFH) feature, performs coarse registration by using Teaser++ and then performs ICP fine registration to estimate the pose transformation relationship of adjacent frame point clouds; the visual odometry part performs feature tracking processing on the input image frame through Lucas-Kanade (LK) optical flow tracking and corner detection, simultaneously detects new corner features, determines the 3D position of the feature points by triangulation, estimates the pose of the system by solving the minimum re-projection error through perspective-n-point (PnP), and the fusion part fuses the laser odometry and the visual odometry by using error state Kalman filtering (ESKF), predicts the ESKF state by using an inertial measurement unit (IMU), and updates and observes the IMU through the camera and the radar; c. A back-end optimization module designs a loop detection method based on the scan context (SC) description information of the laser point cloud and a visual bag-of-words model, extracts the SC description information from the laser radar point cloud as a global descriptor, adopts a key frame strategy to reduce the calculation amount, uses the bag-of-words model to establish the similarity relationship between images to detect loops, adds the loop frame as a constraint to the back-end optimization, effectively reduces the cumulative error and corrects the global pose, colors the point cloud map through image RGB information to increase the color attribute information, provides a better visual effect and generates a three-dimensional map closer to reality; d. A hardware platform construction module builds a multi-sensor fusion scanner, configures Livox laser radar, a camera, an embedded IMU, a computing platform and other equipment, uses a unmanned aerial vehicle and a unmanned vehicle as a carrier to collect land and air data sets to adapt to the data collection requirements of different scenes, performs sensor extrinsic parameter calibration, converts the data of each sensor into a unified coordinate system by selecting a scene with distinct edge features and using an advanced calibration algorithm, and realizes remote data collection and local real-time processing data by adopting a remote networking and ROS distributed network configuration mode.

2. The complex environment oriented multi-source information fusion SLAM system according to claim 1, wherein, The multi-sensor fusion scanner in the hardware platform construction module comprises selecting appropriate laser radar, a camera, an IMU, a computing platform and other equipment, and combining with various carriers to adapt to the data collection requirements of different scenes, and performing sensor extrinsic parameter calibration by selecting a scene with distinct edge features and using an advanced calibration algorithm to convert the data of each sensor into a unified coordinate system.

3. The complex environment oriented multi-source information fusion SLAM system according to claim 1, wherein, The software implementation part uses Ubuntu20.04 operating system, realizes data communication and processing through robot operating system ROS, uses C++ for programming, uses bash language for running script, uses Vscode for development environment, compiles the code into executable file corresponding to the node in ROS through cross-platform compiler Cmake, establishes laser odometry factor, visual odometry factor and IMU factor based on advanced optimization algorithm for optimizing the final output of laser vision, designs loop detection algorithm of fused laser vision, establishes the relationship between current frame and historical frame through loop detection module, and uses loop frame as constraint to correct global pose and feature points.

4. The complex environment oriented multi-source information fusion SLAM system according to claim 1, wherein, The performance test part carries out positioning accuracy evaluation on public data set and private data set, quantitatively evaluates the overall and local accuracy of the system by calculating absolute trajectory error (ATE) and relative pose error (RPE), carries out algorithm robustness evaluation in challenging scenes, including laser degradation, visual texture missing and the like to verify the system performance, analyzes the time consumption of the system in processing one frame of data and compares it with other multi-sensor fusion frameworks to evaluate the computing efficiency.

5. The complex environment oriented multi-source information fusion SLAM system according to claim 1, wherein, The map-based repositioning and high-quality color point cloud analysis part uses NDT algorithm to reposition in the existing map to verify the effectiveness of the map, and analyzes the generated color point cloud map to evaluate its stability and execution efficiency.

6. A computer device comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, The processor executes the computer program to implement the multi-source information fusion SLAM method for complex environment as claimed in any one of claims 1-5.

7. A computer device comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, The processor executes the computer program to implement the multi-source information fusion SLAM method for complex environment as claimed in any one of claims 1-5.

Images