Multi-sensor fusion synchronous positioning mapping method and device for unmanned equipment

Abstract

The invention provides a multi-sensor fusion synchronous positioning mapping method and device for unmanned equipment, belongs to the technical field of unmanned equipment, and solves the technical problems of sparse features of a weak texture region and insufficient utilization of geometric constraints of a structured scene in an environment in the prior art. The method comprises the following steps: performing IMU pre-integration and laser radar data preprocessing in an SLAM system to obtain laser radar point cloud data subjected to motion distortion correction and IMU motion constraints between adjacent frames; the plane points are classified, straight lines are fitted to form the contour of the environment, virtual edge points are derived by searching intersection points between the straight lines, the virtual edge points and edge points obtained from curvature analysis are combined, and a final edge point set is compiled; feature matching and point cloud registration are carried out, laser odometer factors are generated, a factor graph is constructed in combination with IMU motion constraint, and a global map is constructed by using poses output by back-end optimization. The method is used for providing instant positioning and mapping functions for the unmanned equipment.

Description

Technical Field

[0001] This invention belongs to the field of unmanned equipment technology, and is specifically applied to providing real-time positioning and mapping functions for unmanned equipment. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) is a core enabling technology for the autonomous operation of unmanned equipment (such as autonomous vehicles, service robots, and warehouse robots). In the field of unmanned equipment, SLAM provides the equipment with real-time localization and environmental mapping capabilities, and is an indispensable foundation for realizing higher-level functions such as autonomous navigation, path planning, dynamic obstacle avoidance, and task execution. The positioning accuracy and map quality of unmanned equipment directly determine the reliability and efficiency of its task execution.

[0003] The core challenge of SLAM lies in the need for unmanned devices to simultaneously solve two interdependent problems in unknown environments: self-localization and environmental mapping. Accurate localization depends on an accurate map, while map construction, in turn, depends on accurate localization; the two constitute a continuous iterative optimization process. With Csorba's proof of the convergence of the SLAM problem, SLAM technology has made significant progress, giving rise to SLAM systems based on different sensors, primarily including laser SLAM, visual SLAM, and multi-sensor fusion SLAM systems that integrate multiple sensors.

[0004] Multi-sensor fusion-based SLAM methods leverage the complementary strengths of various sensors (such as LiDAR, IMU, and cameras) to further improve attitude estimation accuracy. This allows them to combine the ranging accuracy of LiDAR SLAM with the rich information content of visual SLAM, better meeting the needs of unmanned equipment in complex environments. Numerous achievements have been made in this area: LIO-Mapping achieves tight coupling between IMU and LiDAR by optimizing measurement residuals; Qin et al. achieved tight coupling through error state filtering, improving mapping efficiency; Shan et al.'s Lio-sam algorithm uses IMU pre-integration to provide initial values ​​for the LiDAR odometry and optimizes IMU bias using the LiDAR odometry results, thus achieving tight coupling between LiDAR and IMU; TixiaoShan and XinTong's Lvi-sam enhances the adaptability of SLAM systems through factor-map-based tight coupling processing of LiDAR-vision-inertial systems.

[0005] Existing SLAM technologies (including the aforementioned fusion methods) still face severe challenges in performing well in common structured scenes (such as corridors, regularly arranged shelves, empty warehouses, and poorly lit corridors). Visual sensors rely on illumination, and many structured scenes have poor lighting conditions (such as nighttime, windowless areas, and dim lighting) or drastic changes in illumination, making visual feature extraction difficult or ineffective, which seriously affects the performance of visual SLAM. Furthermore, laser point cloud features are scarce. In structured scenes, the inter-frame features of point clouds obtained by laser radar scanning are highly similar and lack discriminativeness (such as repetitive line and surface structures), making robust feature matching difficult and resulting in decreased pose estimation (localization) accuracy.

[0006] For unmanned equipment operating in structured environments, a more stable and reliable SLAM solution is urgently needed. This invention addresses this core pain point by proposing a multi-sensor fusion SLAM method specifically designed for unmanned equipment. This method deeply integrates LiDAR and inertial measurement units, aiming to effectively solve the problems of decreased positioning accuracy and cumulative error drift caused by feature scarcity in LiDAR SLAM in structured scenes. Ultimately, it significantly improves the positioning accuracy and map building quality of unmanned equipment in complex structured environments, ensuring the stability and reliability of its autonomous operation. Summary of the Invention

[0007] In view of this, the present invention aims to propose a synchronous localization and mapping method and apparatus for multi-sensor fusion for unmanned equipment, so as to solve the technical problems of insufficient utilization of sparse features in weak texture regions of the environment and geometric constraints of structured scenes in the prior art.

[0008] To achieve the above objectives, the present invention adopts the following technical solution: This invention proposes a synchronous localization and mapping method for multi-sensor fusion in unmanned equipment, the method comprising: S1. Perform IMU pre-integration and lidar data preprocessing in the SLAM system to obtain lidar point cloud data with motion distortion correction and IMU motion constraints between adjacent frames. S2. Based on the motion distortion corrected lidar point cloud data, the planar points are classified, and then the classified points are fitted with straight lines to form the contour of the environment. Virtual edge points are derived by finding the intersection points between the straight lines, and the virtual edge points are merged with the edge points obtained from curvature analysis to compile the final edge point set. S3. Based on the final edge point set, perform feature matching and point cloud registration to generate laser odometry factors, and construct a factor map by combining the IMU motion constraints. Use the pose output by the backend optimization to construct a global map.

[0009] Furthermore, the process of S1 includes: S11. Pre-integrate the IMU data and calculate the relative motion increment between adjacent lidar frames by using the translational acceleration and attitude angular velocity of the object at each moment acquired by the IMU. S12. The rotation and translation matrices of the object at each moment obtained by the IMU pre-integration are used for spherical linear interpolation to convert all point cloud data to the start or end time of each frame of point cloud scanning, so as to achieve point cloud distortion removal.

[0010] Furthermore, the S2 process includes: S21. Divide each laser scanning line into 6 segments, process each segment independently, sort the points of each segment in descending order of curvature, select the first 20 points as corner points starting from the point with the largest curvature and mark the surrounding points, and select points with curvature below the threshold to obtain plane points. S22. Use covariance matrix decomposition to calculate the point cloud normal vector, and use the included angle formula to determine whether it is a ground point. Based on the characteristics of the surface normal vector, distinguish the planar points into ground points and non-ground points. S23. Ground points participate in the generation of virtual points. In the divided independent subspace, the ground points are separated into geometrically continuous subclusters. A straight line hypothesis is constructed by random sampling and the interior points are screened. The straight line with the most interior points is selected. S24. Calculate the intersection point of any two straight lines. If the included angle is greater than the threshold and the intersection point is located near the boundary of the subspace, add the intersection point to the virtual point set and project the non-ground points onto the ground plane. Check the projected points. If they are located in the edge area of ​​the ground block, add them to the virtual point set. S25. Using the intensity information of the laser point cloud as an auxiliary feature, the intensity value is filtered by mean. Each frame of point cloud is divided into six sub-regions. An adaptive threshold for intensity features is set based on the histogram value of each sub-region. Points with intensity greater than the threshold and local intensity change rate greater than the minimum change rate threshold are selected as edge points. S26. Merge the edge points obtained from curvature analysis, the edge points extracted based on intensity information, and the virtual points to compile the final edge point set.

[0011] Furthermore, S3 includes: S31. Perform point cloud registration for edge points and planar points respectively: For each edge point in the current frame, find the nearest edge line in the previous frame or local map and calculate the distance from the point to the line; for each planar point in the current frame, find the nearest planar block in the previous frame and local map and calculate the distance from the point to the plane. S32, Joint Optimization of Hierarchical Robust Cost Function: Establish geometric residual equations for edge points to lines and plane points to surfaces, and introduce motion constraints provided by IMU pre-integration as regularization terms; An M-estimator kernel function is introduced to automatically suppress the interference of outlier matching points on the optimization objective by dynamically adjusting the residual weights; the M-estimator kernel function includes Huber or Cauchy. The Levenberg-Marquardt algorithm is used to iteratively solve the nonlinear cost function, and the sparsity of the Hessian matrix is ​​used to quickly update the pose estimate. S33. The point cloud registration results are used as laser odometry factors, which together with IMU factors form a factor map. The factors are grouped using a graph optimization method. Two frames of pose are obtained from step-by-step loop closure detection for global pose optimization. The optimized pose is then used to construct a global map.

[0012] Furthermore, in S11, when pre-integrating the IMU data, the rotation integral of the IMU data within the time window of two lidar frames is represented by quaternions; the rotation integral formula is as follows: (1) In the formula, These are gyroscope measurements. To achieve zero bias in the gyroscope, For time intervals.

[0013] Furthermore, the point cloud distortion removal described in S12 uses the Slerp spherical linear interpolation method, and the interpolation parameters for the spherical linear interpolation are: (4) In the formula, , is the point collection time; Interpolated quaternions for: (5) in, , For adjacent IMU times, , Let q be the rotation quaternion corresponding to adjacent IMU times; given a unit quaternion q, the formula for calculating its corresponding rotation matrix is: (6) Point Transform from the coordinate system of the acquisition time to the reference time The coordinate system is: (7) in, and yes and The corresponding rotation matrix, and It is a translation vector.

[0014] Furthermore, in S21, the eigenvalue method of the covariance matrix is ​​used to calculate the curvature. The formula for calculating the covariance matrix of neighborhood points is: (9) in, Let be the centroid of the neighboring points, and k be the number of neighboring points. Let be the coordinates of the j-th point in the neighborhood; decompose the covariance matrix C to obtain the eigenvalues ​​λ1≤λ2≤λ3; The formula for calculating curvature is: (10) A larger value indicates a more curved surface, corresponding to edge feature corner points. The smaller the value, the flatter the surface, corresponding to planar feature points.

[0015] Furthermore, in S25, the intensity data undergoes mean filtering. The mean filtering formula for a window size of 2 is as follows: (11) in, The intensity value of the k-th point in the original point cloud. This represents the intensity value at point i after filtering. The formula for calculating the standard deviation of intensity in a sub-region is: (12) in, Let be the average intensity of sector s. Let N be the number of points in sector s, and N be the set of point clouds within the sub-region. and Let i be the intensity at points i and j.

[0016] Furthermore, the formulas for calculating the residuals at edge points and plane points in S32 are as follows: (twenty three) (twenty four) in, For the edge points of the current frame, As the starting point of the reference frame edge line, Let the line direction vector be... For the reference frame plane normal vector, Let be a point on the plane.

[0017] This invention also proposes a multi-sensor fusion synchronous localization and mapping device for unmanned equipment, the device comprising: Data preprocessing module; used to perform IMU pre-integration and lidar data preprocessing in the SLAM system to obtain lidar point cloud data with motion distortion correction and IMU motion constraints between adjacent frames; Edge point set compilation module; used to classify planar points based on the motion distortion corrected lidar point cloud data, then fit the classified points to straight lines to form the contour of the environment, derive virtual edge points by finding the intersection points between the straight lines, and merge the virtual edge points with the edge points obtained from curvature analysis to compile the final edge point set; A global map construction module is used to perform feature matching and point cloud registration based on the final edge point set, generate laser odometry factors, construct a factor map in combination with the IMU motion constraints, and construct a global map using the pose output by the backend optimization.

[0018] Compared with the prior art, the beneficial effects of the present invention are: This invention proposes a multi-sensor fusion-based simultaneous localization and mapping (SLAM) method for unmanned equipment. Compared to traditional methods like Lego-LOAM and Lio-SAM that rely solely on geometric curvature, this invention effectively compensates for the shortcomings of LiDAR in structured scenes such as corridors and warehouses by introducing adaptive threshold extraction of point cloud intensity information and virtual edge point derivation based on ground plane constraints. This addresses the deficiencies of LiDAR in terms of insufficient vertical resolution and lack of geometric features. This allows the system to obtain a sufficient number of highly discriminative feature points even in weakly textured and highly repetitive geometric environments, fundamentally solving the registration failure problem caused by feature sparsity. Simultaneously, the use of covariance matrix decomposition and normal vector angle constraints for ground point classification more accurately removes the ground background, providing a reliable benchmark for subsequent virtual point generation and ensuring the high stability of unmanned equipment in complex indoor ground environments. This significantly improves the robustness and richness of feature extraction in structured scenes. Furthermore, this method uses factor graph optimization and step-by-step loop closure detection to achieve high-precision localization and globally consistent mapping in degraded environments. By integrating IMU pre-integration constraints, laser odometry factors, and step-by-step loop closure detection factors through a factor graph optimization framework, real-time correction of sensor bias and cumulative errors is achieved. In particular, after obtaining precise attitude constraints between two frames through step-by-step detection, a grouped graph optimization method is used for global adjustment, significantly reducing absolute trajectory errors during long-distance operation and ensuring the geometric consistency and seamless stitching of the global map. Step one involves IMU pre-integration and LiDAR data preprocessing in the SLAM system. Step two involves classifying planar points, fitting these points to straight lines to represent the contours of the environment, deriving virtual edge points by finding intersections between these lines, and finally merging these predicted edge points with edge points obtained from curvature analysis to compile the final edge point set. Step three uses the pose output from the backend optimization to construct the global map. This invention belongs to the field of unmanned equipment technology and is specifically applied to provide real-time localization and mapping functions for unmanned equipment.

[0019] This invention belongs to the field of unmanned equipment technology, and is specifically applied to providing real-time positioning and mapping functions for unmanned equipment. Attached Figure Description

[0020] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0021] Figure 1 This is a schematic diagram of a synchronous positioning and mapping method for multi-sensor fusion of unmanned equipment as described in this invention.

[0022] Figure 2 This is a schematic diagram of point cloud distortion in this invention.

[0023] Figure 3 This is a schematic diagram of Slerp interpolation in this invention.

[0024] Figure 4 This is a flowchart of the multi-feature extraction process in this invention.

[0025] Figure 5 This is a schematic diagram of the radar curvature in this invention.

[0026] Figure 6 This is a schematic diagram of point feature extraction in this invention.

[0027] Figure 7 This is a schematic diagram of the features extracted by the method described in this invention.

[0028] Figure 8 This is a schematic diagram of the pose and observation point in the factor graph of this invention.

[0029] Figure 9 The flowchart illustrates the multi-feature extraction process using the method described in this invention.

[0030] Figure 10 The absolute trajectory error of each algorithm in the corridor sequence of the VECtor dataset is represented by the method described in this invention.

[0031] Figure 11 The absolute trajectory error of each algorithm in the school sequence of the VECtor dataset is represented by the method described in this invention. Detailed Implementation

[0032] 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.

[0033] Specific implementation method one, such as Figure 1 As shown in this embodiment, a synchronous localization and mapping method for multi-sensor fusion in unmanned equipment includes: The method begins with step one, in which IMU pre-integration and lidar data preprocessing are performed in the SLAM system to obtain lidar point cloud data corrected for motion distortion and IMU motion constraints between adjacent frames; the process in S1 includes: S11. Pre-integrate the IMU data and calculate the relative motion increment between adjacent lidar frames by using the translational acceleration and attitude angular velocity of the object at each moment acquired by the IMU. S12. The rotation and translation matrices of the object at each moment obtained by the IMU pre-integration are used for spherical linear interpolation to convert all point cloud data to the start or end time of each frame of point cloud scanning, so as to achieve point cloud distortion removal.

[0034] In step one, to calculate the relative motion increment between adjacent lidar frames using IMU data and avoid repeatedly integrating all state variables, the IMU data is first pre-integrated. By using the translational acceleration and attitude angular velocity of the object at each moment acquired by the IMU, the rotation and translation matrices of the object at each moment are obtained through IMU pre-integration. Pre-integration only calculates the relative motion increment (ΔR, Δv, Δp), significantly improving computational efficiency. Assume the time window between two lidar frames is... arrive The rotation integral of the IMU data within this window uses quaternions to represent the rotation, avoiding the singularity problem of Euler angles. The rotation integral is as follows: (1) in, These are gyroscope measurements. To achieve zero bias in the gyroscope, The time interval is defined. Velocity and position within the window are calculated using median integration, balancing accuracy and computational efficiency. The integral is as follows: (2) in, The value is the accelerometer measurement. Let g be the acceleration due to gravity, and g be the zero bias of the accelerometer. In factor graph optimization, and As a state variable, it is estimated in real time, and the pre-integration result is used to make a first-order approximation correction to the change in zero bias: (3) The results of IMU pre-integration are used as motion constraints (IMU factors) between adjacent lidar frames, and together with the lidar matching factor, they optimize the pose. Optimization variables include pose. , ,speed , and IMU zero bias and Between lidar frames, high-frequency motion prediction is provided through IMU pre-integration to correct point cloud distortion. The optimized state is fed back to the pre-integration module, and then the zero-bias estimate is updated to form a closed loop.

[0035] During the data acquisition process of a lidar system, movement causes the lidar coordinate system to be in motion relative to the world coordinate system. This results in changes to the start and end points of a scan within a single frame of point cloud data, even if the coordinate systems of the scanned lidar point clouds are not unified. Consequently, point cloud data that should be in the same coordinate system may be classified into different coordinate systems, leading to point cloud distortion. Figure 2 This is a schematic diagram of point cloud distortion.

[0036] This invention combines the above-described IMU data processing with a motion distortion correction method for point cloud information interpolation. It uses the rotation and translation matrices of the object obtained through IMU pre-integration at each moment to calculate the position and orientation of the point cloud at the start and end times of every two frames of lidar point cloud data. Then, it applies spherical linear interpolation (Slerp) theory... Figure 3 The diagram shown illustrates Slerp interpolation. It uses IMU data to calculate the rotation components at the start and end times of the current laser point cloud frame. By converting all point cloud data to the start or end time of each frame of point cloud scanning, point cloud distortion removal can be achieved.

[0037] If the current k-th frame The laser point cloud at any given time is At that time, the rotation quaternion for adjacent IMU times (time )and (time ), point collection time The interpolation parameters are: (4) Interpolated quaternions for: (5) Given a unit quaternion The corresponding rotation matrix is: (6) Point Transform from the coordinate system of the acquisition time to the reference time The coordinate system is: (7) in, and yes and The corresponding rotation matrix, and It is a translation vector. After obtaining the rotation amount corresponding to each moment in the start and end times of the lidar point cloud frame, the increment between the rotation amount at the start time of the lidar frame and the rotation amount at the start time is calculated. The laser point at each moment is transformed to the coordinate system of the start time using the rotation increment and translation increment at each moment, thereby realizing point cloud distortion correction.

[0038] Then, step two is executed. In step two, based on the motion-distortion-corrected lidar point cloud data, the planar points are classified, and then the classified points are fitted with straight lines to form the contour of the environment. Virtual edge points are derived by finding the intersections between the straight lines, and the virtual edge points are merged with the edge points obtained from curvature analysis to compile the final edge point set; the process of S2 includes: S21. Divide each laser scanning line into 6 segments, process each segment independently, sort the points of each segment in descending order of curvature, select the first 20 points as corner points starting from the point with the largest curvature and mark the surrounding points, and select points with curvature below the threshold to obtain plane points. S22. Use covariance matrix decomposition to calculate the point cloud normal vector, and use the included angle formula to determine whether it is a ground point. Based on the characteristics of the surface normal vector, distinguish the planar points into ground points and non-ground points. S23. Ground points participate in the generation of virtual points. In the divided independent subspace, the ground points are separated into geometrically continuous subclusters. A straight line hypothesis is constructed by random sampling and the interior points are screened. The straight line with the most interior points is selected. S24. Calculate the intersection point of any two straight lines. If the included angle is greater than the threshold and the intersection point is located near the boundary of the subspace, add the intersection point to the virtual point set and project the non-ground points onto the ground plane. Check the projected points. If they are located in the edge area of ​​the ground block, add them to the virtual point set. S25. Using the intensity information of the laser point cloud as an auxiliary feature, the intensity value is filtered by mean. Each frame of point cloud is divided into six sub-regions. An adaptive threshold for intensity features is set based on the histogram value of each sub-region. Points with intensity greater than the threshold and local intensity change rate greater than the minimum change rate threshold are selected as edge points. S26. Merge the edge points obtained from curvature analysis, the edge points extracted based on intensity information, and the virtual points to compile the final edge point set.

[0039] like Figure 4 The flowchart shown illustrates the multi-feature extraction process. In step two, feature extraction employs a multi-dimensional approach, reducing the limitations of single features through multi-feature fusion, enhancing environmental understanding, and introducing intensity information to assist feature extraction. Since the curvature of radar point clouds encodes information about the environmental surface, this paper adopts a curvature-based geometric feature extraction method. Figure 5This is a schematic diagram of the curvature of a lidar sensor. The curvature of a point is calculated from the average distance between its position and adjacent points on the same laser beam. If the distance is less than a threshold, the point is classified as a planar point; otherwise, it is simply a corner point. The conventional curvature calculation formula calculates the curvature of a point... Its neighboring points Find the norm of the sum of vectors and normalize it. The curvature is estimated as shown in equation (8). Its essence is to measure the point... The curvature of the surrounding neighborhood is lower if the distribution of neighborhood points is symmetrical and the vector sum approaches zero, and higher if the distribution is asymmetrical.

[0040] (8) This invention optimizes the estimation of neighborhood points by focusing on their distribution characteristics rather than their relative positions. It directly employs the eigenvalue method of calculating the covariance matrix of curvature. The covariance matrix provides rich geometric information, such as normal vectors, principal directions, and geometric types, by analyzing the distribution characteristics of the local neighborhood, making it more suitable for point cloud feature extraction in structured environments. The geometric type of the local surface is determined based on the distribution characteristics of the neighborhood points, and the magnitude of the eigenvalues ​​is used to distinguish between planes and corners.

[0041] (9) (10) The covariance C of the neighborhood points is given by the following formula. Let be the centroid of the neighborhood points. Decompose the covariance matrix. , to obtain eigenvalues The curvature is calculated to obtain equation (10). A larger value indicates a more curved surface, which is an edge feature (corner point). The smaller the value, the flatter the surface, indicating a planar feature (planar point). Each laser scan line is divided into 6 segments, each processed independently. Points within each segment are sorted in descending order of curvature. Starting with the point of maximum curvature, the first 20 points are selected as corner points. Surrounding points are marked (5 points before and 5 points after) to avoid duplicate selection. Points with curvature below a threshold are selected for further filtering to obtain planar points. Figure 6 This is a schematic diagram of point feature extraction.

[0042] Because the geometric features of structured environments are relatively sparse, when dealing with lidar point cloud frames... After completing motion correction and extracting the corresponding geometric features, this paper uses the intensity information of the laser point cloud as auxiliary features to provide more constraints for point cloud feature matching. Intensity information often contains noise; if intensity features are extracted directly, many noisy points will be extracted as features. Therefore, the intensity data needs to be preprocessed. First, the intensity values ​​of the original point cloud are subjected to mean filtering to reduce the impact of noise on feature extraction. The following formula represents the mean filtering with a window size of 2: (11) (12) in, The intensity value of the k-th point in the original point cloud. Let be the intensity value of the i-th point after filtering. In order to ensure the uniform distribution of intensity feature points, the point cloud of each frame of the horizontal field of view of the lidar is divided into six sub-regions. The standard deviation of the intensity of each sector is given by equation (12). The intensity change rate of each point in the sub-region is dynamically calculated by equation (12). Let be the average intensity of sector s. Let N be the number of points in sector s, and N be the set of point clouds within the sub-region. and Let i be the intensity at points i and j.

[0043] (13) (14) The histogram value of each sub-region is set as the adaptive threshold for the intensity feature of that sub-region. The adaptive threshold is negatively correlated with the standard deviation, thus adaptively adjusting the number of intensity features extracted. Once the rotation matrix R is determined, the translation vector t can be determined using the following formula.

[0044] (15) Select each sub-region with an intensity greater than Points are selected as candidate intensity features, and the local intensity change rate is calculated for each candidate intensity feature. , The larger the value, the more likely the point is in a region of abrupt change in intensity. Intensity points exceeding the set minimum rate of change threshold are considered as candidates. When the curvature exceeds the threshold... Greater than It is then determined to be an edge point.

[0045] To mitigate the shortcomings of overly simplistic feature extraction and enhance environmental understanding, virtual points are extracted at the interface between the ground and obstacles to compensate for the insufficient vertical resolution of the LiDAR. Planar points can be categorized based on the orientation of their respective normal vectors, and further distinguished as ground points and non-ground points based on the features of their associated surface normal vectors. Only ground points participate in the generation of virtual points. Covariance matrix decomposition is used to calculate the point cloud normal vectors for each point... Its neighborhood point covariance matrix is: (16) in, With the centroid as the center, the eigenvector corresponding to the smallest eigenvalue of the covariance matrix is ​​the normal vector n. The direction of gravity g = (0, 0, 1), and the angle formula is: (17) Among them, if < For the ground point, if 180°- < If it is, then it is another point; otherwise, it is a non-ground point.

[0046] The set of independent subspaces partitioned along the X / Y axes after the classification plane point. For point After coordinate mapping, its subspace index is: (18) in, This is the width of the subspace, meaning that this width is less than the minimum structural size of the environment.

[0047] Combinatorial subspace is This is the union of all X / Y axis subspaces. Within each subspace, the separated point cloud is a geometrically continuous subcluster. Initialization is performed first, processing points within the subspace according to their classification categories. Then, for each point… , Clustering criteria are determined, and if the following conditions are met, the clusters are classified into the same sub-cluster.

[0048] Condition 1: Distance constraint

[0049] Condition 2: Consistency of normal vectors.

[0050] Condition 3: Same category, and Points belonging to the same class Based on the above conditions, the sub-cluster set is obtained. ,in Let i be the i-th sub-cluster. By analyzing the obtained sub-clusters... Perform random sampling, from Two points are randomly selected from the middle. , Construct the linear assumption: (19) Then, interior point filtering is performed, and the distances from other points to the line are calculated. ,like These are then marked as interior points. This process is repeated sequentially, selecting the line with the most interior points. This yields the fitted lines for all subclusters. Then for any two straight lines , Calculate the intersection point; if the included angle is greater than a threshold, calculate the intersection point. : (20) If the intersection point is located near the boundary of the subspace, then the point is added to the set of virtual points.

[0051] After supplementing the geometric features through the intersection points of straight lines, we consider adding non-ground projection points to enhance the obstacle outline. After obtaining ground and non-ground points, virtual edge points are generated at the intersection of ground and non-ground points. The ground points are then divided into blocks, and the centroid C and normal vector n are calculated for each block to obtain the plane equation: (twenty one) For each ground block, calculate the height difference between the non-ground points and the ground within the search radius. If h is greater than the height threshold, then the non-ground point p will be projected onto the ground plane: (twenty two) By inspection If a point is located on the edge of a ground block, i.e., the distance from the subspace boundary is less than the distance threshold of the edge region, then it is added to the virtual point set.

[0052] In summary, as follows: Figure 7 The diagram shows the features extracted by this method. Yellow squares represent extracted virtual edge points, colored squares (excluding yellow) represent extracted corner points, and small colored circles represent extracted planar points. By combining multiple dimensions for feature extraction, the method significantly increases the number of features, reducing the limitations of relying on single features.

[0053] Finally, step three is executed. In step three, feature matching and point cloud registration are performed based on the final edge point set to generate laser odometry factors. These factors are then combined with the IMU motion constraints to construct a factor map, and the pose output from the backend optimization is used to construct a global map. S3 includes: S31. Perform point cloud registration for edge points and planar points respectively: For each edge point in the current frame, find the nearest edge line in the previous frame or local map and calculate the distance from the point to the line; for each planar point in the current frame, find the nearest planar block in the previous frame and local map and calculate the distance from the point to the plane. S32, Joint Optimization of Hierarchical Robust Cost Function: Establish geometric residual equations for edge points to lines and plane points to surfaces, and introduce motion constraints provided by IMU pre-integration as regularization terms; An M-estimator kernel function is introduced to automatically suppress the interference of outlier matching points on the optimization objective by dynamically adjusting the residual weights; the M-estimator kernel function includes Huber or Cauchy. The Levenberg-Marquardt algorithm is used to iteratively solve the nonlinear cost function, and the sparsity of the Hessian matrix is ​​used to quickly update the pose estimate. S33. The point cloud registration results are used as laser odometry factors, which together with IMU factors form a factor map. The factors are grouped using a graph optimization method. Two frames of pose are obtained from step-by-step loop closure detection for global pose optimization. The optimized pose is then used to construct a global map.

[0054] Before proceeding to step three, feature matching is required. After finding the geometric relationship between the feature points in the current frame and the corresponding features in historical frames or the map, point cloud registration is performed to estimate the pose transformation between adjacent frames.

[0055] This invention employs optimization based on direct feature matching and a hierarchical robust cost function. Because the feature types differ, point cloud registration needs to be performed separately for both types of features to obtain optimal pose estimation between frames. First, feature matching is performed. Edge point matching involves finding the nearest edge line (by fitting a straight line to adjacent points) in the previous frame or local map for each edge point in the current frame, and calculating the distance from the point to the line. Plane point matching involves finding the nearest planar patch (by fitting a plane to adjacent points) in the previous frame or local map for each planar point in the current frame, and calculating the distance from the point to the plane.

[0056] Using the IMU pre-integration motion constraints obtained in S1 as prior terms, and combining them with the edge point residuals and planar point residuals calculated in S31, a nonlinear least squares optimization objective function is constructed. This function comprehensively considers the smoothness constraints of inter-frame motion (IMU factor) and the geometric alignment constraints of environmental features (laser factor).

[0057] To suppress interference from dynamic objects or mismatched points, a robust kernel function is introduced into the geometric residual term. The kernel function performs a nonlinear mapping on the residuals. When the residual of an observation point exceeds a preset threshold, the weight of that matching pair in the optimization objective is automatically reduced, thus achieving robust suppression of outliers without explicitly removing point clouds.

[0058] Using the pose output from the IMU pre-integration as the initial value for iteration, the Levenberg-Marquardt algorithm is used to iteratively solve the objective function. In each iteration, the Jacobian and Hessian matrices of the objective function with respect to the pose increment are calculated, and the pose update is solved by linearization until the residual converges or the maximum number of iterations is reached.

[0059] Specific implementation method two: The synchronous positioning and mapping device for multi-sensor fusion of unmanned equipment described in this implementation method includes: Data preprocessing module; used to perform IMU pre-integration and lidar data preprocessing in the SLAM system to obtain lidar point cloud data with motion distortion correction and IMU motion constraints between adjacent frames; Edge point set compilation module; used to classify planar points based on the motion distortion corrected lidar point cloud data, then fit the classified points to straight lines to form the contour of the environment, derive virtual edge points by finding the intersection points between the straight lines, and merge the virtual edge points with the edge points obtained from curvature analysis to compile the final edge point set; A global map construction module is used to perform feature matching and point cloud registration based on the final edge point set, generate laser odometry factors, construct a factor map in combination with the IMU motion constraints, and construct a global map using the pose output by the backend optimization.

[0060] Specific implementation method three addresses the issue of cumulative drift in structured environment SLAM systems by employing a two-stage loop closure detection framework combining geometric and semantic features. First, a hybrid descriptor is used to concatenate the polar coordinate grid geometric features (maximum height statistics of loop sectors) of Scan-Context (SC) based on BEV projection with the semantic graph features (histogram of fixed instance category distribution and topological relationships) extracted by the SGLC module. L2 normalization is applied to balance the weights of the two features, enhancing the discriminability between different regions. Then, a hierarchical loop closure detection strategy is employed. Key features of the SC loop are used for rapid initial screening of candidate keyframes. Precise screening is then achieved through geometric-semantic similarity calculation using the hybrid descriptor and semantic graph node matching driven by the Hungarian algorithm (selecting the subgraph with the highest consistency). Finally, semantic-assisted pose optimization is combined. Based on the center coordinates of the matched nodes, the coarse registration transformation matrix is ​​solved using Singular Value Decomposition (SVD). This result is used as the initial value, and the point cloud alignment is iteratively optimized using the Iterative Closest Point (ICP) algorithm.

[0061] In the backend map construction process, constraints from frame to sub-map and frame to global map are employed to achieve high-precision pose output. Factor graph optimization is used for map optimization. The factor graph consists of nodes and edges, where factors provide different constraints for the system. Relative motion between adjacent keyframes is predicted using inertial measurement unit (IMU) data and used as a constraint in the factor graph. For laser point cloud data, the system establishes local map constraints through point cloud feature matching and uses the matching result between the current frame and the local map as a laser odometry factor for constraint, connecting the pose nodes of adjacent keyframes. At the global optimization level, loop closure detection is used to detect the matching relationship between historical keyframes and the current frame, and the relative pose of the two is constrained. This hierarchical constraint mechanism achieves global pose optimization, thereby optimizing the map. Figure 8 This is a schematic diagram of the pose and observation point in the factor graph.

[0062] Specific implementation method four: This implementation method verifies the synchronous positioning and mapping method for multi-sensor fusion for unmanned equipment described in this invention through experiments; Figure 9 The flowchart illustrates the multi-feature extraction process using the method described in this invention. Figure 10 The absolute trajectory error of each algorithm in the corridor sequence of the VECtor dataset is the method described in this invention. Figure 11 The method described in this invention is used in the VECtor dataset.

Claims

1. A synchronous localization and mapping method for multi-sensor fusion in unmanned equipment, characterized in that, The method includes: S1. Perform IMU pre-integration and lidar data preprocessing in the SLAM system to obtain lidar point cloud data with motion distortion correction and IMU motion constraints between adjacent frames. S2. Based on the motion distortion corrected lidar point cloud data, the planar points are classified, and then the classified points are fitted with straight lines to form the contour of the environment. Virtual edge points are derived by finding the intersection points between the straight lines, and the virtual edge points are merged with the edge points obtained from curvature analysis to compile the final edge point set. S3. Based on the final edge point set, perform feature matching and point cloud registration to generate laser odometry factors, and construct a factor map by combining the IMU motion constraints. Use the pose output by the backend optimization to construct a global map.

2. The synchronous localization and mapping method for multi-sensor fusion of unmanned equipment according to claim 1, characterized in that, The process of S1 includes: S11. Pre-integrate the IMU data and calculate the relative motion increment between adjacent lidar frames by using the translational acceleration and attitude angular velocity of the object at each moment acquired by the IMU. S12. The rotation and translation matrices of the object at each moment obtained by the IMU pre-integration are used for spherical linear interpolation to convert all point cloud data to the start or end time of each frame of point cloud scanning, so as to achieve point cloud distortion removal.

3. The synchronous localization and mapping method for multi-sensor fusion of unmanned equipment according to claim 1, characterized in that, The process of S2 includes: S21. Divide each laser scanning line into 6 segments, process each segment independently, sort the points of each segment in descending order of curvature, select the first 20 points as corner points starting from the point with the largest curvature and mark the surrounding points, and select points with curvature below the threshold to obtain plane points. S22. Use covariance matrix decomposition to calculate the point cloud normal vector, and use the included angle formula to determine whether it is a ground point. Based on the characteristics of the surface normal vector, distinguish the planar points into ground points and non-ground points. S23. Ground points participate in the generation of virtual points. In the divided independent subspace, the ground points are separated into geometrically continuous subclusters. A straight line hypothesis is constructed by random sampling and the interior points are screened. The straight line with the most interior points is selected. S24. Calculate the intersection point of any two straight lines. If the included angle is greater than the threshold and the intersection point is located near the boundary of the subspace, add the intersection point to the virtual point set and project the non-ground points onto the ground plane. Check the projected points. If they are located in the edge area of ​​the ground block, add them to the virtual point set. S25. Using the intensity information of the laser point cloud as an auxiliary feature, the intensity value is filtered by mean. Each frame of point cloud is divided into six sub-regions. An adaptive threshold for intensity features is set based on the histogram value of each sub-region. Points with intensity greater than the threshold and local intensity change rate greater than the minimum change rate threshold are selected as edge points. S26. Merge the edge points obtained from curvature analysis, the edge points extracted based on intensity information, and the virtual points to compile the final edge point set.

4. The synchronous localization and mapping method for multi-sensor fusion of unmanned equipment according to claim 1, characterized in that, S3 include: S31. Perform point cloud registration for edge points and planar points respectively: For each edge point in the current frame, find the nearest edge line in the previous frame or local map and calculate the distance from the point to the line; for each planar point in the current frame, find the nearest planar block in the previous frame and local map and calculate the distance from the point to the plane. S32, Joint Optimization of Hierarchical Robust Cost Function: Establish geometric residual equations for edge points to lines and plane points to surfaces, and introduce motion constraints provided by IMU pre-integration as regularization terms; An M-estimator kernel function is introduced to automatically suppress the interference of outlier matching points on the optimization objective by dynamically adjusting the residual weights; the M-estimator kernel function includes Huber or Cauchy. The Levenberg-Marquardt algorithm is used to iteratively solve the nonlinear cost function, and the sparsity of the Hessian matrix is ​​used to quickly update the pose estimate. S33. The point cloud registration results are used as laser odometry factors, which together with IMU factors form a factor map. The factors are grouped using a graph optimization method. Two frames of pose are obtained from step-by-step loop closure detection for global pose optimization. The optimized pose is then used to construct a global map.

5. The synchronous localization and mapping method for multi-sensor fusion of unmanned equipment according to claim 1, characterized in that, In S11, when pre-integrating the IMU data, the rotation integral of the IMU data within the time window of two lidar frames is represented by quaternions; the rotation integral formula is as follows: （1） In the formula, These are gyroscope measurements. To achieve zero bias in the gyroscope, For time intervals.

6. The synchronous localization and mapping method for multi-sensor fusion of unmanned equipment according to claim 1, characterized in that, The point cloud distortion removal described in S12 uses the Slerp spherical linear interpolation method, and the interpolation parameters for the spherical linear interpolation are: （4） In the formula, , is the data collection time; Interpolated quaternions for: (5) in, , For adjacent IMU times, , Let q be the rotation quaternion corresponding to adjacent IMU times; given a unit quaternion q, the formula for calculating its corresponding rotation matrix is: （6） Point Transform from the coordinate system of the acquisition time to the reference time The coordinate system is: （7） in, and yes and The corresponding rotation matrix, and It is a translation vector.

7. The synchronous localization and mapping method for multi-sensor fusion of unmanned equipment according to claim 1, characterized in that, In S21, the curvature is calculated using the eigenvalue method of the covariance matrix. The formula for calculating the covariance matrix of neighborhood points is: （9） in, Let be the centroid of the neighboring points, and k be the number of neighboring points. Let be the coordinates of the j-th point in the neighborhood; decompose the covariance matrix C to obtain the eigenvalues ​​λ1≤λ2≤λ3; The formula for calculating curvature is: (10) A larger value indicates a more curved surface, corresponding to edge feature corner points. The smaller the value, the flatter the surface, corresponding to planar feature points.

8. The synchronous localization and mapping method for multi-sensor fusion of unmanned equipment according to claim 1, characterized in that, In S25, the intensity data undergoes mean filtering. The mean filtering formula for a window size of 2 is as follows: （11） in, The intensity value of the k-th point in the original point cloud. This represents the intensity value at point i after filtering. The formula for calculating the standard deviation of intensity in a sub-region is: （12） in, Let be the average intensity of sector s. Let N be the number of points in sector s, and N be the set of point clouds within the sub-region. and Let i be the intensity at points i and j.

9. The synchronous localization and mapping method for multi-sensor fusion of unmanned equipment according to claim 1, characterized in that, The formulas for calculating the residuals at edge points and plane points in S32 are as follows: （23） （24） in, For the edge points of the current frame, As the starting point of the reference frame edge line, The direction vector of the line. For the reference frame plane normal vector, Let be a point on the plane.

10. A multi-sensor fusion synchronous localization and mapping device for unmanned equipment, characterized in that, The device includes: Data preprocessing module; used to perform IMU pre-integration and lidar data preprocessing in the SLAM system to obtain lidar point cloud data with motion distortion correction and IMU motion constraints between adjacent frames; Edge point set compilation module; used to classify planar points based on the motion distortion corrected lidar point cloud data, then fit the classified points to straight lines to form the contour of the environment, derive virtual edge points by finding the intersection points between the straight lines, and merge the virtual edge points with the edge points obtained from curvature analysis to compile the final edge point set; A global map construction module is used to perform feature matching and point cloud registration based on the final edge point set, generate laser odometry factors, construct a factor map in combination with the IMU motion constraints, and construct a global map using the pose output by the backend optimization.

Images