A robust multi-sensor fusion slam method and system

Abstract

This invention discloses a degradation-resistant multi-sensor fusion SLAM method and system, belonging to the field of robot simultaneous localization and mapping technology. The method includes acquiring raw data from a camera, LiDAR, inertial measurement unit, and global navigation satellite system; performing front-end preprocessing and initial estimation of the raw data based on a VIG tightly coupled optimization unit to obtain the global pose; performing motion distortion correction and feature extraction processing based on the raw data and global pose using a LIG tightly coupled optimization unit; and fusing LiDAR features and GNSS observation information in two stages using a two-stage factor graph optimizer to perform global optimization, outputting a high-precision, globally consistent pose and map. This invention overcomes the limitations of single sensors, suppresses accumulated errors, and achieves high-precision, highly robust, and globally consistent localization and mapping in large-scale complex outdoor scenes.

Description

Technical Field

[0001] This invention relates to the field of robot synchronous localization and mapping technology, specifically to a degradation-resistant multi-sensor fusion SLAM method and system. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) technology is the core of enabling autonomous movement of intelligent agents. However, single sensors (such as cameras, LiDAR, and inertial measurement units (IMUs)) have inherent limitations and are difficult to handle complex and ever-changing real-world application scenarios. Therefore, multi-sensor fusion has become an inevitable choice to improve the robustness of SLAM systems. Currently, the most mature and widely used SLAM schemes are based on vision-inertial (VI) and laser-inertial (LI) fusion.

[0003] Existing technologies suffer from multiple drawbacks, such as blind spots in basic solutions, cumulative errors in advanced solutions, and rigid GNSS fusion strategies. These shortcomings make it difficult for them to intelligently adapt to complex real-world environments, thus failing to achieve high-precision and robust positioning across the entire domain.

[0004] To address the aforementioned issues, there is an urgent need for a degradation-resistant multi-sensor fusion SLAM method and system to solve the problems associated with traditional methods. Summary of the Invention

[0005] The purpose of this invention is to provide a degradation-resistant multi-sensor fusion SLAM method and system, which solves the problem of insufficient perception capability of basic vision or laser fusion schemes in extreme environments, corrects the global accuracy loss caused by accumulated errors in advanced fusion schemes, and overcomes the defect of existing GNSS correction methods in complex signal environments where reliability decreases due to simple and rigid fusion strategies. Thus, it achieves high-precision and robust positioning and mapping in all scenarios.

[0006] To achieve the above objectives, the technical solution adopted by the present invention is as follows: A degradation-resistant multi-sensor fusion SLAM system includes: a VIG tightly coupled optimization unit, a LIG tightly coupled optimization unit, and a two-stage factor graph optimizer; The VIG tightly coupled optimization unit, with a sliding window optimizer at its core, is used to fuse raw data from the camera, lidar, inertial measurement unit, and global navigation satellite system, and to perform initial estimation to obtain the global pose. The LIG tightly coupled optimization unit is used to perform motion distortion correction and feature extraction based on the original data and global pose. The two-stage factor graph optimizer includes a factor graph model and a corresponding nonlinear least squares solver, which is used to fuse laser features and GNSS observation information in two stages, perform global optimization, and output high-precision, globally consistent pose and map.

[0007] Furthermore, the LIG tightly coupled optimization unit includes a motion distortion correction module, a feature extraction module, and a data synchronization module.

[0008] This invention also provides a degradation-resistant multi-sensor fusion SLAM method, applied to the aforementioned degradation-resistant multi-sensor fusion SLAM system, comprising: Step 1: Acquire raw data from the camera, lidar, inertial measurement unit, and global navigation satellite system; Step 2: Based on the VIG tightly coupled optimization unit, perform front-end preprocessing and initial estimation on the raw data to obtain the global pose; Step 3: Based on the LIG tightly coupled optimization unit, motion distortion correction and feature extraction are performed using the original data and global pose. Step 4: Based on the two-stage factor graph optimizer, the laser features and GNSS observation information are fused in two stages to perform global optimization and output high-precision, globally consistent pose and map.

[0009] Further, in step 2, the original data is preprocessed and initially estimated based on the VIG tightly coupled optimization unit to obtain the global pose, specifically as follows: The VIG tightly coupled optimization unit, based on a sliding window optimizer, outputs high-precision global pose in real time by minimizing visual reprojection error, inertial measurement unit pre-integration error, and global navigation satellite system observation residuals.

[0010] Furthermore, in step 3, the LIG tightly coupled optimization unit performs motion distortion correction and feature extraction processing based on the original data and global pose, specifically as follows: The motion distortion correction module of the LIG tightly coupled optimization unit performs motion compensation on the original laser point cloud in the original data based on the global pose. The feature extraction module of the LIG tightly coupled optimization unit performs curvature analysis on the motion-compensated laser point cloud to extract stable geometric features. The data synchronization module of the LIG tightly coupled optimization unit uses a nearest neighbor matching algorithm to clock-align the asynchronous data streams from the lidar, inertial measurement unit, and global navigation satellite system.

[0011] Furthermore, the geometric features include edge geometric feature points and planar geometric feature points.

[0012] Furthermore, in step 4, the first stage is the joint optimization stage, specifically: The two-stage factor graph optimizer constructs a unified factor graph and incorporates geometric feature factors and global pose factors. By minimizing the total residual of all factors, it achieves preliminary tight coupling alignment between the laser local map and the global coordinate system of the global navigation satellite system, thus completing the global pose correction.

[0013] Furthermore, in step 4, the second stage is the constrained smoothing stage, specifically as follows: The two-stage factor graph optimizer constructs global constraints based on the solution values ​​of the global navigation satellite system calculated in the joint optimization stage, performs graph optimization, and further suppresses noise and smooths the trajectory, ultimately outputting a globally consistent and high-precision pose and map.

[0014] In summary, the present invention has at least one of the following beneficial technical effects: 1. Significantly Enhanced Robustness Across All Scenarios: By employing a parallel and collaborative architecture of vision-inertial and laser-inertial dual subsystems, an equal and redundant perception path is constructed. When a single sensor (such as vision in low-light environments or laser in corridor environments) or even multiple sensors fail alternately, the system can still maintain reliable positioning through the other path and the IMU. This fundamentally solves the problems of insufficient adaptability and perception blind spots in existing solutions under specific degraded environments, achieving stable operation in all weather and all scenarios.

[0015] 2. Significantly Improved Global Accuracy and Consistency: Thanks to the introduction of GNSS and a deeply coupled two-stage optimization mechanism, this invention effectively suppresses pose drift caused by error accumulation in traditional visual / laser SLAM. Field tests show that in open areas, the system can achieve centimeter-level positioning accuracy; even in scenarios with intermittent GNSS signals, through the constraints of the optimized framework, the long-term absolute trajectory error is reduced by more than 70% compared to LIO-SLAM or VIO-SLAM schemes without GNSS, completely solving the problem of global consistency loss caused by accumulated errors.

[0016] 3. Intelligent Adaptation and High Reliability in Complex Signal Environments: The hierarchical optimization mechanism proposed in this invention possesses inherent intelligence. The first stage utilizes raw GNSS observations, while the second stage relies on robust solutions after convergence. This process automatically reduces the impact of disturbances such as signal jumps or multipath effects. Compared to simple loosely coupled or weighted fusion schemes, this invention reduces the variance of positioning output by more than 50% in complex environments such as urban canyons where GNSS signal quality dynamically changes. This effectively avoids the misleading influence of poor-quality GNSS signals on the system, and the overall system reliability is significantly superior to existing technologies. Attached Figure Description

[0017] Figure 1 This is a schematic diagram of the overall architecture of the degradation-resistant multi-sensor fusion SLAM system according to an embodiment of the present invention; Figure 2 A schematic diagram of the factor graph model constructed for the two-stage factor graph optimizer.

[0018] Figure 3 This is a schematic diagram of the method flow of the present invention. Detailed Implementation

[0019] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention. Furthermore, the technical features involved in the various embodiments of this invention described below can be combined with each other as long as they do not conflict with each other.

[0020] like Figure 1 As shown, Figure 1 The core components of the system are shown. This invention provides a degradation-resistant multi-sensor fusion SLAM system, including: a vision-inertial-satellite navigation (VIG) tightly coupled optimization unit, a laser-inertial-satellite navigation (LIG) tightly coupled optimization unit, and a two-stage factor graph optimizer; The VIG tightly coupled optimization unit, with a sliding window optimizer at its core, is used to fuse raw data from the camera, lidar, inertial measurement unit (IMU), and global navigation satellite system (GNSS) receiver, and to perform initial estimation to obtain the global pose. The LIG tightly coupled optimization unit is used to perform motion distortion correction and feature extraction based on the original data and global pose. The two-stage factor graph optimizer includes a factor graph model and a corresponding nonlinear least squares solver, which is used to fuse laser features and GNSS observation information in two stages, perform global optimization, and output high-precision, globally consistent pose and map.

[0021] The LIG tightly coupled optimization unit includes a motion distortion correction module, a feature extraction module, and a data synchronization module.

[0022] like Figure 3 As shown, the present invention also provides a degradation-resistant multi-sensor fusion SLAM method, applied to the aforementioned degradation-resistant multi-sensor fusion SLAM system, comprising: Step 1: Acquire raw data from the camera, lidar, inertial measurement unit, and global navigation satellite system; Step 2: Based on the VIG tightly coupled optimization unit, perform front-end preprocessing and initial estimation on the raw data to obtain the global pose; Step 3: Based on the LIG tightly coupled optimization unit, motion distortion correction and feature extraction are performed using the original data and global pose. Step 4: Based on the two-stage factor graph optimizer, the laser features and GNSS observation information are fused in two stages to perform global optimization and output high-precision, globally consistent pose and map.

[0023] In step 2, the raw data is preprocessed and initially estimated based on the VIG tightly coupled optimization unit to obtain the global pose, specifically as follows: The VIG tightly coupled optimization unit, based on a sliding window optimizer, outputs high-precision global pose in real time by minimizing visual reprojection error, inertial measurement unit pre-integration error, and global navigation satellite system observation residuals.

[0024] In step 3, the LIG tightly coupled optimization unit performs motion distortion correction and feature extraction processing based on the original data and global pose, specifically as follows: The motion distortion correction module of the LIG tightly coupled optimization unit performs motion compensation on the original laser point cloud in the original data based on the global pose. The feature extraction module of the LIG tightly coupled optimization unit performs curvature analysis on the motion-compensated laser point cloud to extract stable geometric features. The data synchronization module of the LIG tightly coupled optimization unit performs clock alignment on the asynchronous data streams of the lidar, inertial measurement unit, and global navigation satellite system based on a nearest neighbor matching algorithm with a strict time tolerance threshold.

[0025] The geometric features include edge geometric feature points and planar geometric feature points.

[0026] In step 4, the laser features and GNSS observation information are fused in two stages based on the two-stage factor graph optimizer to perform global optimization and output a high-precision, globally consistent pose and map. Specifically: The two-stage factor graph optimizer is the core of the system's backend fusion. It consists of a factor graph model and a corresponding nonlinear least squares solver, performing deep optimization in two stages. The first stage (joint optimization) involves the optimizer constructing a unified factor graph, incorporating laser geometric feature factors from LIG cells and GNSS observation factors from VIG cells. By minimizing the total residual of all factors, initial tight coupling alignment between the local laser map and the global GNSS coordinate system is achieved, completing global pose correction. The second stage (constrained smoothing) utilizes the more accurate GNSS solutions from the first stage to construct stronger global constraints, performing graph optimization again to further suppress noise and smooth the trajectory, ultimately outputting a globally consistent, high-precision pose and map.

[0027] in, Figure 2This diagram illustrates the factor graph model constructed for the two-stage factor graph optimizer. It specifically shows the various constraint factors (such as GNSS observation factors, lidar feature factors, IMU pre-integration factors, loop closure detection factors, etc.) involved in the optimization process and their connections with system state nodes (such as pose nodes and velocity offset nodes), intuitively demonstrating the optimization framework of deep fusion of multi-source information.

[0028] The present invention also provides an embodiment to verify the effectiveness of the present invention, specifically as follows: This unmanned vehicle performs routine material delivery within the campus. The scenarios include open plazas, tree-lined roads, indoor-outdoor transition areas, and narrow strips of land between buildings where GNSS signals are partially obstructed, posing a comprehensive challenge to the accuracy and robustness of the positioning system. To achieve autonomous navigation covering the entire campus, the delivery vehicle is equipped with the GLVI-SLAM system of this invention. Its sensor configuration includes: a vision component: a global shutter monocular camera mounted at the front of the vehicle, approximately 1.2 meters above the ground, with a 15-degree forward tilt to optimize road observation; a lidar component: a 16-line mechanical lidar mounted at the center of the roof, approximately 1.8 meters above the ground, with a 360-degree horizontal field of view; an inertial measurement unit (IMU): mounted near the vehicle's center of gravity and rigidly connected to the vehicle body; and a dual-frequency GNSS receiver with its antenna fixed to the roof, installed adjacent to the lidar to reduce boom error.

[0029] All sensors acquire and process data through an onboard industrial computer (equipped with an Intel i7 processor), and the system software runs on the Robot Operating System (ROS) framework under the Ubuntu system.

[0030] System Deployment and Workflow: After the system starts up, follow the workflow below: 1) Initialization and Calibration: After the vehicle is powered on, the system first performs sensor timestamp synchronization and extrinsic parameter calibration. A preset inter-sensor conversion matrix is ​​used, and the aforementioned data synchronization module is used to ensure time alignment of the data streams from each sensor.

[0031] 2) Online Operation: The VIG unit takes as input forward camera images, IMU data, and raw GNSS observations, performs tightly coupled optimization within a sliding window, and outputs a global pose at a real-time frequency of 20Hz, providing motion priors for the LIG unit. The LIG unit uses the pose provided by the VIG unit to correct motion distortion in the lidar point cloud and extract line and area features. The two-stage factor map optimizer initiates optimization each time it receives a new frame of lidar features. In open areas with good GNSS signal, the first stage of optimization fully utilizes pseudorange / Doppler factors to quickly align the local trajectory to the global coordinate system. When the vehicle enters a tree-lined road or between buildings, the GNSS signal quality degrades, and the system automatically relies on the constraints formed by loop closure detection and lidar features in the second stage to maintain trajectory consistency.

[0032] This SLAM system acts as the "brain" of the delivery vehicle, providing it with core state estimation. It publishes high-precision pose, velocity, and real-time local maps to other functional modules via ROS topics: Path planning module: Subscribes to the globally consistent map and current pose provided by the SLAM system for global path planning and real-time obstacle avoidance. Decision control module: Based on the precise positioning information provided by the SLAM system, controls the vehicle's speed and steering to achieve precise docking.

[0033] After a week of continuous field testing, the system performed exceptionally well: Global Accuracy: In open areas, its absolute positioning error remained consistently within 5 centimeters, meeting the requirements for precise parking of delivery vehicles. Robustness: On a one-kilometer stretch of tree-lined road (where GNSS signals were intermittent and lighting changes occurred due to tree movement), the system did not experience any positioning loss or jumps, maintaining stable tracking throughout. Effectiveness Verification: Compared to the previously implemented single LIO-SLAM system, its maximum cumulative drift on the same path was reduced from over 5 meters (preventing loop closure) to below 5 centimeters, effectively resolving the long-term global consistency issue.

[0034] This case demonstrates that the system described in this invention can effectively support the long-term, stable, and high-precision operation of autonomous driving applications in complex park environments.

[0035] The implementation steps are described below: 1. Sensor installation and mechanical integration Installation reference established: First, a vehicle coordinate system is established with the vehicle's center of gravity as the theoretical reference origin. High-precision measuring instruments (such as a total station) are used to mark the theoretical installation positions of each sensor on the chassis.

[0036] GNSS antenna installation: Fix the dual-frequency GNSS receiver antenna at the highest point of the vehicle roof, in the center of the plane with the least obstruction, using a special magnetic chuck or flange for rigid connection, ensuring that the antenna plane is horizontal, and reliably connect the coaxial feeder.

[0037] LiDAR installation: The 16-line LiDAR is rigidly mounted on the roof of the vehicle using a bracket, as close as possible to the GNSS antenna to reduce arm error, and its level is adjusted to ensure an unobstructed 360-degree field of view.

[0038] Camera and IMU Installation: Mount the global shutter camera onto the front bumper structure of the vehicle body, adjust its pitch angle (tilt forward by approximately 15 degrees) and secure it firmly. Rigidly mount the IMU near the vehicle's center of gravity using a bracket, aligning its sensitive axis with the vehicle's coordinate system axis.

[0039] 2. System debugging and parameter calibration Electrical connection check: Before powering on, use a multimeter to check the voltage and polarity of all power interfaces to ensure they are correct. Power on each sensor in turn and observe the status of its indicator lights to confirm that it is working properly.

[0040] Intrinsic parameter calibration: In the laboratory, the camera's intrinsic parameters (focal length, principal point, distortion coefficients) are calibrated using a checkerboard calibration board. Noise parameters of the IMU (such as angular random walk, accelerometer bias) are calibrated using specialized tools or specific rotation sequences.

[0041] External parameter calibration: The vehicle is placed in an open area and subjected to low-speed, multi-directional movement to collect data from multiple sensors. Offline calibration algorithms (such as hand-eye calibration or optimization-based methods) are used to accurately calculate the spatial transformation relationships between the camera and IMU, and between the lidar and IMU. The boom vector between the GNSS antenna and the IMU can be estimated through precise measurement or by incorporating it into the optimization process.

[0042] Software parameter configuration and debugging: In the ROS environment, start each node of the system. Use professional visualization debugging tools (such as Rviz) to observe the status of point cloud, image feature tracking, trajectory estimation, etc. in real time. For typical park scenarios (open areas, tree-lined roads), fine-tune the noise model parameters, sliding window size, feature point extraction threshold, etc. in the optimizer to achieve the best balance between accuracy and computational efficiency.

[0043] 3. Operation and maintenance Routine Inspection: Before each mission, perform a quick visual inspection to ensure that all sensor lenses and optical windows are clean and free of dirt, and that connecting cables are not loose or worn. Inspect the surface of the GNSS antenna for damage.

[0044] Periodic calibration: After every 200 hours of operation or after a severe collision, external parameters need to be recalibrated in a controlled environment to correct for parameter changes that may be caused by deformation or loosening of the mechanical structure.

[0045] Performance monitoring and log analysis: During system operation, key performance indicators such as GNSS signal-to-noise ratio (SNR), number of feature tracks, and optimizer residuals are monitored in real time. Operation logs are saved, and the smoothness and closure error of the positioning trajectory are analyzed periodically to promptly identify potential problems.

[0046] Consumable parts and software updates: Regularly check the performance degradation of sensors such as cameras and LiDAR, and calibrate or replace them as necessary according to the user manual. Regularly update the system software to the latest stable version to obtain performance optimizations and bug fixes.

[0047] 4. Implementation Results: After installation, debugging, and maintenance according to the above specifications, the system performed excellently in a week of continuous field testing, effectively supporting the fully autonomous operation of the unmanned delivery vehicle. This case demonstrates that the system described in this invention has clear engineering feasibility, and through standardized installation, debugging, and maintenance procedures, it can ensure long-term, stable, and high-precision operation in complex park environments.

[0048] Embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0049] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0050] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0051] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0052] Contents not described in detail in this specification are prior art known to those skilled in the art. It is hereby indicated that the above description is intended to help those skilled in the art understand this invention, but does not limit the scope of protection of this invention. Any equivalent substitutions, modifications, improvements, or simplifications of the above descriptions that do not depart from the essential content of this invention fall within the scope of protection of this invention.

Claims

1. A robust multi-sensor fusion SLAM system, characterized by, include: Visual-inertial-satellite navigation (VIG) tightly coupled optimization unit, laser-inertial-satellite navigation (LIG) tightly coupled optimization unit, and two-stage factor graph optimizer; The Visual-Inertial-Satellite Navigation (VIG) tightly coupled optimization unit, with a sliding window optimizer at its core, is used to fuse raw data from the camera, lidar, inertial measurement unit, and global navigation satellite system, and to perform initial estimation to obtain the global pose. The laser-inertial-satellite navigation LIG tightly coupled optimization unit is used for motion distortion correction and feature extraction based on raw data and global pose. The two-stage factor graph optimizer includes a factor graph model and a corresponding nonlinear least squares solver, which is used to fuse laser features and GNSS observation information in two stages, perform global optimization, and output high-precision, globally consistent pose and map. The first phase is the joint optimization phase, which specifically includes: The two-stage factor graph optimizer constructs a unified factor graph and adds geometric feature factors and global pose factors. By minimizing the total residual of all factors, it achieves the initial tight coupling alignment between the laser local map and the global coordinate system of the global navigation satellite system, and completes the global pose correction. The second stage is the constrained smoothing stage, specifically: The two-stage factor graph optimizer constructs global constraints based on the solution values ​​of the global navigation satellite system calculated in the joint optimization stage, performs graph optimization, and further suppresses noise and smooths the trajectory, ultimately outputting a globally consistent and high-precision pose and map.

2. The anti-degradation multi-sensor fusion SLAM system of claim 1, wherein, The laser-inertial-satellite navigation LIG tightly coupled optimization unit includes a motion distortion correction module, a feature extraction module, and a data synchronization module.

3. The anti-degradation multi-sensor fusion SLAM method applied to the anti-degradation multi-sensor fusion SLAM system of any of claims 1-2, characterized in that, include: Step 1: Acquire raw data from the camera, lidar, inertial measurement unit, and global navigation satellite system; Step 2: Based on the Vision-Inertial-Satellite Navigation (VIG) tightly coupled optimization unit, the raw data is preprocessed and initially estimated to obtain the global pose; Step 3: Based on the tightly coupled optimization unit of laser-inertial-satellite navigation LIG, motion distortion correction and feature extraction are performed using the original data and global pose. Step 4: Based on the two-stage factor graph optimizer, the laser features and GNSS observation information are fused in two stages to perform global optimization and output high-precision, globally consistent pose and map.

4. The anti-degradation multi-sensor fusion SLAM method of claim 3, wherein, In step 2, the raw data is preprocessed and initially estimated based on the Vision-Inertial-Satellite Navigation (VIG) tightly coupled optimization unit to obtain the global pose, specifically as follows: The Visual-Inertial-Satellite Navigation (VIG) tightly coupled optimization unit, based on a sliding window optimizer, outputs high-precision global pose in real time by minimizing visual reprojection error, inertial measurement unit pre-integration error, and global navigation satellite system observation residuals.

5. The anti-degradation multi-sensor fusion SLAM method of claim 4, wherein, In step 3, the laser-inertial-satellite navigation LIG tightly coupled optimization unit performs motion distortion correction and feature extraction processing based on the original data and global pose, specifically as follows: The motion distortion correction module of the laser-inertial-satellite navigation LIG tightly coupled optimization unit performs motion compensation on the original laser point cloud in the original data based on global pose. The feature extraction module of the laser-inertial-satellite navigation LIG tightly coupled optimization unit performs curvature analysis on the motion-compensated laser point cloud to extract stable geometric features; The data synchronization module of the laser-inertial-satellite navigation LIG tightly coupled optimization unit uses the nearest neighbor matching algorithm to clock-align the asynchronous data streams from the lidar, inertial measurement unit, and global navigation satellite system.

6. The degradation-resistant multi-sensor fusion SLAM method according to claim 5, characterized in that, The geometric features include edge geometric feature points and planar geometric feature points.

Images