Multi-sensor joint optimization SLAM method and system based on adaptive weight

Abstract

The invention provides a multi-sensor joint optimization SLAM method and system based on adaptive weight. The method comprises the following steps: synchronously acquiring observation data of a laser radar, a visual sensor and an inertial measurement unit; determining the degradation state of each sensor, and correcting the uncertainty matrix of state estimation based on the degradation determination result; calculating information entropy based on the corrected eigenvalue distribution of the uncertainty matrix of each sensor, and combining determinant calculation of the matrix to obtain a quantitative reliability evaluation value; dynamically generating a fusion weight according to the quantitative reliability evaluation value so as to suppress the influence of the sensor which is judged to be degraded; and carrying out weighted fusion on the observation residual errors of the multiple sensors based on the fusion weight, and solving the optimal pose and map estimation of the system.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of intelligent vehicle navigation, and particularly relates to a multi-sensor joint optimization SLAM method and system based on adaptive weights. BACKGROUND

[0002] With the rapid development of automatic driving technology, simultaneous localization and mapping (SLAM) as an important technology for intelligent vehicles to achieve autonomous navigation usually relies on sensors such as visual cameras, laser radars or inertial measurement units (IMUs) for environment perception and pose estimation. Visual SLAM is prone to feature loss and tracking failure in low-texture, strong light change or high-speed motion scenes. Laser SLAM has high-precision ranging capability, but is easily disturbed by point cloud sparsity and mismatch in rain, fog, open areas or repetitive structure environments. IMU can provide high-frequency motion information in a short time, but is difficult to support long-term stable positioning due to integral drift problems. Therefore, a single sensor solution cannot maintain stable performance in complex and variable real road environments.

[0003] To solve the above problems of a single sensor, existing technologies begin to use a multi-sensor fusion SLAM method to effectively integrate the complementary advantages of heterogeneous sensors. However, the existing fusion method still has some problems:

[0004] (1) In terms of fusion strategy, most systems use a fixed weight allocation mechanism (such as a pre-set covariance matrix) or a static scheduling rule based on a simple threshold judgment, which cannot dynamically evaluate the confidence of the sensor according to its current working state (such as image blurriness, point cloud density, IMU acceleration mutation, etc.), resulting in still giving it too high a weight when the sensor performance degrades, introducing obvious noise or even errors, and thus polluting the state estimation results.

[0005] (2) In terms of motion distortion processing, laser radar point clouds often produce geometric distortion due to the continuous motion of the carrier within the scanning period, but existing methods ignore this distortion or only use a uniform speed model for rough compensation, without fully fusing the high-frequency motion priors provided by IMU or vision, resulting in a decrease in point cloud registration accuracy, especially in non-uniform motion scenes such as sharp turns and bumpy roads. In addition, in the loop detection link, the mainstream approach often relies on a single modality (such as using only laser point cloud descriptors or only based on visual bag-of-words models), lacking a cross-modality consistency checking mechanism. When there is local structural similarity (such as long corridors, parking lot column arrays) or light intensity changes, false detection is easily triggered, and once the false loop is included in the optimization framework, the global map will be severely distorted or even the trajectory will collapse.

[0006] (3) Existing systems generally lack the ability to actively identify and isolate sensor degradation. In typical degradation scenarios such as tunnels, underground garages, dense fog, and strong backlight, they cannot suppress the input of unreliable sensors in time, nor have they established a degradation operation strategy under degradation conditions, which leads to a sharp deterioration in positioning accuracy and may even result in long-term drift or complete failure. Summary of the Invention

[0007] To address the shortcomings and deficiencies of existing technologies, this invention provides a multi-sensor joint optimization SLAM method and system based on adaptive weights. This method solves problems such as easy degradation of single sensors, fixed weights in multi-sensor fusion, coarse compensation for motion distortion of LiDAR point clouds, and high false detection rate of loop closure detection, thereby enabling high-precision positioning and mapping of intelligent vehicles in complex environments.

[0008] This invention employs multi-sensor fusion using lidar, vision sensors, and an inertial measurement unit (IMU). First, a unified spatiotemporal reference is established. Observational data from the three types of sensors are aligned through time synchronization and spatial extrinsic parameter calibration. Then, the system is initialized using an IMU pre-integration model and multi-constraint joint solution to ensure consistency in subsequent fusion. In front-end processing, high-frequency motion priors provided by the IMU and vision sensors are combined with a quaternion spherical interpolation method to eliminate geometric distortions in the lidar point cloud caused by carrier motion, providing a high-quality data foundation for pose estimation.

[0009] To address the sensor degradation problem, this invention determines the working status of each sensor frame by frame: the reliability of the three types of sensors is evaluated by the eigenvalue distribution of the IMU pre-integration covariance matrix, the number of feature points tracked and the reprojection error of the visual sensor, and the eigenvalue of the inter-frame matching correlation matrix of the lidar. The degree of concentration of sensor information is quantified based on information entropy, and the reliability assessment value is calculated by combining the volume characteristics of the information matrix. Adaptive fusion weights are dynamically generated. Through normalization processing with temperature parameters and sliding window low-pass filtering, the weights are prevented from jumping drastically due to noise in a single frame. At the same time, a minimum lower bound is set to prevent the sensor weights from returning to zero, thereby suppressing the influence of unreliable information when the sensor degrades.

[0010] In terms of optimization strategy, this invention employs a sliding window layered edge detection and local optimization strategy. It differentiates the processing of observation data based on the information content differences between key frames and non-key frames, maintaining local optimization within limited computing resources. Figure OneConsistency; Through cross-modal loop closure detection and joint optimization, a visual bag-of-words model is used to generate loop closure candidates and guide the loop closure search of the LiDAR subgraph, while retaining an independent LiDAR loop closure detection channel. A joint optimization objective function containing dual-modal residuals is constructed, and geometric consistency verification reduces the risk of false loop closure detection. The backend adopts a factor graph tightly coupled global optimization method to integrate observation information from LiDAR measurements, IMU pre-integration, visual reprojection, and loop closure constraints. Maximum a posteriori estimation is used to eliminate accumulated errors at the front end, improving long-term positioning accuracy and ground-based positioning performance. Figure One To the point of being responsive.

[0011] Simulation experiments and real vehicle tests show that the present invention can achieve global accuracy comparable to mainstream algorithms under normal conditions, and has better local consistency. In sensor degradation scenarios, it can maintain trajectory continuity and positioning stability, effectively avoid drift and failure, and significantly improve the robustness and reliability of SLAM system in complex environments.

[0012] The specific technical solution adopted by this invention to solve its technical problem is as follows:

[0013] A multi-sensor joint optimization SLAM method based on adaptive weights includes:

[0014] Simultaneously acquire observation data from lidar, vision sensors, and inertial measurement units;

[0015] For each sensor, its degradation state is determined, and the uncertainty matrix of its state estimation is corrected based on the degradation determination result;

[0016] The information entropy is calculated based on the eigenvalue distribution of the corrected uncertainty matrix of each sensor, and the quantitative reliability assessment value is obtained by combining the determinant of the matrix.

[0017] The fusion weights are dynamically generated based on the quantified reliability assessment values ​​to suppress the impact of sensors that are deemed to be degraded.

[0018] Based on the fusion weights, the residuals of multi-sensor observations are weighted and fused to solve for the optimal pose and map estimation of the system.

[0019] Furthermore, the method for determining the degradation state of the sensor is as follows:

[0020] For inertial measurement units, the ratio of the maximum to the minimum singular value of their pre-integrated covariance matrix is ​​used as an auxiliary criterion. If the ratio exceeds a preset threshold, it is determined to be degraded.

[0021] For visual sensors, if the number of feature points successfully tracked in the current frame is lower than a preset threshold, or the average reprojection error is higher than a preset threshold, it is considered a degradation.

[0022] For lidar, if the normalized eigenvalue of the Hessian matrix constructed based on inter-frame matching is lower than a preset threshold in a certain direction, then that direction is considered degraded.

[0023] Furthermore, the quantitative reliability assessment value is calculated as follows: taking into account the volume characteristics and information reliability of the uncertainty matrix after correction of each sensor, wherein the volume characteristics are represented by the root of the state dimension of the determinant of the matrix, and the information reliability is calculated by an exponential function that is negatively correlated with the information entropy. The final quantitative reliability assessment value is the product of the volume characteristics, the information reliability and the normalization constant. The information entropy is calculated based on the distribution of the normalized eigenvalues ​​of the corrected uncertainty matrix. The normalized eigenvalues ​​satisfy the constraint that the summation is 1. The information entropy is used to characterize the concentration of matrix information.

[0024] Furthermore, the dynamic generation process of the fusion weights is as follows:

[0025] The quantified reliability assessment value is normalized with a temperature parameter to obtain an initial weight, and a lower bound is set for the initial weight to prevent any sensor weight from returning to zero.

[0026] A sliding window low-pass filter is applied to the initial weights to avoid drastic changes in weights due to single-frame noise, thus obtaining the final fused weights.

[0027] Furthermore, after synchronously acquiring the observation data, the following steps are also included:

[0028] LiDAR point cloud motion distortion correction: Combining the high-frequency motion priors provided by the inertial measurement unit and the visual sensor, the quaternion spherical interpolation method is used to correct the geometric distortion of the original point cloud to improve the geometric fidelity of the point cloud.

[0029] Multi-sensor spatiotemporal unification and initialization: The timestamps of the lidar, vision sensor and inertial measurement unit are aligned through a time synchronization mechanism. Singular value decomposition is used for spatial extrinsic parameter calibration to solve for the rotation matrix and translation vector that satisfy orthogonality and determinant constraints. The inertial measurement unit pre-integration and residual model is constructed. By combining visual feature point reprojection constraints, inertial measurement unit velocity constraints and gravity direction fixity, the initial position, velocity, rotation, inertial measurement unit zero bias and scale factor are solved to establish a unified spatiotemporal reference.

[0030] Furthermore, the process of the weighted fusion solution system for optimal pose and map estimation adopts a factor graph tightly coupled global optimization method, which integrates the residual terms of lidar measurement, inertial measurement unit pre-integration, visual reprojection and loop closure constraints, and achieves the optimal solution by minimizing the global error;

[0031] The weighted fusion process employs a sliding window marginalization and local optimization strategy: the state variables within the sliding window include the pose, velocity, inertial measurement unit (IMU) bias, scale factor, and sensor extrinsic parameters for each frame; when the window size exceeds a preset value, a layered marginalization strategy is adopted, first marginalizing all variables except camera pose, and then marginalizing camera pose; for key frames, the oldest key frame and its associated IMU data are completely marginalized as prior factors; for non-key frames, only IMU measurements are retained to maintain motion continuity, while visual observations are discarded.

[0032] Furthermore, the process of solving for the optimal pose and map estimation of the system also includes cross-modal loop closure detection and joint optimization steps:

[0033] Features are extracted from visual sensor observation data, and visual loop closure candidates are generated by combining a bag-of-words model with weight calculation.

[0034] Based on the timestamp of the visual loop closure candidate, loop closure detection of the LiDAR subgraph is performed in the temporal neighborhood, while retaining an independent LiDAR subgraph loop closure detection channel.

[0035] A joint optimization objective function incorporating visual and laser loop closure residuals is constructed, and valid loop closures are confirmed through geometric consistency verification and incorporated into the global optimization.

[0036] Furthermore, the sliding window edge-out strategy adopts a hierarchical edge-out approach based on Schur complement. First, all state variables except camera pose are edge-out, and then the camera pose is edge-out, compressing historical information into prior factors of the remaining states.

[0037] Furthermore, the normalization process uses a softmax function with a temperature parameter, and the sliding window low-pass filtering is implemented as follows: the current sensor fusion weight is the weighted sum of the fusion weight of the previous moment and the initial weight calculated in this moment, and the ratio is controlled by the forgetting factor, wherein the proportion of the weight of the previous moment is equal to the forgetting factor, and the proportion of the initial weight in this moment is the difference between 1 and the forgetting factor.

[0038] And, a multi-sensor joint optimization SLAM system based on adaptive weights, comprising:

[0039] The data acquisition module is used to simultaneously acquire observation data from lidar, vision sensors, and inertial measurement units;

[0040] The degradation determination and matrix correction module is used to determine the degradation state of each sensor and correct the uncertainty matrix of its state estimation based on the degradation determination result.

[0041] The reliability assessment and weight generation module is used to calculate the information entropy based on the eigenvalue distribution of the corrected uncertainty matrix of each sensor, obtain the quantified reliability assessment value by combining the determinant of the matrix, and dynamically generate fusion weights based on the quantified reliability assessment value to suppress the influence of the sensor that is determined to be degraded.

[0042] The weighted fusion optimization module is used to perform weighted fusion of the multi-sensor observation residuals based on the fusion weights, solve for the optimal pose and map estimation of the system, and output the results.

[0043] And a computer device including a memory, a processor, and a computer program stored in the memory, wherein the processor executes the computer program to implement the method described above.

[0044] A non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the method described above.

[0045] Compared with the prior art, the present invention and its preferred embodiments have at least the following beneficial effects:

[0046] It effectively solves the problems of easy degradation of single sensors and fixed weights in existing multi-sensor fusion. By sensing the working status of each sensor frame by frame and dynamically adjusting the fusion weights, it can actively suppress the interference of unreliable information in sensor degradation scenarios, significantly improve the positioning stability and anti-interference ability of SLAM system in complex environments, and avoid trajectory drift or failure.

[0047] By integrating high-frequency motion priors with quaternion spherical interpolation, the motion distortion of LiDAR point clouds is accurately corrected, significantly improving the point cloud registration accuracy and providing high-quality data support for subsequent pose estimation and map construction, especially performing better in non-uniform motion scenarios.

[0048] By adopting a cross-modal loop closure detection and joint optimization strategy, combining the complementary advantages of vision and LiDAR and geometric consistency verification, the risk of false detection in single-modal loop closure detection is effectively reduced, global map distortion is avoided, and global map consistency is ensured.

[0049] By leveraging a sliding window hierarchical marginalization and a tightly coupled global optimization mechanism with factor graphs, local optimization can be efficiently maintained under limited computational resources. Figure One It achieves consistency and integrates multi-source observation information to eliminate front-end accumulated errors, thus achieving a good balance between positioning accuracy and computational efficiency. This ensures both long-term positioning accuracy and meets real-time requirements.

[0050] The overall solution achieves global accuracy comparable to mainstream algorithms under normal conditions and has better local consistency through multi-module collaborative optimization. In complex scenarios such as sensor degradation, drastic changes in lighting, and repetitive structures, its robustness and reliability are significantly improved, providing more stable and accurate technical support for autonomous navigation of intelligent vehicles. Attached Figure Description

[0051] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments:

[0052] Figure 1 This is a schematic diagram of the overall technical process of an embodiment of the present invention;

[0053] Figure 2 This is a schematic diagram illustrating the principle framework of the adaptive weight optimization front-end fusion module in an embodiment of the present invention.

[0054] Figure 3 This is a schematic diagram comparing the trajectory errors of the algorithm of the present invention and the LVI-SAM algorithm in a normal scenario according to an embodiment of the present invention;

[0055] In the figure, the left figure is the trajectory error diagram of the algorithm of the present invention, and the right figure is the trajectory error diagram of the LVI-SAM algorithm;

[0056] Figure 4 This is a schematic diagram showing the fitting comparison between the fused trajectory of the algorithm of the present invention and the LVI-SAM algorithm and the real trajectory under normal scenarios in an embodiment of the present invention.

[0057] In the figure, the left figure shows the fitting result of the algorithm of the present invention, and the right figure shows the fitting result of the LVI-SAM algorithm.

[0058] Figure 5 This is a schematic diagram comparing the trajectory errors of the algorithm of this invention and the LVI-SAM algorithm in a LiDAR degradation scenario according to an embodiment of this invention;

[0059] In the figure, the left figure is the trajectory error diagram of the algorithm of the present invention, and the right figure is the trajectory error diagram of the LVI-SAM algorithm;

[0060] Figure 6 This is a schematic diagram showing the fitting comparison between the fusion trajectory of the algorithm of this invention and the LVI-SAM algorithm and the real trajectory in a LiDAR degradation scenario according to an embodiment of this invention.

[0061] In the figure, the left figure shows the fitting result of the algorithm of the present invention, and the right figure shows the fitting result of the LVI-SAM algorithm.

[0062] Figure 7 This is a schematic diagram comparing the trajectories of the algorithm of this invention and the LVI-SAM algorithm in a real-vehicle experiment according to an embodiment of this invention;

[0063] Figure 8This is a schematic diagram showing a partial comparison of the trajectories of the algorithm of the present invention and the LVI-SAM algorithm in a real vehicle experiment according to an embodiment of the present invention;

[0064] Figure 9 This is a schematic diagram comparing the positioning error curves of the algorithm of the present invention and the LVI-SAM algorithm in a real vehicle experiment according to an embodiment of the present invention;

[0065] Figure 10 This is a partial comparison diagram of the positioning error curves of the algorithm of the present invention and the LVI-SAM algorithm in a real vehicle experiment according to an embodiment of the present invention. Detailed Implementation

[0066] To make the features and advantages of the present invention more apparent and understandable, specific embodiments are described below in detail:

[0067] It should be noted that the following detailed descriptions are exemplary and intended to provide further explanation of this application. Unless otherwise specified, all technical and scientific terms used in this specification have the same meaning as commonly understood by one of ordinary skill in the art to which this application pertains.

[0068] It should be noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the exemplary embodiments according to this application. As used herein, the singular form is intended to include the plural form as well, unless the context clearly indicates otherwise. Furthermore, it should be understood that when the terms "comprising" and / or "including" are used in this specification, they indicate the presence of features, steps, operations, devices, components, and / or combinations thereof.

[0069] The purpose of this invention is to provide a multi-sensor joint optimization SLAM method based on adaptive weights to overcome the problems of single sensor degradation, large system cumulative error, and poor loop closure detection in the prior art, so as to achieve high-precision real-time positioning and mapping of intelligent vehicles in complex environments.

[0070] This invention achieves multi-sensor fusion using LiDAR, camera, and inertial measurement unit (IMU), establishes an IMU pre-integration and residual model, and extracts visual and LiDAR odometry features to unify the temporal and spatial characteristics of the three sensors. An adaptive weight optimization front-end algorithm based on degradation detection is proposed, dynamically adjusting weights to improve fusion pose accuracy when sensors degrade, while quaternion spherical interpolation is used to eliminate LiDAR motion distortion. The back-end employs a loop closure detection strategy based on vision-assisted LiDAR and a factor graph optimization method, accelerating visual factor generation through sliding window edge detection, thus improving system robustness and optimization speed. Simulation and real-vehicle experimental results demonstrate that the algorithm effectively improves the accuracy and reliability of the SLAM system in complex environments.

[0071] Please see Figures 1-10The multi-sensor joint optimization SLAM method based on adaptive weights provided in this embodiment of the invention can be implemented by referring to the following steps:

[0072] Step 1: Multi-sensor spatiotemporal unification and initialization modeling to establish a unified spatiotemporal benchmark, laying the foundation for subsequent multi-source information fusion;

[0073] Step 2: Front-end point cloud matching and motion distortion pre-correction to provide high-frequency, low-latency initial pose values ​​and eliminate geometric distortions in the lidar point cloud caused by carrier motion.

[0074] Step 3: Multi-source degradation sensing and adaptive weight fusion are used to dynamically evaluate the reliability of each sensor in the current environment and adjust its contribution weight in state estimation in real time accordingly.

[0075] Step 4: Sliding window edge detection and local optimization to maintain a locally consistent and efficient optimization window under limited computing resources;

[0076] Step 5: Cross-modal loop closure detection and joint optimization, in order to achieve high-precision loop closure detection with low false detection rate by fusing complementary information from vision and lidar;

[0077] Step 6: Global backend optimization of the factor graph to integrate all available observation information and perform global consistency optimization to eliminate small errors accumulated at the front end.

[0078] In the above-mentioned multi-sensor joint optimization SLAM method based on adaptive weights, step 1 aims to establish a unified spatiotemporal benchmark through multi-sensor spatiotemporal unification and initialization modeling, laying the foundation for subsequent multi-source information fusion.

[0079] Specifically, Step 1 first establishes a kinematic model that maps the front wheel steering angle input to the instantaneous radius of curvature and linear velocity of the rear axle center, thus providing reasonable prior constraints for IMU pre-integration. Subsequently, an IMU pre-integration and residual model is constructed. This model numerically integrates the raw measurements from the accelerometer and gyroscope within discrete time steps to obtain the relative pose increment, velocity increment, and covariance propagation, avoiding the high computational overhead of re-integration in each optimization iteration. Based on this, a high-precision time synchronization mechanism is used to obtain the timestamp alignment relationship between the visual camera, LiDAR, and IMU, and spatial extrinsic parameter calibration is performed to obtain their rotation and translation relationships T. ic ∈SE(3). The external parameter calibration process adopts the hand-eye calibration principle, with minimizing the rotation error as the objective function, and uses singular value decomposition (SVD) to solve for the numerically stable rotation matrix. The calculation formula is as follows:

[0080]

[0081] in, and Let R represent the same set of spatial points observed in two different coordinate systems, and let R be the rotation matrix to be solved.

[0082] After completing the external parameter calibration, the system enters the initialization phase. This phase first uses IMU pre-integration to achieve zero bias in the gyroscope. Initialization is performed by constructing the pre-integrated rotational residual. And linearize it to form a zero-biased increment. The overdetermined system of equations is solved using the least squares method to obtain the initial zero bias. Based on this, a system is further constructed by combining the reprojection constraints of visual feature points, the velocity constraints of IMU pre-integration, and the fixity of the gravity direction in the world coordinate system, and incorporating a scale factor. The joint least squares problem can be solved in one step to obtain the initial position. ,speed Rotation accelerometer zero bias and scale This allows the three types of heterogeneous sensors to have a consistent scale reference and initial motion state in a unified coordinate system, providing a reliable starting point for subsequent tight coupling optimization.

[0083] In the above-mentioned multi-sensor joint optimization SLAM method based on adaptive weights, step 2, front-end point cloud matching and motion distortion pre-correction, aims to provide high-frequency, low-latency initial pose values ​​and eliminate geometric distortions in the lidar point cloud caused by carrier motion.

[0084] Specifically, step 2 first uses the classic ICP method to perform inter-frame point cloud matching. Its optimization objective function is to minimize the sum of squared Euclidean distances between corresponding point pairs in the source and target point clouds, and the calculation formula is as follows:

[0085]

[0086] This optimization process reduces computation by downsampling, simplifies the problem by removing translation bias using the centroid, and finally solves for the optimal rotation matrix using SVD decomposition. With translation vector This achieves iterative convergence.

[0087] To further improve matching accuracy and robustness, the system adopts a Scan-to-Map strategy, matching the current laser scan frame with a local sub-map constructed from the most recent keyframes, rather than just matching with the previous frame, thus helping to suppress accumulated errors. Simultaneously, the PL-ICP method is introduced to first fit effective line segment features in the local sub-map, and then use the perpendicular distance from the points in the current scan to these fitted lines as the error term, calculated as follows:

[0088]

[0089] Where p is the current scan point, For any point on the network, This is the normal vector of the line. This error term is more sensitive to planar and linear structures in the point cloud, and can effectively improve the registration accuracy in structured environments. Finally, through the LM nonlinear optimization algorithm, the pose parameters are jointly optimized to obtain a high-precision inter-frame pose transformation. At the same time, in order to solve the problem of point cloud "stretching" or "folding" distortion caused by continuous carrier movement within a single scan cycle of the lidar, motion distortion pre-correction is adopted. This correction process does not assume uniform motion, but uses the high-frequency fused pose output in step 3 as the motion prior. For the original points acquired at any time α (α∈[0,1]) within the scan cycle The system calculates the precise pose at that moment using quaternion spherical interpolation (SLEP). Let the rotation quaternion corresponding to the pose at the start of the scan be... The end time is The angle between the two is Then the rotation quaternion at the intermediate time is:

[0090]

[0091] in, and This corresponds to the translation vector. Finally, the original point... Correction points transformed to the world coordinate system Its transformation formula is ,in, Depend on This correction process effectively improves the geometric fidelity of the point cloud, providing a high-quality data foundation for subsequent high-precision matching and mapping.

[0092] In the above-mentioned multi-sensor joint optimization SLAM method based on adaptive weights, step 3, the fusion of multi-source degradation perception and adaptive weights, aims to dynamically evaluate the reliability of each sensor in the current environment and adjust its contribution weight in state estimation in real time accordingly.

[0093] Specifically, step 3 first determines the degradation state of each sensor frame by frame. For the IMU, its pre-integrated covariance matrix is ​​used. condition number As a criterion for degradation, among which... and These are the maximum and minimum singular values ​​of the covariance matrix, respectively. If (like This indicates that the IMU information is highly uncertain in certain directions, so the system will introduce a scaling factor to suppress the information matrix in the corresponding directions. For visual sensors, if the number of feature points successfully tracked in the current frame is... Less than the preset threshold (e.g., 50), or average reprojection error Greater than the threshold If the value is 2 pixels, then the visual information is deemed unreliable. In this case, the visual information matrix is... ( Eigenvalue truncation is performed on the reprojected Jacobian matrix, i.e., eigenvalues ​​smaller than a certain threshold are set to zero to reduce their influence on optimization. For LiDAR, this is based on the Hessian matrix constructed during inter-frame matching. normalized eigenvalues Make a judgment if the eigenvalue in a certain direction Then, the amount of information in that direction is corrected. This is done after completing the information matrix for each sensor. After correction, the system calculates its normalized eigenvalue set. (satisfy ), and define information entropy. This quantifies the concentration of information. A lower information entropy indicates that the information is concentrated in a few directions, and therefore, its reliability is higher. The system calculates a credibility index. in, >0 represents the scaling factor.

[0094] Raw score Taking into account the volume of the information matrix (derived from the determinant) (represented by the power root) and confidence level, i.e. (n is the state dimension, (This is a normalization constant). Final weights By temperature parameter The softmax function is mapped to:

[0095]

[0096] And introduce a very small lower bound. (e.g., 0.01) To prevent any sensor weights from being completely zeroed out, the weight vector is then renormalized. The residuals of the three types of sensors in the Lie algebra space are then calculated. Jacobian matrix According to the calculated weights By performing weighted combinations, a weighted normal equation is constructed:

[0097]

[0098] The pose increment is solved using the damped LM method. Then, the system pose is updated through exponential mapping. Furthermore, to enhance the system's robustness to outliers, all residual terms are weighted using the Huber kernel function. To prevent drastic weight jumps due to single-frame noise, the system also applies a sliding window low-pass filter to the weight sequence. (a) w This is the forgetting factor, such as 0.8. This dynamic weighting mechanism ensures that the system prioritizes the most reliable sensor under any operating condition, thereby improving overall robustness.

[0099] In the above-mentioned multi-sensor joint optimization SLAM method based on adaptive weights, step 4, sliding window marginalization and local optimization, aims to maintain a locally consistent and efficient optimization window under limited computing resources.

[0100] In sliding window marginalization, for key frames, the oldest frame and its IMU data are marginalized as prior factors, while for non-key frames, visual observations are discarded and only IMU measurements are retained to maintain motion continuity.

[0101] Specifically, step 4 defines the complete state variables within the sliding window. This includes all poses, velocities, IMU bias, scale factors, and extrinsic parameters from frame 0 to frame N. The system constructs an optimization objective function containing multiple residual terms: ,in, For robust kernel functions, It includes residuals from priors, IMU pre-integration measurement residuals, and visual reprojection residuals. This is its corresponding weight matrix. When a new frame is added and causes the window to exceed the preset size, the system needs to marginalize the oldest state to maintain the window size.

[0102] This invention employs a hierarchical marginalization strategy based on Shure complement to efficiently process high-dimensional state variables. The system first performs marginalization on all variables except camera pose, transforming the information matrix into a dense submatrix related to camera pose. Subsequently, marginalization is performed on the camera pose itself, ultimately compressing the entire historical information into a prior factor about the remaining states. For keyframes, the system completely marginalizes the oldest keyframe and all associated IMU data into a prior factor; while for non-keyframes, due to their lower information content, the system only retains their IMU measurements to maintain the continuity of the motion model, without incorporating their complete states into long-term optimization. This ensures local stability while maintaining high-level optimization. Figure One While ensuring consistency, the computational complexity was effectively controlled, achieving a good balance between real-time performance and accuracy.

[0103] In the above-mentioned multi-sensor joint optimization SLAM method based on adaptive weights, step 5, cross-modal loop closure detection and joint optimization, aims to achieve high-precision loop closure detection with a low false detection rate by fusing complementary information from vision and lidar.

[0104] Specifically, step 5 first extracts ORB feature points from the visual images and then uses a large number of unordered images to train an offline visual bag-of-words model. During online execution, the system calculates the weight of each visual word using the TF-IDF algorithm, with the following formula: ,in, It is a word The number of times it appears in the current image. It is the total number of words in the current image. It is the total number of images in the training set. It contains words The system calculates the similarity score between the current frame and historical keyframes based on the number of images. This generates a candidate list for visual loop closures.

[0105] To overcome the vulnerability of pure vision methods to changes in illumination, this invention uses visual loop closure detection results as prior guidance for the loop closure search of the LiDAR. The system precisely aligns the timestamps of high-resolution visual loop closure candidate frames with the LiDAR frames, and uses this as the center to perform loop closure detection of the LiDAR submap within its temporal neighborhood. Simultaneously, the system maintains an independent LiDAR submap loop closure detection channel to handle scenarios where vision is completely ineffective (such as dense fog or darkness). After obtaining loop closure candidates from both modalities, the system constructs a joint loop closure optimization objective function:

[0106]

[0107] The first term is the visual loop closure residual, and the second term is the laser loop closure residual. and These represent the number of matched feature points or feature pairs, respectively, and are used as weighting coefficients, r. i ,r j For the corresponding cyclic residual, Ω i Ω j Let its covariance matrix be denoted as . Through this weighted merging method, the system forms a unified nonlinear optimization problem. Only when the constraints of the two modes are geometrically consistent will the loop be finally identified and incorporated into the global optimization, thereby significantly reducing the risk of false detections caused by a single mode.

[0108] In the above-mentioned multi-sensor joint optimization SLAM method based on adaptive weights, step 6, global back-end optimization of the factor graph, aims to integrate all available observation information and perform global consistency optimization to eliminate small errors accumulated at the front end.

[0109] Specifically, step 6 constructs a factor graph model with states (pose, velocity, zero bias, etc.) at each key time stamp as nodes and various observations (IMU pre-integration, laser odometry, visual reprojection, loop closure constraints) as edges. The goal of global optimization is to minimize the weighted sum of squares of all factors, and its global error function is:

[0110]

[0111] Where L is the set of loop closure pairs, and each factor corresponds to laser odometry, IMU pre-integration, visual reprojection, and loop closure constraint, respectively.

[0112] According to Bayesian inference principles, the posterior probability of a system's state is proportional to the product of all its factors, i.e. ,in, As a priori factor, The observation constraint factor is used. The system employs an efficient iterative nonlinear minimization algorithm (such as the LM algorithm) to maximize this posterior probability, thereby solving for the globally optimal pose sequence.

[0113] The following is a more specific application example to further demonstrate and introduce the above solutions of the present invention:

[0114] based on Figure 1 The overall technical process shown Figure 2 Based on the principle of adaptive weight optimization in the front-end fusion module, this invention runs the KITTI dataset on the ROS platform, employing both the industry-leading LVI-SAM algorithm and the algorithm of this invention, testing the localization performance of the two algorithms under the same ROSbag data source. The core evaluation metrics for the tests are the trajectory error of the two algorithms in normal scenes and when LiDAR degradation occurs, and the fitting between the fused trajectory and the real trajectory. Specifically, the trajectory error comparison in normal scenes is... Figure 3 As shown, the fitting effect between the fused trajectory and the real trajectory is as follows: Figure 4 As shown; trajectory error in lidar degradation scenarios, for example... Figure 5 As shown, the fitting effect between the fused trajectory and the real trajectory is as follows: Figure 6 As shown.

[0115] The results show that, in normal sensor environments (without degradation), the overall accuracy of the two algorithms is similar, but the present invention exhibits better trajectory smoothness, achieving global accuracy comparable to LVI-SAM, and possesses higher local consistency. Figure 3 Error distribution and Figure 4This advantage can be clearly observed in the trajectory fitting curve; in sensor degradation scenarios (simulating LiDAR degradation), LVI-SAM exhibits significant drift and transient failure during degradation, while this invention, due to its adaptive weight adjustment mechanism, can reduce the LiDAR weight and increase the visual / IMU weight during degradation, resulting in no significant trajectory divergence and maintaining overall trajectory continuity. Figure 5 Error comparison and Figure 6 The trajectory fitting results can be used to visually verify that the robustness of this invention under sensor degradation conditions is significantly better than that of LVI-SAM.

[0116] To further verify the feasibility of the algorithm in real-world scenarios, an Ackerman steering intelligent vehicle was tested in a real test area with irregular obstacles arranged in a 10m×15m long corridor. Laser, visual, and IMU data were collected simultaneously, and real-time positioning was performed using LVI-SAM and the algorithm of this invention, respectively. Due to the similar characteristics of the long corridor, sensors are prone to degradation in this environment. The overall trajectory comparison and local detail comparison of the real vehicle experiment are shown below. Figure 7 , Figure 8 As shown, the positioning error curve and a magnified local comparison are shown. Figure 9 , Figure 10 As shown.

[0117] Experimental results show that the LVI-SAM trajectory drifts and briefly fails; while the trajectory of the present invention is stable and continuous, with no significant noise accumulation, normal positioning output, and pose estimation fluctuations are significantly lower than the control algorithm. Figures 7-10 The actual test data provides ample evidence for this. Real-vehicle experiments further validated the usability and robustness of the method in complex environments.

[0118] In summary, compared with the prior art, the advantages of the present invention include:

[0119] (1) The dynamic weighting mechanism of this invention is superior to fixed weight fusion. Traditional SLAM often uses fixed weights or simple switching strategies, which are prone to noise pollution when the sensors degrade. The adaptive weighting mechanism proposed in this invention is based on the theories of information observability and information entropy, quantifies the reliability of each sensor in real time, and dynamically adjusts the fusion weights, which can effectively improve the robustness and stability of the system in extreme scenarios such as textureless, tunnel, and open areas.

[0120] (2) The high-precision motion distortion correction of the present invention is superior to the uniform velocity model assumption. Existing methods often assume that the carrier moves at a uniform velocity during lidar scanning, which leads to severe distortion of point clouds under sharp turns or bumpy road conditions. The present invention uses high-frequency fusion pose combined with quaternion spherical interpolation to perform spatiotemporal alignment of point clouds at each moment, effectively eliminating distortions such as stretching and folding, and improving the geometric fidelity of point cloud registration.

[0121] (3) The cross-modal loop closure detection of the present invention is superior to the single-modal scheme. Traditional laser SLAM is prone to false matching in structurally repetitive environments, while pure visual loop closure is greatly affected by changes in illumination. The present invention integrates visual semantic prior with laser geometric consistency, guides laser loop closure search through visual bag-of-words model, and implements weighted joint optimization, which greatly reduces the false detection rate and improves the accuracy and recall of loop closure.

[0122] (4) The marginalization strategy of this invention balances efficiency and consistency. Compared with the high computational overhead brought about by full graph optimization, this invention adopts a hierarchical Shur complement marginalization strategy, which efficiently eliminates old states within the sliding window, retains key historical constraints, and controls computational complexity, thus achieving a balance between real-time performance and local consistency.

[0123] (5) Existing systems often employ loose coupling or two-stage optimization, making it difficult to fully utilize complementary information from multiple sensors. This invention unifies the modeling of IMU, vision, laser, and loop closure into a factor graph, achieving tightly coupled global optimization through maximum a posteriori estimation. This effectively suppresses front-end cumulative errors and improves performance. Figure One Consistency and long-term positioning accuracy.

[0124] Based on the same inventive concept, this invention also provides a computer device, comprising: one or more processors, and a memory for storing one or more computer programs; the programs include program instructions, and the processor executes the program instructions stored in the memory. The processor may be a Central Processing Unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. It is the computing and control core of the terminal, used to implement one or more instructions, specifically for loading and executing one or more instructions stored in a computer storage medium to implement the above-described method.

[0125] It should be further explained that, based on the same inventive concept, the present invention also provides a computer storage medium storing a computer program, which, when executed by a processor, performs the above-described method. This storage medium can be any combination of one or more computer-readable media. A computer-readable medium can be a computer-readable signal medium or a computer-readable storage medium. A computer-readable storage medium can be, for example, but not limited to, an electrical, magnetic, optical, infrared, or semiconductor system, apparatus, or device, or any combination thereof. More specific examples of computer-readable storage media (a non-exhaustive list) include: an electrical connection having one or more wires, a portable computer disk, a hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage device, magnetic storage device, or any suitable combination thereof. In the present invention, a computer-readable storage medium can be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution system, apparatus, or device.

[0126] It should be noted that, unless otherwise defined, the technical or scientific terms used in this invention should have the ordinary meaning understood by one of ordinary skill in the art to which this invention pertains. The terms "first," "second," and similar terms used in this invention do not indicate any order, quantity, or importance, but are merely used to distinguish different components. Terms such as "comprising" or "including" mean that the element or object preceding the word encompasses the elements or objects listed following the word and their equivalents, without excluding other elements or objects. Terms such as "connected" or "linked" are not limited to physical or mechanical connections, but can include electrical connections, whether direct or indirect. Terms such as "upper," "lower," "left," and "right" are used only to indicate relative positional relationships; when the absolute position of the described object changes, the relative positional relationship may also change accordingly.

[0127] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention in any other way. Any person skilled in the art may make changes or modifications to the above-disclosed technical content to create equivalent embodiments. However, any simple modifications, equivalent changes, and modifications made to the above embodiments based on the technical essence of the present invention without departing from the scope of the present invention shall still fall within the protection scope of the present invention.

[0128] This invention is not limited to the preferred embodiment described above. Anyone inspired by this invention can derive other forms of multi-sensor joint optimization SLAM method and system based on adaptive weights. All equivalent changes and modifications made within the scope of the claims of this invention shall fall within the scope of this invention.

Claims

1. A multi-sensor joint optimization SLAM method based on adaptive weights, characterized in that, include: Simultaneously acquire observation data from lidar, vision sensors, and inertial measurement units; For each sensor, its degradation state is determined, and the uncertainty matrix of its state estimation is corrected based on the degradation determination result; The information entropy is calculated based on the eigenvalue distribution of the corrected uncertainty matrix of each sensor, and the quantitative reliability assessment value is obtained by combining the determinant of the matrix. The fusion weights are dynamically generated based on the quantified reliability assessment values ​​to suppress the impact of sensors that are deemed to be degraded. Based on the fusion weights, the residuals of multi-sensor observations are weighted and fused to solve for the optimal pose and map estimation of the system.

2. The multi-sensor joint optimization SLAM method based on adaptive weights according to claim 1, characterized in that: The method for determining the degradation state of the sensor is as follows: For inertial measurement units, the ratio of the maximum to the minimum singular value of their pre-integrated covariance matrix is ​​used as an auxiliary criterion. If the ratio exceeds a preset threshold, it is determined to be degraded. For visual sensors, if the number of feature points successfully tracked in the current frame is lower than a preset threshold, or the average reprojection error is higher than a preset threshold, it is considered a degradation. For lidar, if the normalized eigenvalue of the Hessian matrix constructed based on inter-frame matching is lower than a preset threshold in a certain direction, then that direction is considered degraded.

3. The multi-sensor joint optimization SLAM method based on adaptive weights according to claim 1, characterized in that: The quantitative reliability assessment value is calculated as follows: taking into account the volume characteristics and information reliability of the uncertainty matrix after correction of each sensor, the volume characteristics are represented by the root of the state dimension of the determinant of the matrix, and the information reliability is calculated by an exponential function that is negatively correlated with the information entropy. The final quantitative reliability assessment value is the product of the volume characteristics, the information reliability and the normalization constant. The information entropy is calculated based on the distribution of the normalized eigenvalues ​​of the corrected uncertainty matrix. The normalized eigenvalues ​​satisfy the constraint that the summation is 1. The information entropy is used to characterize the concentration of matrix information.

4. The multi-sensor joint optimization SLAM method based on adaptive weights according to claim 1, characterized in that: The dynamic generation process of the fusion weights is as follows: The quantified reliability assessment value is normalized with a temperature parameter to obtain an initial weight, and a lower bound is set for the initial weight to prevent any sensor weight from returning to zero. A sliding window low-pass filter is applied to the initial weights to avoid drastic changes in weights due to single-frame noise, thus obtaining the final fused weights.

5. The multi-sensor joint optimization SLAM method based on adaptive weights according to claim 1, characterized in that: After synchronously acquiring the observation data, the following steps are also included: LiDAR point cloud motion distortion correction: Combining the high-frequency motion priors provided by the inertial measurement unit and the visual sensor, the quaternion spherical interpolation method is used to correct the geometric distortion of the original point cloud to improve the geometric fidelity of the point cloud. Multi-sensor spatiotemporal unification and initialization: The timestamps of the lidar, vision sensor and inertial measurement unit are aligned through a time synchronization mechanism. Singular value decomposition is used for spatial extrinsic parameter calibration to solve for the rotation matrix and translation vector that satisfy orthogonality and determinant constraints. The inertial measurement unit pre-integration and residual model is constructed. By combining visual feature point reprojection constraints, inertial measurement unit velocity constraints and gravity direction fixity, the initial position, velocity, rotation, inertial measurement unit zero bias and scale factor are solved to establish a unified spatiotemporal reference.

6. The multi-sensor joint optimization SLAM method based on adaptive weights according to claim 1, characterized in that: The process of the weighted fusion solution system for optimal pose and map estimation adopts a factor graph tightly coupled global optimization method, which integrates the residual terms of lidar measurement, inertial measurement unit pre-integration, visual reprojection and loop closure constraints, and achieves the optimal solution by minimizing the global error; The weighted fusion process employs a sliding window marginalization and local optimization strategy: the state variables within the sliding window include the pose, velocity, inertial measurement unit (IMU) bias, scale factor, and sensor extrinsic parameters for each frame; when the window size exceeds a preset value, a layered marginalization strategy is adopted, first marginalizing all variables except camera pose, and then marginalizing camera pose; for key frames, the oldest key frame and its associated IMU data are completely marginalized as prior factors; for non-key frames, only IMU measurements are retained to maintain motion continuity, while visual observations are discarded.

7. The multi-sensor joint optimization SLAM method based on adaptive weights according to claim 1, characterized in that: The process of finding the optimal pose and map estimation for the system also includes cross-modal loop closure detection and joint optimization steps: Features are extracted from visual sensor observation data, and visual loop closure candidates are generated by combining a bag-of-words model with weight calculation. Based on the timestamp of the visual loop closure candidate, loop closure detection of the LiDAR subgraph is performed in the temporal neighborhood, while retaining an independent LiDAR subgraph loop closure detection channel. A joint optimization objective function incorporating visual and laser loop closure residuals is constructed, and valid loop closures are confirmed through geometric consistency verification and incorporated into the global optimization.

8. The multi-sensor joint optimization SLAM method based on adaptive weights according to claim 1, characterized in that: The sliding window edge-out strategy adopts a hierarchical edge-out approach based on Schur complement. First, all state variables except camera pose are edge-out, and then the camera pose is edge-out, compressing historical information into prior factors of the remaining states.

9. The multi-sensor joint optimization SLAM method based on adaptive weights according to claim 1, characterized in that: The normalization process uses a softmax function with a temperature parameter. The sliding window low-pass filtering is implemented as follows: the current sensor fusion weight is the weighted sum of the fusion weight of the previous moment and the initial weight calculated in this moment, and the ratio is controlled by the forgetting factor. The proportion of the weight of the previous moment is equal to the forgetting factor, and the proportion of the initial weight is the difference between 1 and the forgetting factor.

10. A multi-sensor joint optimization SLAM system based on adaptive weights, characterized in that, include: The data acquisition module is used to simultaneously acquire observation data from lidar, vision sensors, and inertial measurement units; The degradation determination and matrix correction module is used to determine the degradation state of each sensor and correct the uncertainty matrix of its state estimation based on the degradation determination result. The reliability assessment and weight generation module is used to calculate the information entropy based on the eigenvalue distribution of the corrected uncertainty matrix of each sensor, obtain the quantified reliability assessment value by combining the determinant of the matrix, and dynamically generate fusion weights based on the quantified reliability assessment value to suppress the influence of the sensor that is determined to be degraded. The weighted fusion optimization module is used to perform weighted fusion of the multi-sensor observation residuals based on the fusion weights, solve for the optimal pose and map estimation of the system, and output the results.

Images