SLAM tunnel positioning method and system based on intensity auxiliary and degradation perception

Abstract

This invention belongs to the field of vehicle positioning and navigation technology, specifically a SLAM tunnel positioning method and system based on intensity-assisted and degradation-aware perception. The method involves acquiring a 3D point cloud of the tunnel, projecting it to generate an intensity image, and extracting intensity line features. A state prior is constructed using pre-integration by an inertial measurement unit (IMU). A factor graph optimization model is built using pre-integration factors, geometric feature factors, and intensity line feature factors as constraint factors. A degradation factor is calculated based on the Jacobian matrix of the geometric feature factors to determine the longitudinal degradation direction and degree. An adaptive weighting function is designed based on degradation detection, reducing the geometric weight and increasing the intensity line weight in the degradation direction to suppress longitudinal drift. The optimized model is solved to output the pose and a 3D point cloud map. This method solves the drift caused by insufficient longitudinal constraints in long, straight tunnels, achieving high-precision positioning.

Description

Technical Field

[0001] This invention belongs to the field of vehicle positioning and navigation technology, specifically a SLAM tunnel positioning method and system based on intensity-assisted and degradation-sensing. Background Technology

[0002] The statements in this section only refer to the background art related to this invention and do not necessarily constitute prior art.

[0003] Highway tunnels, as vital transportation infrastructure connecting mountainous areas and crossing rivers, are characterized by their long routes, enclosed spaces, and dramatic changes in lighting. With the rapid development of autonomous driving and advanced driver assistance systems, continuous and high-precision autonomous vehicle positioning within tunnels has become a critical requirement for ensuring driving safety.

[0004] However, the tunnel environment completely blocks navigation satellite system signals, and vehicles cannot rely on satellite positioning to obtain absolute position information. Therefore, a simultaneous positioning and mapping scheme that integrates lidar and inertial measurement unit is usually adopted to achieve pose estimation by continuously sensing the three-dimensional geometry of the tunnel.

[0005] Due to the elongated and narrow structure of tunnels, the geometry of long, straight sections is uniform, with nearly parallel side walls and little variation along the depth direction. Geometric feature-based laser inertial SLAM (Simultaneous Localization and Mapping) relies primarily on geometric constraints such as the tunnel walls to estimate vehicle position when calculating pose. However, in long, straight sections, the insufficient geometric constraints along the tunnel's depth direction lead to a lack of constraint in existing geometric feature-based localization methods in this direction. This causes the vehicle's longitudinal pose estimation to drift continuously with increasing travel distance, resulting in a continuous accumulation of errors. Ultimately, this causes the localization result to deviate significantly from the true position, making it impossible to maintain lane-level positioning accuracy. Summary of the Invention

[0006] This invention provides a SLAM tunnel localization method and system based on strength assistance and degradation perception. By introducing tunnel wall strength information to construct longitudinal constraints, it effectively suppresses positioning drift in the vehicle's forward direction and achieves long-term, high-precision pose estimation in tunnel scenarios.

[0007] To achieve the above objectives, the present invention adopts the following technical solution: The first aspect of this invention discloses a SLAM tunneling localization method based on intensity-assisted and degradation-sensing methods, comprising the following steps: The three-dimensional point cloud data of the tunnel is acquired, and a two-dimensional intensity image is generated by projection and intensity line features are extracted. The intensity line features are the pixel points on the tunnel wall where the laser reflection intensity changes abruptly. After being back-projected into three-dimensional space, they form a linear feature that reflects the physical structure of the tunnel wall with differences in reflection intensity. The three-axis acceleration and three-axis angular velocity are obtained. During the time interval between two adjacent keyframes, the inter-frame pose increment is calculated based on integration, and the state prior is constructed by combining the statistical covariance. The six-degree-of-freedom pose, velocity and zero bias of each keyframe are used as state variables, and the pre-integration factor obtained based on integration, the geometric feature factor obtained based on laser point cloud and the intensity line feature factor are used as constraint factors to construct a factor graph optimization model. The degradation factor is calculated based on the Jacobian matrix of the geometric feature factors, and the longitudinal degradation direction and degree of the tunnel are determined based on the set degradation judgment threshold. Based on the obtained degradation factor detection results, an adaptive weighting function is designed to dynamically adjust the weights of the residual terms of geometric feature factors and intensity line feature factors. In the degradation direction, the weights of geometric features are reduced and the weights of intensity line features are enhanced to suppress longitudinal pose drift. Based on the factor graph optimization model, the residual terms corresponding to the pre-integral factor, geometric feature factor and intensity line feature factor are jointly iteratively solved to output the vehicle's six-degree-of-freedom pose estimate and the tunnel's three-dimensional point cloud map, thus completing the tunnel positioning.

[0008] Furthermore, a two-dimensional intensity image is generated by projection and intensity line features are extracted, specifically as follows: The acquired 3D point cloud data of the tunnel is used to generate a 2D intensity image by cylindrical projection based on the number of lidar beams and the horizontal angular resolution. The pixel value records the laser reflection intensity of the corresponding 3D point. The two-dimensional intensity image is denoised and edge pixels with abrupt changes in intensity are extracted. The edge pixels correspond to at least the concrete joints of the tunnel wall, the segment joints, and the pipeline supports. Based on the geometric mapping relationship of cylindrical projection, the edge pixels are back-projected into three-dimensional space to obtain a set of three-dimensional intensity edge points; Euclidean distance clustering is performed on the three-dimensional intensity edge points to aggregate spatially adjacent edge points into independent intensity line feature segments. The center point coordinates and direction vector of each intensity line feature segment are calculated and used as intensity line feature observation units in factor graph optimization.

[0009] Furthermore, the geometric feature factors include geometric surface feature factors and geometric line feature factors obtained based on laser point clouds. The residual of the geometric surface feature factor is the distance from the point to the plane, and the residuals of the geometric line feature factor and the intensity line feature factor are both the distance from the point to the line. The optimal pose is solved by iteratively minimizing the weighted sum of squared residuals.

[0010] Furthermore, the degradation factor is calculated based on the Jacobian matrix of the geometric eigenfactors, specifically as follows: Obtain the residual Jacobian matrix corresponding to all geometric feature observations in the current optimization window, and construct an approximate expression for the information matrix; Singular value decomposition is performed on the information matrix to obtain multiple singular values, which correspond to the constraint strength in the six degrees of freedom of translation and rotation in pose estimation. The overall degradation factor and the degradation factor for each degree of freedom direction are calculated based on the singular values. The overall degradation factor reflects the sufficiency of the overall geometric constraints of the current scene. The degradation factors for translation and rotation directions are the ratios of the corresponding singular values ​​to the maximum singular values ​​in the corresponding directions.

[0011] Furthermore, the longitudinal degradation direction and degree of the tunnel are determined based on the set degradation judgment threshold. Specifically, a degradation judgment threshold is set. When the degradation factor of a certain degree of freedom direction is less than the degradation judgment threshold, it is determined that there is geometric degradation in that direction and it is marked as a degradation state. In the highway tunnel scenario, the smaller the singular value corresponding to the vehicle's forward direction, the more severe the longitudinal constraint degradation.

[0012] Furthermore, an adaptive weighting function is designed, specifically as follows: For geometric feature factors, a weight decay coefficient is defined. When the degradation factor is less than the degradation judgment threshold, the weight decay coefficient decreases linearly as the degree of degradation increases. For the intensity line characteristic factor, a weight enhancement coefficient is defined and a gain adjustment parameter is set. When the degradation factor is less than the degradation judgment threshold, the weight enhancement coefficient increases linearly with the degree of degradation. Multiply the geometric feature residual by the weight decay coefficient and the intensity line feature residual by the weight enhancement coefficient, and substitute them into the overall objective function of factor graph optimization.

[0013] Furthermore, for degrees of freedom directions not marked as degenerate, both the weight decay coefficient and the weight enhancement coefficient remain at 1, maintaining the original weight configuration unchanged and ensuring that optimization in normal directions is not affected.

[0014] Furthermore, during the joint iterative solution process, the method also includes constructing an intensity edge context descriptor for loop closure detection, specifically: The keyframe point cloud is projected onto a two-dimensional horizontal plane. A polar coordinate system is established with the center of the lidar as the origin. Rings are evenly divided along the radial direction to cover different distance ranges, and sectors are evenly divided along the angular direction to cover a 360° field of view, forming a polar coordinate grid. For each grid cell, the number of geometric edge points falling into it, the number of intensity edge points, and the average reflection intensity value of the intensity edge points are counted. The three statistics obtained are concatenated into the feature vector of the grid cell, and all grid features are normalized. The grid features are expanded into a one-dimensional descriptor vector in row and column order, while retaining the grid index information corresponding to each element.

[0015] Furthermore, a two-stage hierarchical matching strategy is adopted for loop closure detection, specifically as follows: The first stage uses the global statistical features of descriptors for initial screening to exclude candidate frames that are obviously mismatched. The second stage involves a grid-by-grid fine comparison of the descriptors of the candidate frames and the current frame, calculating the cosine similarity of the features of each grid and taking the mean as the final similarity score. When the similarity score exceeds the preset threshold and the time interval and spatial distance between the current frame and the candidate frame both meet the loop closure judgment conditions, a loop closure is confirmed to be detected. The loop closure constraint is added to the factor graph as a loop closure factor. The pose of historical key frames is uniformly adjusted through global pose graph optimization to generate a globally consistent 3D tunnel point cloud map.

[0016] The second aspect of the present invention discloses a SLAM tunnel positioning system based on intensity-assisted and degradation-sensing, including a vehicle-mounted lidar, an inertial measurement unit, a processor, and a memory. The memory stores a computer program, and the processor executes the computer program to implement the above-mentioned laser inertial SLAM tunnel positioning method based on intensity-assisted and degradation-sensing.

[0017] Compared with existing technologies, one or more of the above technical solutions have the following beneficial effects: The inherent physical structures of tunnel walls, such as joints and seams, create intensity differences in laser reflection. This characteristic is utilized to form intensity line features, which serve as supplementary constraints in the longitudinal direction. Combined with the pre-integration factor of the inertial measurement unit and geometric feature factors, a factor graph optimization model is constructed. This transforms the positioning problem of insufficient longitudinal geometric constraints in long, straight tunnel sections into a factor graph optimization problem with longitudinal constraints based on intensity line features. This leverages the inherent structural characteristics of the tunnel walls to compensate for the insufficient geometric constraints in the longitudinal direction of long, straight tunnels. Furthermore, an adaptive weighting function is incorporated to ensure that the intensity line features play a dominant role in the direction with the weakest geometric constraints, achieving targeted suppression of longitudinal pose drift. Finally, based on the factor graph optimization model, a six-degree-of-freedom vehicle pose estimate and a 3D point cloud map of the tunnel are output, maintaining lane-level positioning accuracy in tunnel scenarios. Attached Figure Description

[0018] The accompanying drawings, which form part of this invention, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an improper limitation of the invention.

[0019] Figure 1This is a flowchart illustrating the SLAM tunnel localization method based on intensity assistance and degradation perception provided in an embodiment of the present invention. Figure 2 This is a schematic diagram illustrating the principle of intensity line feature extraction and longitudinal constraint enhancement provided in an embodiment of the present invention; Figure 3 This is a schematic diagram of the degradation factor calculation and degradation direction determination process provided in an embodiment of the present invention; Figure 4 A schematic diagram of the adaptive weight allocation mechanism provided in an embodiment of the present invention; Figure 5 This is a schematic diagram of the factor graph optimization framework provided in an embodiment of the present invention. Detailed Implementation

[0020] The present invention will be further described below with reference to the accompanying drawings and embodiments.

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

[0022] As described in the background section, existing technologies rely solely on three-dimensional geometric features to construct positioning constraints in long, straight tunnel sections. Because the tunnel wall geometry is highly uniform along the longitudinal direction, the observability of constraints in this direction is extremely low, leading to a lack of longitudinal pose constraints, i.e., longitudinal degradation. Ultimately, this causes the vehicle's pose estimation in the forward direction to continuously drift and diverge with the accumulated travel distance, which is the fundamental cause of tunnel positioning degradation.

[0023] Therefore, this solution provides a SLAM tunnel localization method and system based on intensity-assisted and degradation-aware features. By leveraging the differences in laser reflection intensity of physical structures such as tunnel wall joints and segment joints, intensity line features are extracted as supplementary constraints for geometric degradation direction, thereby effectively suppressing longitudinal pose drift.

[0024] The relevant technical terms used in this solution are explained below.

[0025] Intensity refers to the intensity of the lidar echo reflection, which is the reflected energy after the laser hits the surface of an object. It is related to the material and color of the object, rather than the brightness of image pixels. The significant difference in reflection intensity between the tunnel wall concrete and the joints is the source of information for feature extraction in this scheme.

[0026] An intensity image is a two-dimensional image that unfolds a three-dimensional point cloud through cylindrical projection. Each pixel value records the laser reflection intensity of the corresponding three-dimensional point and serves as an intermediate carrier for extracting intensity line features.

[0027] Intensity line features, in this scheme, refer to the linear features formed by pixels on the tunnel wall with abrupt changes in reflectance intensity (corresponding to physical structures such as concrete joints, segment joints, and pipeline supports) after edge detection and back projection into 3D space. These features serve as observation factors to supplement longitudinal constraints. They are simply called line features and are different from geometric edges (corners, structural edges) in conventional SLAM. In this scheme, they are two independent feature factors participating in optimization.

[0028] Degradation refers to a state where geometric constraints are insufficient or constraints have low observability in a certain degree of freedom direction, rather than a decrease in algorithm performance. In tunnels, the longitudinal direction of travel degrades due to the uniform shape of the tunnel walls, which is the root cause of drift.

[0029] Edges refer to pixels in an intensity image where the intensity value changes abruptly. This scheme uses the Canny edge detection operator to extract them, corresponding to physical structures such as tunnel wall seams, rather than object contour boundaries.

[0030] The SLAM tunneling localization method based on intensity-assisted and degradation-aware methods includes the following steps: The three-dimensional point cloud data of the tunnel is acquired, and a two-dimensional intensity image is generated by projection and intensity line features are extracted. The intensity line features are the linear features formed by back-projecting the pixels on the tunnel wall where the laser reflection intensity changes abruptly into three-dimensional space. They are used to reflect the physical structure of the tunnel wall with differences in reflection intensity. The three-axis acceleration and three-axis angular velocity are obtained. During the time interval between two adjacent keyframes, the inter-frame pose increment is calculated based on integrals and the state prior is constructed by combining the statistical covariance. The six-degree-of-freedom pose, velocity and zero bias of each keyframe are used as state variables. The pre-integration factor obtained based on integral calculation, the geometric feature factor obtained based on laser point cloud and the intensity line feature factor are used as constraint factors to construct a factor graph optimization model. The degradation factor is calculated based on the Jacobian matrix of the geometric feature factors, and the longitudinal degradation direction and degree of the tunnel are determined based on the set degradation judgment threshold. Based on the obtained degradation factor detection results, an adaptive weighting function is designed to dynamically adjust the weights of the residual terms of geometric feature factors and intensity line feature factors. In the degradation direction, the weights of geometric features are reduced and the weights of intensity line features are enhanced to suppress longitudinal pose drift. Based on the factor graph optimization model, the residual terms corresponding to the pre-integral factor, geometric feature factor and intensity line feature factor are jointly iteratively solved to output the vehicle's six-degree-of-freedom pose estimate and the tunnel's three-dimensional point cloud map, thus completing the tunnel positioning.

[0031] The inherent physical structures of tunnel walls, such as joints and seams, create intensity differences in laser reflection. This characteristic is utilized to form intensity line features, which serve as supplementary constraints in the longitudinal direction. Combined with the pre-integration factor of the inertial measurement unit and the geometric feature factor cylinder as constraints, a factor graph optimization model is constructed. This transforms the positioning problem of insufficient longitudinal geometric constraints in long, straight tunnel sections into a factor graph optimization problem with longitudinal constraints from intensity line features. This leverages the inherent structural characteristics of the tunnel walls to compensate for the insufficient geometric constraints in the longitudinal direction of long, straight tunnels. Furthermore, an adaptive weighting function is incorporated to allow the intensity line features to play a dominant role in the direction with the weakest geometric constraints, achieving targeted suppression of longitudinal pose drift. Finally, based on the factor graph optimization model, a six-degree-of-freedom vehicle pose estimate and a 3D point cloud map of the tunnel are output, maintaining lane-level positioning accuracy in tunnel scenarios.

[0032] This embodiment combines Figure 1 The provided flowchart illustrates the basic process of the SLAM tunneling localization method based on intensity-assisted and degradation-aware features, including the following steps: S1: The vehicle-mounted LiDAR collects the 3D point cloud of the tunnel, generates a 2D intensity image by cylindrical projection, extracts the intensity abrupt change pixels by edge detection and projects them back to the 3D space to obtain the intensity line feature points of the corresponding tunnel wall joints, splices and other features. S2: Based on inertial measurement unit data, pre-integration is used to calculate inter-frame pose increments and covariance, constructing state priors. A factor graph optimization framework is established, with state variables including six-DOF pose, velocity, and zero bias for each keyframe. Pre-integration factors, point cloud geometric surface features, line feature factors, and intensity line feature factors are jointly optimized. The surface feature residual is the distance from a point to a plane, while the line and intensity line feature residuals are the distances from a point to a line. The optimal pose is solved by iteratively minimizing the weighted sum of squared residuals. The principles of intensity line feature extraction and longitudinal constraint enhancement are as follows: Figure 2 As shown; S3: Online calculation of degradation factors. The singular value decomposition of the geometric constraint Jacobian matrix yields six singular values ​​λ1≥…≥λ6. The degradation factor for each direction is the ratio of the corresponding singular value to the largest singular value in that direction. A threshold τ=0.1 is set; when the degradation factor is less than τ, that direction is considered degraded. The direction of travel in a tunnel is typically a degradation direction. The degradation factor calculation and degradation direction determination process is as follows: Figure 3 As shown; S4: Adaptive weighting is designed based on degradation detection. The geometric feature weight coefficient α = min(1, d / τ), and the intensity line feature weight coefficient β = 1 + γ·max(0, 1-d / τ), where γ ∈ [0.5, 2.0]. In the degradation direction, the geometric weight is reduced and the intensity weight is increased, while the weights in the non-degradation direction remain unchanged. The adaptive weight allocation mechanism is as follows: Figure 4 As shown; S5: Construct an intensity edge context descriptor for loop closure detection. An M×N polar coordinate grid is established on the projected point cloud plane. For each grid cell, the number of geometric edge points, the number of intensity edge points, and the average intensity value are counted to form a feature vector, which is then L2 normalized. Two-stage matching is used to detect loop closures. Loop closure constraints are added to the factor graph, and global pose graph optimization eliminates accumulated drift. The framework for loop closure detection and factor graph optimization is as follows: Figure 5 As shown; S6: Outputs the vehicle's six-DOF pose and a 3D point cloud map.

[0033] The specific process of this embodiment is described in detail below.

[0034] S1: Acquire 3D point cloud data of the tunnel using vehicle-mounted LiDAR, generate a 2D intensity image through cylindrical projection, and extract intensity line features. The specific process is as follows: S1-1 performs cylindrical projection of the original 3D point cloud according to the radar scanning structure to generate a 2D intensity image of size H×W, where H is the number of lidar beams and W is the number of horizontal sampling points in a single frame. Each pixel value records the laser reflection intensity of the corresponding 3D point.

[0035] This scheme does not restrict the radar's structural type. For example, for a mechanically rotating radar, the horizontal angle θ = atan2(y,x) and the vertical angle are calculated from the point coordinates P(x,y,z). =atan2(z, ),according to Determine the row number, and calculate the column number w = round((θ) based on θ and the horizontal angular resolution Δθ. θ min ) / Δθ ) mod W, where the pixel value is the corresponding laser reflection intensity.

[0036] For solid-state / hybrid solid-state radars, an equivalent two-dimensional grid mapping is performed based on their inherent point cloud arrangement.

[0037] S1-2: Gaussian filtering is applied to the intensity image to smooth it out and suppress noise. Then, the Canny edge detection operator is used to extract edge pixels where the intensity changes abruptly. These pixels correspond to physical features with significant differences in reflection intensity, such as concrete joints, segment joints, and pipeline supports on the tunnel wall. S1-3: Based on the geometric mapping relationship of cylindrical projection, the extracted two-dimensional edge pixels are back-projected into three-dimensional space to obtain a set of three-dimensional intensity edge points; S1-4: Perform Euclidean distance clustering on the three-dimensional intensity edge points, aggregating spatially adjacent edge points into independent intensity line feature segments, and calculate the center point coordinates and direction vector of each line feature segment as the intensity line feature observation unit in subsequent factor graph optimization.

[0038] Step S1 addresses the issue of the source of longitudinal constraint information. Since the geometry of long, straight tunnel sections is highly uniform along the longitudinal direction, relying solely on geometric features cannot provide effective constraints for longitudinal pose. Furthermore, the physical structures of the tunnel walls, such as concrete joints and segment joints, exhibit significant differences in laser reflection intensity, serving as naturally existing and stable spatial markers within the tunnel. By using cylindrical projection to unfold the 3D point cloud into a 2D intensity image, extracting pixels with abrupt intensity changes using edge detection and projecting them back into 3D space, and then clustering them into intensity line feature segments using Euclidean distance, the inherent inhomogeneities of the tunnel walls can be transformed into linear observation units that can be used for optimization. This provides a source of constraint information for the longitudinal degradation direction without adding any external facilities, forming the material basis for subsequent degradation perception and adaptive weighting.

[0039] S2: Construct a state prior based on inertial measurement unit data, and jointly incorporate geometric features and intensity line features into factor graph optimization to solve for the optimal pose. The specific process is as follows: S2-1: Inertial Measurement Unit Data Acquisition and Preprocessing. The vehicle-mounted inertial measurement unit acquires raw measurement data of the vehicle's three-axis acceleration and three-axis angular velocity at high frequency, and performs noise reduction processing on the measurement data to eliminate the influence of high-frequency noise on subsequent integration.

[0040] S2-2: Inter-frame pre-integration calculation of pose increment and covariance. During the time interval between two adjacent keyframes, all acceleration and angular velocity measurements collected by the inertial measurement unit are integrated to accumulate the relative displacement, velocity change, and attitude change of the vehicle during that time interval, which serves as the inter-frame pose increment. Simultaneously, the uncertainty of this pose increment estimation is statistically analyzed, generating corresponding covariance information to characterize the reliability of the constraint.

[0041] In this scheme, the selection of key frames is based on a dual judgment of motion change and time interval: when the displacement or attitude change of the vehicle relative to the previous key frame exceeds the set threshold, or the time interval from the previous key frame exceeds the maximum allowable duration, the current frame is marked as a new key frame.

[0042] S2-3: Constructing State Priors. Using the inter-frame pose increment obtained from pre-integration, combined with the optimal pose estimation of the previous keyframe, the initial pose, velocity, and zero bias of the current keyframe are calculated as initial state estimates for factor graph optimization, providing a convergence starting point for subsequent optimizations.

[0043] S2-4: Establish a factor graph optimization framework. Using the six-degree-of-freedom pose, velocity, and zero bias of each keyframe as the state variables to be solved, a factor graph optimization model is constructed. The inertial measurement unit pre-integration factor, point cloud geometric surface feature factor, geometric line feature factor, and intensity line feature factor are added to the factor graph as constraint edges to form a tightly coupled joint optimization structure.

[0044] S2-5: Construct residuals for various feature factors. For point cloud geometric features, construct residual terms based on the distance from the point to the plane to constrain the fit between the vehicle and the tunnel wall plane; for geometric line features and intensity line features, construct residual terms based on the distance from the point to the line to constrain the fit between the vehicle and the tunnel structure edge line and the wall joint line, respectively.

[0045] S2-6: Joint optimization to find the optimal pose. The residual terms corresponding to all factors in the factor graph are weighted and summed. The pose, velocity, and zero bias of each keyframe are iteratively adjusted to minimize the weighted sum of squared residuals. After iterative convergence, the optimal pose estimate for each keyframe within the current optimization window is output.

[0046] Intensity line feature extraction and longitudinal constraint enhancement principle as follows Figure 2 As shown: In long, straight sections of tunnels, vehicles travel longitudinally (in the X direction). This direction becomes a degenerate direction due to the uniform geometry of the tunnel walls, and geometric features alone cannot effectively constrain longitudinal displacement. For example... Figure 2 As shown, the vehicle scans the tunnel wall using an onboard LiDAR. The laser reflection intensity at physical structures such as concrete joints and segment joints on the wall differs significantly from the surrounding wall surface. A two-dimensional intensity image is generated via cylindrical projection, and edge pixels are extracted. This image is then projected back into three-dimensional space to obtain the intensity line features distributed along the tunnel's longitudinal direction. Between adjacent frames, feature matching constraints between feature points in the previous and current frames establish a temporal correspondence between the intensity line features. Furthermore, intensity line feature factors are constructed based on the point-to-line distance and incorporated into the factor map optimization. This intensity line feature extends longitudinally, and its normal direction coincides with the vehicle's forward direction, thus providing a clear displacement constraint in the longitudinal degradation direction. This effectively suppresses the cumulative drift of the inertial / odometer in the forward direction, achieving stable enhancement of longitudinal pose estimation in tunnel corridor scenarios.

[0047] This step utilizes multi-source information collaboration for pose constraints. The inertial measurement unit (IMU) acquires acceleration and angular velocity at high frequency. Direct participation in optimization would impose a computational burden. Pre-integration compresses the measurements between adjacent keyframes into inter-frame pose increments with covariance, preserving motion constraints while providing their reliability. Using keyframe pose, velocity, and zero bias as state variables, pre-integration factors, geometric feature factors, and intensity line feature factors are jointly incorporated into the factor graph optimization model. This ensures that constraints from different sources mutually constrain each other in the same optimization problem. Geometric features lock the direction perpendicular to the wall, intensity line features supplement longitudinal constraints, and pre-integration ensures smooth short-term motion, thus preventing the drift of a single constraint from accumulating in isolation and achieving tightly coupled joint optimization.

[0048] S3: Online calculation of lidar degradation factor, quantitative assessment of constraint observability in each pose degree of freedom direction, and determination of degradation direction and degree. The specific process of S3 is as follows... Figure 3As shown, it includes the following steps: S3-1: Collect the residual Jacobian matrices corresponding to all geometric feature observations in the current optimization window, and construct an approximate expression for the information matrix Λ≈J. J; Where J is the unweighted residual Jacobian matrix, which is intended to reflect the inherent constraint strength of the geometry itself and does not include the influence of subsequent adaptive weights.

[0049] In this scheme, the current optimization window adopts a fixed-length sliding window mechanism, which contains the most recent keyframes (set to the most recent 20 in this embodiment). The window slides as new keyframes are added, and the oldest keyframe is removed to maintain computational efficiency.

[0050] In this scheme, the Jacobian matrix used for detection is obtained during optimization iteration.

[0051] In this scheme, the partial derivative of the Jacobian matrix is ​​only used for the pose of the six degrees of freedom, and the velocity and zero bias are not included in the degradation analysis.

[0052] S3-2: Perform singular value decomposition on the information matrix Λ, i.e., Λ=UΣV We obtain six singular values ​​λ1≥λ2≥λ3≥λ4≥λ5≥λ6, which correspond to the constraint strengths of the six degrees of freedom of translation and rotation in pose estimation, respectively; where λ1, λ2, and λ3 represent the X, Y, and Z directions, respectively; and λ4, λ5, and λ6 represent the roll angle (around the X-axis), pitch angle (around the Y-axis), and yaw angle (around the Z-axis), respectively.

[0053] S3-3: Calculate the overall degradation factor as λ6 / λ1, which reflects the sufficiency of the overall geometric constraints of the current scene. The closer this value is to 1, the more balanced the constraints are in all directions.

[0054] S3-4: Calculate the degradation factor d for each degree of freedom direction. i =λ i / λ max The translational degradation factor is the ratio of the corresponding singular value to the maximum singular value in the translational direction, and the same applies to the rotational direction.

[0055] The six singular values ​​are divided into translation and rotation groups based on the first three terms (λ1, λ2, λ3) and the last three terms (λ4, λ5, λ6). Based on this, the degradation factor d for each degree of freedom is calculated. i =λ i / λ maxThe translation group is normalized using the largest singular value λ1 as the denominator, i.e., d1=1, d2=λ2 / λ1, d3=λ2 / λ1; the rotation group is normalized using the largest singular value λ4 as the denominator, i.e., d4=1, d5=λ5 / λ4, d6=λ6 / λ4. The purpose of grouping normalization is to compare the relative strength of constraints in each direction within the same motion type, avoiding deviations in degradation judgments due to differences in the physical dimensions of translation and rotation.

[0056] S3-5: Set the degradation judgment threshold τ=0.1. When the degradation factor d in a certain direction... i When the value is less than τ, geometric degradation is determined to exist in that direction, and it is marked as a degenerate state. In highway tunnel scenarios, the singular value corresponding to the vehicle's forward direction is usually the smallest, and the longitudinal constraint is severely degraded.

[0057] In this scheme, the weakest direction is determined by comparing six singular values, with the smallest value corresponding to the weakest direction.

[0058] The physical explanation is as follows: In long straight sections of highway tunnels, since the two side walls are nearly parallel and their geometric shapes are highly consistent along the longitudinal direction, the partial derivative of the observation equation based on geometric features along the vehicle's forward direction (longitudinal direction) is approximately zero. This results in the singular values ​​in the corresponding direction of the information matrix being numerically smaller than those in other directions, meaning that the longitudinal constraint strength is the lowest.

[0059] Step S3 quantitatively identifies directions with insufficient constraints. In step S3, degradation factor detection constructs an information matrix from the Jacobian matrix of geometric feature factors and performs singular value decomposition, quantifying the constraint strength of each degree of freedom direction into a degradation factor. This transforms the previously difficult-to-observe question of "which direction has insufficient constraints and to what extent" into a calculable quantitative indicator. In highway tunnel scenarios, the singular value corresponding to the vehicle's forward direction is usually the smallest. Based on this, the longitudinal degradation direction and its degree can be clearly determined, providing an accurate decision-making basis for subsequent adaptive weighting and avoiding indiscriminate processing of all directions.

[0060] S4: Design an adaptive weighting function based on the degradation detection results to dynamically adjust the optimization contribution weights of intensity line features and geometric features.

[0061] In S4, the adaptive weight allocation mechanism is as follows: Figure 4 As shown, specifically: S4-1: For geometric feature factors, define the weight decay coefficient α=min(1, d / τ), where d is the degradation factor in the current degradation direction and τ is the degradation judgment threshold. When d<τ, α decreases linearly with the degree of degradation. S4-2: For the intensity line characteristic factor, the weight enhancement coefficient is defined as β=1+γ·max(0, 1-d / τ), where γ is the gain adjustment parameter, and the value range is from 0.5 to 2.0. When d<τ, β increases linearly with the degree of degradation, and reaches the maximum gain 1+γ when the degradation is most severe. S4-3: Multiply the original geometric feature residual by the weight attenuation coefficient α, multiply the original intensity line feature residual by the weight enhancement coefficient β, and substitute them into the overall objective function of factor graph optimization; S4-4: For directions of freedom that are not marked as degenerate, both α and β are kept at 1, that is, the original weight configuration is maintained unchanged, ensuring that the optimization in the normal direction is not affected.

[0062] In step S4, the weights of geometric features are reduced and the weights of intensity line features are increased in the degradation direction, allowing the intensity line features to play a dominant role in the direction with the weakest geometric constraints, thus achieving targeted suppression of longitudinal drift. Simultaneously, the original weight configuration is maintained for the non-degraded direction to ensure that the optimization accuracy in the normal direction is not disturbed. Through a closed-loop design of detection-decision-adjustment, the contribution of intensity line features dynamically changes with the degree of degradation, increasing its contribution when degradation is severe and returning to normal when constraints are sufficient. This ensures positioning accuracy in degradation scenarios while avoiding the side effects of excessive weighting.

[0063] S5: Construct an intensity edge context descriptor, combine geometric and intensity information for loop closure detection, and achieve globally consistent localization through pose graph optimization; the loop closure detection and factor graph optimization framework in S5 is as follows: Figure 5 As shown, specifically: S5-1: Project the keyframe point cloud onto a two-dimensional horizontal plane, establish a polar coordinate system with the center of the lidar as the origin, uniformly divide M rings along the radial direction to cover different distance ranges, and uniformly divide N sectors along the angular direction to cover a 360° field of view, forming an M×N polar coordinate grid.

[0064] S5-2: For each grid cell, count the number of geometric edge points falling into it, g. ij Number of intensity edge points i ij And the average reflection intensity value r at the intensity edge point ij This forms a three-dimensional statistical quantity.

[0065] In this scheme, the geometric edge point g ij The three-dimensional geometric structure derived from laser point clouds is determined by calculating the change in normal vector or curvature gradient within the neighborhood of each point in the point cloud. Points whose normal vector or curvature changes exceed a set threshold are marked as geometric edge points, corresponding to abrupt changes in geometric structure such as tunnel walls and road edges.

[0066] S5-3: Concatenate the three statistics into the feature vector f of the grid cell.ij =[g ij i ij , r ij Furthermore, L2 normalization is performed on all M×N grid features to eliminate the influence of point cloud density differences on similarity calculation.

[0067] S5-4: Expand the M×N grid features into a one-dimensional descriptor vector in row and column order, while retaining the grid index information corresponding to each element to support position alignment in the subsequent fine matching stage.

[0068] S5-5: A two-stage hierarchical matching strategy is adopted for loop closure detection: the first stage uses the global statistical features of the descriptor to perform a fast initial screening and exclude candidate frames that are obviously mismatched.

[0069] In this scheme, global statistical features include summary statistics such as the mean, variance, maximum, minimum and L2 norm of the descriptor vectors. These are represented by histogram statistics to characterize the overall intensity and edge distribution contours, allowing for the rapid elimination of candidate frames with large differences without grid-by-grid comparison.

[0070] The second stage involves a grid-by-grid fine comparison of the descriptors of the candidate frames and the current frame, calculating the cosine similarity of the features of each grid and taking the average as the final similarity score.

[0071] S5-6: When the similarity score exceeds the preset threshold and the time interval and spatial distance between the current frame and the candidate frame both meet the loop closure judgment conditions, a loop closure is confirmed to be detected.

[0072] The time interval determination requires that the time difference between two frames be greater than the set minimum time interval threshold of 30 seconds, so as to exclude false matching caused by similar local scenes in a short period of time; the spatial distance determination requires that the position difference between two frames be less than the set maximum spatial distance threshold of 5 meters, that is, the two frames must be close enough in space to form a valid loop.

[0073] After confirming the loop closure, the loop closure constraint is added to the factor graph as a closure factor. The pose of historical keyframes is uniformly adjusted through global pose graph optimization to eliminate the drift error accumulated during long-distance travel and generate a globally consistent 3D tunnel point cloud map.

[0074] S5-7: Intensity Line Feature Validity Determination and Downgrade Processing. During factor graph optimization, the number of intensity line feature factors and their average observation quality within the current sliding window are statistically analyzed in real time, serving as a quantitative indicator of the effectiveness of intensity line features. When the number of intensity line features within the window is less than a set threshold of 5, or the average observation residual exceeds a set threshold, it is determined that the current scene's intensity line features are insufficiently extracted.

[0075] To address the issue of insufficient intensity line characteristics, the following graded downgrading measures are adopted: Level 1: When the number of intensity line features is below the threshold but not completely missing, a small number of extracted intensity line features are retained for optimization. At the same time, the intensity line feature gain adjustment parameter γ in the adaptive weighting function is increased from the default range [0.5, 2.0] to [1.0, 3.0] to increase the contribution weight of the limited intensity line features and maximize the use of existing information.

[0076] Level 2: When intensity line features are completely missing or the average observation residuals severely exceed limits, the weighted enhancement mechanism of intensity line feature factors is turned off, degenerating into a pure geometry-inertial positioning mode. In this mode, pose estimation relies solely on pre-integration factors and geometric feature factors, and a degraded state marker is output. This allows downstream modules to perceive the decrease in current positioning reliability and adjust planning and control strategies accordingly. The loop closure detection module adjusts synchronously: if intensity information is missing, the number of intensity edge points and the average intensity value in the descriptor are set to zero, relying only on the distribution of geometric edge points for matching, ensuring that the loop closure detection function can still operate normally in degraded mode.

[0077] In step S5, the residual terms corresponding to the pre-integral factors, geometric feature factors, and intensity line feature factors are jointly iteratively solved based on the factor graph optimization model. This enables simultaneous optimization of the pose, velocity, and zero bias of all keyframes within a unified framework, minimizing the weighted sum of squared residuals for various constraints, and outputting a six-degree-of-freedom pose estimate for the vehicle and a 3D point cloud map of the tunnel. Since the pose and map are jointly solved in the unified optimization, they maintain strict consistency, providing directly usable localization and mapping results for downstream modules, thus completing a complete closed loop from constraint construction to final output.

[0078] S6: Output the vehicle's six-DOF pose estimation and the tunnel's 3D point cloud map within the global optimization framework. Specifically: S6-1: Obtain the globally optimized pose result. Based on the convergence result of the global pose graph optimization in S5-6, extract the six-degree-of-freedom pose of each historical keyframe after unified adjustment. This pose sequence has eliminated the drift error accumulated during long-distance driving, which is the globally consistent driving trajectory of the vehicle in the tunnel.

[0079] S6-2: Stitching and Generating a 3D Point Cloud Map. Based on the optimized poses of each keyframe, the 3D point clouds of the tunnel corresponding to each keyframe are transformed to a unified world coordinate system according to the pose transformation relationship, and then stitched and fused to generate a globally consistent 3D point cloud map of the tunnel covering the entire vehicle travel process.

[0080] S6-3: Output pose sequence and point cloud map. The globally consistent rut pose sequence and the 3D point cloud map of the tunnel are output as the final localization and mapping results, which are used by downstream modules of the autonomous driving system such as path planning, vehicle control and high-precision map construction to achieve continuous and high-precision autonomous localization and map construction in tunnel scenarios.

[0081] At this point, map construction in the tunnel scenario was completed, and localization was achieved. This solution leverages the differences in laser reflection intensity caused by physical structures such as tunnel wall joints and segment splices, extracting intensity line features as supplementary constraints for geometric degradation directions to effectively suppress longitudinal pose drift.

[0082] Correspondingly, the SLAM tunnel positioning system based on intensity-assisted and degradation-sensing includes an onboard LiDAR, an inertial measurement unit, a processor, and a memory. The memory stores a computer program, and the processor executes the computer program to implement the aforementioned laser inertial SLAM tunnel positioning method based on intensity-assisted and degradation-sensing.

[0083] The above are merely preferred embodiments of the present invention and are not intended to limit the present invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A SLAM tunneling localization method based on intensity-assisted and degradation-sensing methods, characterized in that, Includes the following steps: The three-dimensional point cloud data of the tunnel is acquired, and a two-dimensional intensity image is generated by projection and intensity line features are extracted. The intensity line features are the pixel points on the tunnel wall where the laser reflection intensity changes abruptly. After being back-projected into three-dimensional space, they form a linear feature that reflects the physical structure of the tunnel wall with differences in reflection intensity. The three-axis acceleration and three-axis angular velocity are obtained. During the time interval between two adjacent keyframes, the inter-frame pose increment is calculated based on the integral, and the state prior is constructed by combining the statistical covariance. Using the six-degree-of-freedom pose, velocity, and zero bias of each keyframe as state variables, and the pre-integration factor obtained based on integral calculation, the geometric feature factor obtained based on laser point cloud, and the intensity line feature factor as constraint factors, a factor graph optimization model is constructed. The degradation factor is calculated based on the Jacobian matrix of the geometric feature factors, and the longitudinal degradation direction and degree of the tunnel are determined based on the set degradation judgment threshold. Based on the obtained degradation factor detection results, an adaptive weighting function is designed to dynamically adjust the weights of the residual terms of geometric feature factors and intensity line feature factors. In the degradation direction, the weights of geometric features are reduced and the weights of intensity line features are enhanced to suppress longitudinal pose drift. Based on the factor graph optimization model, the residual terms corresponding to the pre-integral factor, geometric feature factor and intensity line feature factor are jointly iteratively solved to output the vehicle's six-degree-of-freedom pose estimate and the tunnel's three-dimensional point cloud map, thus completing the tunnel positioning.

2. The SLAM tunneling localization method based on intensity-assisted and degradation-sensing as described in claim 1, characterized in that, A two-dimensional intensity image is generated by projection, and intensity line features are extracted, specifically as follows: The acquired 3D point cloud data of the tunnel is used to generate a 2D intensity image by cylindrical projection based on the number of lidar beams and the horizontal angular resolution. The pixel value records the laser reflection intensity of the corresponding 3D point. The two-dimensional intensity image is denoised and edge pixels with abrupt changes in intensity are extracted. The edge pixels correspond to at least the concrete joints of the tunnel wall, the segment joints, and the pipeline supports. Based on the geometric mapping relationship of cylindrical projection, the edge pixels are back-projected into three-dimensional space to obtain a set of three-dimensional intensity edge points; Euclidean distance clustering is performed on the three-dimensional intensity edge points to aggregate spatially adjacent edge points into independent intensity line feature segments. The center point coordinates and direction vector of each intensity line feature segment are calculated and used as intensity line feature observation units in factor graph optimization.

3. The SLAM tunneling localization method based on intensity-assisted and degradation-sensing as described in claim 1, characterized in that, The geometric feature factors include geometric surface feature factors and geometric line feature factors obtained based on laser point clouds. The residual of the geometric surface feature factor is the distance from the point to the plane, and the residuals of the geometric line feature factor and the intensity line feature factor are both the distance from the point to the line. The optimal pose is solved by iteratively minimizing the weighted sum of squared residuals.

4. The SLAM tunneling localization method based on intensity-assisted and degradation-sensing as described in claim 1, characterized in that, The degradation factor is calculated based on the Jacobian matrix of the geometric eigenfactors, specifically as follows: Obtain the residual Jacobian matrix corresponding to all geometric feature observations in the current optimization window, and construct an approximate expression for the information matrix; Singular value decomposition is performed on the information matrix to obtain multiple singular values, which correspond to the constraint strength in the six degrees of freedom of translation and rotation in pose estimation. The overall degradation factor and the degradation factor for each degree of freedom direction are calculated based on the singular values. The overall degradation factor reflects the sufficiency of the overall geometric constraints of the current scene. The degradation factors for translation and rotation directions are the ratios of the corresponding singular values ​​to the maximum singular values ​​in the corresponding directions.

5. The SLAM tunneling localization method based on intensity-assisted and degradation-sensing as described in claim 4, characterized in that, Based on the set degradation judgment threshold, the longitudinal degradation direction and degree of the tunnel are determined. Specifically, a degradation judgment threshold is set. When the degradation factor of a certain degree of freedom direction is less than the degradation judgment threshold, it is determined that there is geometric degradation in that direction and it is marked as a degradation state. In the highway tunnel scenario, the smaller the singular value corresponding to the vehicle's forward direction, the more severe the longitudinal constraint degradation.

6. The SLAM tunneling localization method based on intensity-assisted and degradation-sensing as described in claim 1, characterized in that, Design an adaptive weighting function, specifically: For geometric feature factors, a weight decay coefficient is defined. When the degradation factor is less than the degradation judgment threshold, the weight decay coefficient decreases linearly as the degree of degradation increases. For the intensity line characteristic factor, a weight enhancement coefficient is defined and a gain adjustment parameter is set. When the degradation factor is less than the degradation judgment threshold, the weight enhancement coefficient increases linearly with the degree of degradation. Multiply the geometric feature residual by the weight decay coefficient and the intensity line feature residual by the weight enhancement coefficient, and substitute them into the overall objective function of factor graph optimization.

7. The SLAM tunneling localization method based on intensity-assisted and degradation-sensing as described in claim 6, characterized in that, For degrees of freedom directions not marked as degenerate, both the weight decay coefficient and the weight enhancement coefficient remain at 1, maintaining the original weight configuration and ensuring that optimization in normal directions is not affected.

8. The SLAM tunneling localization method based on intensity-assisted and degradation-sensing as described in claim 1, characterized in that, During the joint iterative solution, the process also includes constructing an intensity edge context descriptor for loop closure detection, specifically: The keyframe point cloud is projected onto a two-dimensional horizontal plane. A polar coordinate system is established with the center of the lidar as the origin. Rings are evenly divided along the radial direction to cover different distance ranges, and sectors are evenly divided along the angular direction to cover a 360° field of view, forming a polar coordinate grid. For each grid cell, the number of geometric edge points falling into it, the number of intensity edge points, and the average reflection intensity value of the intensity edge points are counted. The three statistics obtained are concatenated into the feature vector of the grid cell, and all grid features are normalized. The grid features are expanded into a one-dimensional descriptor vector in row and column order, while retaining the grid index information corresponding to each element.

9. The SLAM tunneling localization method based on intensity assistance and degradation perception as described in claim 8, characterized in that, A two-stage hierarchical matching strategy is used for loop closure detection, specifically: The first stage uses the global statistical features of descriptors for initial screening to exclude candidate frames that are obviously mismatched. The second stage involves a grid-by-grid fine comparison of the descriptors of the candidate frames and the current frame, calculating the cosine similarity of the features of each grid and taking the mean as the final similarity score. When the similarity score exceeds the preset threshold and the time interval and spatial distance between the current frame and the candidate frame both meet the loop closure judgment conditions, a loop closure is confirmed to be detected. The loop closure constraint is added to the factor graph as a loop closure factor. The pose of historical key frames is uniformly adjusted through global pose graph optimization to generate a globally consistent 3D tunnel point cloud map.

10. A SLAM tunneling localization system based on intensity-assisted and degradation-sensing methods, characterized in that, It includes an onboard lidar, an inertial measurement unit, a processor, and a memory. The memory stores a computer program, and when the processor executes the computer program, it implements the SLAM tunneling localization method based on intensity-assisted and degradation-sensing as described in any one of claims 1-9.

Images