A long-term robust positioning method for scene changes

Abstract

This invention discloses a long-term robust localization method accommodating scene changes, relating to the field of autonomous navigation and localization technology for mobile robots. The method comprises three stages: a front-end LiDAR inertial odometry stage, which updates the initial pose and distortion-free point cloud through IMU motion integration and ESIKF updates; a fusion map construction stage, which uses map matching scores and pose consistency as dual criteria, combined with incremental octrees to dynamically insert and delete point clouds, generating a fusion map; and a LiDAR matching optimization stage, which extracts ground point, corner point, and area point features, and outputs high-precision localization results through KNN search and L-M optimization. This invention can simultaneously address changes in the environment both inside and outside the prior map area, improving long-term localization robustness and accuracy while reducing computational resource consumption, making it suitable for scenarios such as logistics delivery and autonomous driving.

Description

Technical Field

[0001] This invention relates to the field of autonomous navigation and positioning technology for mobile robots, specifically a long-term robust positioning method for changing scenarios. Background Technology

[0002] In practical applications such as logistics and delivery, autonomous driving, service cleaning, and industrial exploration, the core prerequisite for mobile robots to achieve autonomous navigation is to achieve accurate and robust localization in complex and dynamically changing environments. Simultaneous Localization and Mapping (SLAM) technology, as a key support for the localization module, can output the sensor's position and orientation in a reference coordinate system in real time, simultaneously save environmental information, and build a globally unified map, providing core data support for robot localization.

[0003] However, the aforementioned application scenarios generally feature large-scale coverage, repeated location visits, and dynamic environmental changes. If the current environment map is rebuilt every time the SLAM system is started, it will not only waste a significant amount of system resources but also easily lead to cumulative drift in localization, severely impacting localization stability. Therefore, the industry typically adopts a "build an environment map first, then perform localization based on that map" approach, which reduces resource consumption and improves localization accuracy. However, during long-term robot operation, the environment inevitably undergoes various changes, including natural environmental changes due to seasonal variations and positional shifts of semi-static objects such as warehouse goods handling and construction scaffolding erection. These changes can cause the pre-built map of the SLAM system to become invalid during the real-time localization phase, and may even result in the map becoming outdated before its construction is complete, posing a serious challenge to the robot's long-term stable localization.

[0004] Existing long-term localization schemes still have significant shortcomings and are difficult to meet the localization needs in complex scenarios. On the one hand, some schemes cannot comprehensively cope with various map changes. For example, the LiLoc framework judges environmental changes only by calculating the overlap range between the current frame's LiDAR point cloud and the prior sub-map. It can only identify scenarios where the robot is outside the prior map range and cannot perceive environmental changes within the prior map range. Moreover, its method of enhancing prior constraints by allocating scan matching factor weights during the Joint Factor Graph Optimization (JFGO) process relies solely on prior map information, which can easily lead to localization failure when the environment within the map range changes significantly. On the other hand, some schemes fail to fully integrate prior map and current observation information. For example, although the ROLL framework can judge map changes by the inlier matching rate and adopt a temporary mapping strategy to cope with situations such as prior map changes and exceeding the map range, the temporary mapping strategy requires switching from map-based localization to incremental localization. It only uses current observations for matching localization, which can easily lead to deviation from the prior map coordinate system due to cumulative drift.

[0005] In summary, existing technologies struggle to handle multiple complex scenarios simultaneously, such as environmental changes within the scope of the prior map and situations beyond the prior map's boundaries. Furthermore, they fall short in balancing resource utilization efficiency and positioning accuracy. There is an urgent need for a long-term, high-precision, robust positioning method that can adapt to various scenario changes and fully integrate prior and real-time observation information. Summary of the Invention

[0006] The purpose of this invention is to provide a long-term robust localization method for scene changes, so as to solve the problems mentioned in the background art.

[0007] To achieve the above objectives, the present invention provides the following technical solution: a long-term robust localization method oriented towards scene changes, comprising the following steps: Step S1: Output the initial pose and the distorted point cloud through the state estimation and point cloud distortion correction module and the observation update module; Step S2: Through the admission judgment module and the dynamic insertion and deletion module, output the merged map formed by combining the static map and the dynamic map; Step S3: Starting from the initial pose output in step S1 and using the fused map output in step S2 as the data source, iterative optimization is performed through the feature extraction module and the matching optimization module until convergence, and then a high-precision positioning result is output.

[0008] Preferably, the working process of the state estimation and point cloud distortion correction module in step S1 is as follows: Starting from the pose updated by the previous observation residual, motion integration is performed using the acceleration and angular velocity provided by the IMU data to obtain the pose of each point in the lidar scan frame and the pose of the last point. By transforming the extrinsic parameters between the IMU and the lidar, all points in the current lidar scan frame are projected to the end of the scan frame, thus completing the point cloud distortion removal and obtaining the lidar prior pose of the current frame. Preferably, the operation of the observation update module in step S1 is as follows: Based on the prior pose of the LiDAR, the points in the current scan frame are projected onto the map coordinate system to complete the matching between the scan frame and the map and to construct the point-area distance constraint. The state is updated based on the error state iterative Kalman filter, and the initial pose is output for use in step S3.

[0009] Preferably, the specific process for constructing the point-to-surface distance constraint is as follows: Traverse the points in the current LiDAR scan frame and project them onto the global map; The KNN search is used to find the n nearest neighbors of each point, the covariance matrix of the n points is calculated and the plane normal vector is obtained by SVD eigenvalue decomposition. Select the closest point among n points and calculate the point-to-surface distance residual using the vector dot product.

[0010] Preferably, the operation of the admission judgment module in step S2 is as follows: After the LiDAR matching optimization converges, the point cloud of the current scan frame is projected onto the map coordinate system using the optimized pose. KNN search is performed on each point. If the distance to the nearest point is less than the threshold of the distance to the inner point, the matching is considered successful. The proportion of successfully matched points to the total number of points is used as the map matching score. The relative pose error (RPE) between the LiDAR matched optimized pose and the LiDAR inertial odometry pose was calculated, and the pose consistency was determined to be good. Based on the map matching score and pose consistency, a dynamic map is inserted and merged with the static map to form a fused map.

[0011] Preferably, the working process of the dynamic insertion and deletion module in step S2 is as follows: An incremental octree is used as the map data structure to maintain a local map centered on the robot's current position. Based on the admission criteria and the distance between two adjacent frames, the point cloud of the current frame is inserted into the map as a keyframe; When the robot translates beyond a set distance in any of the X, Y, or Z axes, the new local map boundary and the offset from the old map are calculated. A temporary bounding box is set according to the positive or negative offset, and points within the box are deleted. The center coordinates of the local map are then updated to the current robot position.

[0012] Preferably, the working process of the feature extraction module in step S3 is as follows: The distortion-free point cloud is projected into a distance image, and the row index, column index and distance value of each point are saved. Traverse each column of points in the distance image, calculate the cosine of the angle between the vector formed by adjacent points in the same column and the ground normal vector, and mark it as a ground point if the value is less than a preset value; For non-ground points, the curvature of the point cloud is calculated using the curvature formula. Points with larger curvature are classified as corner points, and points with smaller curvature are classified as surface points. The curvature formula is: , where represents the curvature of the i-th point in the scan line, S is the set of n points to the left and right of the i-th point, and is the distance between the i-th point and the radar center.

[0013] Preferably, the working process of the matching optimization module in step S3 is as follows: Based on the previous matching optimization results and the pose increment of the lidar inertial odometry, the initial pose of the current frame is estimated using a formula; The keyframes within the merged map are used to construct local corner point maps, local area point maps, and local ground point maps, which are then stored in three incremental octrees. Traverse the corner points, face points, and ground points of the current frame, find the corresponding points through KNN search, and construct point-line matching pairs and point-face matching pairs. After iterative convergence based on LM optimization, output high-precision positioning results.

[0014] Compared with existing technologies, the beneficial effects of this invention are as follows: This invention outputs accurate initial pose and distortion-free point cloud through the front-end LiDAR inertial odometry stage, providing a reliable foundation for localization; in the map construction stage, it achieves accurate dynamic map insertion by using map matching scores and pose consistency as dual judgments, and maintains the local map using an incremental octree data structure to dynamically delete point clouds, efficiently integrating prior maps and real-time observation information; in the LiDAR matching optimization stage, it extracts multiple types of feature points and optimizes them through LM iteration, ultimately achieving multi-scene adaptation. This method effectively addresses complex situations such as environmental changes within the prior map range and exceeding the prior map range, solving the problem of insufficient localization robustness in existing solutions. Furthermore, it reduces computational resource consumption through local map maintenance and keyframe filtering, while improving long-term localization accuracy, providing stable and reliable technical support for the autonomous navigation of mobile robots in scenarios such as logistics delivery and autonomous driving. Attached Figure Description

[0015] Figure 1 is an overall flowchart of the present invention; Figure 2 is a schematic diagram of the dynamic insertion and deletion module. Detailed Implementation

[0016] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. 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 are within the scope of protection of the present invention.

[0017] Please see Figures 1 to 2This invention provides a technical solution: a long-term robust localization method oriented towards scene changes, comprising a front-end LiDAR inertial odometry stage (step S1), a fusion map construction stage (step S2), and a LiDAR matching optimization stage (step S3). Step S1 includes a state estimation and point cloud distortion correction module and an observation update module, outputting an initial pose and a distorted point cloud; Step S2 includes an admission judgment module and a dynamic insertion and deletion module, outputting a fusion map formed by merging static and dynamic maps; Step S3 includes a feature extraction module and a matching optimization module, using the pose provided by the LiDAR inertial odometry stage as the initial pose, using the fusion map formed by merging static and dynamic maps as the map data source for LiDAR matching optimization, iterating until convergence, and outputting a high-precision localization result.

[0018] In one embodiment of the present invention, the working process of the state estimation and point cloud distortion correction module in step S1 is as follows: Starting from the pose updated by the previous observation residual, motion integration is performed using the acceleration and angular velocity provided by the IMU data to obtain the pose of each point in the lidar scan frame and the pose of the last point. By transforming the extrinsic parameters between the IMU and the lidar, all points in the current lidar scan frame are projected to the end of the scan frame, thus completing the point cloud distortion removal and obtaining the lidar prior pose of the current frame.

[0019] In this embodiment, the pose updated from the previous observation residual is used as the starting point. Then, motion integration is performed based on the acceleration and angular velocity provided by the received IMU data to obtain the pose of each point in the lidar scan frame. pose of the last point in the lidar scan frame Define the extrinsic parameters between the lidar and the IMU as follows: By using this extrinsic parameter transformation, all points in the current lidar scan frame are projected to the end of the lidar scan frame, thus completing the lidar point cloud distortion removal and obtaining the lidar prior pose of the current frame. In one embodiment of the present invention, the operation of the observation update module in step S1 is as follows: Based on the prior pose of the LiDAR, the points in the current scan frame are projected onto the map coordinate system to complete the matching between the scan frame and the map and to construct the point-area distance constraint. The state is updated based on the error state iterative Kalman filter, and the initial pose is output for use in step S3.

[0020] In this embodiment, after obtaining the prior pose of the lidar, motion observation is provided through the lidar point cloud for state updates. For the point cloud data observation provided by the lidar, the points in the current scan frame are projected onto the map coordinate system based on the obtained lidar prior pose, completing the matching of the scan frame and the map, and constructing point-area distance constraints.

[0021] In one embodiment of the present invention, the specific process of constructing point-to-surface distance constraints is as follows: Traverse the points in the current LiDAR scan frame and project them onto the global map; The KNN search is used to find the n nearest neighbors of each point, the covariance matrix of the n points is calculated and the plane normal vector is obtained by SVD eigenvalue decomposition. Select the closest point among n points and calculate the point-to-surface distance residual using the vector dot product.

[0022] Specifically, it iterates through the points in the current lidar scan frame. Projected onto the global map: The KNN search is used to find the n nearest neighbors of the current point in the map, the covariance matrix of these n points is calculated, eigenvalue decomposition is performed using SVD, and the plane normal vector is calculated based on the eigenvalues ​​and eigenvectors. Select n points with distance nearest point The point-to-surface distance residual is calculated using vector dot product. Then, the state is updated based on the Error-State Iterative Kalman Filter (ESIKF), and the initial pose is output for use in the lidar matching optimization stage.

[0023] In one embodiment of the present invention, the operation of the admission judgment module in step S2 is as follows: After the LiDAR matching optimization converges, the optimized pose is used to project the point cloud of the current scan frame onto the map coordinate system. A KNN search is performed on each point. If the nearest neighbor distance is less than a set threshold for the interior point distance, the match is considered successful. The proportion of successfully matched points to the total number of points is counted as the map matching score. For example, a successful match is considered to be achieved if the nearest neighbor distance is less than the interior point distance threshold of 0.2m. The relative pose error (RPE) between the LiDAR-matched optimized pose and the LiDAR-inertial odometry pose was calculated, and good pose consistency was determined. Good pose consistency was determined when the translational error of the RPE was ≤0.1m and the rotational error was ≤1°.

[0024] Based on the map matching score and pose consistency, for example, when the map matching score is below 70% but the pose consistency is good, dynamic map insertion is triggered and merged with the static map into a fused map.

[0025] In this example, the admission criteria include two judgments: map matching score judgment and pose consistency judgment. The map matching score represents the degree of matching between the current LiDAR frame and the map. The calculation process is as follows: after each LiDAR matching optimization phase converges, the points in the current LiDAR scan frame are projected onto the map coordinate system using the optimized pose. Then, a KNN search is performed on each point in the current LiDAR scan frame to find the nearest neighbor. If the distance to the found nearest neighbor is less than the inlier distance threshold of 0.2m, the match is considered successful; otherwise, the match is considered unsuccessful. The proportion of successfully matched points in the current LiDAR frame to the total number of points is counted as the map matching score. Pose consistency refers to the RPE (Relative Pose Error) between the LiDAR matching optimized pose and the LiDAR inertial odometry pose. The LiDAR matching optimized poses of two consecutive frames are stored separately. , With lidar inertial odometry pose , ,in It is the pose of the previous frame, and it is the pose of the current frame.

[0026] Specifically, the pose difference between the LiDAR matching optimization and the LiDAR inertial odometry are calculated separately for two consecutive frames: Then calculate the relative pose error RPE: If the translation error of the RPE is less than the threshold of 0.1m and the rotation error is less than the threshold of 1°, the pose consistency is considered good. When the map matching score is below the threshold of 70% and the pose consistency is good, a dynamic map is inserted and merged with the static map to form a fused map.

[0027] In one embodiment of the present invention, the operation of the dynamic insertion and deletion module in step S2 is as follows: An incremental octree is used as the map data structure to maintain a local map centered on the robot's current location, with each dimension and height being 100m. If the admission criteria are met and the distance between two adjacent frames is ≥0.2m, the point cloud of the current frame will be inserted into the map as a keyframe. When the robot translates more than 0.5m in any of the X, Y, or Z axes, calculate the boundary of the new local map and the offset from the old map. Set a temporary bounding box based on the positive or negative offset and delete the points inside the box. Update the center coordinates of the local map to the current robot position.

[0028] In this implementation, such as Figure 2As shown, to improve the efficiency of the LiDAR matching optimization stage, the entire map is not used for subsequent LiDAR matching optimization. Instead, a local map centered on the robot's current position is maintained for matching optimization. The map data structure adopts an incremental octree (i-octree), which can realize point insertion, deletion, and KNN search functions. After each LiDAR matching optimization stage converges, an admission judgment is performed. If the admission judgment condition is met, point cloud insertion is performed according to keyframes. That is, when the distance between two adjacent frames is greater than the threshold of 0.2m, the point cloud of the current frame is inserted to build a fused map in real time. At the same time, as the robot moves, points are not inserted indefinitely, but a local map centered on the robot's current position is maintained. A local map centered on a point, with dimensions of 100m (length, width, and height). The specific operation involves the robot... axis, axis, After translating in any direction of the axis beyond a distance threshold of 0.5m, a temporary bounding box is first created and initialized to the old bounding box. Then, the new position is calculated at... axis, axis, The system calculates the maximum and minimum boundary values ​​on each axis, as well as the offsets of the new and old positions on each axis. It then sequentially checks the sign of the offset on each axis. If the offset is positive, the maximum boundary value of the temporary bounding box is set to the minimum boundary value of the new position, and points within the temporary bounding box are binned. If the offset is negative, the minimum boundary value of the temporary bounding box is set to the maximum boundary value of the new position, points within the temporary bounding box are binned, and the center coordinates of the local map are updated to the new position. This enables dynamic insertion and deletion of point clouds, ensuring efficient utilization of system resources.

[0029] In one embodiment of the present invention, the operation of the feature extraction module in step S3 is as follows: The distortion-free point cloud is projected into a distance image, and the row index, column index and distance value of each point are saved. Traverse each column of the distance image, calculate the cosine of the angle between the vector formed by adjacent points in the same column and the ground normal vector. If the value is less than a preset value, mark it as a ground point. The preset value can be 0.085. For non-ground points, the curvature of the point cloud is calculated using the curvature formula. Points with larger curvature are classified as corner points, and points with smaller curvature are classified as surface points. The curvature formula is: , where represents the curvature of the i-th point in the scan line, S is the set of n points to the left and right of the i-th point, and is the distance between the i-th point and the radar center.

[0030] like Figure 1As shown, for the distortion-corrected LiDAR point cloud, feature points for ground points, corner points, and area points are extracted. First, each point in the original LiDAR point cloud is traversed, and the original LiDAR point cloud is projected into a distance image, saving the row index, column index, and distance value for each point. Next, each column of points in the distance image is traversed, performing row-by-row calculations within a specified row index range to obtain the current point and the points in the same column of the next row, and calculating their vectors. Then, the cosine of the angle between this vector and the ground normal vector is calculated. If the cosine of the angle is less than 0.085, the line formed by these two points is considered to be close to the ground direction, and such points are marked as ground points; otherwise, they are non-ground points. For non-ground points, corner and area point features are extracted by calculating the curvature of the point cloud on the LiDAR frame.

[0031] In one embodiment of the present invention, the operation of the matching optimization module in step S3 is as follows: Based on the previous matching optimization results and the pose increment of the lidar inertial odometry, the initial pose of the current frame is estimated using a formula; The keyframes within the merged map are used to construct local corner point maps, local area point maps, and local ground point maps, which are then stored in three incremental octrees. Traverse the corner points, face points, and ground points of the current frame, find the corresponding points through KNN search, and construct point-line matching pairs and point-face matching pairs. After iterative convergence based on LM optimization, output high-precision positioning results.

[0032] In this embodiment, based on the results of the previous lidar matching optimization phase... Integrating with lidar inertial odometry Pose increment within time It can estimate the initial pose of the current frame. The calculation formula is: .

[0033] Initial pose based on the current frame The current frame point cloud can be projected onto the map coordinate system. Simultaneously, keyframes within the fused map are used to create local corner maps, local polygon maps, and local ground point maps, stored in three incremental octrees for faster searching during optimization. Then, the corner, polygon, and ground points in the current frame point cloud are traversed, and KNN search is used to find corresponding points and construct point-line and point-polygon matching pairs. Iterative optimization based on Learning Model (LM) is performed until convergence. After multiple rounds of matching pair construction and LM optimization, a relatively accurate pose estimation result for the current frame can be obtained. .

[0034] In summary, the specific embodiments of this invention elaborate on the execution logic of each step of the long-term robust localization method for scene changes. The various parameters and thresholds involved, including the interior point distance setting threshold of 0.2m, the translation error threshold of 0.1m and the rotation error threshold of 1° for relative pose error (RPE), the map matching score threshold of 70%, the local map size (length, width, and height are all 100m), the distance threshold between adjacent frames for keyframe insertion of 0.2m, the distance threshold for robot translation triggering map updates of 0.5m, and the preset value of the cosine of the included angle for ground point determination of 0.085, can all be adaptively adjusted according to the sensor accuracy, the frequency of dynamic changes in the environment, the system computing power configuration, and the positioning accuracy requirements under the actual application scenario.

[0035] The specific numerical values ​​listed in this embodiment are merely illustrative demonstrations and clear explanations of the technical solution of the present invention, intended to enable those skilled in the art to accurately reproduce the technical effects of the present invention, and do not constitute the sole limitation of the present invention. It should be understood that any reasonable adjustments, substitutions, or optimizations made to the above parameters within the core conceptual framework of the present invention—"providing initial pose through LiDAR-IMU fusion, triggering dynamic map updates through a dual-judgment mechanism, and achieving high-precision positioning through multi-feature extraction and LM optimization"—do not depart from the technical essence and protection scope of the present invention, and should be included within the protection scope defined by the claims of the present invention.

[0036] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A long-term robust localization method for scene changes, characterized in that, Includes the following steps: Step S1: Output the initial pose and the distorted point cloud through the state estimation and point cloud distortion correction module and the observation update module; Step S2: Through the admission judgment module and the dynamic insertion and deletion module, output the merged map formed by combining the static map and the dynamic map; Step S3: Starting from the initial pose output in step S1 and using the fused map output in step S2 as the data source, iterative optimization is performed through the feature extraction module and the matching optimization module until convergence, and then a high-precision positioning result is output.

2. The long-term robust localization method for scene changes according to claim 1, characterized in that: The working process of the state estimation and point cloud distortion correction module in step S1 is as follows: Starting from the pose updated by the previous observation residual, motion integration is performed using the acceleration and angular velocity provided by the IMU data to obtain the pose of each point in the lidar scan frame and the pose of the last point. By transforming the extrinsic parameters between the IMU and the lidar, all points in the current lidar scan frame are projected to the end of the scan frame, thus completing the point cloud distortion removal and obtaining the lidar prior pose of the current frame.

3. The long-term robust localization method for scene changes according to claim 2, characterized in that: The operation of the observation update module in step S1 is as follows: Based on the prior pose of the LiDAR, the points in the current scan frame are projected onto the map coordinate system to complete the matching between the scan frame and the map and to construct the point-area distance constraint. The state is updated based on the error state iterative Kalman filter, and the initial pose is output for use in step S3.

4. The long-term robust localization method for scene changes according to claim 3, characterized in that: The specific process for constructing the point-to-surface distance constraint is as follows: Traverse the points in the current LiDAR scan frame and project them onto the global map; The KNN search is used to find the n nearest neighbors of each point, the covariance matrix of the n points is calculated and the plane normal vector is obtained by SVD eigenvalue decomposition; Select the closest point among n points and calculate the point-to-surface distance residual using the vector dot product.

5. The long-term robust localization method for scene changes according to claim 4, characterized in that: The working process of the admission judgment module in step S2 is as follows: After the LiDAR matching optimization converges, the point cloud of the current scan frame is projected onto the map coordinate system using the optimized pose. KNN search is performed on each point. If the distance to the nearest point is less than the threshold of the distance to the inner point, the matching is considered successful. The proportion of successfully matched points to the total number of points is used as the map matching score. Calculate the relative pose error (RPE) between the LiDAR matched optimized pose and the LiDAR inertial odometry pose, and determine pose consistency; Based on the map matching score and pose consistency, a dynamic map is inserted and merged with the static map to form a fused map.

6. The long-term robust localization method for scene changes according to claim 5, characterized in that: The working process of the dynamic insertion and deletion module in step S2 is as follows: An incremental octree is used as the map data structure to maintain a local map centered on the robot's current position. Based on the admission criteria and the distance between two adjacent frames, the point cloud of the current frame is inserted into the map as a keyframe; When the robot translates beyond a set distance in any of the X, Y, or Z axes, the new local map boundary and the offset from the old map are calculated. A temporary bounding box is set according to the positive or negative offset, and points within the box are deleted. The center coordinates of the local map are then updated to the current robot position.

7. The long-term robust localization method for scene changes according to claim 6, characterized in that: The working process of the feature extraction module in step S3 is as follows: The distortion-free point cloud is projected into a distance image, and the row index, column index and distance value of each point are saved. Traverse each column of points in the distance image, calculate the cosine of the angle between the vector formed by adjacent points in the same column and the ground normal vector, and mark it as a ground point if the value is less than a preset value; For non-ground points, the curvature of the point cloud is calculated using the curvature formula. Points with larger curvature are classified as corner points, and points with smaller curvature are classified as surface points. The curvature formula is: ,in Let represent the curvature of the i-th point in the scan line, S be the set of n points to the left and right of the i-th point, and represent the distance between the i-th point and the radar center.

8. The long-term robust localization method for scene changes according to claim 7, characterized in that: The matching optimization module in step S3 works as follows: Based on the previous matching optimization results and the pose increment of the lidar inertial odometry, the initial pose of the current frame is estimated using a formula; The keyframes within the merged map are used to construct local corner point maps, local area point maps, and local ground point maps, which are then stored in three incremental octrees. Traverse the corner points, face points, and ground points of the current frame, find the corresponding points through KNN search, and construct point-line matching pairs and point-face matching pairs. After iterative convergence based on LM optimization, output high-precision positioning results.

Images