Multi-sensor fusion SLAM real-time positioning method based on observability adaptive adjustment

Abstract

The invention discloses a multi-sensor fusion SLAM (Simultaneous Localization and Mapping) real-time positioning method based on observability adaptive adjustment, which is used in degraded scenes such as tunnels, corridors, open planes and the like to improve the positioning stability and precision. Observability analysis is carried out on an environmental geometric structure, the degeneration subspace direction is explicitly identified and described, and directivity error constraint caused by unified weight descending is avoided; introducing the degradation information into the construction of a laser observation information matrix, and carrying out anisotropic adaptive adjustment on the information matrix; the multi-sensor is combined with sliding window optimization, anisotropic observation constraint adjustment is realized, and continuous self-adaption to environment structure change is realized, so that false constraints in a degradation direction are effectively inhibited, and the robustness of back-end optimization to a degradation environment is improved; a front-end closed-loop feedback correction mechanism effectively inhibits front-end accumulated drift.

Description

A real-time localization method for multi-sensor fusion SLAM based on observability adaptive adjustment. Technical Field

[0001] This invention relates to the field of simultaneous localization and mapping (SLAM) technology based on lidar, and in particular to a multi-sensor fusion SLAM real-time localization method. Background Technology

[0002] With the development of mobile robots, autonomous vehicles, and intelligent equipment, lidar-based simultaneous localization and mapping (SLAM) technology has been widely used for high-precision real-time positioning in complex environments. Existing lidar SLAM systems typically fuse information from multiple sources, such as lidar, inertial measurement units (IMUs), and wheel speed sensors, to improve the system's robustness and positioning accuracy.

[0003] However, in environments such as tunnels, long straight corridors, large-scale open planes, and sparse outdoor scenes, due to the geometric characteristics of the environmental structure, laser point clouds lack effective constraints on some translational or rotational degrees of freedom, which can easily lead to geometric constraint degradation, resulting in rapid accumulation of positioning errors and even causing system divergence.

[0004] Existing technologies primarily address degradation issues by including:

[0005] 1) A unified weighting method based on the overall residual, condition number, or trace value of the Hessian matrix;

[0006] 2) Detect degradation scenarios based on empirical rules and switch positioning modes accordingly;

[0007] 3) The laser observations are adjusted by overall weighting based on the statistical residuals.

[0008] The methods described above typically assume that laser observations have consistent reliability across all spatial degrees of freedom, or only scale the overall observations by a scalar scale. They fail to clearly distinguish the specific spatial direction in which degradation occurs. When multiple degrees of freedom degrade simultaneously, they are prone to introducing directional error constraints in the back-end optimization, leading to systematic bias in the fusion estimation results.

[0009] Therefore, a multi-sensor fusion SLAM real-time localization method is needed that can explicitly analyze the geometric observability of the environment and adaptively adjust the laser observation constraint intensity in different spatial directions to improve the stability and accuracy of the system in structurally degraded environments. Summary of the Invention

[0010] Purpose of the invention: To address the above-mentioned problems, the purpose of this invention is to provide a multi-sensor fusion SLAM real-time positioning method that can explicitly analyze the geometric observability of the environment and adaptively adjust the laser observation constraint intensity in different spatial directions, so as to improve the stability and accuracy of the system in structurally degraded environments.

[0011] Technical solution: A real-time localization method for multi-sensor fusion SLAM based on observability adaptive adjustment, comprising the following steps:

[0012] S1: Multi-source sensor data acquisition and time synchronization;

[0013] S2: Front-end laser odometry estimation based on laser and inertial information;

[0014] S3: Point cloud normal vector estimation and effective matching set construction;

[0015] S4: Environmental geometric observability analysis and degradation subspace determination;

[0016] S5: Anisotropic adaptive adjustment of laser observation information matrix based on degradation information;

[0017] S6: Multi-source factor joint error optimization and front-end / back-end feedback correction based on sliding window.

[0018] Further, step S1 specifically involves: acquiring lidar point cloud, inertial measurement unit data, and wheel speed sensor data; performing time synchronization and preprocessing on the multi-source data; and sending the synchronized data to the front-end positioning module.

[0019] Further, step S2 specifically involves: based on the matching relationship between the current laser point cloud and the local map or the point cloud of the previous frame, constructing a point-to-surface residual model, solving for the initial pose estimation of the current frame relative to the previous frame, and obtaining the output pose of the front-end laser odometry, denoted as... , It represents a three-dimensional Euclidean transformation group, which includes translation and rotation information.

[0020] Preferably, the laser odometry implementation methods include ICP matching, NDT matching, and FASTLIO2.

[0021] Furthermore, step S3 specifically involves: performing local plane fitting on the point cloud participating in the matching, and calculating the unit normal vector for each valid matching point. Construct a set of normals for environmental objectivity analysis:

[0022] Formula (1),

[0023] in, For the set of real numbers, It represents a three-dimensional real vector space.

[0024] Furthermore, step S4 specifically involves:

[0025] S4.1 Construct an observability approximation matrix to quantify the constraint strength:

[0026] Construct a translation observability matrix to reflect the constraint strength in the translation direction:

[0027] Formula (2),

[0028] Construct a rotation observability matrix to reflect the constraint strength in the rotation direction:

[0029] Formula (3),

[0030] in, To effectively match the number of point clouds, For a specific point cloud;

[0031] S4.2, Determining the translational degenerate subspace:

[0032] Eigendecomposition of the translation observability matrix:

[0033] Formula (4),

[0034] Among them, features , Indicates the direction of translation The constraint strength, The eigenvector matrix;

[0035] Based on the eigenvalue ratios, construct a set of translational degenerate subspace indices:

[0036] Formula (5),

[0037] in, For a preset threshold, the set The corresponding eigenvectors span the translational degenerate subspace of the current environment;

[0038] S4.3, Determining Rotational Degenerate Subspaces:

[0039] Eigendecomposition of the rotation observability matrix:

[0040] Formula (6),

[0041] Among them, features , Indicates the direction of rotation The constraint strength, The eigenvector matrix;

[0042] Based on the eigenvalue ratios, construct a set of rotationally degenerate subspace indices:

[0043] Formula (7),

[0044] in, For a preset threshold, the set The corresponding eigenvectors span the rotational degenerate subspace of the current environment;

[0045] S4.4 Forming a comprehensive and observable degradation index:

[0046] Formula (8),

[0047] in, , is used to measure the weakest level of the overall geometric constraints of the system.

[0048] Furthermore, step S5 specifically involves:

[0049] S5.1 Construct a degradation intensity mapping function and adjust the weights according to the degree of degradation:

[0050] Formula (9),

[0051] in, The minimum weight threshold;

[0052] S5.2, Anisotropy Adjustment of Translation / Rotation Information Matrix:

[0053] Constructing the translation information matrix:

[0054] Formula (10),

[0055] Constructing the rotation information matrix:

[0056] Formula (11),

[0057] The laser observation factor information matrix has been updated as follows:

[0058] Formula (12),

[0059] in, The initial laser observation information matrix is ​​determined by the inherent accuracy of the sensor.

[0060] Preferred, .

[0061] Furthermore, step S6 specifically involves:

[0062] S6.1, at the current moment Within the sliding window, the window size Using all pose states within the window To optimize the target object, a joint least squares optimization objective function based on information matrix weighting is constructed:

[0063] Formula (13),

[0064] in:

[0065] Laser odometry bilateral residuals ;

[0066] Wheel speed odometer bilateral residuals ;

[0067] : Map matching factor, representing the matching residual from a point in frame j to the map;

[0068] , , : These represent the information matrices of the corresponding factors;

[0069] S6.2 Feedback Correction:

[0070] Based on the backend optimization results, calculate the frontend pose correction amount:

[0071] Formula (14);

[0072] Correct the initial pose for the next frame:

[0073] Formula (15),

[0074] Among them, the smoothing coefficient .

[0075] Beneficial effects: This invention has the following advantages: By performing observability analysis on the environmental geometry, the direction of the degenerate subspace is explicitly identified and characterized, avoiding directional misconstraints caused by uniform weighting; this degradation information is introduced into the construction of the laser observation information matrix, and the anisotropy of the information matrix is ​​adaptively adjusted; multi-sensor joint sliding window optimization realizes the adjustment of anisotropic observation constraints, continuously adapts to changes in environmental structure, thereby effectively suppressing spurious constraints in the degradation direction and improving the robustness of back-end optimization to the degradation environment; the front-end and back-end closed-loop feedback correction mechanism effectively suppresses the cumulative drift of the front end; and significantly improves positioning stability and accuracy in degradation scenarios such as tunnels, corridors, and open planes. Detailed Implementation

[0077] The present invention will be further illustrated below with reference to specific embodiments.

[0078] A real-time localization method for multi-sensor fusion SLAM based on observability adaptive adjustment includes the following steps S1 to S6.

[0079] S1: Multi-source sensor data acquisition and time synchronization.

[0080] Specifically, the process involves acquiring lidar point cloud data, inertial measurement unit data, and wheel speed sensor data; performing time synchronization and preprocessing on the multi-source data; and then sending the synchronized data to the front-end positioning module.

[0081] S2: Front-end laser odometry estimation based on laser and inertial information.

[0082] Specifically, based on the matching relationship between the current laser point cloud and the local map or the point cloud of the previous frame, a point-to-surface residual model is constructed. The initial pose estimation of the current frame relative to the previous frame is then solved to obtain the output pose of the front-end laser odometry, denoted as... , It represents a three-dimensional Euclidean transformation group, which includes translation and rotation information.

[0083] Laser odometry can be implemented through ICP matching, NDT matching, and FASTLIO2, but is not limited to these methods.

[0084] S3: Point cloud normal vector estimation and effective matching set construction.

[0085] Specifically, this involves performing local plane fitting on the point cloud participating in the matching, and calculating a unit normal vector for each valid matching point. Construct a set of normals for environmental objectivity analysis:

[0086] Formula (1),

[0087] in, For the set of real numbers, It represents a three-dimensional real vector space.

[0088] S4: Environmental geometric observability analysis and degenerate subspace determination. Specifically, this involves the following steps S4.1 to S4.4.

[0089] S4.1 Construct an observability approximation matrix to quantify the constraint strength:

[0090] Construct a translation observability matrix to reflect the constraint strength in the translation direction:

[0091] Formula (2),

[0092] Construct a rotation observability matrix to reflect the constraint strength in the rotation direction:

[0093] Formula (3),

[0094] in, To effectively match the number of point clouds, For a specific point cloud.

[0095] S4.2, Determining the translational degenerate subspace:

[0096] Eigendecomposition of the translation observability matrix:

[0097] Formula (4),

[0098] Among them, features , Indicates the direction of translation The constraint strength, The eigenvector matrix;

[0099] Based on the eigenvalue ratios, construct a set of translational degenerate subspace indices:

[0100] Formula (5),

[0101] in, For a preset threshold, the set The corresponding eigenvectors span the translational degenerate subspace of the current environment.

[0102] S4.3, Determining Rotational Degenerate Subspaces:

[0103] Eigendecomposition of the rotation observability matrix:

[0104] Formula (6),

[0105] Among them, features , Indicates the direction of rotation The constraint strength, The eigenvector matrix;

[0106] Based on the eigenvalue ratios, construct a set of rotationally degenerate subspace indices:

[0107] Formula (7),

[0108] in, For a preset threshold, the set The corresponding eigenvectors span the rotational degenerate subspace of the current environment.

[0109] S4.4 Forming a comprehensive and observable degradation index:

[0110] Formula (8),

[0111] in, , is used to measure the weakest level of the overall geometric constraints of the system.

[0112] S5: Adaptive anisotropy adjustment of the laser observation information matrix based on degradation information. Specifically, this involves the following steps S5.1 to S5.2.

[0113] S5.1 Construct a degradation intensity mapping function and adjust the weights according to the degree of degradation:

[0114] Formula (9),

[0115] in, The minimum weight threshold is preferred. Avoid completely discarding constraints.

[0116] S5.2, Anisotropy Adjustment of Translation / Rotation Information Matrix:

[0117] Constructing the translation information matrix:

[0118] Formula (10),

[0119] Constructing the rotation information matrix:

[0120] Formula (11),

[0121] The laser observation factor information matrix has been updated as follows:

[0122] Formula (12),

[0123] in, The initial laser observation information matrix is ​​determined by the inherent accuracy of the sensor.

[0124] S6: Multi-source factor joint error optimization and front-end / back-end feedback correction based on sliding window. Specifically, it consists of the following steps S6.1 to S6.2.

[0125] S6.1, at the current moment Within the sliding window, the window size Using all pose states within the window To optimize the target object, a joint least squares optimization objective function based on information matrix weighting is constructed:

[0126] Formula (13),

[0127] in:

[0128] Laser odometry bilateral residuals ;

[0129] Wheel speed odometer bilateral residuals ;

[0130] : Map matching factor, representing the matching residual from a point in frame j to the map;

[0131] , , : These represent the information matrices of the corresponding factors.

[0132] Each information matrix is ​​not a fixed constant, but is adaptively adjusted based on the comprehensive observability degradation index obtained in step S4.4 and the information matrix constructed in step S5.2, in order to suppress the interference of unreliable constraints in the degradation direction on the joint optimization results.

[0133] S6.2 Feedback Correction:

[0134] Based on the backend optimization results, calculate the frontend pose correction amount:

[0135] Formula (14);

[0136] Correct the initial pose for the next frame:

[0137] Formula (15),

[0138] Among them, the smoothing coefficient Avoid over-correction.

[0139] Example 1

[0140] This embodiment calculates the degradation direction for a long, straight indoor corridor application scenario (a scenario prone to translational / rotational constraint degradation). The long corridor extends along the X-axis, with almost no effective geometric constraints in the X direction, while the Y and Z directions are strongly constrained by the side walls and the top surface. Therefore, the constructed translational observability matrix has very small eigenvalues ​​in the X direction, below the preset degradation threshold. The rotational observability matrix is ​​constructed according to the same rules. Observability analysis determines that translation along the X-axis, rotation around the X-axis (roll), and rotation around the Z-axis (heading) are all unobservable dimensions. By performing eigenvalue decomposition on the translational and rotational observability matrices respectively, the minimum / maximum eigenvalue ratios of the translational and rotational observability matrices are calculated, thus forming a comprehensive observability degradation index. Substituting the comprehensive observability index into the degradation intensity mapping function yields the translational anisotropic scaling matrix and the rotational anisotropic scaling matrix. Since X-axis translation and rotation around the X-axis exhibit degradation, the weights of the information matrix in the X-axis translation and rotation around the X-axis dimensions are reduced, and the weights of the degradation dimension around the Z-axis are reduced accordingly, while the remaining non-degradation directions retain their original information content. Finally, the laser observation information matrix is ​​replaced with an adaptively adjusted information matrix for the long straight corridor scene, which significantly reduces the residual weights in the degradation directions of X-axis translation, rotation around the X-axis, and rotation around the Z-axis, thus suppressing longitudinal drift in the indoor long straight corridor scene.

[0141] The advantages of the method of the present invention are:

[0142] 1. By performing observability analysis on the environmental geometry, the direction of the degenerate subspace is explicitly identified and characterized, avoiding directional misconstraints caused by uniform weight reduction;

[0143] 2. Incorporate this degradation information into the construction of the laser observation information matrix, and adaptively adjust the anisotropy of the information matrix;

[0144] 3. Multi-sensor joint sliding window optimization enables anisotropic observation constraint adjustment and continuous adaptation to changes in environmental structure, thereby effectively suppressing spurious constraints in the degradation direction and improving the robustness of back-end optimization to the degradation environment;

[0145] 4. A closed-loop feedback correction mechanism between the front-end and back-end effectively suppresses cumulative drift at the front end;

[0146] 5. Significantly improves positioning stability and accuracy in degraded scenarios such as tunnels, corridors, and open flat surfaces.

Claims

1. A multi-sensor fusion SLAM real-time localization method based on observability adaptive adjustment, characterized in that... Includes the following steps: S1: Multi-source sensor data acquisition and time synchronization; S2: Front-end laser odometry estimation based on laser and inertial information; S3: Point cloud normal vector estimation and effective matching set construction; S4: Environmental geometric observability analysis and degradation subspace determination; S5: Anisotropic adaptive adjustment of laser observation information matrix based on degradation information; S6: Multi-source factor joint error optimization and front-end / back-end feedback correction based on sliding window.

2. The multi-sensor fusion SLAM real-time localization method based on observability adaptive adjustment according to claim 1, characterized in that: Step S1 specifically involves: acquiring lidar point cloud data, inertial measurement unit data, and wheel speed sensor data; performing time synchronization and preprocessing on the multi-source data; and sending the synchronized data to the front-end positioning module.

3. The multi-sensor fusion SLAM real-time localization method based on observability adaptive adjustment according to claim 1, characterized in that: Step S2 specifically involves: based on the matching relationship between the current laser point cloud and the local map or the point cloud of the previous frame, constructing a point-to-surface residual model, solving for the initial pose estimation of the current frame relative to the previous frame, and obtaining the output pose of the front-end laser odometry, denoted as... ， It represents a three-dimensional Euclidean transformation group, which includes translation and rotation information.

4. The multi-sensor fusion SLAM real-time localization method based on observability adaptive adjustment according to claim 3, characterized in that: Laser odometry can be implemented through ICP matching, NDT matching, and FASTLIO2.

5. The multi-sensor fusion SLAM real-time localization method based on observability adaptive adjustment according to claim 1, characterized in that: Step S3 specifically involves: performing local plane fitting on the point cloud participating in the matching, and calculating the unit normal vector for each valid matching point. Construct a set of normals for environmental objectivity analysis: Formula (1), where, For the set of real numbers, It represents a three-dimensional real vector space.

6. The multi-sensor fusion SLAM real-time localization method based on observability adaptive adjustment according to claim 1, characterized in that: Step S4 specifically includes: S4.1, constructing an observability approximation matrix to quantify constraint strength: constructing a translation observability matrix to reflect the constraint strength in the translation direction: Formula (2) is used to construct the rotation observability matrix to reflect the constraint strength in the rotation direction: Formula (3), where, To effectively match the number of point clouds, For a specific point cloud; S4.2, Determining the translational degenerate subspace: Perform eigenvalue decomposition on the translation observability matrix: Formula (4), where, feature ， Indicates the direction of translation The constraint strength, The eigenvector matrix is ​​used as an example; based on the proportional relationship of eigenvalues, a set of translated degenerate subspace indices is constructed: Formula (5), where, For a preset threshold, the set The corresponding eigenvectors span the translational degenerate subspace of the current environment; S4.3, Determining the rotational degenerate subspace: Perform eigenvalue decomposition on the rotation observability matrix: Formula (6), where, feature ， Indicates the direction of rotation The constraint strength, Given an eigenvector matrix; construct a set of rotationally degenerate subspace indices based on the eigenvalue ratios: Formula (7), where, For a preset threshold, the set The corresponding eigenvectors span the rotational degeneracy subspace of the current environment; S4.4, forming a comprehensive observability degradation index: Formula (8), where, , is used to measure the weakest level of the overall geometric constraints of the system.

7. The multi-sensor fusion SLAM real-time localization method based on observability adaptive adjustment according to claim 1, characterized in that: Step S5 specifically involves: S5.1, constructing a degradation intensity mapping function and adjusting the weights according to the degree of degradation: Formula (9), where, Minimum weight threshold; S5.2, Anisotropy adjustment of translation / rotation information matrix: Constructing the translation information matrix: Formula (10) is used to construct the rotation information matrix: Formula (11) updates the laser observation factor information matrix as follows: Formula (12), where, The initial laser observation information matrix is ​​determined by the inherent accuracy of the sensor.

8. The multi-sensor fusion SLAM real-time localization method based on observability adaptive adjustment according to claim 7, characterized in that: 。 9. The multi-sensor fusion SLAM real-time localization method based on observability adaptive adjustment according to claim 1, characterized in that: Step S6 specifically includes: S6.1, at the current time... Within the sliding window, the window size Using all pose states within the window To optimize the target object, a joint least squares optimization objective function based on information matrix weighting is constructed: Formula (13), where: Laser odometry bilateral residuals ； Wheel speed odometer bilateral residuals ； : Map matching factor, representing the matching residual from a point in frame j to the map; 、 、 : These represent the information matrices of the corresponding factors; S6.2, Feedback Correction: Based on the backend optimization results, calculate the frontend pose correction amount: Formula (14); Correct the initial pose of the next frame: Formula (15), where the smoothing coefficient is... 。