A laser SLAM navigation method based on bidirectional closed-loop adjustment of point cloud processing and pose optimization

Abstract

This invention proposes a laser SLAM navigation method based on bidirectional closed-loop adjustment of point cloud processing and pose optimization, comprising: using the pose uncertainty scalar of the previous frame and the loop closure importance scalar of the previous frame, a point cloud filtering method is used to filter the current frame point cloud in spherical coordinates to obtain the point cloud of the current frame that is retained; based on the point cloud of the current frame that is retained, it is determined whether the current frame is a key frame; only when the current frame is a key frame is the current frame saved to a key frame set, and based on the key frame set, a pose optimization method is used to obtain the current optimal pose and the pose uncertainty scalar of the current frame; the current optimal pose is used as the current positioning result of the navigation system.

Description

Technical Field

[0001] This invention relates to the field of laser synchronous positioning and mapping technology, and in particular to a laser SLAM navigation method with bidirectional closed-loop adjustment of point cloud processing and pose optimization. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) is a core technology in the fields of autonomous driving and intelligent robot navigation. With the continuous development of related applications, SLAM technology has become an increasingly popular research topic in order to achieve long-term, high-precision autonomous positioning and navigation in environments with limited satellite signals. LiDAR, as a highly efficient and stable sensor, plays an indispensable role in SLAM methods by emitting laser beams and receiving reflected signals to obtain 3D point cloud data of the environment. However, the large-scale point cloud data generated by LiDAR often contains a large amount of redundant point cloud data, which not only consumes a lot of computing resources, but the noise points contained within also reduce the optimization accuracy of SLAM methods. Especially in scenarios involving long-term SLAM operation or multi-robot collaborative work, maintaining a large-scale point cloud map requires significant memory resources, forcing existing systems to reduce map resolution or update frequency, thus affecting the navigation accuracy optimized by SLAM.

[0003] To alleviate computational and storage pressures and improve point cloud processing, existing research has proposed various SLAM methods that incorporate point cloud filtering and feature enhancement. However, while these methods demonstrate good performance in reducing data size, improving computational efficiency, and preserving salient geometric features, they are still inherently limited by unidirectional information flow and static parameter settings in their internal point cloud processing and pose optimization processes. On the one hand, the point cloud processing flow of such systems is usually based on fixed rules and cannot adaptively adjust according to key feedback information during the optimization process. This makes it difficult to achieve an optimal balance between data density, feature saliency, and computational load in complex scenes or dynamic motion. On the other hand, the extracted point cloud feature information also lacks systematic uncertainty modeling, making it impossible to perform fine-grained weighted optimization based on the actual quality of each observation point, thus restricting the improvement of the overall navigation accuracy of SLAM.

[0004] Therefore, in the process of autonomous driving and intelligent robot navigation, in order to effectively improve the accuracy of backend optimization, reduce the scale of point cloud data, and construct a sparse map with clear features, it is necessary to propose a laser SLAM method with bidirectional closed-loop adjustment of point cloud processing and pose optimization. Summary of the Invention

[0005] Purpose of the invention: To address the problems of wasted computational resources and limited navigation pose estimation accuracy caused by the mismatch between point cloud processing and pose optimization in existing laser SLAM methods, this invention proposes a laser SLAM navigation method with bidirectional closed-loop adjustment of point cloud processing and pose optimization.

[0006] Technical Solution: This invention proposes a laser SLAM navigation method with bidirectional closed-loop adjustment of point cloud processing and pose optimization, comprising:

[0007] Using the pose uncertainty scalar and the loop closure importance scalar of the previous frame, a point cloud filtering method is used to filter the current frame point cloud in the spherical coordinate system to obtain the point cloud that is retained in the current frame; the current frame point cloud is a non-ground point cloud that has been distorted, and each point in the current frame point cloud in the spherical coordinate system is represented by the range value, vertical angle and horizontal angle.

[0008] Based on the point cloud preserved in the current frame, determine whether the current frame is a key frame. If the current frame is a key frame only, save the current frame to the key frame set. Based on the key frame set, use the pose optimization method to obtain the current optimal pose and the pose uncertainty scalar of the current frame.

[0009] The current optimal pose is used as the current positioning result of the navigation system, and the current frame pose uncertainty scalar participates in the selection of the point cloud in the next frame.

[0010] The point cloud filtering method includes the following steps:

[0011] The current frame point cloud in spherical coordinates is projected onto a grid map. The resolution of the grid map is obtained based on the pose uncertainty scalar of the previous frame and the loop closure importance scalar of the previous frame. For each grid cell in the grid map, if at least one point is projected onto the grid cell, then the grid cell is a valid grid cell, and each valid grid cell corresponds to a reference point.

[0012] Based on the geometric properties of each effective grid cell in the current frame, the pose uncertainty scalar of the previous frame, and the loop closure importance scalar of the previous frame, calculate the comprehensive feature score of each effective grid cell in the current frame.

[0013] Based on the comprehensive feature score of each valid raster unit in the current frame, the valid raster units to be retained in the current frame are selected, and the point cloud to be retained in the current frame is obtained based on the valid raster units to be retained in the current frame.

[0014] The pose optimization method includes the following steps:

[0015] Based on the second-order differential synchronization factors of all keyframes, a global cost function is constructed; the global cost function is solved to obtain the current optimal pose and the pose uncertainty scalar of the current frame.

[0016] The second-order differential synchronization factor is obtained based on the second-order differential angle residuals of the effective grid cells in the horizontal and vertical directions.

[0017] The current frame loop closure importance scalar is obtained according to the following: The current frame loop closure importance scalar is used to select the point cloud for the next frame:

[0018] The comprehensive feature score of the retained effective raster cells in the current frame is mapped to the three-dimensional observation covariance matrix of the reference point in the effective raster cell. Multiple key frames are selected from all historical key frames within a preset time window, centered on the current frame. Candidate frames for loop closure detection are then selected from these key frames. The three-dimensional observation covariance matrix is ​​used as the optimization weight for ICP optimization. ICP optimization is performed between the current frame and the candidate frames to obtain the ICP goodness of fit. Based on the ICP goodness of fit and the ratio of the number of overlapping effective raster cells in the current frame and the candidate frames to the total number of effective raster cells in the candidate frames, the loop closure importance scalar of the current frame is obtained.

[0019] Furthermore, the projection of the current frame point cloud in the spherical coordinate system onto the grid map, wherein the resolution of the grid map is obtained based on the pose uncertainty scalar of the previous frame and the loop closure importance scalar of the previous frame, specifically includes:

[0020] Assuming a distortion-corrected set of non-ground point clouds The non-ground point cloud set after distortion correction is calculated according to the following formula. Points in the Cartesian coordinate system Transform to spherical coordinates :

[0021]

[0022] in, Indicates the distance measurement value. Indicates the vertical angle. Indicates a horizontal angle;

[0023] Let the first The vertical and horizontal resolutions of a frame raster map are denoted as follows: and The resolution adjustment coefficient is calculated using the following formula: :

[0024]

[0025] in, and These are the lower and upper limits of the adjustment coefficient, respectively. and They are respectively and The weighting coefficients, and These represent the pose uncertainty scalar and the loop closure importance scalar, respectively.

[0026] Based on the set base resolution The resolution of the current frame raster map is obtained as follows: :

[0027]

[0028]

[0029] in, Indicates rounding up;

[0030] Each effective grid cell corresponds to a reference point, including: in each effective grid cell, taking the average of the distance values ​​of all points projected onto the effective grid cell, and setting the point whose distance value is close to the average value as the reference point of the corresponding effective grid cell.

[0031] Furthermore, the calculation of the comprehensive feature score for each effective grid cell in the current frame based on the geometric attributes of each effective grid cell in the current frame, the pose uncertainty scalar of the previous frame, and the loop closure importance scalar of the previous frame specifically includes:

[0032] The geometric properties of the current frame's valid raster unit include spatial coverage area, distance measurement value, and center line-of-sight direction;

[0033] Based on the spatial coverage area of ​​each effective raster cell in the current frame, the feature evaluation index of the point cloud density within each effective raster cell in the current frame is calculated.

[0034] A ranging value sample set is formed by extracting the ranging values ​​of all valid grid cells within a preset neighborhood centered on any valid grid cell in the current frame. The median of the ranging value sample set is calculated, and the median absolute deviation is used. Based on the median, a feature evaluation index reflecting the degree of noise in local depth values ​​is calculated.

[0035] For each valid grid cell in the current frame, its horizontal and vertical gradients are estimated using the ranging values ​​of its adjacent valid grid cells. Based on the estimated horizontal and vertical gradients, a feature evaluation index is calculated as the geometric complexity of the valid grid cell.

[0036] Using IMU pre-integration, the prior relative pose of the current frame and the nearest key frame is obtained, and the translational motion direction is extracted from the prior relative pose. For each effective grid cell in the current frame, the motion perception feature evaluation index of each effective grid cell in the current frame is calculated based on the cosine alignment between its center line of sight and the translational motion direction.

[0037] Based on the pose uncertainty scalar and the loop closure importance scalar of the previous frame, all feature evaluation indicators are weighted and summed to obtain the comprehensive feature score of each effective grid cell in the current frame.

[0038] Furthermore, the mapping of the comprehensive feature score of the valid raster cells retained in the current frame to the three-dimensional observation covariance matrix of the reference points in that valid raster cell specifically includes:

[0039] Based on the comprehensive feature scores of the valid raster cells retained in the current frame, the arithmetic mean of the comprehensive feature scores of the entire column of valid raster cells in the horizontal direction is calculated to obtain the feature score in the horizontal direction; the arithmetic mean of the comprehensive feature scores of the entire row of valid raster cells in the vertical direction is calculated to obtain the feature score in the vertical direction.

[0040] Based on the difference between the maximum and minimum ranging values ​​of points within the valid grid cells retained in the current frame and the comprehensive feature score of the valid grid cells retained in the current frame, the noise of the ranging value is modeled to obtain the noise variance of the ranging value.

[0041] Based on the feature scores in the horizontal direction, the noise in the horizontal angular direction is modeled to obtain the noise variance in the horizontal angular direction;

[0042] Based on the feature scores in the vertical direction, the noise in the vertical angle direction is modeled to obtain the noise variance in the vertical angle direction;

[0043] Based on the noise variance of the ranging value, the noise variance of the horizontal angle direction, and the noise variance of the vertical angle direction, the three-dimensional observation covariance matrix of the reference points in the effective grid cell is obtained.

[0044] Furthermore, determining whether the current frame is a key frame based on the point cloud retained in the current frame includes: setting the first frame after the point cloud filtering method as the initial key frame.

[0045] For each subsequent frame after the point cloud filtering method, the relative pose change of the current frame relative to the nearest key frame is obtained based on IMU pre-integration; at the same time, the current frame grid map is transformed to the grid map coordinate system of the nearest key frame, the number of overlapping effective grid cells of the two is counted, and the amount of new information is evaluated based on the change in the number of point clouds within the overlapping effective grid cells.

[0046] A keyframe score is obtained based on the relative pose change and new information of the current frame relative to its nearest keyframe; when the keyframe score exceeds a preset threshold, the current frame is determined to be a keyframe.

[0047] Furthermore, the second-order differential synchronization factor is obtained based on the second-order differential angular residuals of the effective grid cells in the horizontal and vertical directions, specifically including:

[0048] Based on the measured angle values ​​of each effective grid cell in the previous frame of the grid map in the horizontal and vertical directions, the horizontal and vertical angle increments of the current frame grid map relative to the previous frame grid map are obtained by pre-integration between adjacent frames using the IMU.

[0049] The kinematic prediction of the angle values ​​of the effective grid cells in the current frame grid map in the horizontal and vertical directions is performed by using the horizontal angle increment and the vertical angle increment, so as to obtain the predicted angle values ​​of each effective grid cell in the current grid map in the horizontal and vertical directions.

[0050] Obtain the measured angle values ​​of each valid grid cell in the current frame of the raster map in the horizontal and vertical directions;

[0051] The difference between the measured angle and the predicted angle of the effective grid cells in the current frame of the raster map in the horizontal direction is calculated as the difference between the measured angle and the predicted angle of the effective grid cells in the horizontal direction in the previous frame of the raster map. This yields the second-order angle difference of each effective grid cell in the horizontal direction. Similarly, the difference between the measured angle and the predicted angle of the effective grid cells in the current frame of the raster map in the vertical direction is calculated as the difference between the measured angle and the predicted angle of the effective grid cells in the vertical direction in the previous frame of the raster map. This yields the second-order angle difference of each effective grid cell in the vertical direction.

[0052] The second-order differential synchronization factor is obtained based on the second-order angular difference of each effective grid cell in the horizontal direction and the second-order angular difference of each effective grid cell in the vertical direction.

[0053] Furthermore, a global cost function is constructed based on the second-order differential synchronization factors of all keyframes; solving the global cost function yields the current optimal pose and the current frame pose uncertainty scalar, specifically including:

[0054] Define the state variables for each keyframe, including the absolute pose in the world coordinate system;

[0055] A global cost function is constructed by weighted sum of squares of the lidar odometry factor, IMU pre-integration factor, loop closure factor, and second-order differential synchronization factor for all keyframes. The global cost function is then solved using a factor graph optimizer to obtain the optimal state variables for all keyframes that minimize the global cost function.

[0056] Extract the optimal state variables of the keyframe closest to the current frame stored in the factor graph optimizer, and perform IMU pre-integration on the absolute pose in the optimal state variables to obtain the current optimal pose; extract the pose uncertainty information of the keyframe closest to the current frame stored in the factor graph optimizer, and compress it into scalar form to obtain the pose uncertainty scalar.

[0057] Furthermore, the noise models for the lidar odometer factor and the loop closure factor are both obtained from the Hessian matrix after ICP optimization and convergence.

[0058] Beneficial effects: Compared with the prior art, the present invention has the following advantages:

[0059] (1) The point cloud processing method of the present invention integrates point cloud geometric information and measurement uncertainty to improve pose optimization accuracy. At the same time, pose optimization can feed back state information to dynamically adjust the point cloud processing strategy, thereby achieving an adaptive balance between navigation accuracy and computational overhead.

[0060] (2) The method of the present invention drives the adjustment of grid map resolution by using the pose uncertainty scalar and loop closure importance scalar of pose optimization feedback, and performs high-quality point cloud screening, which significantly improves the effective observation quality and navigation system stability during navigation signal degradation or critical loop closure stages.

[0061] (3) The method of the present invention utilizes the three-dimensional observation covariance matrix of the reference points in the effective grid cells to perform ICP registration and factor map noise modeling for scan tomap and loop closure detection, thereby improving the robustness and accuracy of the system pose optimization;

[0062] (4) The method of the present invention uses the angle increment of the IMU pre-integration output to perform kinematic prediction of the lidar observation angle, and constructs a second-order differential angle residual as a second-order differential synchronization factor, so that while optimizing the pose, the time offset between the lidar and the IMU can be estimated and corrected online, and the navigation and positioning error caused by the time deviation can be suppressed. Attached Figure Description

[0063] Figure 1 The flowchart shows a laser SLAM navigation method for point cloud processing and pose optimization bidirectional closed-loop adjustment proposed in an embodiment of the present invention.

[0064] Figure 2A motion trajectory diagram of sequence 08 in the KITTI dataset;

[0065] Figure 3 This is a motion trajectory diagram of sequence 06 in the KITTI dataset;

[0066] Figure 4 A motion trajectory diagram of sequence 09 in the KITTI dataset;

[0067] Figure 5 This is a comparison diagram of the original point cloud map and the point cloud map processed by the method of this embodiment of the invention. Detailed Implementation

[0068] This invention proposes a laser SLAM navigation method with bidirectional closed-loop adjustment of point cloud processing and pose optimization. By establishing a closed-loop feedback mechanism between point cloud processing and pose optimization, the grid map resolution and point cloud filtering strategy can be dynamically adjusted based on pose optimization information, thereby improving the quality and adaptability of point cloud processing. Simultaneously, the comprehensive feature score of the point cloud is modeled as an adaptive covariance matrix, and optimization weights are dynamically allocated accordingly during pose optimization, thus constructing a complete feedback loop from pose optimization to point cloud processing, significantly improving the localization and mapping accuracy of laser SLAM navigation. Furthermore, this method integrates inertial measurement unit (IMU) pre-integration information and lidar observation information for timestamp alignment and motion distortion correction, effectively improving the accuracy of data fusion and system robustness.

[0069] The technical solution of the present invention will now be further described in conjunction with the accompanying drawings and embodiments.

[0070] This invention proposes a laser SLAM navigation method with bidirectional closed-loop adjustment of point cloud processing and pose optimization, such as... Figure 1 As shown, it includes:

[0071] Step 1: Project the current frame point cloud in spherical coordinates onto the grid map. The resolution of the grid map is obtained based on the pose uncertainty scalar of the previous frame and the loop closure importance scalar of the previous frame. In this embodiment of the invention, the current frame point cloud is a non-ground point cloud that has been distorted. In the spherical coordinate system, each point in the current frame point cloud is represented by a distance value, a vertical angle, and a horizontal angle. For each grid cell in the grid map, if at least one point is projected onto the grid cell, then the grid cell is a valid grid cell. Each valid grid cell corresponds to a reference point.

[0072] As one implementation method, step 1 specifically includes:

[0073] S100: Motion distortion correction is performed on the lidar point cloud using IMU pre-integration. The distorted point cloud is then segmented and non-ground point clouds are extracted to obtain the distorted non-ground point cloud. Specific operations include:

[0074] Let the laser radar scan the first Frame start time is The scanning cycle is Within the scan of this frame, the first... Let there be points, and let their time offset relative to the start of the scan be... Then the timestamp of the lidar corresponding to that point is The time offset between the IMU timestamp and the lidar timestamp is defined to be estimated. Its value is the difference between the IMU timestamp and the lidar timestamp.

[0075] Assume the IMU at time... The measured angular velocity value is Angular velocity with zero bias The time interval between two adjacent IMU measurements is In the first laser radar scan Frame interval Within the scan period, the angular velocity measured by the IMU is pre-integrated to obtain the angle increment at any time. The specific calculation formula is as follows:

[0076]

[0077] in, Indicates the time from the start of the lidar scan to the specified time. Inside, the angle increment of the IMU in the x, y, and z directions.

[0078] For any point corresponding to the IMU timestamp At the IMU sampling times of its two adjacent frames and Above, by adjusting the angle increment and Linear interpolation is performed according to the time ratio to obtain the angle increment at that point at that moment. And use the exponential mapping to transform it into a rotation matrix. .

[0079] Assume the IMU at time... The linear acceleration measurement is The acceleration is zero bias. Gravity is In the first laser radar scan Frame interval Within the scan period, the acceleration and velocity measured by the IMU are pre-integrated to obtain the translation increment at any time. The specific calculation formula is as follows:

[0080]

[0081]

[0082] in, and These represent the time from the start of the lidar scan to the specified time. Inside, the IMU's velocity increments and translation increments in the x, y, and z directions. For a moment The rotation matrix at a given point represents the transformation from the IMU coordinate system to the world coordinate system. Similarly, for any IMU timestamp at a given point... At the IMU sampling times of its two adjacent frames and Above, through the Linear interpolation is performed according to the time ratio to obtain the translation increment of the point at that moment. .

[0083] Based on the rotation matrix described above With translation increment This can represent the first... The pose transformation matrix from a point to the starting position at any time within the frame scan period is:

[0084]

[0085] Let the first The coordinates of any point in the frame point cloud are The distortion-corrected points are obtained by compensating them in the coordinate system of the scan start time. :

[0086]

[0087] Subsequently, existing algorithms were used to perform ground segmentation on the distortion-corrected point cloud, dividing the point cloud into ground point cloud sets. Non-terrestrial point cloud collection .in It serves as input for subsequent projection onto the raster map and feature evaluation.

[0088] non-ground point cloud collection Points in the Cartesian coordinate system Transform to spherical coordinates The specific formula is as follows:

[0089]

[0090] in, This represents the distance from the point to the lidar sensor, corresponding to the ranging value in the grid map. This is the elevation angle in spherical coordinates, corresponding to the vertical angle in a raster map, and is simply called the vertical angle. This is the azimuth angle in spherical coordinates, corresponding to the horizontal angle in a raster map, and is simply called the horizontal angle.

[0091] S110: Aggregate non-terrestrial point clouds Projected onto a raster map, its resolution is determined by the pose uncertainty scalar of the previous frame. Importance scalar of loop closure from the previous frame Perform adaptive adjustments. Specifically, this includes:

[0092] Since fixed-resolution raster maps are difficult to adapt to pose changes during different stages of SLAM operation, this invention constructs a set of adaptive horizontal and vertical partitioning rules to facilitate mapping the converted point cloud onto the raster map plane. This is based on the pose uncertainty scalar... Scalar of importance of loops The resolution of the raster map is adaptively adjusted.

[0093] Let the first The vertical and horizontal resolutions of a frame raster map are denoted as follows: and Define the resolution adjustment factor as The specific calculation formula is as follows:

[0094]

[0095] in, and These are the lower and upper limits of the adjustment coefficient, respectively. and These are the weighting coefficients, based on the set base resolution. The current frame resolution is obtained as follows: The specific calculation formula is as follows:

[0096]

[0097]

[0098] in, Indicates rounding up. When or When it increases, Increasing the resolution enhances geometric representation; conversely, decreasing the resolution reduces computational overhead.

[0099] This generates a picture. A raster map, defining the first raster map. Line number The column's grid cell number is For each grid cell, if at least one point is projected onto that grid cell, then that grid cell is defined as a valid grid cell; within each valid grid cell, the distance measurements of all points projected onto that valid grid cell are... The average value is taken, and the points whose distance values ​​are close to this average value are designated as the reference points for the corresponding valid grid cells. In subsequent calculations, the distance value of each valid grid cell is defined as the distance value of its reference point.

[0100] Step 2: Calculate the comprehensive feature score of each effective grid cell in the current frame based on the geometric properties of each effective grid cell in the current frame, the pose uncertainty scalar of the previous frame, and the loop closure importance scalar of the previous frame.

[0101] As one implementation method, step 2 specifically includes:

[0102] In this embodiment of the invention, the geometric attributes of the effective grid cell in the current frame include the spatial coverage area, the ranging value, and the center line-of-sight direction.

[0103] Based on the spatial coverage area of ​​each effective raster cell in the current frame, a feature evaluation index for the point cloud density within each effective raster cell in the current frame is calculated. Specifically:

[0104] Define valid grid cells The spatial coverage area is The specific calculation formula is as follows:

[0105]

[0106] in, It is an effective grid cell The distance measurement value, Take the horizontal angular resolution of a single scan of the lidar. The elevation angle interval between adjacent vertical beams of the lidar is taken. This is the effective grid unit. Point cloud density within The calculation formula is as follows:

[0107]

[0108] Subsequently, the feature evaluation index of the point cloud density within the effective raster cell was obtained. The specific calculation formula is as follows:

[0109]

[0110] in, This indicates that the value inside the parentheses is normalized. It is a very small constant used to prevent the denominator from being 0.

[0111] A ranging value sample set is constructed by extracting the ranging values ​​of all valid grid cells within a preset neighborhood centered on any valid grid cell in the current frame. The median of the ranging value sample set is calculated, and the median absolute deviation is used. Based on the median, a feature evaluation index reflecting the degree of local depth noise is calculated. Specifically:

[0112] Extracting effective raster cells The sample set consists of the ranging values ​​of all valid grid cells within the preset neighborhood of the center. And calculate its median, denoted as . Subsequently, the Median Absolute Deviation (MAD) was used to measure the dispersion of depth values ​​in this region, in order to eliminate the influence of individual noise points on the overall evaluation. The specific calculation formula is as follows:

[0113]

[0114] Finally, the calculated results use After normalization, feature evaluation indicators reflecting the noise level of local depth values ​​are obtained. .

[0115] For each valid raster cell in the current frame, its horizontal and vertical gradients are estimated using the ranging values ​​of its adjacent valid raster cells (top, bottom, left, and right). Based on the estimated horizontal and vertical gradients, a feature evaluation index is calculated as the geometric complexity of that valid raster cell. Specifically:

[0116] For each effective grid cell, its horizontal gradient is estimated using the distance measurements of its adjacent effective grid cells (up, down, left, and right) through the central difference method. with vertical gradient The specific calculation formula for the geometric complexity of this effective grid cell is as follows:

[0117]

[0118] Finally, this indicator was used Normalization is performed to obtain the geometric complexity index score. .

[0119] The prior relative pose between the current frame and the nearest keyframe is obtained using IMU pre-integration, and the translational motion direction is extracted from the prior relative pose. For each effective grid cell in the current frame, the motion-aware feature evaluation index of each effective grid cell in the current frame is calculated based on the cosine alignment between its center line of sight and the translational motion direction. Specifically:

[0120] To better serve the optimization tasks of the SLAM system, this invention introduces a motion perception mechanism. Its core idea is to identify high-value observation regions located along the vehicle's forward direction and with long dwell times in the field of view. First, IMU pre-integration is used to obtain the prior relative pose between the current frame and the nearest keyframe. ,in It is the rotation matrix from the keyframe pose to the current frame pose. This is the translation matrix from the keyframe pose to the current frame pose. Next, the translation direction is extracted from the relative pose; the specific calculation formula is as follows:

[0121]

[0122] At the same time, define grid cells The central line of sight is The specific calculation formula is as follows:

[0123]

[0124] in, and They represent the first Column and number The center angle of the angle range corresponding to the row grid cell is determined by the preset horizontal and vertical angle coverage range. and Divide into equal parts and determine the result.

[0125] After that, Transform from the current frame coordinate system to the keyframe coordinate system and normalize to obtain... The specific calculation formula is as follows:

[0126]

[0127] Subsequently, the cosine alignment between the line of sight and the direction of motion is defined as follows: ,pass The specific calculation formula for determining whether the area lies on the forward or backward axis of the carrier is as follows:

[0128]

[0129] Considering that objects in the lateral region typically move out of the field of view quickly, making it difficult to provide stable observation constraints, this index only retains the positive region consistent with the direction of motion. An index is also introduced. As a focusing factor, it further suppresses the weights of non-positive directions, forcing backend optimization resources to tilt towards the core vision directly in front. The specific calculation formula is as follows:

[0130]

[0131] Finally, the preset distance band weight is defined as follows: The weights are designed with a high mid-range and low near- and far-range bandpass to match the optimal observation range of the SLAM algorithm. Subsequently, combined with... The final motion perception score is obtained. The specific calculation formula is as follows:

[0132]

[0133] Based on the above formula, the result is then used... Normalization process is performed to obtain .

[0134] Based on the pose uncertainty scalar and loop closure importance scalar of the previous frame, a weighted sum of all feature evaluation metrics is performed to obtain the comprehensive feature score for each valid raster cell in the current frame. Specifically:

[0135] First, based on the aforementioned four characteristic evaluation indicators to The comprehensive feature score based on the calculation is as follows:

[0136]

[0137] in This represents the weight corresponding to the i-th feature indicator.

[0138] Subsequently, two types of supplementary incentives for specific tasks are defined: degradation terms. With loop term .in, for and The summation aims to retain more sparse points and edge features to stabilize pose calculation when the system pose uncertainty is high. for and The summation aims to preserve point clouds of key regions with significant structural features and key regions along the direction of motion when loop closure occurs, in response to loop closure optimization.

[0139] Finally, the comprehensive feature score for each raster unit in the current frame is: The specific calculation formula is as follows:

[0140]

[0141] in, and These are all parameters that adjust the weighting of degenerate and cyclic terms. The score is then normalized to... The interval is obtained after normalization of the comprehensive feature score .

[0142] Step 3: Based on the comprehensive feature score of each valid raster unit in the current frame, select the valid raster units to be retained in the current frame, and obtain the point cloud to be retained in the current frame based on the valid raster units to be retained in the current frame.

[0143] As one implementation method, step 3 specifically includes:

[0144] Given a comprehensive feature scoring threshold ,like If the raster cell is not selected, it will be retained and participate in the subsequent inverse transformation; otherwise, it will be removed.

[0145] By using the inverse spherical coordinate transformation formula, all reference points in the retained effective raster cells are mapped back to three-dimensional Cartesian space, while the remaining points in the effective raster cells are discarded, thus outputting the final high-quality sparse point cloud.

[0146] Step 4: Based on the final high-quality sparse point cloud of the current frame, determine whether the current frame is a keyframe. If it is a keyframe, save the current frame to the keyframe set. One implementation method includes: setting the first frame of point cloud scanned by the LiDAR at the start as the initial keyframe. For subsequent frames, obtain the relative pose change of the current frame relative to the nearest keyframe based on IMU pre-integration; simultaneously, transform the current frame's raster map to the coordinate system of the nearest keyframe's raster map, calculate the overlap of the effective raster cells, and evaluate the amount of new information within the overlapping raster cells based on the change in the number of point clouds. Combine the pose change and the amount of new information to obtain a keyframe score; when the keyframe score exceeds a preset threshold, the current frame is determined as a keyframe and saved. In the following steps, if the current frame itself is a keyframe, then the nearest keyframe to the current frame is the current frame itself.

[0147] Step 5: Map the comprehensive feature score of the retained effective raster cells in the current frame to the 3D observation covariance matrix of the reference point in that effective raster cell. Using the current frame as the center, select multiple keyframes from all historical keyframes within a preset time window; select candidate frames for loop closure detection from these keyframes; use the 3D observation covariance matrix as the optimization weight for ICP optimization; perform ICP optimization between the current frame and candidate frames to obtain the ICP goodness of fit; based on the ICP goodness of fit and the proportion of the number of overlapping effective raster cells in the current frame and candidate frames to the total number of effective raster cells in the candidate frames, obtain the loop closure importance scalar.

[0148] As one implementation method, step 5 specifically includes:

[0149] S500: Because raster maps require mapping continuous 3D point clouds onto discrete raster cell grids, this process introduces errors due to the mapping process. At raster map resolutions... Under this rule, each point is assigned to a corresponding grid cell based on its measured distance and angle, and the average measured distance is used to represent the measured distance of all points within that valid grid cell. This process introduces bounded quantization errors. To uniformly analyze these errors, this embodiment of the invention defines... For grid cells The difference between the maximum and minimum distance values ​​of an interior point.

[0150] Comprehensive feature scoring based on grid cells For each grid cell The horizontal and vertical serial numbers are counted, for the serial number in the specified horizontal direction. Calculate the arithmetic mean of the comprehensive feature scores of the entire column of grid cells corresponding to the given number to obtain the feature score in the horizontal direction. For the specified vertical sequence number Calculate the arithmetic mean of the comprehensive feature scores of the entire row of grid cells corresponding to the given number to obtain the feature score in the vertical direction. .

[0151] For noise modeling of ranging values, the variance is defined. The score consists of the fundamental noise introduced by the quantization error of the ranging value difference and the comprehensive feature score of the grid cells. The specific calculation formula is as follows:

[0152]

[0153] The first item describes the quantization error of the distance measurement difference. The inherent quantization error introduced by the second term is addressed through the coefficient. Mapping feature scores To characterize measurement noise.

[0154] For noise modeling in the horizontal angular direction, the variance is defined. The horizontal scanning angular resolution of the lidar Introduced basic noise and horizontal feature score The composition, and the specific calculation formula are as follows:

[0155]

[0156] The first item describes the horizontal scanning angular resolution of the lidar. The inherent quantization error introduced by the second term is addressed through the coefficient. Mapping horizontal feature scores To characterize measurement noise.

[0157] For noise modeling in the vertical angular direction, the variance is defined. The elevation angle interval between adjacent vertical beams of the lidar Introduced basic noise and vertical feature scoring The composition, and the specific calculation formula are as follows:

[0158]

[0159] The first item describes the pitch angle spacing of the vertically adjacent laser radar beams. The inherent quantization error introduced by the second term is addressed through the coefficient. Mapping vertical feature scores To characterize measurement noise.

[0160] Combining the above three components, for any valid grid cell It is possible to construct its diagonal quantization error covariance matrix in spherical coordinates. The specific form is as follows:

[0161]

[0162] Finally, the Jacobian matrix from spherical coordinates to Cartesian coordinates is used. The covariance matrix Mapping to three-dimensional space yields the three-dimensional observation covariance matrix of the reference point in the effective grid cell. The specific calculation formula is as follows:

[0163]

[0164] final, This will be passed to the scan to map and loop closure detection ICP optimization and factor graph, providing an optimization weight for each point participating in pose optimization, making pose optimization more accurate.

[0165] S510: Using the current frame as the center, select multiple keyframes from all historical keyframes within a preset time window, and then select the keyframe with the highest keyframe score as the candidate frame for loop closure detection.

[0166] After determining the candidate frames for loop closure, The optimization weights are set as ICP optimization weights. A weighted point-to-surface ICP optimization algorithm is used, performing iterative optimization only between the current frame and candidate frames. The 3D distance error of the inlier matching obtained from the loop closure detection ICP is statistically calculated, its root mean square is calculated, and its reciprocal is normalized to obtain the ICP goodness of fit. .

[0167] Based on this, the instantaneous loop score is defined as follows: If a valid candidate frame is detected in the current frame, then calculate... ,in, This represents the proportion of the number of overlapping valid raster cells in the current frame and the candidate frame raster maps to the total number of valid raster cells in the candidate frame raster map; otherwise... To avoid fluctuations in the scalar value of the loop closure importance caused by abrupt changes in the loop closure signal, exponential smoothing in the time dimension is employed. The specific calculation formula is as follows:

[0168]

[0169] in, This is the time smoothing coefficient. This represents the loop closure importance scalar of the previous frame. The loop closure importance scalar works by rapidly increasing when a loop closure occurs and then slowly decreasing over several subsequent frames, thereby continuously improving the quality of the front-end point cloud processing during the critical window period of loop closure.

[0170] Step 6: Based on the second-order differential synchronization factors of all keyframes, construct a global cost function. By solving the global cost function, obtain the optimal pose and pose uncertainty scalar. In this embodiment of the invention, the second-order differential synchronization factor is obtained based on the second-order differential angle residuals of the effective grid cells in the horizontal and vertical directions.

[0171] As one implementation method, step 6 specifically includes:

[0172] S600: Define state variables, including absolute pose in the world coordinate system, velocity vectors in each direction in three-dimensional space, and IMU gyroscope and accelerometer zero bias. Specifically: Define the first... The state variables of each keyframe are .in, Indicates the first The absolute pose of each keyframe in the world coordinate system; Indicates the first The velocity vectors of each keyframe in three-dimensional space in each direction; Indicates the first The IMU gyroscope zero bias and accelerometer zero bias of each keyframe.

[0173] S610: A weighted sum of squares is calculated for the lidar odometry factor, IMU pre-integration factor, loop closure factor, and second-order differential synchronization factor across all keyframes to construct a global cost function, expressed as:

[0174]

[0175] in, This represents the weighted L2 norm. The above formula is composed of the lidar odometer factor, the IMU pre-integration factor, the loop closure factor, and the second-order differential synchronization factor.

[0176] In this embodiment of the invention, the calculation formulas for the residual term and noise model of the lidar odometry factor are as follows:

[0177]

[0178]

[0179] in, It is the relative transformation pose of adjacent keyframes, which is obtained by weighted ICP through scan to map. The point cloud of the first keyframe and the point cloud formed by the first keyframe The local map constructed from each keyframe is registered to obtain the optimal rigid body transformation matrix. The value of this transformation is .in, It is a unit array. The Hessian matrix is ​​the result of convergence of the weighted ICP scan to map. Indicates from The logarithmic mapping to its Lie algebra is used to represent rotational errors as three-dimensional vectors.

[0180] In this embodiment of the invention, the Hessian matrix at the convergence of the weighted ICP of scan to map is... The following steps will yield the following:

[0181] To construct a local point cloud map, multiple keyframes are selected from all historical keyframes within a preset time window, centered on the current frame. Further selection is then made based on the keyframes related to the current frame. The point cloud of the nearest keyframes in the plane of each keyframe is fused to form a local point cloud map, which is used as the map in scan to map.

[0182] The first The 3D observation covariance matrix of the reference points in the valid raster cells retained in each keyframe As optimization weights for ICP optimization, the residual terms of ICP optimization for point cloud frame-to-map matching (scan to map) are weighted to achieve adaptively weighted ICP optimization for point cloud frame-to-map matching (scan to map), making pose optimization more accurate. Specifically, this includes:

[0183] Let the first The source point is the point cloud of each keyframe after distortion removal. The geometric residual between the transformed local map coordinate system and the corresponding target plane for:

[0184]

[0185] in, To characterize the Lie algebra variables of this transformation, This represents the rigid body transformation matrix to be estimated from the current frame coordinate system to the map coordinate system. It is the corresponding point found in the local map point cloud that has the closest Euclidean distance to the transformed source point. for The local surface normal vector.

[0186] Then, the source point is obtained at the 1st... Covariance matrix in keyframe coordinate system And using the prior rotation matrix obtained by IMU pre-integration Will The specific calculation formula for transforming to a local map coordinate system is as follows:

[0187]

[0188] Subsequently, the transformed covariance matrix Project the residual along the normal vector direction of the target plane and calculate the variance of the residual along that normal vector. The specific calculation formula is as follows:

[0189]

[0190] Next, construct the covariance weights. This weight is inversely proportional to the projection variance, and the specific calculation formula is as follows:

[0191]

[0192] in, It is a very small constant used to prevent the denominator from being 0.

[0193] In practical optimization, it is compared with the Huber kernel robust kernel function. The weights are combined to form the final total weights, which are then substituted into the normal equation of the Gauss-Newton (LM) optimization algorithm to construct the Hessian matrix. The specific formula is as follows:

[0194]

[0195] in, For residuals relative to Lie algebra variables The Jacobian matrix. Ultimately, the ICP algorithm, through continuous iteration, will optimize to find the best value from the first... Rigid body transformation matrix from keyframe coordinate system to map coordinate system And thus obtain

[0196] In this embodiment of the invention, the calculation method of the residual term of the IMU pre-integration factor and its noise model is as follows:

[0197]

[0198] In the formula, , , They are respectively from the first From the first keyframe to the second The rotation, velocity, and position increments are obtained by pre-integrating the angular velocity and linear acceleration measured by the IMU within a keyframe time interval. and They are respectively Rotation and translation components within, For the first The first keyframe and the first The time interval between keyframes.

[0199] This is the noise covariance matrix corresponding to the IMU pre-integration measurement, used to characterize the magnitude of the measurement error accumulated during the pre-integration process, thus weighting the IMU pre-integration factor in factor map optimization. Its value is determined by the IMU sensor itself.

[0200] In this embodiment of the invention, the calculation method of the loop closure factor residual term and its noise model is as follows:

[0201]

[0202]

[0203] in, This represents the absolute pose of the candidate loop frame in the world coordinate system. and The first The optimal relative pose and Hessian matrix after weighted ICP optimization of each keyframe and its corresponding candidate frame.

[0204] In this embodiment of the invention, the residual term of the second-order difference synchronization factor is calculated using the following formula:

[0205]

[0206] in, For the first The set of valid raster units that are retained in each keyframe. For the first The number of valid raster cells retained in each keyframe.

[0207] For any valid grid cell , define the first The second-order difference residuals of each keyframe are The specific calculation formula is as follows:

[0208]

[0209] In the formula, , They represent the first The second-order difference angular residuals of the effective raster cells retained in each keyframe in the horizontal and vertical directions are calculated as follows:

[0210] Because lidar scanning has time extension and there is a time stamp deviation with IMU, it will cause systematic errors in distortion correction and pose estimation. In order to achieve accurate time alignment between lidar and IMU and suppress low-frequency timestamp drift, this embodiment of the invention adopts a second-order difference time synchronization strategy based on the IMU prediction angle and lidar observation angle.

[0211] Specifically, based on the measured angle values ​​of each valid grid cell in the previous frame of the raster map in the horizontal and vertical directions, the IMU is used to pre-integrate between adjacent frames to obtain the first... Frame raster map relative to the first Horizontal angle increment of frame raster map With vertical angle increment Then reuse and Kinematic prediction is performed on the angle values ​​of the effective grid cells in the current frame of the raster map in the horizontal and vertical directions to obtain the predicted angle values ​​of each effective grid cell in the current frame of the raster map in the horizontal and vertical directions. The specific calculation formula is as follows:

[0212]

[0213] in, and These are the predicted angle values ​​of the effective raster cells in the current frame of the raster map in the horizontal and vertical directions, respectively. and These represent the measured angle values ​​of the effective grid cells in the previous frame of the raster map in the horizontal and vertical directions, respectively.

[0214] Similarly, the measured angle values ​​of the effective grid cells in the horizontal and vertical directions are obtained from the current frame raster map in the same manner and denoted as... and Subsequently, the difference between the measured and predicted angle values ​​of the effective grid cells in the current raster map in the horizontal direction is calculated, and the difference between the measured and predicted angle values ​​of the effective grid cells in the horizontal direction in the previous frame raster map is also calculated. This yields the second-order angle difference of each effective grid cell in the horizontal direction. Similarly, the difference between the measured and predicted angle values ​​of the effective grid cells in the current raster map in the vertical direction and the difference between the measured and predicted angle values ​​of the effective grid cells in the vertical direction in the previous frame raster map is also calculated. This yields the second-order angle difference of each effective grid cell in the vertical direction. The specific calculation formula is as follows:

[0215]

[0216] In the formula, , These represent the predicted angle values ​​of the effective raster cell in the previous frame of the raster map in the horizontal and vertical directions, respectively. If the... If a frame is selected as a keyframe, then the corresponding and It will be used as a second-order difference residual in the calculation of the second-order difference synchronization factor.

[0217] It should be noted that, since there are no historical differences at the initial stage of the system, this embodiment of the invention adopts a hierarchical initialization strategy for the first two frames of the raster map: in the first frame, only the measured angle value of each valid raster unit is cached, and in the second frame, only the first-order difference between the current measurement and the prediction is cached. From the third frame onwards, the second-order difference is calculated and used to provide second-order difference factors for the backend factor map optimization.

[0218] The noise covariance matrix corresponding to the second-order difference synchronization factor is: Its value is set based on experience.

[0219] S620: The global cost function is modeled as a factor graph, and the factor graph is solved to obtain the optimal state variables that minimize the global cost function. Specifically, this includes modeling the global cost function as a factor graph and solving it using Incremental Smoothing and Mapping 2 (iSAM2). When adding new keyframes or constraints, iSAM2 only performs local linearization and incremental updates on the affected variables, thus ensuring online running efficiency. Periodic relinearization ensures the stability and accuracy of convergence. Finally, the system saves the absolute poses of all optimized keyframes as the navigation trajectory. Subsequently, the absolute pose of the latest keyframe saved in the factor graph optimizer is extracted. If the current frame is a keyframe, then this absolute pose is the current optimal pose; otherwise, IMU pre-integration is performed on the absolute pose to obtain the current optimal pose, which is then output as the real-time navigation positioning result.

[0220] Due to the time offset When the correct value is obtained, the IMU predicted angle and the lidar measured angle are aligned in time, and the second-order difference residual... Therefore, when the global cost function is optimizing the state variables... At the same time, it will also affect the time offset. Optimization is performed to achieve online time calibration between the lidar and the IMU, suppressing systematic optimization errors caused by time deviation. Time offset It will be used for distortion compensation in the point cloud preprocessing stage, and its deviation will directly affect the motion compensation accuracy based on IMU; after the time offset is calibrated online, the IMU and the lidar can be synchronized, thereby improving the geometric consistency and accuracy of the distorted point cloud.

[0221] To achieve closed-loop coupling between point cloud processing and pose state estimation, this embodiment of the invention extracts the pose uncertainty information of the latest keyframe node from the factor graph optimizer and compresses it into a scalar form as the pose uncertainty scalar. Among them, subscript This represents the sequence number of the current frame. Let the covariance matrix of the latest keyframe pose output by the factor graph optimizer under the Lie algebra perturbation be... , here Indicates the sequence number of the keyframe.

[0222] in, It can be understood by dividing it into translation and rotation: the translation part corresponds to the uncertainty of three-dimensional position perturbation, and the rotation part corresponds to the uncertainty of attitude perturbation.

[0223] Considering that the primary concern in ground-based vehicle scenarios is the stability of in-plane drift and yaw angle, this embodiment of the invention only calculates the uncertainties corresponding to in-plane drift and yaw angle. Let the covariance submatrix corresponding to in-plane drift be denoted as... And denot the variance of the yaw angle as Both take values ​​from the covariance matrix. Based on this, the planar drift is calculated respectively. With yaw angle The standard deviation of is calculated using the following formula:

[0224]

[0225]

[0226] in, Represents the trace operation of a matrix.

[0227] Subsequently, the standard deviation was normalized using a preset empirical threshold. The plane drift uncertainty term after normalization is denoted as... The yaw uncertainty term after normalization is .

[0228] Finally, the two are weighted and fused to obtain the pose uncertainty scalar. The specific calculation method is as follows:

[0229]

[0230] in, and They are respectively and The calculated weights.

[0231] pose uncertainty scalar The pose state feedback is transmitted to the point cloud processing to participate in the adaptive fusion process of comprehensive feature scoring. This improves the attention to key observation areas when the system pose is unstable or the degradation trend is enhanced, thus realizing closed-loop adjustment between point cloud processing and pose optimization.

[0232] To verify the effectiveness of the present invention, experiments were conducted on the existing KITTI dataset using the above-mentioned SLAM system as a benchmark, and the currently advanced factor graph backend optimization-based SLAM algorithm LIO-SAM was selected as a control method. Figure 2 , Figure 3 and Figure 4The results of comparing the optimized pose trajectories of the LIO-SAM method and the method proposed in this invention on KITTI 08, 06, and 09 sequences are presented. Experiments show that the trajectory generated by the method proposed in this invention is closer to the true trajectory, while the LIO-SAM method exhibits significant offsets in some regions. Figure 5 The paper presents a comparison between the original point cloud and the point cloud after front-end filtering using the method proposed in this invention. The original point cloud contains 65,492 points, while the point cloud after front-end filtering retains only 19,875 points. The point cloud map processed by the method proposed in this invention effectively enhances the expression of feature information while maintaining the integrity of the original environmental information, and significantly reduces the interference of noise points and artifacts. Table 1 presents a performance comparison between the method of this invention and the LIO-SAM method on three KITTI sequences. Experimental data show that compared with the LIO-SAM method, the method of this invention consumes less memory in single-frame point cloud processing and pose optimization, with a reduction of up to 33.6%. Furthermore, the SLAM system proposed in this invention significantly outperforms the LIO-SAM method in terms of absolute trajectory error (RMSE).

[0233] Table 1. Comparative experimental results on the KITTI dataset.

[0234] .

Claims

1. A method of laser SLAM navigation with bidirectional closed-loop adjustment of point cloud processing and pose optimization, characterized in that: include: Using the pose uncertainty scalar and the loop closure importance scalar of the previous frame, a point cloud filtering method is used to filter the current frame point cloud in the spherical coordinate system to obtain the point cloud that is retained in the current frame; the current frame point cloud is a non-ground point cloud that has been distorted, and each point in the current frame point cloud in the spherical coordinate system is represented by the range value, vertical angle and horizontal angle. Based on the point cloud preserved in the current frame, determine whether the current frame is a key frame. If the current frame is a key frame only, save the current frame to the key frame set. Based on the key frame set, use the pose optimization method to obtain the current optimal pose and the pose uncertainty scalar of the current frame. The current optimal pose is used as the current positioning result of the navigation system, and the current frame pose uncertainty scalar participates in the selection of the point cloud in the next frame. The point cloud filtering method includes the following steps: The current frame point cloud in spherical coordinates is projected onto a grid map. The resolution of the grid map is obtained based on the pose uncertainty scalar of the previous frame and the loop closure importance scalar of the previous frame. For each grid cell in the grid map, if at least one point is projected onto the grid cell, then the grid cell is a valid grid cell, and each valid grid cell corresponds to a reference point. Based on the geometric properties of each effective grid cell in the current frame, the pose uncertainty scalar of the previous frame, and the loop closure importance scalar of the previous frame, calculate the comprehensive feature score of each effective grid cell in the current frame. Based on the comprehensive feature score of each valid raster unit in the current frame, the valid raster units to be retained in the current frame are selected, and the point cloud to be retained in the current frame is obtained based on the valid raster units to be retained in the current frame. The pose optimization method includes the following steps: Based on the second-order differential synchronization factors of all keyframes, a global cost function is constructed; the global cost function is solved to obtain the current optimal pose and the pose uncertainty scalar of the current frame. The second-order differential synchronization factor is obtained based on the second-order differential angle residuals of the effective grid cells in the horizontal and vertical directions. The current frame loop closure importance scalar is obtained according to the following: The current frame loop closure importance scalar is used to select the point cloud for the next frame: The comprehensive feature score of the retained effective raster cells in the current frame is mapped to the three-dimensional observation covariance matrix of the reference point in the effective raster cell. Multiple key frames are selected from all historical key frames within a preset time window, centered on the current frame. Candidate frames for loop closure detection are then selected from these key frames. The three-dimensional observation covariance matrix is ​​used as the optimization weight for ICP optimization. ICP optimization is performed between the current frame and the candidate frames to obtain the ICP goodness of fit. Based on the ICP goodness of fit and the ratio of the number of overlapping effective raster cells in the current frame and the candidate frames to the total number of effective raster cells in the candidate frames, the loop closure importance scalar of the current frame is obtained. 2.The laser SLAM navigation method of claim 1, wherein: The projection of the current frame point cloud in the spherical coordinate system onto a grid map, wherein the resolution of the grid map is obtained based on the pose uncertainty scalar of the previous frame and the loop closure importance scalar of the previous frame, specifically includes: set of undistorted non-ground points The points in the set of undistorted non-ground points are converted from a Cartesian coordinate system to a spherical coordinate system according to the following formula: wherein, represents a range value, represents a vertical angle, represents a horizontal angle; Set the first The vertical and horizontal resolutions of the frame grid map are respectively denoted as And The resolution adjustment coefficient is calculated according to the following formula : in, and These are the lower and upper limits of the adjustment coefficient, respectively. and They are respectively and The weighting coefficients, and These represent the pose uncertainty scalar and the loop closure importance scalar, respectively. Based on the set base resolution The resolution of the current frame raster map is obtained as follows: : in, Indicates rounding up; Each effective grid cell corresponds to a reference point, including: in each effective grid cell, taking the average of the distance values ​​of all points projected onto the effective grid cell, and setting the point whose distance value is close to the average value as the reference point of the corresponding effective grid cell.

3. The laser SLAM navigation method based on point cloud processing and pose optimization with bidirectional closed-loop adjustment according to claim 1, characterized in that: The calculation of the comprehensive feature score for each valid raster cell in the current frame based on the geometric attributes of each valid raster cell in the current frame, the pose uncertainty scalar of the previous frame, and the loop closure importance scalar of the previous frame specifically includes: The geometric properties of the current frame's valid raster unit include spatial coverage area, ranging value, and center line-of-sight direction; Based on the spatial coverage area of ​​each effective raster cell in the current frame, the feature evaluation index of the point cloud density within each effective raster cell in the current frame is calculated. A ranging value sample set is formed by extracting the ranging values ​​of all valid grid cells within a preset neighborhood centered on any valid grid cell in the current frame. The median of the ranging value sample set is calculated, and the median absolute deviation is used. Based on the median, a feature evaluation index reflecting the degree of noise in local depth values ​​is calculated. For each valid grid cell in the current frame, its horizontal and vertical gradients are estimated using the ranging values ​​of its adjacent valid grid cells. Based on the estimated horizontal and vertical gradients, a feature evaluation index is calculated as the geometric complexity of the valid grid cell. Using IMU pre-integration, the prior relative pose of the current frame and the nearest key frame is obtained, and the translational motion direction is extracted from the prior relative pose. For each effective grid cell in the current frame, the motion perception feature evaluation index of each effective grid cell in the current frame is calculated based on the cosine alignment between its center line of sight and the translational motion direction. Based on the pose uncertainty scalar and the loop closure importance scalar of the previous frame, all feature evaluation indicators are weighted and summed to obtain the comprehensive feature score of each effective grid cell in the current frame.

4. The laser SLAM navigation method based on point cloud processing and pose optimization with bidirectional closed-loop adjustment according to claim 1, characterized in that: The mapping of the comprehensive feature score of the valid raster cells retained in the current frame to the three-dimensional observation covariance matrix of the reference points in the valid raster cells specifically includes: Based on the comprehensive feature scores of the valid raster cells retained in the current frame, the arithmetic mean of the comprehensive feature scores of the entire column of valid raster cells in the horizontal direction is calculated to obtain the feature score in the horizontal direction; the arithmetic mean of the comprehensive feature scores of the entire row of valid raster cells in the vertical direction is calculated to obtain the feature score in the vertical direction. Based on the difference between the maximum and minimum ranging values ​​of points within the valid grid cells retained in the current frame and the comprehensive feature score of the valid grid cells retained in the current frame, the noise of the ranging value is modeled to obtain the noise variance of the ranging value. Based on the feature scores in the horizontal direction, the noise in the horizontal angular direction is modeled to obtain the noise variance in the horizontal angular direction; Based on the feature scores in the vertical direction, the noise in the vertical angle direction is modeled to obtain the noise variance in the vertical angle direction; Based on the noise variance of the ranging value, the noise variance of the horizontal angle direction, and the noise variance of the vertical angle direction, the three-dimensional observation covariance matrix of the reference points in the effective grid cell is obtained.

5. The laser SLAM navigation method based on point cloud processing and pose optimization with bidirectional closed-loop adjustment according to claim 1, characterized in that: The step of determining whether the current frame is a keyframe based on the point cloud preserved in the current frame includes: Set the first frame after the point cloud filtering method as the initial keyframe; For each subsequent frame after the point cloud filtering method, the relative pose change of the current frame relative to the nearest key frame is obtained based on IMU pre-integration; at the same time, the current frame grid map is transformed to the grid map coordinate system of the nearest key frame, the number of overlapping effective grid cells of the two is counted, and the amount of new information is evaluated based on the change in the number of point clouds within the overlapping effective grid cells. A keyframe score is obtained based on the relative pose change and new information of the current frame relative to its nearest keyframe; when the keyframe score exceeds a preset threshold, the current frame is determined to be a keyframe.

6. The laser SLAM navigation method based on point cloud processing and pose optimization with bidirectional closed-loop adjustment according to claim 1, characterized in that: The second-order differential synchronization factor is obtained based on the second-order differential angular residuals of the effective grid cells in the horizontal and vertical directions, specifically including: Based on the measured angle values ​​of each effective grid cell in the previous frame of the grid map in the horizontal and vertical directions, the horizontal and vertical angle increments of the current frame grid map relative to the previous frame grid map are obtained by pre-integration between adjacent frames using the IMU. The kinematic prediction of the angle values ​​of the effective grid cells in the current frame grid map in the horizontal and vertical directions is performed by using the horizontal angle increment and the vertical angle increment, so as to obtain the predicted angle values ​​of each effective grid cell in the current grid map in the horizontal and vertical directions. Obtain the measured angle values ​​of each valid grid cell in the current frame of the raster map in the horizontal and vertical directions; The difference between the measured angle and the predicted angle of the effective grid cells in the current frame of the raster map in the horizontal direction is calculated as the difference between the measured angle and the predicted angle of the effective grid cells in the horizontal direction in the previous frame of the raster map. This yields the second-order angle difference of each effective grid cell in the horizontal direction. Similarly, the difference between the measured angle and the predicted angle of the effective grid cells in the current frame of the raster map in the vertical direction is calculated as the difference between the measured angle and the predicted angle of the effective grid cells in the vertical direction in the previous frame of the raster map. This yields the second-order angle difference of each effective grid cell in the vertical direction. The second-order differential synchronization factor is obtained based on the second-order angular difference of each effective grid cell in the horizontal direction and the second-order angular difference of each effective grid cell in the vertical direction.

7. The laser SLAM navigation method based on point cloud processing and pose optimization with bidirectional closed-loop adjustment according to claim 1, characterized in that: A global cost function is constructed based on the second-order differential synchronization factors of all keyframes; solving the global cost function yields the current optimal pose and the current frame pose uncertainty scalar, specifically including: Define the state variables for each keyframe, including the absolute pose in the world coordinate system; A global cost function is constructed by weighted sum of squares of the lidar odometry factor, IMU pre-integration factor, loop closure factor, and second-order differential synchronization factor for all keyframes. The global cost function is then solved using a factor graph optimizer to obtain the optimal state variables for all keyframes that minimize the global cost function. Extract the optimal state variables of the keyframe closest to the current frame stored in the factor graph optimizer, and perform IMU pre-integration on the absolute pose in the optimal state variables to obtain the current optimal pose; extract the pose uncertainty information of the keyframe closest to the current frame stored in the factor graph optimizer, and compress it into scalar form to obtain the pose uncertainty scalar.

8. The laser SLAM navigation method based on point cloud processing and pose optimization with bidirectional closed-loop adjustment according to claim 7, characterized in that: The noise models for the lidar odometer factor and the loop closure factor are both obtained from the Hessian matrix after ICP optimization and convergence.

Images