Degradation environment positioning method based on point cloud intensity information assistance and related equipment

Abstract

The invention relates to the technical field of mobile robot positioning and environment perception, and discloses a degraded environment positioning method based on point cloud intensity information assistance and related equipment, and the method comprises the steps: mapping a three-dimensional point cloud intensity image into a first-level projection intensity image through a channel, and carrying out the image processing of the projection intensity image, and obtaining a second-level projection intensity image; performing gradient calculation in the secondary projection intensity image and determining intensity gradient features, performing multi-stage screening on the intensity gradient features to obtain intensity features, and performing feature tracking and feature updating on the intensity features in sequence to obtain a new round of map feature points; inputting a new round of map feature points into the map intensity feature point projection model, and outputting to obtain a map feature current frame projection; and inputting the predicted pose and the current frame projection of the map features into a pre-constructed intensity and geometry combined observation model for fusion calculation to obtain optimal pose estimation. According to the invention, the problem of positioning in a degraded environment is solved without the assistance of an external sensor.

Description

Technical Field

[0001] This invention relates to the field of mobile robot localization and environmental perception technology, specifically to a method and related equipment for localization in degraded environments assisted by point cloud intensity information. Background Technology

[0002] With the rapid development of autonomous driving, digital underground spaces, and intelligent inspection of energy infrastructure, lidar, due to its high precision and robustness, is widely used in enclosed environments such as hydropower station water pipelines, urban underground passages, wind turbine towers, and cable shafts. In these scenarios lacking Global Navigation Satellite System (GNSS) signals, lidar-based Simultaneous Localization and Mapping (SLAM) systems have become a core technology for robot autonomous navigation and environmental mapping.

[0003] Current mainstream laser SLAM methods mostly employ matching mechanisms based on point cloud geometric features, such as Iterative Closest Point (ICP), Normal Distributions Transform (NDT), or registration strategies based on local features like edges / planes. These methods achieve six-degree-of-freedom pose estimation by comparing point cloud data at different times and minimizing the residuals. In structured open environments, they can provide relatively accurate localization results, but in practical engineering applications, they face the serious challenge of "geometric degradation."

[0004] Geometric degradation is common in environments with strong structural regularity, such as long, straight cylindrical tunnels, smooth, straight pipes, or industrial passages with repetitive internal structures. In such scenarios, the structural changes of point clouds between different frames are minimal, especially when translating along the structural axis or rotating around it. The registration residuals are not sensitive enough, resulting in poor observability of the optimization problem in certain dimensions, affecting system convergence and pose accuracy, and even causing serious problems such as divergence and drift.

[0005] Traditional methods for mitigating degradation primarily rely on introducing external observation information, such as fusing multimodal sensors like inertial measurement units (IMUs), vision, ultra-wideband (UWB), and magnetic navigation to enhance state constraints. However, these methods suffer from numerous drawbacks in enclosed spaces such as underground locations, pipelines, and tunnels. Extremely poor lighting conditions make it difficult for cameras or optical flow-based vision sensors to function properly; UWB and Wi-Fi communication-based positioning systems are difficult to deploy and costly; while fusing IMUs can provide short-term compensation, long-term integration error accumulation still requires correction using external observation information. These methods are highly environmentally dependent, complex in system integration, and have limited applicability, making it difficult to meet the demands of industrial sites for stable, lightweight, and low-cost positioning systems.

[0006] In recent years, intensity information in lidar point clouds has attracted researchers' attention. Intensity values ​​reflect the reflection intensity of the laser beam interacting with the surface of an object, and are influenced by factors such as the object's material, surface condition, and angle. In areas with paint, rust, or water stains on the inner wall of a pipe, the laser reflection intensity exhibits spatial distribution differences, forming "texture" features. These intensity features are relatively stable in space and have no direct strong coupling relationship with the geometric structure distribution, providing additional recognition capabilities for SLAM systems in the direction of geometric degradation.

[0007] However, current technologies still face numerous challenges: In regularly structured environments, LiDAR point clouds exhibit regular shapes, repetitive structures, and unidirectional geometric features, failing to adequately constrain six-degree-of-freedom pose and easily leading to localization degradation. Furthermore, existing mitigation methods rely on external sensors, resulting in complex deployments and poor environmental adaptability. The raw form of LiDAR point cloud intensity information is a sparse, unstructured three-dimensional attribute, making it difficult to directly utilize for efficient feature extraction and matching; an intensity image projection model is required. Features extracted from intensity images exhibit uneven distribution and poor stability; direct use in registration may introduce error interference, necessitating the development of evaluation metrics to screen features and the design of a tracking and update mechanism. Existing LiDAR SLAM systems largely rely on geometric errors to construct observation models, making it difficult to introduce compensation information in the degradation dimension; a joint observation model is needed to achieve optimization through the fusion of geometric and intensity information.

[0008] Therefore, there is an urgent need for a technology that can enhance the observability of SLAM systems by utilizing laser point cloud intensity information and through a series of processing steps when the geometric degradation direction is known, thereby achieving stable localization and trajectory constraints in degraded scenarios. Summary of the Invention

[0009] In order to overcome the shortcomings of the existing technology, the purpose of this invention is to provide a degradation environment localization method and related equipment based on point cloud intensity information, so as to solve the technical problem of how to enhance the pose constraint capability in the degradation direction without the assistance of external sensors.

[0010] This invention is achieved through the following technical solution: In a first aspect, the present invention provides a method for locating degraded environments based on point cloud intensity information, comprising: A three-dimensional point cloud intensity image of the laser point cloud is acquired, and the three-dimensional point cloud intensity image is mapped into a projection intensity image through a channel. The projection intensity image is then processed to obtain a second-level projection intensity image. In the secondary projection intensity image, the intensity gradient features are determined by gradient calculation. After multiple levels of filtering, the intensity gradient features are obtained. After feature tracking and feature updating are performed on the intensity features, a new round of map feature points are obtained. After inputting a new round of map feature points into the map intensity feature point projection model, the output is the current frame projection of the map features; IMU data frames are obtained based on IMU measurements. The predicted pose is determined by forward recursion and backpropagation of the IMU data frames. The predicted pose and map features of the current frame are projected into a pre-constructed intensity and geometry joint observation model and fused to obtain the optimal pose estimate. The location of the degraded environment is determined by the optimal pose estimate.

[0011] Preferably, the three-dimensional point cloud intensity image is mapped into a first-level projection intensity image via a channel. The three-dimensional point cloud intensity image is obtained by mapping the three-dimensional point cloud intensity image to the first-level projection intensity image based on the point cloud index projection and the true angle projection using a rotating mechanical radar projection model.

[0012] Preferably, the image processing procedure includes deinterlacing of projected image pixels, point cloud intensity calibration based on environmental geometry information, and removal of horizontal interference stripes; The process of deinterlacing the projected image pixels includes introducing a horizontal pixel compensation mechanism between image rows to correct the position of each row of pixels in order to restore the alignment relationship in the actual orientation. The point cloud intensity calibration process based on environmental geometry information includes: a laser head emitting a laser beam and receiving the target reflection signal; calculating the distance to the point using the time of flight; combining the emission angle to obtain the three-dimensional position of the point; the ratio of the intensity of the received signal to the intensity of the emitted signal represents the intensity value of the point; calibrating the intensity value of the point to obtain the object reflectivity; and calibrating the reflectivity of the intensity value of each laser point before projection onto a two-dimensional image using the distance and incident angle obtained by laser measurement to generate a calibrated intensity image. The process of removing horizontal interference stripes includes constructing a high-pass FIR filter with a cutoff frequency lower than the stripe interference frequency in the vertical direction of the image to extract the stripe information; constructing a low-pass FIR filter in the horizontal direction to retain only the low-frequency components corresponding to the stripes; after two filtering processes, the horizontal stripe interference in the image can be separated; and the stripe removal is completed by subtracting the original intensity map from the extracted interference map.

[0013] Preferably, the intensity gradient features are determined by gradient calculation in the secondary projection intensity image, including: using pixel blocks as intensity feature regions, extracting intensity features based on image gradient information within the intensity feature regions, and calculating the image gradient using a Sobel-like operator on the extracted intensity features; constructing horizontal and vertical convolution kernels based on the image gradient, performing horizontal and vertical convolutions on the image using the cv::filter2D function in OpenCV to obtain gradient maps in the corresponding directions, calculating the average of the absolute values ​​of the gradient maps in the two directions to obtain the average gradient map of the intensity image, and extracting feature blocks from the average gradient map of the intensity image to obtain the intensity gradient features.

[0014] Preferably, the intensity gradient features are obtained after multi-level screening, including: a first-level screening result is obtained based on the distant points, boundary points, intensity anomalies and cluster points in the intensity gradient features, and a second-level screening result is obtained based on the translation and rotation degradation contribution of the first-level screening result.

[0015] Preferably, a new round of map feature points is obtained by sequentially performing feature tracking and feature updating on the intensity features, including: the feature tracking includes removing points outside the masking area, occluded or disappeared points, dissimilar feature blocks, and points whose duration exceeds the limit. The feature update includes removing invalid feature blocks, updating existing feature block information, and adding new feature blocks to supplement them.

[0016] Preferably, the predicted pose and map features projected onto the current frame are input into a pre-constructed intensity and geometry joint observation model for fusion calculation to obtain the optimal pose estimate. The optimal pose estimate is used to determine the location of the degraded environment. The intensity and geometry joint observation model includes a geometry observation model and an intensity observation model. The predicted pose and map features projected onto the current frame are input into the geometry observation model and the intensity observation model for fusion to obtain the overall residual, Jacobian matrix, and measurement noise covariance matrix. Based on the overall residual, Jacobian matrix, and measurement noise covariance matrix, the error state is iteratively updated using measurement information to obtain the current optimal estimation state. The point cloud and intensity information of the current frame are added to the map with the current optimal estimation state to obtain the optimal pose estimate. The location of the degraded environment is then determined using the optimal pose estimate.

[0017] Secondly, the present invention also provides a degradation environment localization system based on point cloud intensity information, comprising: The three-dimensional point cloud intensity image preprocessing module is used to acquire the three-dimensional point cloud intensity image of the laser point cloud, map the three-dimensional point cloud intensity image into a projection intensity image through the channel, and obtain a secondary projection intensity image after image processing of the projection intensity image. The intensity gradient feature processing module is used to determine the intensity gradient features in the secondary projection intensity image by gradient calculation. After multi-level filtering of the intensity gradient features, the intensity features are obtained. After feature tracking and feature updating are performed on the intensity features in sequence, a new round of map feature points are obtained. The map feature current frame projection output module is used to input a new round of map feature points into the map intensity feature point projection model and then output the map feature current frame projection. The degradation environment localization module is used to acquire IMU data frames based on IMU measurements. The IMU data frames are used to determine the predicted pose through forward recursion and backpropagation. The predicted pose and map features of the current frame are projected into a pre-constructed intensity and geometry joint observation model for fusion calculation to obtain the optimal pose estimate. The localization of the degradation environment is determined by the optimal pose estimate.

[0018] Thirdly, the present invention also provides a mobile terminal, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the degradation environment localization method based on point cloud intensity information as described above.

[0019] Fourthly, the present invention also provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the degradation environment localization method based on point cloud intensity information as described above.

[0020] Compared with the prior art, the present invention has the following beneficial technical effects: This invention provides a localization method for degraded environments based on point cloud intensity information. In a secondary projected intensity image, intensity gradient features are determined through gradient calculation, and intensity features are obtained through multi-level filtering. This captures representative features from the point cloud intensity information. After feature tracking and updating of these intensity features, a new round of map feature points is obtained. Feature tracking maintains the correlation of features across consecutive laser frames, ensuring the continuity and consistency of feature information. The predicted pose and the current frame projection of map features are input into a constructed intensity and geometry joint observation model for fusion calculation to obtain the optimal pose estimate. The optimization process comprehensively considers multiple aspects of information, resulting in more accurate pose estimation and improved localization accuracy and robustness. This invention utilizes the inherent intensity information in laser point clouds to enhance the observability of the system in directions with insufficient geometric information through a series of processing steps, effectively solving the localization problem in degraded environments, even without external sensor assistance.

[0021] Furthermore, this invention constructs a projected intensity image from the current frame's lidar point cloud projection and performs image preprocessing operations such as normalization and filtering. Based on this, feature points with local intensity gradients are extracted, and their contribution is evaluated in conjunction with the degradation direction. Intensity feature points that can enhance the observation capability of degradation degrees of freedom are selected, and correlations are established between frames for stable tracking. Intensity matching residuals are introduced as supplementary observation terms into the SLAM backend, jointly constructing a optimization model with traditional point-to-surface geometric residuals, and completing real-time pose updates within the error state extended Kalman filter framework. During the optimization process, the intensity and geometric residuals are dynamically weighted to ensure that the system obtains effective estimation constraints in the degradation direction, avoiding pose drift or divergence. Attached Figure Description

[0022] Figure 1 This is a flowchart of the degradation environment localization method based on point cloud intensity information in an embodiment of the present invention; Figure 2 This is a framework diagram of the degradation environment localization method based on point cloud intensity information in an embodiment of the present invention; Figure 3 This is a schematic diagram of the laser radar point cloud intensity map projection imaging method in an embodiment of the present invention; Figure 4 This is a schematic diagram of the internal imaging principle of the rotating lidar in an embodiment of the present invention; Figure 5 This is a diagram showing the deinterlacing effect of the point cloud projection intensity map in an embodiment of the present invention. Figure 6 This is a comparison diagram of the intensity map before and after intensity correction in an embodiment of the present invention; Figure 7 This is a comparison image of the intensity map before and after removing the horizontal interference strips in an embodiment of the present invention; Figure 8 This is an intensity projection image gradient map in an embodiment of the present invention; Figure 9 This is a magnified view of the distribution and local area of ​​environmental intensity characteristics of the water supply pipeline in an embodiment of the present invention; Figure 10 This is a schematic diagram of the intensity features colored according to the contribution of mitigating translation degradation in an embodiment of the present invention; Figure 11 This is a schematic diagram of the intensity features colored according to the contribution of mitigating rotational degradation in an embodiment of the present invention; Figure 12 This is a schematic diagram illustrating the principle of laser point cloud distortion removal in an embodiment of the present invention; Figure 13 This is a schematic diagram illustrating the principle of strength residual calculation in an embodiment of the present invention; Figure 14 This is a schematic diagram of a degradation environment localization system based on point cloud intensity information in an embodiment of the present invention. In the figure: 1. 3D point cloud intensity image preprocessing module; 2. Intensity gradient feature processing module; 3. Map feature current frame projection output module; 4. Degraded environment localization module. Detailed Implementation

[0023] To enable those skilled in the art to better understand the present invention, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.

[0024] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this invention are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate so that the embodiments of the invention described herein can be implemented in orders other than those illustrated or described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover a non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0025] The purpose of this invention is to provide a degradation environment localization method and related equipment based on point cloud intensity information, so as to solve the technical problem of how to enhance the pose constraint capability in the degradation direction without the assistance of external sensors.

[0026] The present invention will now be described in further detail with reference to the accompanying drawings: See Figure 1 and Figure 2 In one embodiment of the present invention, a method for localizing degraded environments based on point cloud intensity information is provided, comprising: Step 1: Obtain the 3D point cloud intensity image of the laser point cloud, map the 3D point cloud intensity image into a first-level projection intensity image through the channel, and obtain a second-level projection intensity image after image processing of the projection intensity image. Specifically, the three-dimensional point cloud intensity image is mapped into a first-level projection intensity image via a channel. The three-dimensional point cloud intensity image is obtained by mapping the three-dimensional point cloud intensity image to the point cloud index projection and the true angle projection through a rotating mechanical radar projection model.

[0027] The specific process in this embodiment is as follows: The projection of LiDAR points into a two-dimensional image requires a three-dimensional to two-dimensional mapping relationship, as shown in Equation 1: (1) In Equation 1 The coordinates in the image after projection. For a frame of lidar point cloud For a point obtained from a continuous scan, a projection transformation method is needed. To perform point cloud projection, the projection method is as follows: Figure 3 As shown, C represents the image coordinate system, and P represents the coordinates of a point. It represents a projection relationship from three dimensions to two dimensions.

[0028] Taking the OS0-128-U model lidar as an example, this high-resolution rotating lidar features a ring-shaped repetitive scanning system. 128 laser heads rotate within the lidar, continuously probing the surrounding environment. Due to the lidar's ring-shaped scanning structure, a spherical projection point cloud mapping method is used to generate a two-dimensional intensity image. (The last sentence appears to be incomplete and possibly refers to spherical coordinates.) With image coordinates The transformation relationship between them is shown in Equation 2: (2) in The width of the image. For the OS0-128-U model LiDAR, the height of the image is... For 2048, It is 128. It is the azimuth angle. For pitch angle, The vertical field of view of the lidar is calculated using Equation 3: (3) in This is the maximum elevation angle for the lidar. This is the minimum elevation angle for the lidar, specifically for the OS0-128-U model lidar. It is 45°. -45° It is 90°.

[0029] The imaging principle of a rotating lidar is as follows: The internal imaging principle of the OS0-128-U rotating lidar is as follows... Figure 4As shown, the rotating lidar has a row of laser heads that can emit laser beams, as indicated by the yellow circle in the figure. This row of laser beams is mounted on a rotating structure, as indicated by the black part in the figure. The black part rotates around the black axis in the figure at a certain angular velocity in the direction of the blue arrow. During the rotation, the laser heads continuously emit laser beams outward, as indicated by the red line in the figure, and simultaneously record the emission time. When the laser beam hits an obstacle similar to a car in the figure, it is reflected. The light receiving device on the rotating mechanism receives the reflected laser and records information such as the time and intensity of the laser return.

[0030] By utilizing the laser's emission and reception times, and combining the Time of Flight (TOF) principle with the speed of light, the distance from the measurement point to the radar can be calculated. This distance information, along with the laser head's azimuth and elevation angles, allows for the calculation of the point's three-dimensional coordinates. Superimposing the point clouds obtained during each 360° rotation of the laser head yields a single frame of three-dimensional point cloud.

[0031] Projection is performed using the imaging structure and point indexing of a lidar. As can be seen from the imaging principle of a rotating lidar, one laser head rotates once and performs 2048 samples, numbering the lidar points in this circle as follows: There are a total of 128 laser heads, and the laser heads are also numbered. When calculating the projected image, these two numbers are used directly. , Therefore, the transformation from 3D point cloud to spherical coordinates through this projection is given by Equation 4: (4) In the formula, the subscript This means that the point was collected by a certain laser head. This projection method lays out each ring of points in the lidar to become a row in the image. There are a total of 128 laser heads, which form 128 rows, and finally generate an image with a resolution of 2048*128. These are the horizontal and vertical angles, respectively, based on the point index projection method; h represents the laser head serial number; It is the angle of maximum frontal field of view; It is the field of view; for .

[0032] After the initial image is constructed, the projected image needs to be further corrected by combining the structural calibration parameters provided by the LiDAR manufacturer in order to improve the spatial geometric consistency of the image and enhance the accuracy of subsequent intensity feature extraction.

[0033] Specifically, in the secondary projection intensity image obtained after image processing of the projection intensity image, the image processing process includes deinterlacing of projection image pixels, point cloud intensity calibration based on environmental geometric information, and removal of horizontal interference stripes. The process of deinterlacing projected image pixels includes the following: In the process of projecting LiDAR point clouds into two-dimensional images, because the LiDAR internal laser heads adopt a staggered left-right arrangement structure, although each round of sampling is completed by all laser heads at approximately the same time, directly mapping them into images according to the scanning output order will result in horizontal misalignment between pixel columns in the vertical direction.

[0034] This misalignment necessitates the introduction of a horizontal pixel compensation mechanism between image rows to correct the position of each row of pixels in order to restore the actual alignment.

[0035] Considering that the LiDAR has a factory calibration function, its metadata file contains a field "pixel_shift_by_row" which records the required horizontal pixel offset for each laser head. Therefore, the column index can be corrected when generating the image as follows: (5) Where Offset is the horizontal pixel offset required for each laser head, u and v are pixel coordinates, and index is the pixel index number.

[0036] The effect after applying this inter-row pixel offset compensation is as follows Figure 5 As shown, structural misalignment in the image is significantly reduced, feature boundaries are clearer, and image quality is significantly improved, which can effectively support subsequent intensity feature extraction and tracking tasks.

[0037] The point cloud intensity calibration process based on environmental geometric information is as follows: The basic principle of lidar point cloud generation is as follows: the laser head emits a laser beam and receives the reflected signal from the target. The distance to the point is calculated using the time of flight, and the three-dimensional position of the point is obtained by combining the emission angle. The ratio of the received signal intensity to the emitted signal intensity represents the intensity value of that point. The received power of the laser point can be expressed by formula 6: (6) in, This is the distance from the laser head to the point where the laser beam hits the object. The emission power of the laser beam. It is the aperture of the receiver. It is the system transfer factor. It is an atmospheric transport factor. It is the reflectivity of the reflecting object. It is the angle of incidence between the object's surface and the laser beam. To simplify the calculation, some constants are combined into a unified coefficient, and the relationship between point intensity, distance, and angle of incidence can be expressed as Equation 7: (7) in, This means all constants The constants represented by multiplication are uniformly represented.

[0038] The original intensity value is calibrated to restore reflectivity information that accurately reflects the material properties. According to Formula 8, the reflectivity of an object is related to the intensity, distance, and incident angle obtained from laser measurement. If the distance and incident angle are known, the reflectivity can be restored from the intensity.

[0039] (8) Where I is the reflection intensity of the object.

[0040] Distance can be directly calculated using three-dimensional coordinates, while the incident angle requires estimation of the normal vector of the local surface where the laser point is located. A kd-tree search method is used to obtain the set of neighboring points for each point, and the local normal vector is calculated accordingly. Referring to Equation 9, the incident angle is further estimated using Equation 10. (9) (10) Where n is the normal vector of the local surface where the laser point p is located, p is the three-dimensional coordinate of a laser point in the laser point cloud, p1 and p2 are the neighboring point sets of each point around p obtained by kd-tree search; T represents the transpose of the matrix.

[0041] Based on the estimated distance and incident angle information, the reflectivity of each laser point is calibrated before the two-dimensional image is projected, generating a calibrated intensity image, such as... Figure 6 As shown, the corrected image exhibits a more uniform brightness distribution under different distances and incident angles, which can effectively improve image quality and the stability and accuracy of subsequent intensity feature extraction.

[0042] The process of removing horizontal interference strips includes the following: The non-uniformity of the laser head elevation angle spacing in lidar systems can easily generate horizontal interference bands in the generated intensity map when using point cloud indexing-based image projection methods. This affects the image's vertical gradient and subsequent feature extraction. This phenomenon is particularly pronounced in scenarios with regular structures and a large number of planar components, such as water pipelines.

[0043] To eliminate this type of interference, a two-stage finite impulse response (FIR) filter is used for image preprocessing. First, a high-pass FIR filter with a cutoff frequency slightly lower than the frequency of the stripe interference is constructed in the vertical direction of the image to extract the stripe information. Then, to further separate the interference from the high-pass result, a low-pass FIR filter is constructed in the horizontal direction, retaining only the low-frequency components corresponding to the stripes. After two filtering steps—vertical high-pass and horizontal low-pass—the horizontal stripe interference in the image can be separated. Finally, the difference between the original intensity map and the extracted interference map is calculated to complete the stripe removal.

[0044] The effect is as follows Figure 7 As shown, this method can effectively improve image purity and edge structure clarity, providing a higher quality image foundation for subsequent intensity-based feature point extraction.

[0045] Step 2: In the secondary projection intensity image, the intensity gradient features are determined by gradient calculation. After multiple levels of filtering, the intensity gradient features are obtained. After feature tracking and feature updating are performed on the intensity features in sequence, a new round of map feature points are obtained. Specifically, in determining the intensity gradient features in the secondary projection intensity image through gradient calculation, pixel blocks are used as intensity feature regions. Intensity features are extracted within these regions based on image gradient information, and a Sobel-like operator is used to calculate the image gradient from the extracted intensity features. Based on the image gradient, horizontal and vertical convolution kernels are constructed respectively. The image is then convolved horizontally and vertically using the cv::filter2D function in OpenCV to obtain gradient maps in the corresponding directions. The average gradient map of the intensity image is obtained by averaging the absolute values ​​of the two gradient maps. Feature blocks are then extracted from the average gradient map of the intensity image to obtain the intensity gradient features. The cv::filter2D function is a function in the OpenCV image processing library used for filtering two-dimensional images.

[0046] To extract detailed features from the image, fixed-size N×N pixel blocks are used as intensity feature regions. Feature extraction is based on image gradient information. After the point cloud intensity map is generated, a Sobel-like operator is used to calculate the image gradient. Convolutional kernels in the horizontal and vertical directions are constructed respectively, and the image is convolved in the horizontal and vertical directions using the cv::filter2D function in OpenCV to obtain the gradient maps in the corresponding directions.

[0047] (11) Where, k u The horizontal convolution kernel; k v It is a vertical convolution kernel.

[0048] Then, the absolute values ​​of the two gradient maps are taken and averaged to obtain the average gradient map of the image, as shown below. Figure 8 As shown, this image enhances the edge structure, which helps in the subsequent selection and matching of feature blocks.

[0049] After obtaining the average gradient map of the intensity map and constructing the mask, feature block extraction begins. First, all pixels are sorted by gradient value from the average gradient map, and a non-maximum suppression (NMS) strategy is used to avoid dense distribution of feature points.

[0050] The specific method is as follows: Set a suppression radius parameter, select the point with the largest current gradient from the sorted pixels as the feature point, and mark the suppression region with this point as the center in the mask image; skip pixels that have been covered by the mask and continue to select the next unsuppressed point. This process continues until the number of feature points is met or all candidate points have been processed.

[0051] After feature point selection, an image patch with a side length of N×N is extracted centered on each feature point and used as an intensity feature block for subsequent tracking and matching. The extracted features are as follows: Figure 9 As shown. This strategy ensures the uniformity and representativeness of the feature distribution, laying the foundation for robust matching.

[0052] Specifically, after multiple levels of screening of the intensity gradient features, the intensity features are obtained by first screening based on distant points, boundary points, intensity anomalies and cluster points in the intensity gradient features to obtain the first-level screening results. The first-level screening results are then screened a second time by translation and rotation degradation contribution to obtain the intensity features.

[0053] In this invention, a subset that has a significant effect on mitigating degradation is selected from the detected intensity feature blocks.

[0054] When the degradation direction is obtained through geometric matching results The system then categorizes degradation types into translational degradation and rotational degradation. For each type, an index is constructed to measure the "contribution" of each feature block in the corresponding degradation direction, and the feature blocks are then filtered accordingly.

[0055] Taking translational degradation as an example, two key quantities need to be calculated: one is the principal gradient direction of the feature block in the image, which characterizes the local intensity change trend; the other is the influence of the perturbation of the 3D point cloud in the degradation direction on the image position. The principal gradient direction of the feature block is obtained by calculating its second-order image moments. The gradient covariance matrix of the local region is constructed, and the eigenvector corresponding to the largest eigenvalue is extracted as the principal gradient direction of the block, as shown in Equation 12: (12) Where W(u, v) is a weight function, and u and v are input variables; and Indicates coordinates as pixels in and The pixel gradient in the direction, that is exist and The partial derivatives in the direction are shown in Equation 13: (13) in, and It is the partial derivative of the intensity value I(u,v) in the u and v directions.

[0056] calculate Two eigenvalues and and the corresponding feature vectors and Select the eigenvector with the larger eigenvalue. This serves as the principal gradient direction for the feature block.

[0057] To evaluate the impact of 3D pose changes on image pixels, the projection relationship from 3D points to 2D pixels is analyzed, and the Jacobian matrix of this mapping is differentiated. Since the projection relationship has been formalized into a pinhole camera-like model, the image pixel changes caused by the perturbation of 3D points in the world coordinate system are modeled. The expression for the Jacobian matrix is ​​shown in Equation 14: (14) in, The Jacobian matrix representing the projection relationship from three dimensions to two dimensions; f x and f y The projection relationship is analogous to the focal point of a camera. is the actual distance of the laser point considering the internal error of the lidar; z is the height error of the laser head inside the lidar. It is the actual xy-plane distance of the laser point considering the internal error of the lidar; This is the distance from the center of rotation of the lidar to the laser head; Represents the x-coordinate of a pixel; x represents the ordinate of a pixel; x represents the abscissa of a 3D laser point; y represents the ordinate of a 3D laser point. This represents the three-dimensional coordinates of the laser point in the radar coordinate system.

[0058] Based on the perturbation concept, we define the "pixel translation degradation direction," which is the direction in which the pixel position of a 3D point in the image changes as the platform moves along the degradation direction. Let the degradation direction be... The pixel translation degradation direction is obtained by multiplying the Jacobian matrix by the degradation direction, as shown in Equation 15: (15) in, The direction of pixel translation degradation; t represents translation; x t y t z t The xyz coordinates represent the direction of translational degradation; In the direction of degradation; This represents the distance from the laser head to the object in the xy plane.

[0059] Then, combine the principal direction of the feature block gradient. To quantify the consistency between the image gradient and the degradation direction, a "translation degradation contribution coefficient" is introduced. This coefficient is defined as the absolute value of the unit projection of the feature block gradient direction onto the pixel translation degradation direction, as shown in Equation 16: (16) in, The contribution coefficient of the feature block to translation degradation; The coordinates of the maximum gradient direction of the feature block; This indicates the direction of pixel translation degradation.

[0060] The larger this coefficient, the more significant the image gradient of the feature block in the direction of geometric degradation, which is more beneficial for optimizing and compensating for degradation information. Based on this index, all feature blocks are sorted, and the best ones are selected for subsequent pose estimation.

[0061] Actual visualization effect as follows Figure 10 As shown, high-contribution feature blocks are displayed in red, while low-contribution areas are displayed in blue. It can be observed that pipe wall features perpendicular to the degradation direction are mostly high-contribution regions, while ground features parallel to the degradation direction have lower contributions, verifying the effectiveness of the proposed method.

[0062] The calculation of rotational degradation contribution is similar to that of translation, defining the pixel rotational degradation direction. It can be calculated using Equation 17: (17) in, The direction of rotational degradation; is the spatial dimension; R represents the rotational degradation direction coordinate.

[0063] The gradient direction of the feature block was obtained. and pixel rotation degradation direction Analogous to the method for handling translation degradation, a contribution coefficient of a feature block to translation degradation is defined. , The value of is defined as the absolute value of the normalized projection of the feature block gradient direction onto the pixel rotation direction, and can be calculated by Equation 18: (18) This contribution coefficient describes the extent to which the pixel gradient of the feature block can compensate for rotational degradation if the drone or unmanned vehicle rotates along the direction of rotational degradation. It can serve as a basis for selecting rotationally degraded feature blocks, and the contribution calculation results are as follows: Figure 11 As shown: Specifically, after performing feature tracking and feature updating on the intensity features in sequence to obtain a new round of map feature points, the feature tracking includes removing points outside the masking area, occluded or disappearing points, and dissimilar feature blocks. In this embodiment, a sliding window mechanism is used to manage intensity feature blocks, retaining only feature blocks with high quality and stability at the current moment. When a new frame arrives, the feature blocks retained in the previous frame are tracked first, and replaced only if they are invalid or unavailable, thereby ensuring that the entire system always maintains a set of high-quality features.

[0064] To identify unusable feature blocks, the following four categories of cases should be excluded: Points outside the masked area: If a feature block contains pixels that fall within the masked area, the image edge, or an area with abnormal intensity, the feature block is determined to be unusable.

[0065] Occlusion or vanishing point: The criterion is whether the depth difference of the same feature point in the previous and next frames exceeds a set threshold. If it exceeds the threshold, the feature point is considered invalid.

[0066] Feature block dissimilarity: The similarity between the corresponding feature blocks in the current frame and the previous frame is calculated by normalized cross-correlation (NCC). If the similarity is lower than the set threshold, it is considered a tracking failure.

[0067] Expiration of Duration Limit: The system maintains a duration parameter for each feature block. If the maximum tracking time is exceeded, the feature block will be removed to prevent outdated features from interfering with subsequent tracking.

[0068] The above mechanism constitutes a complete intensity feature block tracking process, enabling continuous utilization and maintenance of high-quality image regions.

[0069] The feature update includes removing invalid feature blocks, updating existing feature block information, and adding new feature blocks to supplement them.

[0070] After the tracking step is completed, the intensity feature block update includes the following three steps: Remove invalid feature blocks: Remove feature blocks that were determined to be unreliable in the previous stage from the map.

[0071] Update existing feature block information: For feature blocks that have been successfully tracked, update their pixel intensity values, projection positions in the current frame image, and 3D coordinates.

[0072] Add new feature blocks to supplement: If the number of valid feature blocks in the current map is less than the set total, select the parts that contribute highly to the degradation direction from the new feature blocks filtered in the current frame and fill them. If there is both translation and rotation degradation, add feature blocks that contribute to both types of degradation evenly.

[0073] The above process ensures that the map always maintains the most representative and binding set of intensity feature blocks during system operation, providing stable support for subsequent pose optimization.

[0074] Step 3: Input the new round of map feature points into the map intensity feature point projection model and output the current frame projection of the map features; Step 4: IMU data frames are acquired based on IMU measurements. The predicted pose is determined through forward recursion and backpropagation of the IMU data frames. The predicted pose and map features projected onto the current frame are input into a pre-constructed intensity and geometry joint observation model for fusion calculation to obtain the optimal pose estimate. The location of the degraded environment is determined through the optimal pose estimate. Specifically, the predicted pose and map features projected onto the current frame are input into a pre-constructed intensity and geometry joint observation model for fusion calculation to obtain the optimal pose estimate. The optimal pose estimate is used to determine the location of the degraded environment. The intensity and geometry joint observation model includes a geometric observation model and an intensity observation model. The predicted pose and map features projected onto the current frame are input into the geometric observation model and the intensity observation model for fusion to obtain the overall residual, Jacobian matrix, and measurement noise covariance matrix. Based on the overall residual, Jacobian matrix, and measurement noise covariance matrix, the error state is iteratively updated using measurement information to obtain the current optimal estimation state. The point cloud and intensity information of the current frame are added to the map with the current optimal estimation state to obtain the optimal pose estimate. The location of the degraded environment is then determined using the optimal pose estimate.

[0075] In this embodiment, an error-state Kalman filter is used for pose estimation. Considering that the system may accumulate large errors during the prediction process, and that error correction under a single point cloud observation is difficult to completely eliminate, an iterative error-state Kalman filter is further introduced to improve estimation accuracy. When each frame of laser point cloud arrives, the error-state Kalman filter iteratively corrects the error state multiple times, repeating the observation update and state correction process until the error is less than a preset threshold, at which point the iteration terminates.

[0076] The forward propagation process occurs with each incoming IMU data frame, assuming it starts from the last radar frame. Start forward propagation at any moment, define a nominal state quantity at time t , initial value The optimal state estimate after correction when the last radar frame arrived. Subsequently, before the arrival of the radar frame, the nominal state variables are updated by forward propagation using Equation 19, based on the kinematic model of the IMU derived above.

[0077] (19) in, for ; For time intervals; It is a function; This is the input for the i-th round; Real state quantity The error between the nominal state quantity and the error state quantity is defined as the error state quantity. The definition of the error state quantity is shown in Equation 20: (20) The relationship between the nominal state variables, the actual state variables, and the error state variables is shown in Equation 21: (twenty one) in, and The attitude and position of the IMU in the world coordinate system. For the speed of the IMU, and These represent the drift of the gyroscope and the drift of the accelerometer in the random walk model of the IMU, respectively. and yes and Gaussian noise, It is the gravity vector representing the position in the world coordinate system. and These are measurements from the IMU gyroscope and accelerometer. and yes and Measurement noise; R represents the rotation matrix, p is the translation matrix, v is the robot velocity, and g is the gravitational acceleration.

[0078] Each time on the nominal state quantity When updating, the error status is also considered. When updating the error state, deviation and noise need to be considered.

[0079] Use Equation 22 to update the error state: (twenty two) in, and The error state transition matrix describes... Time's up The propagation of time error state and offset with noise; xi represents the nominal state quantity, ui is the system input, and wi is the system noise.

[0080] The continuous differential equation of the error state is calculated as shown in Equation 23: (twenty three) Rewrite it in discrete form, as shown in Equation 24: (twenty four) Based on the discrete differential equation of the error state, the error state transition matrix can be obtained after simplification. Process noise influence matrix As in equations 25 and 26: (25) (26) The error state transition matrix has been completed. Process noise influence matrix The derivation of this allows for forward propagation of the nominal state and the error state, but it also requires the determination of the covariance of the error state. Propagation, initial value of covariance The optimal estimate after posterior update based on the radar in the previous frame. The forward propagation of the error state covariance is shown in Equation 27: (27) Where the subscript i represents the i-th iteration, and the superscript T represents the transpose; The covariance of process noise is used in actual calculations. Each update will reset the value to zero, so before the laser frame arrives, we only need to recursively calculate the nominal state quantity according to Equation 19 and the covariance of the error state according to Equation 27.

[0081] Because the movement of the lidar can cause different acquisition times for all laser points in the same frame of the laser point cloud, backpropagation is also necessary to remove the distortion of the laser points. Figure 12 As shown: The pose at the arrival time of each IMU frame is obtained through pose forward propagation. For each laser point, this algorithm needs to convert it from the acquisition time... Coordinate transformation to the end of the radar frame The coordinates of the time are obtained, but the acquisition times of the laser points are much more concentrated than the IMU data times, so Equation 28 is needed to obtain the time of each laser point. arrive Pose changes at any given moment: (28) In this context, the upper left subscript I represents the IMU system, k represents time k, the lower right subscript has the same meaning, i represents time i, and the hat represents the predicted value.

[0082] This formula is a recursive formula, and this algorithm finds... IMU time before and after the laser point at time 1 and The pose transformation of the IMU coordinate system relative to the radar coordinate system at these two moments has been calculated. Therefore, from... By recursively calculating backwards, the acquisition time of each laser point can be obtained. and Relative pose between IMU coordinate systems at time 1 Combined with the extrinsic parameter transformation between the IMU coordinate system and the radar coordinate system given in the Ouster radar metadata file It can Time sampling point Transformed using Equation 29 to This allows for the elimination of point cloud distortion caused by radar motion.

[0083] (29) The specific process of the geometric observation model based on point-surface feature matching in this embodiment is as follows: Record the moment when a new point cloud frame arrives. Iterative updates begin, with the initial prior pose defined as the pose in the recursively derived nominal state. and If the current number is the first In the next iteration update, the prior pose of the current iteration is... and Combine them and denote them as In the Before the next iteration begins, the distortion is first corrected to... Point usage in point clouds at different times Only by transforming to the world coordinate system can residual observations be performed with global features, assuming... In the point cloud of time The first point in the nth point The coordinates of the points are Then, by transforming it to the global coordinate system using Equation 30, the estimated position can be obtained. .

[0084] (30) In this context, the upper left subscript G represents the global coordinate system, j represents the j-th iteration, and L represents the radar coordinate system.

[0085] The transformed point cloud can now be residuald with the global map, using the distance from a point to an area as the residual. It can be found on the global map. The point closest to it is denoted as . ,in Then use this Perform plane fitting on each point, and rewrite and set the constants. By unification, this plane equation can be rewritten as Equation 31: (31) Where xyz are plane normal vectors; ABC are parameters. Found this All points should lie on or be close to the fitted plane, so the normal vector of the fitted plane can be obtained by solving the linear equations represented by Equation 32 using QR decomposition. .

[0086] (32) in, This is the normal vector of the fitted plane.

[0087] Then Normalization yields the unit normal vector of the fitted plane. : (33) Geometric observation model of the system residual Defined as a point The distance to the fitted plane is shown in Equation 34: (34) This completes the residuals of the geometric part of the observation model. The calculation of the geometric observation Jacobian matrix follows.

[0088] Ideally, after transforming the point cloud using the actual pose, each point should coincide with the map and lie on the plane fitted from the map, with the distance residual from the point to the plane being 0, as shown in Equation 35: (35) However, now due to observation noise Existence and state error Translation component and rotational components The existence of [something] introduces bias into the observations. Considering the observations' noise and state errors, and their application in [something]... Perform a first-order Taylor expansion at the given point, as shown in Equation 36: (36) in, That is, observation For error state The Jacobian matrix at 0 is expanded and derived as shown in Equation 37: (37) in, For the first In the nth iteration The observed Jacobian matrix of the nth point can then be obtained by superimposing the matrix of all points. The geometric observation Jacobian matrix of the next iteration As shown in equation 38: (38) This completes the solution for the geometric observation Jacobian matrix in the observation section.

[0089] The specific process of the intensity observation model based on point cloud intensity feature matching in this embodiment is as follows: For point When the coordinates are not integers, this invention first uses operations such as rounding down to find the integer pixel positions in the four directions of up, down, left, and right, as shown in Equation 39: (39) in, This represents rounding down. Next, using the rounded pixel values, we obtain the coordinates of the four points (top left, bottom left, top right, and bottom right) of this sub-pixel, as shown in Equation 40: (40) remember and The decimal parts are as follows: 41: (41) According to the definition of bilinear interpolation, the interpolated pixel value obtained by weighted averaging is shown in Equation 42: (42) in, This represents the intensity value of point p in the pixel coordinate system. For the horizontal gradient; This represents the vertical gradient.

[0090] The bilinear interpolation method described above superimposes the pixel values ​​of the surrounding pixels of the original pixel onto the final pixel according to a certain weighted ratio (distance weighting). This ensures high accuracy in the obtained sub-pixel intensity values, providing a foundation for subsequent residual calculations and other steps. The principle of intensity residual calculation based on the bilinear interpolation method described above is as follows: Figure 13 As shown: If the coordinates of a certain map intensity feature point are Its strength value is The global intensity feature point projection method is used to project the map intensity feature point onto the current intensity image, obtaining the pixel coordinates projected into the current image. The difference between the intensity value of the map intensity feature point and the intensity value of this point is the intensity value of the first pixel. The map intensity feature point at the th th Strength measurement residual at the next iteration As shown in equation 43: (43) The calculation of the intensity residual is now complete. Next, we will calculate the intensity observation Jacobian matrix.

[0091] The calculation of intensity Jacobian is similar to that of geometric Jacobian matrix. For feature points in the projected intensity map, it is also necessary to calculate the Jacobian matrix of the derivative of the intensity with respect to the error state of the drone or robot.

[0092] Equation 44 clearly lists the projection relationship of the laser points and rewrites it in a form similar to a pinhole camera model, so the camera-like intrinsic parameter matrix can be written out. : (44) Where h is the pixel height and w is the width. .

[0093] Next, we will calculate the Jacobian matrix of the intensity error, which is to differentiate the pixel intensity with respect to the error state. We can use the chain rule to decompose the Jacobian matrix: (45) in, This represents the robot's error state. First item By differentiating the pixel intensity value with respect to the pixel coordinates, the pixel gradient can be obtained using the surrounding pixels.

[0094] It has been clarified After the initial calculation, the pixel gradient is calculated using the finite difference method, which involves using the neighboring pixels to calculate the pixel gradient. and The gradient in the direction is calculated as shown in Equation 46: (46) In the formula , , and These are the pixels above, below, left, and right of the current pixel, respectively.

[0095] Second item The Jacobian matrix for the projection relationship has already been obtained during the feature selection stage, as shown in Equation 47: (47) Third item Let be the Jacobian matrix obtained by differentiating the three-dimensional coordinates of a point with respect to the error state.

[0096] The map intensity feature points used for projection need to be... Perform distortion transformation to The process of establishing the radar coordinate system at each moment is shown in Equation 48: (48) in, For the first In the next iteration, the IMU to global system transformation Translation components are represented in the global coordinate system; This is an estimate of the rotation matrix for the j-th iteration from the IMU coordinate system to the global coordinate system.

[0097] Map intensity feature points are obtained after transformation. Representation in IMU coordinate system As shown in equation 49: (49) Then, place the point Transform from the IMU coordinate system to the lidar coordinate system, as shown in Equation 50: (50) in, for; That is, a point with distorted intensity features. Taking the derivative of that point's position with respect to the error state, the error state contains only... and There is an impact, so we differentiate them separately. The derivative of the rotation error with respect to the measurement point is shown in Equation 51: (51) The derivative of the position error with respect to the measurement point is given by Equation 52: (52) Where R is the rotation transformation matrix, I is the IMU coordinate system, G is the global coordinate system, and L is the radar coordinate system.

[0098] In summary, the results are shown in Equation 53: (53) According to the chain rule of differentiation That is, the product of the derivatives of the above three terms. For the first In the nth iteration The observed Jacobian matrix of the nth map intensity feature point can then be obtained by superimposing all the points. The intensity observation Jacobian matrix of the next iteration As shown in equation 54: (54) The intensity observation Jacobian matrix solution for the observation section is now complete.

[0099] In this embodiment, the error state iterative update process of the fusion of geometric observation and intensity observation is as follows: Measurement noise covariance of intensity observation model As shown in equation 55: (55) Where Step is the step function, st is the feature block translation contribution, sr is the rotation contribution, tavg is the average duration of the feature block, and ts is the duration of the current feature block. The average tracking time of the feature blocks is statistically analyzed. In other words, within the time limit mentioned above, the longer the tracking time of a feature block exceeds the average tracking time, the smaller the measurement noise of the feature points in the feature block that contributes more to rotation or translation, and the greater the weight during the update. Step is the step function, st is the translation contribution of the feature block, sr is the rotation contribution, tavg is the average existence time of the feature block, and ts is the existence time of the current feature block. This represents the average tracking time for the statistically analyzed feature blocks.

[0100] The geometric observation noise covariance matrix Covariance matrix of intensity observation noise By combining these, the noise covariance matrix of the fused observation model can be obtained. As shown in equation 56: (56) in, This is geometric noise.

[0101] This completes the fusion of the geometric observation model and the intensity observation model, yielding the fused overall residual. Jacobian matrix and measurement noise covariance matrix The error state can then be iteratively updated using the measurement information.

[0102] Let the actual error state be... , No. The error state obtained in the next iteration is: The relationship between them is shown in Equation 57: (57) when When the quantity is small, it can be approximated as follows: (58) in, True error state Error state of the current iteration The Jacobian matrix at 0.

[0103] Substituting equation 59 into equation 58, we get: (59) The ESKF state estimation problem aims to obtain the current observations... In the case of solving the posterior distribution of the state error However, directly solving for the posterior distribution is quite difficult, but according to Bayes' theorem: (60) in, It is a with Since the quantities are irrelevant, the problem can be transformed into finding the maximum value of the product of the prior probability and the likelihood, which is achieved through iterative solutions. The largest .

[0104] The prior distribution and likelihood distribution of the error state can be obtained: (61) in, This is an estimate of the probability at time k; Let be the iteration of the Jacobian matrix in the j-th iteration.

[0105] As we can see, both the prior probability distribution and the likelihood distribution conform to a Gaussian distribution, so the maximum a posteriori estimation of the observations can be transformed into a least squares problem: (62) The error state of the objective function with respect to the current iteration By taking the derivative and setting it to zero, we can derive the formula for iterative updates: (63) (64) (65) in, Kalman coefficients; Let J be the transpose of the observed Jacobian of the Jth iteration; Equation 63 is the Kalman gain Equation 64 is the formula for updating the error state of the current iteration based on the calculated Kalman gain, and finally, Equation 65 is the formula for updating the nominal state quantity based on the obtained error state.

[0106] IESKF uses an IMU for forward and backward propagation before the arrival of the radar frame. After the radar frame arrives, it uses an observation model to iterate the system several times. During the iteration, the observation and update steps are repeated until the nominal state error between two updates is less than a set threshold, at which point the update stops. This is equivalent to the state error being less than the threshold, as shown in Equation 66. (66) in, The threshold value is used.

[0107] After stopping the iteration, the result can be obtained from The optimal state estimate and covariance after iterative updates of point cloud observations at each time step are calculated using Equation 67: (67) in, State estimation at iteration j+1 State estimation after j iterations State error.

[0108] The point cloud and intensity information of the current frame are added to the map based on the current best estimated state. This process can be repeated to continuously obtain the best state estimate during the movement of the drone or unmanned vehicle.

[0109] In this embodiment, by utilizing the intensity values ​​contained in the point cloud, and through steps such as image processing, feature extraction and filtering, and joint optimization, the observation constraint capability in the degradation direction is effectively improved, providing a robust and adaptable compensation mechanism for SLAM systems in geometrically degraded environments. By projecting the lidar echo intensity into a two-dimensional image and combining it with the original lidar structural parameters for deinterlacing, intensity calibration, and image stripe interference suppression, the geometric consistency and brightness uniformity of the image are significantly improved. Compared to the traditional uncalibrated intensity map construction method, the image generated by this method can more realistically reflect the reflection properties of environmental objects, providing high-quality input for subsequent feature extraction. By introducing the second-order matrix of the image to estimate the principal direction of the intensity features and combining it with the projection relationship of the influence of three-dimensional perturbation on the image gradient, the compensation capability of each feature block in the degradation direction is quantified, realizing a feature point fine-screening mechanism oriented towards the degradation direction. Compared with the traditional feature point detection method based on image gradient magnitude, this method can select observation information that is more helpful in constraining the degrees of freedom of degradation. By introducing intensity residual modeling into the IESKF optimization framework, a highly robust pose optimization of geometry-intensity joint is realized. By dynamically updating high-quality features through a feature block tracking strategy and constructing an observation model by combining image residuals from the point cloud intensity map, the system's localization stability and estimation accuracy in scenarios with weak geometric constraints are improved. Compared to the ICP optimization method that relies solely on geometric matching, this invention effectively integrates intensity information, providing the system with an additional dimension of observation constraints and achieving accurate and robust pose estimation.

[0110] Example 2 according to Figure 14 As shown, this embodiment also provides a degradation environment localization system assisted by point cloud intensity information, including: The three-dimensional point cloud intensity image preprocessing module 1 is used to acquire the three-dimensional point cloud intensity image of the laser point cloud, map the three-dimensional point cloud intensity image into a projection intensity image through the channel, and obtain a secondary projection intensity image after image processing of the projection intensity image. The intensity gradient feature processing module 2 is used to determine the intensity gradient features in the secondary projection intensity image by gradient calculation. After multi-level filtering of the intensity gradient features, the intensity features are obtained. After feature tracking and feature updating are performed on the intensity features in sequence, a new round of map feature points are obtained. The map feature current frame projection output module 3 is used to input a new round of map feature points into the map intensity feature point projection model and then output the map feature current frame projection. The degradation environment localization module 4 is used to acquire IMU data frames based on IMU measurements. The IMU data frames are used to determine the predicted pose through forward recursion and backpropagation. The predicted pose and map features of the current frame are projected into a pre-constructed intensity and geometry joint observation model for fusion calculation to obtain the optimal pose estimate. The localization of the degradation environment is determined by the optimal pose estimate.

[0111] Example 3 This embodiment also provides a mobile terminal, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the degradation environment localization method based on point cloud intensity information as described above.

[0112] Example 4 This embodiment also provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the degradation environment localization method based on point cloud intensity information as described above.

[0113] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that modifications or equivalent substitutions can still be made to the specific implementation of the present invention. Any modifications or equivalent substitutions that do not depart from the spirit and scope of the present invention should be covered within the protection scope of the present invention.

Claims

1. A degenerated environment positioning method based on point cloud intensity information assistance, characterized in that, The method comprises the following steps: obtaining a three-dimensional point cloud intensity image of a laser point cloud, mapping the three-dimensional point cloud intensity image to a first-level projection intensity image through a channel, and obtaining a second-level projection intensity image through image processing on the projection intensity image; determining intensity gradient features in the second-level projection intensity image through gradient calculation, obtaining intensity features through multi-stage screening on the intensity gradient features, and obtaining new round of map feature points through feature tracking and feature updating on the intensity features in sequence; inputting the new round of map feature points into a map intensity feature point projection model to output a current frame projection of the map features; obtaining an IMU data frame based on IMU measurement, determining a predicted pose through forward recursion and back propagation on the IMU data frame, inputting the predicted pose and the current frame projection of the map features into a pre-constructed intensity and geometry joint observation model to obtain an optimal pose estimation through fusion calculation, and determining the positioning of the degenerative environment through the optimal pose estimation.

2. The method of claim 1, wherein, In the process of mapping the three-dimensional point cloud intensity image to the first-level projection intensity image, the three-dimensional point cloud intensity image is mapped to the first-level projection intensity image through a rotating mechanical radar projection model based on point cloud index projection and real angle projection.

3. The method of claim 1, wherein, The image processing process comprises deinterlacing of the projection image pixels, point cloud intensity calibration based on environmental geometric information, and horizontal interference strip removal. The process of deinterlacing the projection image pixels comprises introducing a horizontal pixel compensation mechanism between image rows, and correcting the position of each row of pixels to restore the alignment relationship in the actual orientation. The process of point cloud intensity calibration based on environmental geometric information comprises emitting a laser beam by a laser head and receiving a target reflection signal, calculating the distance of a point through time of flight, and obtaining the three-dimensional position of the point in combination with the emission angle; the ratio of the intensity of the received signal to the emission intensity represents the intensity value of the point, the intensity value of the point is calibrated to obtain the reflectivity of the object, and the reflectivity of each laser point is calibrated through the distance and the incidence angle measured by laser before two-dimensional image projection, thereby generating a calibrated intensity image. The process of horizontal interference strip removal comprises constructing a high-pass FIR filter with a cutoff frequency lower than the strip interference frequency in the vertical direction of the image to extract the strip information; constructing a low-pass FIR filter in the horizontal direction to retain only the low-frequency components corresponding to the strip; after vertical high-pass filtering and horizontal low-pass filtering, the horizontal strip interference in the image can be separated; the original intensity image is subtracted from the extracted interference image to complete the strip removal.

4. The method of claim 1, wherein, The intensity gradient features are determined by gradient calculation in the secondary projection intensity image, including: a pixel block is used as an intensity feature region, intensity features are extracted in the intensity feature region based on image gradient information, and an image gradient is calculated by using a Sobel-like operator on the extracted intensity features; a horizontal direction and a vertical direction convolution kernel are constructed based on the image gradient, horizontal direction and vertical direction convolution is performed on the image by using a cv: filter2D function in OpenCV, a gradient image of the corresponding direction is obtained, an average gradient image of the intensity image is obtained by calculating the average value of the absolute values of the gradient images of the two directions, and intensity gradient features are obtained by feature block extraction on the average gradient image of the intensity image.

5. The method of claim 1, wherein, The intensity features are obtained after multi-stage screening of the intensity gradient features, including: one-stage screening is performed based on distant points, boundary points, intensity abnormal points and aggregation points in the intensity gradient features to obtain a one-stage screening result, and two-stage screening is performed on the one-stage screening result by using translation rotation degradation contribution to obtain the intensity features.

6. The method of claim 1, wherein, The new round of map feature points are obtained after the intensity features are sequentially subjected to feature tracking and feature updating, including: the feature tracking includes respectively eliminating points outside the mask area, occluded or disappeared points, dissimilar feature blocks and survival time exceeding limit; The feature updating includes eliminating invalid feature blocks, updating inventory feature block information and adding new feature blocks.

7. The method of claim 1, wherein, The optimal pose estimation is obtained by inputting the predicted pose and the map feature current frame projection into the pre-constructed intensity and geometric joint observation model for fusion calculation, and the localization of the degraded environment is determined by the optimal pose estimation, including: the intensity and geometric joint observation model includes a geometric observation model and an intensity observation model, the overall residual, Jacobian matrix and measurement noise covariance matrix are obtained by inputting the predicted pose and the map feature current frame projection into the geometric observation model and the intensity observation model for fusion; the error state is iteratively updated based on the overall residual, Jacobian matrix and measurement noise covariance matrix using measurement information to obtain the current optimal estimation state, the point cloud and intensity information are added to the map by using the current optimal estimation state, the optimal pose estimation is obtained, and the localization of the degraded environment is determined by the optimal pose estimation.

8. A degraded environment positioning system based on point cloud intensity information assistance, characterized in that, It includes: A three-dimensional point cloud intensity image preprocessing module is configured to obtain a three-dimensional point cloud intensity image of a laser point cloud, map the three-dimensional point cloud intensity image to a projection intensity image through channel mapping, and obtain a secondary projection intensity image by image processing on the projection intensity image; An intensity gradient feature processing module is configured to determine intensity gradient features in the secondary projection intensity image by gradient calculation, obtain intensity features after multi-stage screening of the intensity gradient features, and obtain new round of map feature points after sequentially performing feature tracking and feature updating on the intensity features; A map feature current frame projection output module is configured to output map feature current frame projection obtained by inputting the new round of map feature points into a map intensity feature point projection model. The degenerative environment positioning module is used for obtaining an IMU data frame based on IMU measurement, determining a predicted pose through forward recursion and back propagation, inputting the predicted pose and a current frame of map features into a pre-constructed intensity and geometry joint observation model for fusion calculation to obtain an optimal pose estimation, and determining the positioning of the degenerative environment through the optimal pose estimation.

9. A mobile terminal comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, The computer program is executed by the processor to implement the degenerative environment positioning method based on point cloud intensity information assistance according to any one of claims 1-7.

10. A computer-readable storage medium storing a computer program, the computer program comprising instructions that, when executed by a computer, cause the computer to perform the method of any one of claims 1 to 9. The computer program is executed by the processor to implement the degenerative environment positioning method based on point cloud intensity information assistance according to any one of claims 1-7.

Images