A multi-source heterogeneous sensor adaptive fusion positioning and mapping method and device

Abstract

The application provides a multi-source heterogeneous sensor adaptive fusion positioning and mapping method and device, and relates to the technical field of automatic driving and mobile robots. The method comprises the following steps: collecting sensor data of a mobile robot in an indoor environment through a sensor data collection module, inputting the sensor data into a front-end data processing module to obtain processed sensor data, inputting the processed sensor data into an environment scene recognition module to obtain an environment scene type recognition result, and inputting the environment scene type recognition result into a back-end state estimation and optimization module to obtain a map construction and pose estimation result. The application solves the problems in the prior art through an innovative front-end feature extraction, back-end optimization mechanism and multi-sensor fusion strategy. By introducing a low-cost magnetometer, the robustness of the system in the absence of GPS signals or in a complex electromagnetic environment is enhanced, and through an adaptive optimization mechanism, the calculation efficiency and accuracy in different environments are improved, and finally, the functions of high-precision and high-robustness autonomous positioning and mapping are realized.

Description

A localization and mapping method and apparatus based on adaptive fusion of multi-source heterogeneous sensors Technical Field

[0001] This invention relates to the fields of autonomous driving and mobile robot technology, and in particular to a localization and mapping method and apparatus for adaptive fusion of multi-source heterogeneous sensors. Background Technology

[0002] With the widespread application of technologies such as autonomous driving and mobile robots in complex indoor environments, the demand for high-precision and robust autonomous localization and mapping (SLAM) technologies in the absence of GPS (Global Positioning System) signals is becoming increasingly urgent. Complex indoor environments often feature intricate structures and sparse or repetitive features, posing a significant challenge to single-sensor SLAM (Simultaneous Localization and Mapping) technologies. Against this backdrop, SLAM solutions integrating multiple sensors, such as LiDAR, vision, and inertial measurement units, have become a key technological path for achieving stable navigation due to their ability to comprehensively utilize the advantages of different sensors. However, achieving deep adaptive fusion of multiple heterogeneous sensors and intelligent processing of core SLAM components remain current technological bottlenecks.

[0003] Current SLAM systems generally consist of three main parts: front-end sensor acquisition and feature processing, back-end optimization, and loop closure detection. The front-end uses devices such as LiDAR, visual sensors, and IMUs (Inertial Measurement Units) to acquire environmental information and achieves real-time mapping and localization through fusion and feature extraction. Back-end optimization uses graph optimization and nonlinear least squares methods to impose global consistency constraints on pose and map. Loop closure detection identifies whether the system revisits a previously visited area during operation and, in conjunction with back-end optimization, eliminates accumulated errors.

[0004] In the front-end sensor module, LiDAR can acquire geometric structure information of the scene through point cloud scanning, visual sensors provide rich color and texture features, IMU is used to supplement kinematic information to improve short-term positioning accuracy, and magnetometer provides global pose information and complements inertial navigation. Current SLAM algorithms mainly fall into two categories in terms of feature processing methods: geometric feature-based methods and deep learning-based methods. Among geometric feature-based methods, patent 202510647251.7 utilizes differentiated geometric feature processing based on LiDAR, combined with front-end matching and global optimization, to achieve high-precision and robust SLAM in complex environments. Among deep learning-based methods, patent 202511058676.0 introduces a lightweight MLP (Multilayer Perceptron) network to perform implicit feature extraction and volumetric modeling of keyframe point clouds, thereby improving the accuracy, robustness, and real-time performance of the autonomous mapping process. In addition, some methods further fuse multimodal data collected by different sensors to improve the system's positioning capability. For example, patent 202210644332.8 uses a magnetometer and an IMU to collaboratively calculate and generate a real-time first heading angle, and then uses a first-order complementary filter to fuse the fusion result with loop closure detection to generate a more reliable second heading angle to support robot repositioning tasks.

[0005] Backend optimization typically uses graph-based optimization methods as its core. The basic idea is to transform sensor observation information into constraint factors, construct a factor graph, and solve the consistency between the trajectory and the map using nonlinear least squares method to construct a representation of the optimization problem. Through this optimization process, smooth and consistent trajectory estimation and map results can be obtained.

[0006] Loop closure detection in SLAM systems is responsible for correcting pose drift accumulated over time. Existing technologies mainly include methods based on the bag-of-words model (BOD) and methods based on point cloud matching. In the BOD method, patent 202510691685.7 performs loop closure detection by calculating the similarity between the current frame and historical keyframes, and combines this with extended Kalman filtering to achieve pose optimization. In the point cloud matching method, patent 202511189688.7 utilizes visual sensor and LiDAR data combined with point cloud registration and graph optimization methods to achieve robust localization and map maintenance in large spatial environments.

[0007] In multi-sensor fusion SLAM schemes, existing technologies similar to this invention use LiDAR as the core, supplemented by an inertial measurement unit (IMU) and a visual sensor, achieving system framework fusion through loose coupling. The core idea is to leverage the complementary advantages of multiple sensors to improve the accuracy and robustness of localization and mapping. Specifically, LiDAR provides high-precision geometric constraints, the visual sensor supplements texture and semantic information, and the IMU ensures motion continuity on a short-term scale. Each sensor independently performs feature extraction and pose estimation at the front end, while the back end fuses the results through time synchronization and factor graph modeling, and performs global consistency optimization using loop closure detection, thereby achieving high-precision trajectory estimation and environmental map construction.

[0008] In existing multi-sensor fusion SLAM solutions, few consider incorporating magnetometers to assist in mapping optimization. Most existing multi-sensor frameworks are limited to the fusion of LiDAR and IMU or vision and IMU, failing to fully utilize the low-cost magnetometer. This results in insufficient positioning accuracy and limited system robustness in environments with missing GPS signals or complex electromagnetic conditions. Furthermore, IMU modules inevitably experience pose drift during prolonged operation. While GPS constraints are typically used for optimization in outdoor environments to mitigate drift effects, GPS signals are often weak or unavailable in indoor scenarios.

[0009] In front-end feature extraction, traditional methods typically extract corner points and planar points directly from the point cloud geometry, which has high computational efficiency and interpretability, but is easily affected by noise, occlusion and sparse point clouds, leading to unstable feature extraction. On the other hand, some deep learning methods rely too much on the dataset and have insufficient generalization ability in new environments, making them unsuitable for application in SLAM systems without prior knowledge.

[0010] In terms of backend optimization, existing SLAM backend optimizations typically use fixed parameters (such as keyframe search radius, voxel sampling density, etc.), which cannot be adaptively adjusted according to environmental complexity or motion state, resulting in high computational overhead in dense environments and insufficient accuracy in sparse environments. Summary of the Invention

[0011] To address the shortcomings of existing multi-sensor fusion SLAM technologies, such as insufficient positioning accuracy and limited system robustness in environments with missing GPS signals or complex electromagnetic fields, and the inevitable pose drift of the IMU module during long-term operation, the present invention provides a localization and mapping method and apparatus based on adaptive fusion of multi-source heterogeneous sensors. The technical solution is as follows:

[0012] On the one hand, a localization and mapping method based on adaptive fusion of multi-source heterogeneous sensors is provided. This method is implemented by a localization and mapping device based on adaptive fusion of multi-source heterogeneous sensors, and includes:

[0013] S1. For mobile robots in indoor environments, sensor data is collected through a sensor data acquisition module; wherein, the sensor data acquisition module includes a first magnetometer, a second magnetometer, an inertial measurement unit, a lidar, and a depth camera; the collected sensor data includes a first geomagnetic sequence, a second geomagnetic sequence, inertial measurement unit data, point cloud data, and depth camera data.

[0014] S2. Input the sensor data into the front-end data processing module to obtain processed sensor data. The front-end data processing module includes an inertial enhancement submodule, a three-line laser feature extraction submodule, and a visual feature extraction submodule. The inertial enhancement submodule processes the first geomagnetic sequence, the second geomagnetic sequence, and inertial measurement unit data to obtain enhanced velocity estimates. The three-line laser feature extraction submodule processes point cloud data to obtain point cloud features. The visual feature extraction submodule processes depth camera data to obtain visual feature points.

[0015] S3. Input the processed sensor data into the environmental scene recognition module to obtain the environmental scene type recognition result.

[0016] S4. Input the processed sensor data and environmental scene type recognition results into the backend state estimation and optimization module to obtain map construction and pose estimation results. The backend state estimation and optimization module includes a loop closure detection submodule and a global optimization submodule. The loop closure detection submodule is used to provide return determination information to the global optimization submodule by fusing loop closure detection based on multi-dimensional sequences, laser loop closure, and visual loop closure. The global optimization submodule is used to perform map construction and pose estimation based on the return determination information.

[0017] Optionally, the enhanced velocity estimate obtained in S2 by processing the first geomagnetic sequence, the second geomagnetic sequence, and the inertial measurement unit data includes:

[0018] S211. Obtain the main sensitive direction projections of the first geomagnetic sequence and the second geomagnetic sequence respectively, and perform bandpass filtering to obtain the filtered first geomagnetic sequence and the second geomagnetic sequence.

[0019] S212. Construct Hilbert analytic signals for the filtered first and second geomagnetic sequences respectively and expand their phases to obtain the phases of the analytic signals of the first and second geomagnetic sequences.

[0020] S213. Based on the phase of the analytical signals of the first and second geomagnetic sequences, define the average estimate of the instantaneous angular frequency and the phase difference.

[0021] S214. Based on the average estimate of the defined instantaneous angular frequency and the phase difference, a sliding window mechanism is introduced to obtain the time delay.

[0022] S215. Based on the baseline distance and time delay between the first and second magnetometers, obtain the magnetometer velocity estimate and the variance of the magnetometer velocity estimate in the window sense.

[0023] S216. Perform gravity compensation on the inertial measurement unit data and integrate it along the forward direction of the mobile robot to obtain the inertial measurement unit velocity estimate. Use the first-order prediction of the inertial measurement unit velocity estimate to compensate the magnetometer velocity estimate to obtain the compensated magnetometer velocity estimate.

[0024] S217. Based on the compensated magnetometer velocity estimate and the inertial measurement unit velocity estimate, the enhanced velocity estimate is obtained.

[0025] Optionally, the point cloud data is processed in S2 to obtain point cloud features, including:

[0026] S221. Perform distortion correction on the point cloud data, and project the distortion-corrected point cloud data onto a two-dimensional raster image to obtain point cloud depth information.

[0027] S222. Select point cloud data on a lidar scanning line, and obtain multiple segmented intervals by segmenting the lidar scanning line.

[0028] S223. For the point cloud data in each segment interval, select a target point through an algorithm, obtain multiple neighboring points of the target point, calculate the difference between the depth value of the target point and the depth value of the neighboring points, and judge the validity of the target point by comparing the difference between the depth values ​​with the preset depth threshold, thereby obtaining the valid points in each segment interval, and then obtaining the valid points on a lidar scanning line.

[0029] S224. By acquiring the effective points on the three lidar scanning lines, the depth value of the accumulated effective points is obtained, and the curvature of the effective points is calculated based on the depth value of the accumulated effective points.

[0030] S225. Classify the valid points according to their curvature to obtain point cloud features.

[0031] Optionally, in S3, the processed sensor data is input to the environmental scene recognition module to obtain the environmental scene type recognition result, including:

[0032] S31. Determine whether the average distance of the point cloud is greater than a preset first distance threshold and whether the point cloud feature density is less than a preset feature density threshold based on the point cloud features.

[0033] If so, output the third feature value and store it in the scene state manager.

[0034] If not, determine whether the average distance of the point cloud is less than the preset second distance threshold; if yes, output the first feature value and store it in the scene state manager; if no, output the second feature value and store it in the scene state manager.

[0035] S32. Sum the feature values ​​in the scene state manager and identify the current environment scene type based on the summation result; the environment scene type includes open scene, narrow corridor scene and normal scene.

[0036] Optionally, S3 also includes:

[0037] Adjust the density and downsampling ratio of adjacent keyframes according to the environmental scene type.

[0038] This includes adjusting the density and downsampling ratio of adjacent keyframes based on the environmental scene type, including:

[0039] When the environment scene type is an open scene, the density of neighboring keyframes is increased based on the preset standard neighboring keyframe density according to the preset neighboring keyframe density calculation formula, and the downsampling ratio is reduced based on the preset standard downsampling ratio according to the preset dynamic downsampling ratio calculation formula.

[0040] When the environment scene type is a narrow corridor scene, the density of neighboring keyframes is reduced according to the preset neighboring keyframe density calculation formula based on the preset standard neighboring keyframe density, and the downsampling ratio is increased according to the preset dynamic downsampling ratio calculation formula based on the preset standard sampling rate.

[0041] Optionally, in S4, loop closure detection based on multi-dimensional sequences, laser loop closure, and visual loop closure are fused to provide recurrence determination information to the global optimization submodule, including:

[0042] S41. Perform single-point loop closure detection.

[0043] The single-point loop closure detection includes: representing the geomagnetic sequence of the current point cloud and the geomagnetic sequence of the historical point cloud using three-dimensional vectors; calculating the cosine similarity between the current point cloud and the historical point cloud based on their three-dimensional vectors to obtain the geomagnetic similarity between them; acquiring the time of the current point cloud and the historical point cloud respectively, and calculating the time interval between them; acquiring the coordinates of the current point cloud and the historical point cloud respectively, and calculating the spatial distance between them; constructing a loop closure determination function based on the geomagnetic similarity, time interval, and spatial distance, and determining whether the current point cloud is a valid loop closure point based on the loop closure determination function.

[0044] S42. After single-point loop closure detection, perform loop closure detection based on multidimensional sequences.

[0045] The loop closure detection based on multidimensional sequences includes: acquiring two geomagnetic history sequences to be matched; defining a point-by-point local distance metric function based on the geomagnetic history sequences; introducing a cumulative cost matrix based on the point-by-point local distance metric function; and finding the optimal path through dynamic recursion.

[0046] S43. Based on loop closure detection based on multidimensional sequences, laser loop closure and visual loop closure are introduced, and the loop closure detection based on multidimensional sequences, laser loop closure and visual loop closure are fused to construct a loop closure confidence function. Based on the loop closure confidence function, return determination information is provided to the global optimization module.

[0047] Optionally, the global optimization submodule in S4 includes: an adaptive residual optimization method.

[0048] Among them, the adaptive residual optimization method includes:

[0049] Calculate the distance from the current point cloud to the target plane and determine the basic distance threshold.

[0050] Based on the distance from the current point cloud to the target plane and the basic distance threshold, the current point cloud is determined to be a near point or a far point. If it is a near point, the adjustment factor of the current point cloud is calculated according to the preset formula for the near point adjustment factor; if it is a far point, the adjustment factor of the current point cloud is calculated according to the preset formula for the far point adjustment factor. The weight of the current point cloud is obtained based on the adjustment factor of the current point cloud, and the map construction and pose estimation are optimized based on the weight.

[0051] On the other hand, a localization and mapping device for adaptive fusion of multi-source heterogeneous sensors is provided. This device is applied to the localization and mapping method for adaptive fusion of multi-source heterogeneous sensors. The device includes:

[0052] The sensor data acquisition module is used to collect sensor data for mobile robots in indoor environments. The sensor data acquisition module includes a first magnetometer, a second magnetometer, an inertial measurement unit, a lidar, and a depth camera. The collected sensor data includes a first geomagnetic sequence, a second geomagnetic sequence, inertial measurement unit data, point cloud data, and depth camera data.

[0053] The front-end data processing module is used to input sensor data and obtain processed sensor data. This module includes an inertial enhancement submodule, a three-line laser feature extraction submodule, and a visual feature extraction submodule. The inertial enhancement submodule processes the first geomagnetic sequence, the second geomagnetic sequence, and inertial measurement unit data to obtain enhanced velocity estimates. The three-line laser feature extraction submodule processes point cloud data to obtain point cloud features. The visual feature extraction submodule processes depth camera data to obtain visual feature points.

[0054] The environmental scene recognition module is used to input the processed sensor data into the environmental scene recognition module to obtain the environmental scene type recognition result.

[0055] The backend state estimation and optimization module is used to input the processed sensor data and environmental scene type recognition results into the backend state estimation and optimization module to obtain map construction and pose estimation results. The backend state estimation and optimization module includes a loop closure detection submodule and a global optimization submodule. The loop closure detection submodule is used to provide recurrence determination information to the global optimization submodule by fusing loop closure detection based on multi-dimensional sequences, laser loop closure, and visual loop closure. The global optimization submodule is used to perform map construction and pose estimation based on the recurrence determination information.

[0056] On the other hand, a localization and mapping device for adaptive fusion of multi-source heterogeneous sensors is provided. The localization and mapping device for adaptive fusion of multi-source heterogeneous sensors includes: a processor; a memory, wherein the memory stores computer-readable instructions, and when the computer-readable instructions are executed by the processor, any one of the methods described above for localization and mapping of multi-source heterogeneous sensors is implemented.

[0057] On the other hand, a computer-readable storage medium is provided, wherein at least one instruction is stored in the storage medium, the at least one instruction being loaded and executed by a processor to implement any of the above-described methods for localization and mapping of multi-source heterogeneous sensors through adaptive fusion.

[0058] The beneficial effects of the technical solutions provided in the embodiments of the present invention include at least the following:

[0059] In this invention, the magnetometer and IMU are fused to estimate pose, which can effectively suppress drift error.

[0060] A feature extraction method combining geometric stability and projection processing is proposed. Curvature is calculated through a two-dimensional depth projection map and occlusion and parallel points are removed. At the same time, line bundle partitioning is used to improve the robustness of feature classification.

[0061] An adaptive optimization mechanism is proposed, which can automatically identify scene categories based on point cloud density and inter-frame displacement, and dynamically adjust the keyframe search radius, sampling density and residual weight to achieve optimization that balances efficiency and accuracy. Attached Figure Description

[0062] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0063] Figure 1 is a flowchart of a localization and mapping method for adaptive fusion of multi-source heterogeneous sensors provided in an embodiment of the present invention;

[0064] Figure 2 is a schematic diagram of the front and rear magnetometers provided in an embodiment of the present invention;

[0065] Figure 3 is a flowchart of the feature processing provided in an embodiment of the present invention;

[0066] Figure 4 is a schematic diagram of feature segmentation extraction provided in an embodiment of the present invention;

[0067] Figure 5 is a flowchart of the scene judgment provided in an embodiment of the present invention;

[0068] Figure 6 is a flowchart of the multimodal loop closure detection provided in an embodiment of the present invention;

[0069] Figure 7 is a diagram of the overall system structure provided in an embodiment of the present invention;

[0070] Figure 8 is a block diagram of a localization and mapping device for adaptive fusion of multi-source heterogeneous sensors provided in an embodiment of the present invention;

[0071] Figure 9 is a schematic diagram of a positioning and mapping device with adaptive fusion of multi-source heterogeneous sensors provided in an embodiment of the present invention. Detailed Implementation

[0072] The technical solution of the present invention will now be described with reference to the accompanying drawings.

[0073] In embodiments of the present invention, words such as "exemplarily," "for example," etc., are used to indicate that something is an example, illustration, or description. Any embodiment or design described as "exemplary" in the present invention should not be construed as being more preferred or advantageous than other embodiments or designs. Specifically, the use of the word "exemplary" is intended to present the concept in a concrete manner. Furthermore, in embodiments of the present invention, the meaning expressed by "and / or" can be both, or either one.

[0074] In the embodiments of this invention, the terms "image" and "picture" may sometimes be used interchangeably. It should be noted that, without emphasizing the distinction between them, they convey the same meaning. Similarly, the terms "of," "corresponding (relevant)," and "corresponding" may sometimes be used interchangeably. It should be noted that, without emphasizing the distinction between them, they convey the same meaning.

[0075] In this embodiment of the invention, sometimes a subscript such as W1 may be written in a non-subscript form such as W1. When the difference is not emphasized, the meaning they express is the same.

[0076] To make the technical problems, technical solutions and advantages of the present invention clearer, a detailed description will be given below in conjunction with the accompanying drawings and specific embodiments.

[0077] This invention provides a localization and mapping method based on adaptive fusion of multi-source heterogeneous sensors. This method can be implemented by a localization and mapping device based on adaptive fusion of multi-source heterogeneous sensors, which can be a terminal or a server. As shown in Figure 1, the flowchart of the localization and mapping method based on adaptive fusion of multi-source heterogeneous sensors includes the following steps:

[0078] S1. For mobile robots in indoor environments, sensor data is collected through a sensor data acquisition module.

[0079] The sensor data acquisition module includes a first magnetometer, a second magnetometer, an inertial measurement unit, a lidar, and a depth camera; the acquired sensor data includes a first geomagnetic sequence, a second geomagnetic sequence, inertial measurement unit data, point cloud data, and depth camera data.

[0080] In one feasible implementation, this invention proposes a multi-source heterogeneous sensor fusion SLAM method suitable for complex indoor environments, especially under conditions of weak or failed GPS signals. The aim is to achieve high-precision autonomous localization and environmental mapping for mobile robots in the absence of external positioning references. It should be clarified that this scenario is merely for explaining the principles and steps of the localization method in this invention to facilitate reader understanding and does not constitute a limitation on the scope of protection of this invention.

[0081] S2. Input the sensor data into the front-end data processing module to obtain the processed sensor data.

[0082] The front-end data processing module includes an inertial enhancement submodule, a three-line laser feature extraction submodule, and a visual feature extraction submodule. The inertial enhancement submodule is used to process the first geomagnetic sequence, the second geomagnetic sequence, and inertial measurement unit data to obtain enhanced velocity estimates. The three-line laser feature extraction submodule is used to process point cloud data to obtain point cloud features. The visual feature extraction submodule is used to process depth camera data to obtain visual feature points.

[0083] Optionally, the process of processing the first geomagnetic sequence, the second geomagnetic sequence, and the inertial measurement unit data in S2 to obtain the enhanced velocity estimate may include the following steps S211-S217:

[0084] S211. Obtain the main sensitive direction projections of the first geomagnetic sequence and the second geomagnetic sequence respectively, and perform bandpass filtering to obtain the filtered first geomagnetic sequence and the second geomagnetic sequence.

[0085] S212. Construct Hilbert analytic signals for the filtered first and second geomagnetic sequences respectively and expand their phases to obtain the phases of the analytic signals of the first and second geomagnetic sequences.

[0086] S213. Based on the phase of the analytical signals of the first and second geomagnetic sequences, define the average estimate of the instantaneous angular frequency and the phase difference.

[0087] S214. Based on the average estimate of the defined instantaneous angular frequency and the phase difference, a sliding window mechanism is introduced to obtain the time delay.

[0088] S215. Based on the baseline distance and time delay between the first and second magnetometers, obtain the magnetometer velocity estimate and the variance of the magnetometer velocity estimate in the window sense.

[0089] S216. Perform gravity compensation on the inertial measurement unit data and integrate it along the forward direction of the mobile robot to obtain the inertial measurement unit velocity estimate. Velocity estimation using inertial measurement units The first-order prediction is used to compensate for the magnetometer velocity estimate, resulting in a compensated magnetometer velocity estimate. .

[0090] S217. Estimation based on the compensated magnetometer velocity. and inertial measurement unit velocity estimation This yields an enhanced velocity estimate.

[0091] In one feasible implementation, the inertial enhancement submodule employs a velocity estimation enhancement method for inertial measurement units based on dual magnetometers. First, geomagnetic sequences are acquired using dual magnetometers arranged front and rear. The installation method of the dual magnetometers is shown in Figure 2. A rear magnetometer (second magnetometer) and a front magnetometer (first magnetometer) are sequentially positioned on the carrier along the direction of motion. The baseline length of the fixed structure for the two magnetometers is denoted as... The time delay between the two sensors is then calculated using the phase difference, and combined with the known baseline distance, the robot's velocity within that time period is extrapolated, thus forming a drift-free low-frequency velocity estimate. While the IMU (Integrated Mutor Unit) can obtain velocity information with good high-frequency response through acceleration integration, it suffers from low-frequency drift. Combining the characteristics of both, the system employs adaptive complementary filtering for fusion in the frequency domain: the low-frequency component is constrained by the magnetometer velocity to ensure long-term stability; the high-frequency component is compensated by the IMU velocity to ensure dynamic response. An adaptive weighting and time delay compensation mechanism based on signal coherence and noise level is introduced during the fusion process to ensure smooth, accurate, and robust velocity estimation even under magnetic interference or complex environments.

[0092] Specifically, the triaxial sequence is obtained by sampling from the front and rear magnetometers. ,in, Indicates the front magnetometer at A three-axis sequence of time points. This indicates the three-axis sequence of the post-magnetic meter. Indicates time, that is, the magnetometer at various moments. The measurement data, under normal circumstances, It is a continuous time series, representing the sampling times of the sensor at different times. Representing three-dimensional real space. To obtain a phase-interpretable one-dimensional signal, projection onto the principal sensing direction is performed, followed by bandpass filtering to remove DC and suppress high-frequency noise.

[0093] (1)

[0094] In the formula, This represents the one-dimensional magnetic signal of the front magnetometer in the main sensing direction. This indicates that the direction vector is consistent with the direction of the system's motion. Represents the transpose of a vector. This represents the one-dimensional magnetic signal of the magnetometer in the main sensing direction. Indicates time.

[0095] Construct a Hilbert analytic signal from the above one-dimensional signal and expand its phase:

[0096] (2)

[0097] In the formula, This represents the analytical signal obtained after the Hilbert transform of the former magnetometer. This represents the one-dimensional magnetic signal of the front magnetometer in the main sensing direction. Indicates the imaginary part. This represents the Hilbert transform operator, used to construct the imaginary part of a signal. This indicates the instantaneous amplitude of the front magnetometer signal. This indicates the instantaneous phase of the front magnetometer signal. This represents the analytical signal obtained after the Hilbert transform of the magnetometer. This indicates the instantaneous amplitude of the magnetometer signal. This indicates the instantaneous phase of the magnetometer signal.

[0098] Define the average estimate of the instantaneous angular frequency:

[0099] (3)

[0100] In the formula, This represents the average estimate of the instantaneous angular frequency. The time derivative of the phase of the front magnetometer, i.e., the instantaneous angular frequency. This represents the time derivative of the phase of the magnetometer, i.e., the instantaneous angular frequency.

[0101] And the definition of phase difference:

[0102] (4)

[0103] In the formula, Indicates phase difference, This indicates the instantaneous phase of the front magnetometer analytical signal. This indicates the instantaneous phase of the post-magnetic meter analytical signal.

[0104] To address noise, non-stationary disturbances, and local distortions in magnetic field signals in real-world environments, the system incorporates a sliding window mechanism in phase difference calculation. Built-in weights Using weighted least squares to fit the uniform time delay :

[0105] (5)

[0106] In the formula, express The estimated value, This indicates the instantaneous amplitude of the front magnetometer signal. This indicates the instantaneous amplitude of the magnetometer signal. This represents the average estimate of the instantaneous angular frequency. This indicates the phase difference.

[0107] Thus, the magnetometer velocity estimate in the sense of a window is obtained:

[0108] (6)

[0109] In the formula, This represents the magnetometer velocity estimation in the sense of a window. It represents a specific moment within a window of time.

[0110] Its variance is used for subsequent adaptive weighting, and the variance is as follows:

[0111] (7)

[0112] In the formula, This represents the variance of the magnetometer velocity estimate. The variance is the fitted residual.

[0113] Gravity compensation was then applied to the IMU, and the velocity was estimated by integrating along the forward direction. Because the phase method will bring about Due to the window delay, first-order prediction is used for compensation:

[0114] (8)

[0115] In the formula, This represents the estimated magnetometer velocity after compensation. Indicates the discrete sampling time. This indicates the velocity estimated by the magnetometer using the phase method. Indicates the length of the sliding window. This represents the rate of change of IMU velocity.

[0116] The continuous domain of the first-order complementary structure is:

[0117] (9)

[0118] In the formula, This represents the transfer function of a low-pass filter. Represents the Laplace operator. This represents the time constant of the filter. This represents the transfer function of a high-pass filter.

[0119] Discrete implementation is as follows:

[0120] (10)

[0121] In the formula, This represents the velocity estimation result after complementary filtering and fusion. , The smoothed value representing the time constant. This represents the velocity estimate after IMU gravity compensation; the third term is equivalent to... High-pass injection preserves fast dynamics and suppresses low-frequency drift.

[0122] Adaptive adjustment based on the quality index of the magnetometer speed measurement :

[0123] (11)

[0124] In the formula, This represents the adaptively adjusted filter time constant based on the magnetometer's velocities quality index. Represents the initial time constant. Indicates coherence. This represents a small constant that avoids a denominator of zero. This is represented as a weighting factor, used to adjust the degree of influence of quality indicators on the time constant. This represents the standard deviation of the magnetometer speed estimate. Indicates the baseline standard deviation. and The adjustment index represents the control over the effects of variance and signal-to-noise ratio on the time constant, respectively. Indicates the reference signal-to-noise ratio. This represents the amplitude signal-to-noise ratio.

[0125] Then perform exponential smoothing to obtain :

[0126] (12)

[0127] In the formula, This represents the time constant after exponential smoothing. This represents the smoothing coefficient, which controls the degree of smoothing.

[0128] Optionally, processing the point cloud data in S2 to obtain point cloud features may include the following steps S221-S225:

[0129] S221. Perform distortion correction on the point cloud data, and project the distortion-corrected point cloud data onto a two-dimensional raster image to obtain point cloud depth information.

[0130] S222. Select point cloud data on a lidar scanning line, and obtain multiple segmented intervals by segmenting the lidar scanning line.

[0131] S223. For the point cloud data in each segment interval, select a target point through an algorithm, obtain multiple neighboring points of the target point, calculate the difference between the depth value of the target point and the depth value of the neighboring points, and judge the validity of the target point by comparing the difference between the depth values ​​with the preset depth threshold, thereby obtaining the valid points in each segment interval, and then obtaining the valid points on a lidar scanning line.

[0132] S224. By acquiring the effective points on the three lidar scanning lines, the depth value of the accumulated effective points is obtained, and the curvature of the effective points is calculated based on the depth value of the accumulated effective points.

[0133] S225. Classify the valid points according to their curvature to obtain point cloud features.

[0134] In one feasible implementation, the three-line laser feature extraction submodule employs a point cloud feature extraction method based on a three-line scanning intersection structure. First, the point cloud information from the lidar is distorted and projected onto a two-dimensional raster image to store depth information. Then, depth curvature is calculated on each laser beam to identify potential feature points. During point cloud feature extraction, this invention adopts a segmented feature extraction strategy, dividing each frame of laser data into six segments according to the laser beams. High-curvature corner points and low-curvature face points are extracted separately. Unclassified but highly reliable transition points are downsampled and extracted and then included in the face point set to improve mapping density. The corner point and face point sets are stored in a PCL data structure and published to the backend module. The feature extraction process is shown in Figure 3.

[0135] Specifically, in the point cloud processing, a frame of LiDAR data is selected, and an algorithm identifies a target point and iterates through the five neighboring points before and after that target point. During this iteration, the algorithm evaluates the validity of the depth value of each point based on a set threshold. By comparing the depth difference between the target point and its neighboring points, the system can identify invalid points, such as abnormal points caused by noise or uneven reflection. Only when the depth difference between neighboring points is less than the preset threshold are these points considered valid, and their depth values ​​are accumulated and used for curvature calculation.

[0136] After traversing the neighboring points on the three lines, the algorithm infers the geometric curvature of the target point in the local environment by the depth difference between the target point and its surrounding points. This is calculated by subtracting the product of the number of valid points and the current point's depth from the sum of the depths, and then taking the square root to obtain the curvature of the current point. The specific calculation is as follows:

[0137] (13)

[0138] In the formula, Indicates the curvature at the current point. The sum of the depths of the extracted valid points. The depth of the current point. The effective number of point clouds.

[0139] To avoid inaccurate feature extraction due to uneven distribution of feature points, this invention segments the laser beam, dividing the point cloud data from one scan of each lidar line into 6 segments. A limited number of features are extracted from each segment separately, as shown in Figure 4.

[0140] In Figure 4, startRingIndex and endRingIndex are the start and end indices of each scan line. and These are the start and end points of each segment, where... and The calculation is as follows:

[0141] (14)

[0142] In the formula, the point cloud data of each lidar scan line is divided sequentially into... Segment. The start and end positions of each segment are determined by... and Indexing is determined. When dividing each region, to avoid duplicate feature extraction at the start and end points, approximately 10 points within this section will not participate in feature extraction. These points are mainly the acute angles of the scan lines to prevent duplicate feature extraction. Feature extraction for each region is performed cumulatively, gradually extracting features from each segment. Then, the curvature of all points in the point cloud is sorted from smallest to largest to ensure that points with high curvature are preferentially extracted as corner points.

[0143] In feature classification, the system distinguishes different types of points by calculating the curvature of each point. Points with larger curvature are selected as corner points, which are typically located at the edges or junctions of objects. Points with smaller curvature are marked as face points, which are typically located in flat areas. Points with curvature values ​​between corner and face points are considered transition points. They do not fully conform to the characteristics of corner or face points, but still have some value for environment modeling and matching. Therefore, some transition points are extracted and added to the face point set through downsampling. Finally, the processed feature point cloud is published for use by the environment scene recognition module.

[0144] S3. Input the processed sensor data into the environmental scene recognition module to obtain the environmental scene type recognition result.

[0145] In one feasible implementation, the environmental scene recognition module includes scene recognition and parameter adaptive adjustment.

[0146] For scene type determination, this invention proposes an environmental scene recognition method that classifies scenes into three types: open scenes (sparse point clouds and relatively far distances), narrow corridor scenes (dense point clouds and relatively close distances), and ordinary scenes (moderate point cloud density). The system analyzes scene features based on point cloud reflection intensity and point cloud feature density, and stores the corresponding feature values ​​in the scene state manager for scene determination. The determination process is shown in Figure 5.

[0147] After continuously collecting five frames of point cloud data, the system sums the feature values ​​stored in the scene state manager to identify the type of the current environment. If the sum is greater than 8, the scene is described as a large scene with sparse features; if the sum is less than 2, it is described as a spatial scene with sparse features; if the sum is between 2 and 8, the scene is described as a normal ordinary scene.

[0148] For adaptive parameter adjustment, after obtaining the environmental scene type, the system will adjust the density of adjacent keyframes and the downsampling ratio according to the characteristics of different scenes to optimize the construction of local maps and make reasonable use of computing resources.

[0149] Specifically, in open scenes, due to sparse point clouds, the density of adjacent keyframes needs to be increased; in narrow scenes, due to dense point clouds, the density of adjacent keyframes should be appropriately reduced; and in ordinary scenes, the standard density should be maintained. The specific adjustments are calculated using the following formula:

[0150] (15)

[0151] (16)

[0152] in The average point cloud distance for the current scene. and The mean and standard deviation are used to smooth the point cloud distance. The ratio of valid points to total points is used to represent point cloud density. This is a density weighting factor used to control the influence of point cloud density. This is a scene type factor that dynamically adjusts based on the scene type (open, narrow, normal). The density of neighboring keyframes.

[0153] The downsampling ratio is dynamically adjusted based on the scene type and point cloud density. Open scenes, due to sparse point clouds, require retaining more point cloud data and thus necessitate a high sampling rate. Narrow scenes undergo stronger downsampling to reduce computational load. Ordinary scenes use the standard downsampling ratio. The dynamic downsampling ratio is calculated as follows:

[0154] (17)

[0155] In the formula, and These are used to adjust the effects of point cloud density and the number of keyframes on the downsampling ratio, respectively. This indicates the number of keyframes in the current local map. The maximum number of keyframes, The standard downsampling rate.

[0156] S4. Input the processed sensor data and environmental scene type recognition results into the backend state estimation and optimization module to obtain map construction and pose estimation results.

[0157] The backend state estimation and optimization module includes a loop closure detection submodule and a global optimization submodule. The loop closure detection submodule provides recurrence determination information to the global optimization submodule by fusing loop closure detection based on multi-dimensional sequences, laser loop closure, and visual loop closure. The global optimization submodule is used to construct a map and estimate pose based on the recurrence determination information.

[0158] Optionally, step S4 above may include the following steps S41-S43:

[0159] S41. Perform single-point loop closure detection.

[0160] The single-point loop closure detection includes: representing the geomagnetic sequence of the current point cloud and the geomagnetic sequence of the historical point cloud using three-dimensional vectors; calculating the cosine similarity between the current point cloud and the historical point cloud based on their three-dimensional vectors to obtain the geomagnetic similarity between them; acquiring the time of the current point cloud and the historical point cloud respectively, and calculating the time interval between them; acquiring the coordinates of the current point cloud and the historical point cloud respectively, and calculating the spatial distance between them; constructing a loop closure determination function based on the geomagnetic similarity, time interval, and spatial distance, and determining whether the current point cloud is a valid loop closure point based on the loop closure determination function.

[0161] S42. After single-point loop closure detection, perform loop closure detection based on multidimensional sequences.

[0162] The loop closure detection based on multidimensional sequences includes: acquiring two geomagnetic history sequences to be matched; defining a point-by-point local distance metric function based on the geomagnetic history sequences; introducing a cumulative cost matrix based on the point-by-point local distance metric function; and finding the optimal path through dynamic recursion.

[0163] S43. Based on loop closure detection based on multidimensional sequences, laser loop closure and visual loop closure are introduced, and the loop closure detection based on multidimensional sequences, laser loop closure and visual loop closure are fused to construct a loop closure confidence function. Based on the loop closure confidence function, return determination information is provided to the global optimization module.

[0164] In one feasible implementation, the loop closure detection submodule is based on a multimodal revisit determination and path correction method using a dual magnetometer.

[0165] Specifically, magnetometer loop closure detection uses collected geomagnetic data to determine whether the current path intersects with historical paths, thereby enabling revisit determination and path correction. In practical applications, while single-point loop closure detection can quickly trigger loop closure judgment, it carries a certain risk of false loops due to environmental factors. To address this, this invention proposes a loop closure detection method based on geomagnetic sequences. By comprehensively analyzing geomagnetic data from multiple time points, it further refines the determination of whether a recurrence has occurred. Finally, it integrates visual loop closure and laser loop closure data to form a multimodal loop closure detection. The specific process is shown in Figure 6.

[0166] The core of single-point loop closure detection lies in calculating the geomagnetic similarity between the current point and historical points, and combining this with time interval and spatial consistency for judgment. The specific steps are as follows:

[0167] The first step is the extraction and representation of geomagnetic features. The geomagnetic data of the current point and historical points are represented as three-dimensional vectors. These vectors reflect the geomagnetic field strength and direction at the current and historical points. It should also be noted that the triaxial sequence obtained from the sampling of the front and rear magnetometers in step S2 is a continuous time series magnetic signal used for dynamic phase method velocity estimation. Here, it is a geomagnetic three-dimensional fingerprint of a "single-time spatial point" used for static matching and positioning. A single magnetometer can complete this, and dual magnetometers are not required.

[0168] The geomagnetic similarity between the current point and historical points is quantified by calculating cosine similarity. The formula for calculating cosine similarity is as follows:

[0169] (18)

[0170] The closer the ratio of dot product to modulus is to 1, the more similar the geomagnetic fields of the two points are, and they may be loops at the same location.

[0171] At the same time, it is also necessary to ensure that the time interval between loopback points is large enough to avoid false matches. Assume the time intervals of the current point and the historical point are respectively... and Then the time interval between the two points is:

[0172] (19)

[0173] Set a time threshold; if the time interval is less than the threshold, exclude the current loop point.

[0174] Loop closure points not only need to meet geomagnetic similarity and time interval requirements, but also spatial consistency. Calculate the spatial distance between the current point and historical points:

[0175] (20)

[0176] in These are the coordinates of the current point. These are the coordinates of a historical point.

[0177] If the spatial distance between the two exceeds a preset threshold If so, then that point is excluded as a loop point.

[0178] Based on the aforementioned geomagnetic similarity, time interval, and spatial consistency, the loop closure determination function can be obtained from the multi-dimensional factors of revisit consistency analysis. for:

[0179] (twenty one)

[0180] in , , These are weighting factors used to control the influence of geomagnetic similarity, time interval, and spatial consistency. When the set threshold is exceeded, the current point is considered a valid loop closure point.

[0181] After coarse matching using single-point loop closure detection, the system will perform fine-grained loop closure detection based on multi-dimensional sequence matching. By comparing geomagnetic data from multiple time points, the similarity of the geomagnetic sequences is used to further confirm loop closures.

[0182] Let the two geomagnetic history sequences to be matched be as follows: and Each element , All are three-dimensional feature vectors. To measure the differences between points in the sequence, a point-by-point local distance metric function is defined:

[0183] (twenty two)

[0184] Since the samples on the time axis are not strictly aligned, a highly compatible matching method is needed to adapt to time scaling and offset. Therefore, a cumulative cost matrix is ​​introduced. The optimal path is found through dynamic recursion. The recursive formula is:

[0185] (twenty three)

[0186] The cumulative minimum cost of the optimal path, i.e., the overall difference in the final match, is as follows:

[0187] (twenty four)

[0188] in, This represents a legal path that satisfies the boundary, monotonicity, and continuity constraints.

[0189] In large-scale indoor environments, directly performing full computation on all candidate sequences results in a complexity of O(n log n). The computational cost is too high. Therefore, a method of constructing upper and lower bound envelopes is used to quickly filter the sequence. Specifically, given a window... For the reference sequence Each point in the array is defined with upper and lower bounds:

[0190] (25)

[0191] In the formula, Indicates the current reference point index. Indicates the radius of the search window.

[0192] Then, construct the lower bound distance function:

[0193] (26)

[0194] If the lower bound distance of a candidate sequence is greater than the cost of the current optimal path, the sequence can be directly excluded without further calculation. This significantly improves overall matching efficiency while ensuring the accuracy of the results.

[0195] Based on the fine matching of geomagnetic sequences, a multi-source fusion mechanism of laser loop closure and visual loop closure is further introduced to improve the robustness and accuracy of loop closure detection in complex environments.

[0196] Laser loop closure primarily relies on the geometric consistency of point cloud data. Let the point cloud of the current frame be... Candidate historical frame point cloud is A similarity metric is constructed using a scan-based matching approach:

[0197] (27)

[0198] in, Indicates the current time With candidate historical moments The similarity of laser loops, This indicates that the midpoint of the current frame point cloud will be... By rotation matrix Coordinates mapped to the historical frame coordinate system It is a translation vector. For rigid body transformation, Control the sensitivity of the matching.

[0199] Visual loop closure primarily relies on the description and matching of image features. Let the current frame image be... Candidate historical images are Key points are extracted and similarity is calculated using local features:

[0200] (28)

[0201] in, Indicates the current time With candidate historical moments Visual loop similarity, This indicates the number of matched feature points. , These represent the total number of key points in the two frames, respectively.

[0202] During the fusion phase, the system comprehensively considers the results from the three sub-modules: laser, visual, and geomagnetic sequences, and constructs a unified loop closure confidence function:

[0203] (29)

[0204] in, The normalized similarity is based on multidimensional geomagnetic elastic registration. , , The weights for different modalities are adaptively adjusted based on sensor reliability and scene characteristics.

[0205] Optionally, the global optimization submodule in S4 includes: an adaptive residual optimization method.

[0206] In one feasible implementation, the process optimizes pose parameters and minimizes matching errors between point clouds by comparing the current frame point cloud with existing point clouds in the map. This invention employs a frame-to-map matching strategy, utilizing point-to-line and point-to-plane residuals for pose optimization.

[0207] Point-line residuals are a measure of how well each point in a point cloud matches a straight line in a map. Essentially, they are the perpendicular distance from the current point to the nearest straight line in the map, and the formula is as follows:

[0208] (30)

[0209] in , , It is the normal vector of the line. It is a constant term. These are the coordinates of the current point.

[0210] To dynamically adjust the matching accuracy based on the point-line residuals, adaptive weights are introduced in this section. The smaller the distance from a point to a line, the more accurate the match between the point and the line; therefore, a larger weight is assigned to these points, thereby strengthening their influence in the optimization process. The weighting factor is derived from the formula:

[0211] (31)

[0212] in: It is the distance from the point to the line. The set intermediate distance is usually a small threshold. It is an adjustment factor used to control the range of weight variation.

[0213] When matching point clouds with a plane, the point-to-plane residual refers to the distance from a point in the point cloud to a plane in the map. From the set of corner feature points in the map, we find the five closest non-collinear plane points to the current point to construct an overdetermined equation, and then use QR decomposition to solve for the plane coefficients. The point-to-plane residual equation is constructed as follows:

[0214] (32)

[0215] In the formula , , , These are the coefficients of the characteristic equation of the plane. This represents the coordinates of a planar feature point in the lidar coordinate system.

[0216] Then, by taking the partial derivative of the residual from the point to the plane, we can obtain the partial derivative of the residual with respect to the coordinates of the current point. The formula is as follows:

[0217] (33)

[0218] This partial derivative provides the gradient information of the residual with respect to the pose parameters, which can then be used for updates during the optimization process. However, due to the varying emission distances of the laser point cloud, the magnitude of the residual will differ. Therefore, this invention proposes an adaptive residual adjustment method, the specific steps of which are as follows:

[0219] First, calculate the distance from the current point to the target plane, i.e., the residual value. In practical applications, the distance threshold is usually determined by the following radar parameters:

[0220] <1> Maximum measurement range The upper limit of radar measurement range.

[0221] <2> Measurement accuracy The accuracy of radar usually refers to the error range of point cloud data.

[0222] <3> Point cloud density: The number of points in each measurement area, which affects distance calculation and the density of the point cloud.

[0223] Based on these parameters, the basic threshold It can be derived from the following formula:

[0224] (34)

[0225] in , This is an adjustment factor used to adjust the degree of influence of distance on the optimization process.

[0226] Next, based on the distance of the point cloud... Calculate an adaptive adjustment factor for points at different distances. This factor controls the impact of point cloud distance on matching accuracy.

[0227] 1) Closest point ( ):

[0228] For points at close range, radar accuracy is higher, therefore the matching accuracy of these points is more critical. The adjustment factor for close-range points is:

[0229] (35)

[0230] in, This represents the difference between the maximum measurable distance from the point to the radar and the current distance. It is a factor that controls precision.

[0231] 2) Distant point ( ):

[0232] For distant points, radar measurement accuracy is relatively low, therefore the influence of these points should be gradually reduced. The adjustment factor for distant points is:

[0233] (36)

[0234] in It is used to reflect the ratio of the distance from a point to a plane to a threshold.

[0235] Through adaptive adjustment factor It can calculate the weight of each point in the point cloud. The formula is as follows:

[0236] (37)

[0237] The weights reflect the contribution of each point to the optimization process. In each optimization iteration, the weighted residuals from the point to the plane are calculated, and the coefficients of the plane equation are updated in combination with the weights. The entire process is solved using the least squares method, gradually approaching the optimal plane until the optimization converges.

[0238] The overall system structure is shown in Figure 7. It mainly consists of a sensor data acquisition module, a front-end data processing module, an environmental scene recognition module, and a back-end state estimation and optimization module.

[0239] The sensor data acquisition module is responsible for collecting data from various sensors throughout the process, including data from magnetometers, IMUs, lidar, and depth cameras.

[0240] The front-end data processing module consists of three parts: an inertial enhancement submodule, a three-line laser feature extraction submodule, and a visual feature extraction submodule. The inertial enhancement submodule uses magnetic field information collected by a magnetometer to estimate velocity and fuses it with accelerometer data to obtain a more accurate and robust velocity estimate. The three-line laser feature extraction submodule performs a three-line intersecting structure scan on the laser point cloud data collected by radar to extract radar point cloud features. The visual feature extraction module extracts visual feature points through inter-frame feature correlation.

[0241] All processed sensor data will be transferred to the environmental scene recognition module for scene classification. This module is responsible for identifying and classifying the feature points transmitted from the front end. By analyzing the density and reflection intensity of the laser point cloud data, the module can assess the characteristics of the environment and adaptively adjust the parameters of the backend state estimation and optimization module accordingly. This changes the focus of backend optimization and adjusts the confidence level of different sensor data.

[0242] The backend state estimation and optimization module consists of a loop closure detection submodule and a global optimization submodule. Loop closure detection provides re-entry determination information to the global optimization submodule by fusing multimodal loop closure data, namely, dual loop closures from the magnetometer, radar loop closures, and visual loop closures. The global optimization submodule performs pose constraint estimation based on factors provided by multiple sources and outputs the optimized trajectory and state.

[0243] This invention proposes a localization and mapping method and device based on adaptive fusion of multi-source heterogeneous sensors. The method integrates lidar point cloud features, visual image information, inertial measurement unit data, and geomagnetic sensor output. By optimizing front-end point cloud feature extraction and back-end adaptive parameter adjustment, combined with magnetometer-enhanced IMU and multimodal loop closure detection strategy, it achieves highly robust and accurate autonomous localization and dense map construction in a multi-source heterogeneous data environment.

[0244] Specifically, in the fusion structure of the present invention, a feature point extraction method is used in the front-end lidar point cloud processing stage to perform three-line curvature calculation and bubble extraction of corner points, surface points and transition points, as well as a feature classification strategy based on the division of regions by line bundles.

[0245] The adaptive optimization mechanism proposed in this invention in the backend optimization includes automatically identifying scene categories based on environmental point cloud density and inter-frame displacement, dynamically adjusting the keyframe search radius and voxel sampling density, and dynamically adjusting residual weights based on point cloud distance in point-surface residual matching.

[0246] This invention relates to an enhanced velocity estimation method for an inertial measurement unit based on dual front and rear magnetometers. The method includes using the phase difference of the magnetic field curve and the distance to a fixed baseline to estimate the velocity, and then jointly modeling the estimate with the inertial measurement unit data and embedding it into the optimization process in the form of constraint factors.

[0247] This invention relates to a multimodal revisit determination and path correction method based on dual magnetometers, which includes identifying revisit points using geomagnetic data collected by dual magnetometers, and determining single-point loop closures by combining geomagnetic similarity, time interval, and spatial consistency constraints; further, it uses an adaptive multidimensional data matching algorithm to perform a refined comparison of geomagnetic sequences, and combines multimodal data fusion and dynamic pose adjustment mechanisms with visual and laser loop closure information.

[0248] This invention proposes an autonomous localization and mapping framework based on multi-source sensor fusion. This framework integrates data from lidar, visual sensors, inertial measurement units, and dual magnetometers, and combines adaptive feature processing, dynamic optimization, and multimodal loop closure detection mechanisms to construct a high-precision and robust autonomous localization and mapping system in complex environments without GPS signals.

[0249] In this embodiment of the invention, the magnetometer and IMU are fused to estimate the pose, which can effectively suppress drift error.

[0250] A feature extraction method combining geometric stability and projection processing is proposed. Curvature is calculated through a two-dimensional depth projection map and occlusion and parallel points are removed. At the same time, line bundle partitioning is used to improve the robustness of feature classification.

[0251] An adaptive optimization mechanism is proposed, which can automatically identify scene categories based on point cloud density and inter-frame displacement, and dynamically adjust the keyframe search radius, sampling density and residual weight to achieve optimization that balances efficiency and accuracy.

[0252] Figure 8 is a block diagram of a localization and mapping device for adaptive fusion of multi-source heterogeneous sensors according to an exemplary embodiment. This device is used in a localization and mapping method for adaptive fusion of multi-source heterogeneous sensors. Referring to Figure 8, the device includes a sensor data acquisition module 310, a front-end data processing module 320, an environmental scene recognition module 330, and a back-end state estimation and optimization module 340. Wherein:

[0253] The sensor data acquisition module 310 is used to acquire sensor data for mobile robots in indoor environments. The sensor data acquisition module includes a first magnetometer, a second magnetometer, an inertial measurement unit, a lidar, and a depth camera. The acquired sensor data includes a first geomagnetic sequence, a second geomagnetic sequence, inertial measurement unit data, point cloud data, and depth camera data.

[0254] The front-end data processing module 320 is used to input sensor data into the front-end data processing module to obtain processed sensor data. The front-end data processing module includes an inertial enhancement submodule, a three-line laser feature extraction submodule, and a visual feature extraction submodule. The inertial enhancement submodule is used to process the first geomagnetic sequence, the second geomagnetic sequence, and inertial measurement unit data to obtain enhanced velocity estimates. The three-line laser feature extraction submodule is used to process point cloud data to obtain point cloud features. The visual feature extraction submodule is used to process depth camera data to obtain visual feature points.

[0255] The environmental scene recognition module 330 is used to input the processed sensor data into the environmental scene recognition module to obtain the environmental scene type recognition result.

[0256] The backend state estimation and optimization module 340 is used to input the processed sensor data and environmental scene type recognition results into the backend state estimation and optimization module to obtain map construction and pose estimation results. The backend state estimation and optimization module includes a loop closure detection submodule and a global optimization submodule. The loop closure detection submodule is used to provide recurrence determination information to the global optimization submodule by fusing loop closure detection based on multi-dimensional sequences, laser loop closure, and visual loop closure. The global optimization submodule is used to perform map construction and pose estimation based on the recurrence determination information.

[0257] In this embodiment of the invention, the magnetometer and IMU are fused to estimate the pose, which can effectively suppress drift error.

[0258] A feature extraction method combining geometric stability and projection processing is proposed. Curvature is calculated through a two-dimensional depth projection map and occlusion and parallel points are removed. At the same time, line bundle partitioning is used to improve the robustness of feature classification.

[0259] An adaptive optimization mechanism is proposed, which can automatically identify scene categories based on point cloud density and inter-frame displacement, and dynamically adjust the keyframe search radius, sampling density and residual weight to achieve optimization that balances efficiency and accuracy.

[0260] Figure 9 is a schematic diagram of a localization and mapping device for adaptive fusion of multi-source heterogeneous sensors provided in an embodiment of the present invention. As shown in Figure 9, the localization and mapping device for adaptive fusion of multi-source heterogeneous sensors may include the localization and mapping device for adaptive fusion of multi-source heterogeneous sensors shown in Figure 8 above. Optionally, the localization and mapping device 410 for adaptive fusion of multi-source heterogeneous sensors may include a first processor 2001.

[0261] Optionally, the localization and mapping device 410 with adaptive fusion of multi-source heterogeneous sensors may also include a memory 2002 and a transceiver 2003.

[0262] The first processor 2001, memory 2002, and transceiver 2003 can be connected via a communication bus.

[0263] The following section, with reference to Figure 9, provides a detailed description of each component of the localization and mapping device 410, which utilizes adaptive fusion of multi-source heterogeneous sensors:

[0264] The first processor 2001 is the control center of the multi-source heterogeneous sensor adaptive fusion positioning and mapping device 410. It can be a single processor or a collective term for multiple processing elements. For example, the first processor 2001 can be one or more central processing units (CPUs), application-specific integrated circuits (ASICs), or one or more integrated circuits configured to implement embodiments of the present invention, such as one or more digital signal processors (DSPs), or one or more field-programmable gate arrays (FPGAs).

[0265] Optionally, the first processor 2001 can execute various functions of the multi-source heterogeneous sensor adaptive fusion positioning and mapping device 410 by running or executing software programs stored in the memory 2002 and calling data stored in the memory 2002.

[0266] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A localization and mapping method based on adaptive fusion of multi-source heterogeneous sensors, characterized in that, The method includes: S1, for a mobile robot in an indoor environment, acquiring sensor data through a sensor data acquisition module; wherein, the sensor data acquisition module includes a first magnetometer, a second magnetometer, an inertial measurement unit, a lidar, and a depth camera; the acquired sensor data includes a first geomagnetic sequence, a second geomagnetic sequence, inertial measurement unit data, point cloud data, and depth camera data; S2, inputting the sensor data into a front-end data processing module to obtain processed sensor data; wherein, the front-end data processing module includes an inertial enhancement submodule, a three-line laser feature extraction submodule, and a visual feature extraction submodule; the inertial enhancement submodule is used to process the first geomagnetic sequence, the second geomagnetic sequence, and the inertial measurement unit data to obtain an enhanced velocity estimate; the three-line laser... The feature extraction submodule processes point cloud data to obtain point cloud features; the visual feature extraction submodule processes depth camera data to obtain visual feature points; S3, the processed sensor data is input into the environment scene recognition module to obtain the environment scene type recognition result; S4, the processed sensor data and the environment scene type recognition result are input into the backend state estimation and optimization module to obtain map construction and pose estimation results; the backend state estimation and optimization module includes a loop closure detection submodule and a global optimization submodule; the loop closure detection submodule provides return determination information to the global optimization submodule by fusing multi-dimensional sequence-based loop closure detection, laser loop closure, and visual loop closure; the global optimization submodule performs map construction and pose estimation based on the return determination information.

2. The localization and mapping method based on adaptive fusion of multi-source heterogeneous sensors according to claim 1, characterized in that, The enhanced velocity estimate obtained by processing the first geomagnetic sequence, the second geomagnetic sequence, and the inertial measurement unit data in step S2 includes: S211, acquiring the principal sensing direction projections of the first and second geomagnetic sequences respectively, and performing bandpass filtering to obtain the filtered first and second geomagnetic sequences; S212, constructing Hilbert analytic signals for the filtered first and second geomagnetic sequences respectively and expanding the phase to obtain the phase of the analytic signals of the first and second geomagnetic sequences; S213, defining the average estimate of the instantaneous angular frequency and the phase difference based on the phase of the analytic signals of the first and second geomagnetic sequences; S214, based on the defined... S215. Based on the average estimate of the instantaneous angular frequency and phase difference, a sliding window mechanism is introduced to obtain the time delay; S216. Based on the baseline distance and time delay between the first and second magnetometers, the magnetometer velocity estimate and the variance of the magnetometer velocity estimate under the window meaning are obtained; S217. Gravity compensation is performed on the inertial measurement unit data, and the data is integrated along the forward direction of the mobile robot to obtain the inertial measurement unit velocity estimate. The first-order prediction of the inertial measurement unit velocity estimate is used to compensate the magnetometer velocity estimate to obtain the compensated magnetometer velocity estimate; S218. Based on the compensated magnetometer velocity estimate and the inertial measurement unit velocity estimate, the enhanced velocity estimate is obtained.

3. The localization and mapping method based on adaptive fusion of multi-source heterogeneous sensors according to claim 1, characterized in that, The step S2, processing the point cloud data to obtain point cloud features, includes: S221, performing distortion correction on the point cloud data, projecting the distortion-corrected point cloud data onto a two-dimensional raster image to obtain point cloud depth information; S222, selecting point cloud data along a LiDAR scan line, and segmenting the LiDAR scan line to obtain multiple segment intervals; S223, for the point cloud data within each segment interval, selecting a target point using an algorithm, acquiring multiple neighboring points of the target point, calculating the difference between the depth value of the target point and the depth values ​​of the neighboring points, and judging the validity of the target point by comparing the difference between the depth values ​​with a preset depth threshold to obtain valid points within each segment interval, thereby obtaining valid points along a LiDAR scan line; S224, acquiring valid points along three LiDAR scan lines, obtaining the accumulated depth values ​​of the valid points, and calculating the curvature of the valid points based on the accumulated depth values; S225, classifying the valid points based on their curvature to obtain point cloud features.

4. The localization and mapping method based on adaptive fusion of multi-source heterogeneous sensors according to claim 1, characterized in that, The step S3 involves inputting the processed sensor data into the environmental scene recognition module to obtain the environmental scene type recognition result, including: S31, determining whether the average distance of the point cloud is greater than a preset first distance threshold and whether the point cloud feature density is less than a preset feature density threshold based on the point cloud features; if yes, outputting a third feature value and storing it in the scene state manager; if no, determining whether the average distance of the point cloud is less than a preset second distance threshold; if yes, outputting a first feature value and storing it in the scene state manager; if no, outputting a second feature value and storing it in the scene state manager; S32, summing the feature values ​​in the scene state manager and identifying the current environmental scene type based on the summation result; wherein, the environmental scene type includes an open scene, a narrow corridor scene, and a normal scene.

5. The localization and mapping method based on adaptive fusion of multi-source heterogeneous sensors according to claim 1, characterized in that, S3 further includes: adjusting the density and downsampling ratio of adjacent keyframes according to the environmental scene type; wherein, adjusting the density and downsampling ratio of adjacent keyframes according to the environmental scene type includes: when the environmental scene type is an open scene, increasing the density of adjacent keyframes according to a preset adjacent keyframe density calculation formula based on a preset standard adjacent keyframe density, and decreasing the downsampling ratio according to a preset dynamic downsampling ratio calculation formula based on a preset standard downsampling ratio; when the environmental scene type is a narrow corridor scene, decreasing the density of adjacent keyframes according to a preset adjacent keyframe density calculation formula based on a preset standard adjacent keyframe density, and increasing the downsampling ratio according to a preset dynamic downsampling ratio calculation formula based on a preset standard sampling rate.

6. The localization and mapping method based on adaptive fusion of multi-source heterogeneous sensors according to claim 1, characterized in that, The step S4, by fusing loop closure detection based on multi-dimensional sequences, laser loop closure, and visual loop closure, provides recurrence determination information to the global optimization submodule. This includes: S41, performing single-point loop closure detection; wherein, the single-point loop closure detection includes: representing the geomagnetic sequence of the current point cloud and the geomagnetic sequence of the historical point cloud using three-dimensional vectors; calculating the cosine similarity based on the three-dimensional vectors of the current and historical point clouds to obtain the geomagnetic similarity between the current and historical point clouds; obtaining the time of the current and historical point clouds respectively, and calculating the time interval between the current and historical point clouds; obtaining the coordinates of the current and historical point clouds respectively, and calculating the spatial distance between the current and historical point clouds; and constructing a loop closure determination based on the geomagnetic similarity, time interval, and spatial distance. The function determines whether the current point cloud is a valid loop closure point based on the loop closure determination function; S42, after single-point loop closure detection, loop closure detection based on multi-dimensional sequences is performed; wherein, the loop closure detection based on multi-dimensional sequences includes: acquiring two geomagnetic history sequences to be matched, defining a point-by-point local distance metric function based on the geomagnetic history sequence; introducing a cumulative cost matrix based on the point-by-point local distance metric function, and finding the optimal path through dynamic recursion; S43, based on the loop closure detection based on multi-dimensional sequences, laser loop closure and visual loop closure are introduced, and the loop closure detection based on multi-dimensional sequences, laser loop closure and visual loop closure are fused to construct a loop closure confidence function, and providing return determination information to the global optimization module based on the loop closure confidence function.

7. The localization and mapping method based on adaptive fusion of multi-source heterogeneous sensors according to claim 1, characterized in that, The global optimization submodule in S4 includes an adaptive residual optimization method. This method includes: calculating the distance from the current point cloud to the target plane and determining a basic distance threshold; determining whether the current point cloud is a near or far point based on the distance from the current point cloud to the target plane and the basic distance threshold; if it is a near point, calculating the adjustment factor of the current point cloud according to a preset formula for calculating the near point adjustment factor; if it is a far point, calculating the adjustment factor of the current point cloud according to a preset formula for calculating the far point adjustment factor; obtaining the weight of the current point cloud based on its adjustment factor; and optimizing map construction and pose estimation based on the weight.

8. A localization and mapping device for adaptive fusion of multi-source heterogeneous sensors, wherein the localization and mapping device for adaptive fusion of multi-source heterogeneous sensors is used to implement the localization and mapping method for adaptive fusion of multi-source heterogeneous sensors as described in any one of claims 1-7, characterized in that, The device includes: a sensor data acquisition module for acquiring sensor data for a mobile robot in an indoor environment; wherein the sensor data acquisition module includes a first magnetometer, a second magnetometer, an inertial measurement unit, a lidar, and a depth camera; the acquired sensor data includes a first geomagnetic sequence, a second geomagnetic sequence, inertial measurement unit data, point cloud data, and depth camera data; and a front-end data processing module for inputting the sensor data to obtain processed sensor data; wherein the front-end data processing module includes an inertial enhancement submodule, a three-line laser feature extraction submodule, and a visual feature extraction submodule; the inertial enhancement submodule processes the first geomagnetic sequence, the second geomagnetic sequence, and the inertial measurement unit data to obtain an enhanced velocity estimate; the three-line laser feature extraction submodule... The feature extraction submodule processes point cloud data to obtain point cloud features; the visual feature extraction submodule processes depth camera data to obtain visual feature points; the environmental scene recognition module inputs the processed sensor data into the environmental scene recognition module to obtain environmental scene type recognition results; the backend state estimation and optimization module inputs the processed sensor data and environmental scene type recognition results into the backend state estimation and optimization module to obtain map construction and pose estimation results; the backend state estimation and optimization module includes a loop closure detection submodule and a global optimization submodule; the loop closure detection submodule provides recurrence determination information to the global optimization submodule by fusing multi-dimensional sequence-based loop closure detection, laser loop closure, and visual loop closure; the global optimization submodule performs map construction and pose estimation based on the recurrence determination information.

9. A positioning and mapping device based on adaptive fusion of multi-source heterogeneous sensors, characterized in that, The localization and mapping device for adaptive fusion of multi-source heterogeneous sensors includes: a processor; a memory, wherein the memory stores computer-readable instructions, and when the computer-readable instructions are executed by the processor, the method as described in any one of claims 1 to 7 is implemented.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium contains program code that can be invoked by a processor to execute the method as described in any one of claims 1 to 7.

Images