Lightweight high-precision laser / inertial slam method

Abstract

The application discloses a lightweight high-precision laser / inertial SLAM method and belongs to the technical field of satellite sheltered space autonomous positioning and navigation. Regular feature points are extracted by combining IMU pre-integration and point cloud smoothness analysis. The least square section fitting is used to parameterize the feature points into spatial center point coordinates. An information matrix is constructed, and an effective feature subset is screened to reduce the calculation amount of subsequent matching and optimization. Based on principal component analysis, the constraint conditions of features in six degrees of freedom are evaluated, and the uncertainty corresponding to each degree of freedom is obtained. In the joint factor graph optimization process, the point-to-point Euclidean distance residual of the rod-shaped feature to the prior map fitting center is constructed, and the laser observation covariance is adjusted according to the uncertainty. Whether the current frame point cloud participates in the map update is judged according to the posterior evaluation result. The application can reduce the positioning drift, reduce the calculation burden and reduce the influence of false matching on the global map update.

Description

Technical Field

[0001] This invention belongs to the field of satellite-shaded space autonomous positioning and navigation technology, and specifically relates to a lightweight, high-precision laser / inertial SLAM method. Background Technology

[0002] Currently, with the rapid development of autonomous driving, intelligent manufacturing, and underground space development technologies, high-precision spatial positioning and mapping of mobile robots are playing an increasingly important role in fields such as unmanned vehicle navigation, warehousing and logistics, and utility tunnel inspection. Because satellite (GPS) signals are severely limited or even nonexistent in underground utility tunnels or urban canyons, SLAM technology based on LiDAR and inertial measurement units (IMU) has become a core support for solving the positioning challenges in unknown and complex environments, possessing significant social and economic value.

[0003] Traditional laser-inertial SLAM systems primarily rely on geometric feature matching to suppress IMU drift. However, in degraded environments with simple structures such as long corridors and tunnels, the effective normal constraints in the environment are insufficient, easily leading to ill-conditioned Hessian matrices during nonlinear optimization, resulting in a sharp degradation of positioning performance and trajectory drift. To overcome the problem of degraded positioning performance in degraded scenarios, degradation perception and feature selection techniques have been extensively studied. However, existing methods still face the following challenges: First, the feature utilization dimension is limited; existing algorithms are restricted to line and surface features and cannot effectively extract and utilize naturally degrade-resistant vertical rod-like structures in the environment. Second, data association efficiency is low; traditional fixed-resolution hard voxelization cannot assess the amount of feature information, easily causing redundant point cloud accumulation and increasing the computational burden on the front end. Third, degradation perception is severely lagging; existing methods mostly rely on post-evaluation based on optimized matrix eigenvalues, making it difficult to proactively predict risks before alignment failure, which can easily lead to numerical collapse. Fourth, weight allocation is not refined enough; once degradation is detected, laser observations are usually discarded in a coarse manner, lacking adaptive residual weight adjustment for the six independent degrees of freedom in space. Summary of the Invention

[0004] To overcome the numerous shortcomings of existing technologies, this invention provides a lightweight, high-precision laser / inertial SLAM method. It innovatively proposes a set of methods including front-end feature extraction, lightweight feature optimization and selection, PCA observability quantization, and adaptive weighting for degradation direction, along with a dynamic map update method based on posterior evaluation.

[0005] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0006] A lightweight, high-precision laser / inertial SLAM method includes the following steps:

[0007] Step 1: Combine the IMU pre-integration results to perform distortion correction on the current frame point cloud. Based on the depth difference of the local neighborhood and the column index span, identify and remove pseudo feature points caused by parallel beams and physical occlusion, and extract the corner features and surface features of the current frame point cloud.

[0008] Step 2: Based on the corner and surface features of the current frame point cloud extracted in Step 1, project the 3D point cloud onto a 2D depth panorama; use the breadth-first traversal algorithm and geometric topology feature verification, combined with 2D least squares cross-section fitting, to obtain the center coordinates of the rod-shaped objects in the scene; the rod-shaped objects are parameterized as spatial circle center coordinates as non-degenerate features for subsequent pose optimization.

[0009] Step 3: Determine the maximum number of features to retain by calculating the degradation factor of the candidate feature set composed of the corner features, surface features extracted in Step 1 and the rod-shaped features extracted in Step 2, and use a random greedy algorithm to select a subset of features that contribute to pose estimation from the candidate features.

[0010] Step 4: For the feature subset obtained in Step 3, use PCA of local neighborhood to obtain the spatial normal and principal direction; construct a six-degree-of-freedom observability distribution by projecting the position coordinates and normal constraints of the angular features, surface features and rod features, and obtain the prior risk vector corresponding to each independent degree of freedom.

[0011] Step 5: Construct a laser-inertial joint factor map. Based on the parameterized rod features in Step 2, construct the point-to-point Euclidean distance residual from the center coordinates of the rod in the current frame to the fitted center of the corresponding rod in the prior map, and calculate the observation Jacobian matrix of the residual with respect to the pose parameters to form the laser observation factor of the rod.

[0012] Based on the observability risk vector obtained in step 4, the covariance matrix of the laser odometry factor is adjusted in different directions. For the degradation direction, the laser observation constraint is weakened, and the pose constraint is provided by the inertial measurement unit. For the non-degradation direction, the constraint effect of rod features and other geometric features on pose is preserved.

[0013] Finally, the Levenberg-Marquardt algorithm is used to iteratively solve the joint factor graph;

[0014] Step 6: After the nonlinear optimization converges, extract the pose update amount and the eigenvalue distribution of the Hessian matrix for the current frame, and compare them with the risk vector output in Step 4 to determine the matching reliability of the current frame in each degree of freedom. Based on the determination result, decide whether to incorporate the current frame point cloud into the global map, thereby achieving dynamic updating of the global map.

[0015] Furthermore, the specific method for extracting the rod-shaped feature in step 2 is as follows:

[0016] First, the 3D point cloud is projected onto a 2D depth map according to its azimuth and pitch angles, resulting in a map with dimensions of... A two-dimensional depth matrix, where, This represents the number of rows in the depth map, corresponding to the vertical scan resolution. This represents the number of columns in the depth map, corresponding to the horizontal scan resolution.

[0017] In the 2D depth map, after removing invalid background points, the BFS algorithm is used to cluster the remaining points into connected components. If the neighboring points of the current point in the 2D depth map satisfy the depth continuity condition, that is, the depth difference between adjacent points satisfies the condition... <0.2 m, and elevation If a point is located within a preset physical interval, its neighboring point is incorporated into the current connected cluster; where, The depth difference between adjacent points; The elevation coordinates of the neighboring point; the physical interval is used to constrain the height range of the candidate region for the rod-shaped object;

[0018] For each connected cluster, calculate the aspect ratio of its two-dimensional bounding box:

[0019]

[0020] in, The aspect ratio of the current connected cluster; Let be the span of the connected cluster in the vertical direction; Let S be the horizontal span of the connected cluster; when If the physical chord width exceeds a set threshold, the connected cluster is determined not to be a rod-shaped object and is removed.

[0021] For the connected clusters that pass the screening, backproject them to a three-dimensional Cartesian coordinate system and then onto the XY plane; let the point set after projection be... To centralize its processing:

[0022]

[0023] in, For the first The coordinates of each projection point in the XY plane; and These are the mean values ​​of all points in the connected cluster in the X and Y directions, respectively. The coordinates are centered planar coordinates. A two-dimensional circle is fitted based on the centered point set to obtain the center offset. And based on this, calculate the center of the cross-section of the rod:

[0024]

[0025] in, This is the offset of the fitted circle center relative to the centralized origin; Let be the coordinates of the center of the cross-section of the rod in the plane coordinate system; let the spatial center point of the rod be:

[0026]

[0027] in, The spatial center point of the rod-shaped object after its characteristic parameterization; Here are the vertical coordinates of the center point of the circle.

[0028] Furthermore, the feature selection method based on the random greedy algorithm in step 3 is as follows: First, based on the feature set... Constructing an information matrix And define the macroscopic degradation factor:

[0029]

[0030] in, For candidate feature set The constructed information matrix; For determinant operations; This represents the macroscopic degradation factor in the current scenario; The larger the value, the stronger the overall constraint of the current feature set on pose estimation;

[0031] When the degradation factor is higher than the safety threshold ( When the system determines that it is currently in a benign observation state, it selects feature points as the maximum retention base. Conversely, if This indicates that the current scene is at a high level of degradation, and in this case, the maximum retention base should be increased. ;

[0032] in, The threshold for determining degradation; The maximum number of features allowed to proceed to subsequent optimization stages;

[0033] Let the subset of features to be selected be The feature selection problem is then expressed as:

[0034]

[0035] in, This is the subset of target features obtained through filtering; For subset The number of features; For the first The observation Jacobian matrix corresponding to each feature; A matrix of historical prior information; For the first The inverse covariance matrix of the observations corresponding to each feature; for the rod-shaped feature... The residual from the point on the rod to the center of the fitted circle is obtained by differentiating the pose parameters.

[0036] A random greedy algorithm is used for approximate solution. In each iteration, a subset of states is randomly sampled from the remaining candidate features. Its size is defined as:

[0037]

[0038] in, The size of the subset of random states; For candidate feature set The total number of characteristics; This is the tolerance decay factor; in the state subset Internal retrieval enables the objective function The optimal feature point for generating maximum gain is moved into Iterate multiple times until the maximum base is reached. Upon completion, a preferred set of features is obtained for subsequent degradation assessment and joint optimization.

[0039] Furthermore, the specific method for quantitative evaluation of the observability of six-degree-of-freedom features based on PCA in step 4 is as follows: Perform local geometric analysis on the preferred feature set obtained in step 3; for any feature point... Search for it in the prior map Find the nearest neighbors and construct the covariance matrix of that neighborhood. For the matrix Apply eigenvalue decomposition:

[0040]

[0041] in, For feature vectors; For eigenvalues;

[0042] Establish orthogonal basis of global rigid body coordinate system Based on the relationship between the feature point positions and the normal constraints, the spatial moment vector is defined as follows:

[0043]

[0044] in, For the first The spatial moment vector generated by each feature point; This is the normal constraint vector corresponding to the feature point; Project the projections onto the global three axes and sum the projection results to the rotated histogram. In the middle, at the same time, the normal vector The absolute projection results on the global three axes are accumulated into the translation histogram. middle;

[0045] The uncertainty of the translational degree of freedom in the X direction is defined as:

[0046]

[0047] in, This represents the cumulative value in the translation direction corresponding to the X-axis in the translation histogram; This is the cumulative sum of all histogram intervals in the translated histogram;

[0048] The uncertainty of each degree of freedom is denoted as ,and Construct a six-degree-of-freedom prior risk vector:

[0049]

[0050] in, This is the six-degree-of-freedom prior risk vector corresponding to the current frame; superscript Indicates transpose; The closer it is to 1, the higher the risk of degradation of the corresponding degree of freedom.

[0051] Furthermore, the specific method for joint optimization of direction adaptive weight adjustment in step 5 is as follows: Construct a factor graph model jointly using lidar, inertial measurement unit, and prior information. The residual objective function for joint optimization is:

[0052]

[0053] in, This is the pre-integrated residual term; For laser observation residuals; For the prior residual term; , , These correspond to the covariance matrices of each residual term. The laser observation residuals in the objective function described above... It includes residual terms constructed from angular features, surface features, and rod-shaped features. The residuals and Jacobian matrices corresponding to the rod-shaped features are constructed based on the spatial center coordinates of the rod-shaped features obtained in step 2.

[0054] In the joint factor graph, the spatial center coordinates of the rod obtained in step 2 are used as part of the laser observation constraint; let the rod point mapped to the global coordinate system in the current frame be... The center of the fitted circle is Then we have:

[0055]

[0056] in, It is a rotation matrix; It is a translation vector; This represents the position of the rod-shaped feature point in the current frame in the global coordinate system. The coordinates of the fitted center of the corresponding rod-shaped object on the map;

[0057] Define the Euclidean distance residual from the rod-shaped point to the center of the map fitting circle as:

[0058]

[0059] in, Matching residuals for rod-shaped objects; for Coordinates in the plane; for The coordinates in the plane. When solving for the Jacobian matrix of the residual with respect to pose, calculate the partial derivatives of the Euclidean distance residual with respect to the three-axis rotation and three-axis translation components respectively; concatenate the above partial derivative results according to the six-degree-of-freedom pose parameters to obtain the observation Jacobian vector corresponding to the rod-shaped feature:

[0060]

[0061] in, , , These are the partial derivatives of the residuals with respect to the rotation about the X, Y, and Z axes, respectively. , , This is the partial derivative of the residual with respect to the translation along the X, Y, and Z directions; For the first The observation Jacobian vectors corresponding to the features of each rod-shaped object are then used for joint optimization. Next, the observation residual factors of the rod-shaped objects are combined with other laser observation residual factors, IMU pre-integration residual factors, and prior residual factors. A global Jacobian matrix is ​​then constructed from the Jacobian matrix and residual terms corresponding to each residual factor. With residual vector Then the pose increment Solve using normal equations:

[0062]

[0063] Based on the risk vector obtained in step 4 ,right Adjustment is performed per degree of freedom; when the uncertainty in a certain degree of freedom... At that time, the variance of laser observations in the corresponding direction is amplified by the following formula:

[0064]

[0065] in, To adjust the covariance matrix in the th Diagonal elements in one degree of freedom; Based on the observed variance; This is the gain amplification factor; As a safety threshold; with As the value increases, the laser observation weights on the corresponding degrees of freedom decrease.

[0066] Due to the adoption of the above technical solution, the beneficial effects of this invention compared with the prior art are as follows:

[0067] 1. Solve the problem of localization failure in feature degradation scenarios: By extracting the features of vertical rods, additional three-dimensional geometric constraints are provided to the system, thereby making up for the lack of constraints of traditional face points and corner points in a single structural environment.

[0068] 2. Lightweight Feature Point Cloud Processing: By introducing an optimization mechanism based on a random greedy algorithm, coupled with dynamic degradation factor adjustment of sampling, significant compression and lightweight representation of point cloud data are achieved. While retaining the core constraint information entropy, the retrieval and computational overhead of Kd-Tree are reduced.

[0069] 3. Identification and risk assessment of degradation direction: By performing principal component analysis on local features and combining the constraint distribution on each degree of freedom, the risk assessment results of the corresponding degree of freedom are obtained. This allows us to identify the direction with weaker constraints before optimization and adjust the observation weights in the subsequent optimization process accordingly.

[0070] 4. Adaptive weight adjustment and map update reliability: By adjusting the weights of laser observations on different degrees of freedom and controlling map updates in combination with posterior evaluation results, the impact of mismatches on trajectory estimation and map construction in degraded scenarios can be reduced. Attached Figure Description

[0071] Figure 1 This is a flowchart of the rod-assisted laser SLAM method that integrates a random greedy algorithm and degradation perception in an embodiment of the present invention.

[0072] Figure 2 This is a flowchart of alignment risk prediction based on PCA feature observability in an embodiment of the present invention. Detailed Implementation

[0073] The technical solution of the present invention will be described in detail below with reference to the accompanying drawings.

[0074] This invention designs a lightweight, high-precision laser / inertial SLAM method, such as... Figure 1 As shown, it includes the following steps:

[0075] Step 1: Laser point cloud preprocessing and conventional feature extraction. The current frame point cloud is processed to remove distortion based on the IMU pre-integration results. False feature points caused by parallel beams and physical occlusion are identified and removed according to the depth difference and column index span of the local neighborhood. Corner and surface features of the current frame point cloud are then extracted.

[0076] Step 2: Point Cloud Rod Feature Extraction. After separating regular corner points and face points, the 3D point cloud is projected onto a 2D depth panorama. Using a breadth-first search (BFS) algorithm and geometric topological feature verification, combined with 2D least-squares cross-section fitting, the center coordinates of rods in the scene are obtained. The rods are parameterized as non-degenerate features into spatial circle center coordinates for subsequent pose optimization.

[0077] Step 3: Based on the stochastic greedy algorithm, construct the Fisher information matrix to measure the system's state constraint capability. Determine the maximum number of features to retain by calculating the degradation factor of global features, and then use the stochastic greedy algorithm to select a subset of features that contribute significantly to pose estimation from the candidate features, thus achieving feature lightweighting.

[0078] Step 4: Quantitative evaluation of the observability of six-degree-of-freedom features based on Principal Component Analysis (PCA). For the optimized feature subset, the spatial normal and principal direction are obtained using PCA of the local neighborhood. A six-degree-of-freedom observability distribution is constructed based on the projection relationship between the feature point positions and the normal constraints, and the uncertainty vector corresponding to each independent degree of freedom is obtained.

[0079] Step 5: Joint Optimization with Adaptive Weight Adjustment. A laser-inertial joint factor map is constructed. For the parameterized rod-like features, the point-to-point Euclidean distance residual from the rod to the center of the prior map fitting circle is constructed, and the observation Jacobian matrix of this residual with respect to the pose parameters is calculated. Based on the observability uncertainty vector obtained in Step 4, the covariance matrix of the laser odometry factor is adjusted directionally. For degenerate directions, the laser observation constraint is weakened, mainly utilizing the pose constraint provided by the inertial measurement unit; for non-degenerate directions, the constraint effect of the rod-like features and other geometric features on the pose is preserved. Finally, the Levenberg-Marquardt algorithm is used to iteratively solve the joint factor map.

[0080] Step 6: Dynamic Map Update Based on Posterior Evaluation. After the nonlinear optimization converges, the pose update amount and the eigenvalue distribution of the Hessian matrix for the current frame are extracted and compared with the prior uncertainty vector output in Step 4 to determine the matching reliability of the current frame in each degree of freedom. Based on the determination result, it is decided whether to incorporate the current frame point cloud into the global map, thereby achieving dynamic global map update.

[0081] The specific process of step 1 is as follows:

[0082] The 3D point cloud of the current frame, after IMU pre-integration compensation, is acquired, and its geometric structure is analyzed line-by-line. To characterize local curvature, for any point on the scanning line... Select the two before and after it A local neighborhood is formed by two adjacent points. and define points The formula for calculating smoothness is:

[0083]

[0084] in, For point Smoothness; The first in the current scan beam One point; For the point A local neighborhood constructed around a central point; For the index of points in the neighborhood; For point Euclidean distance to the center of the lidar; For the neighboring region The Euclidean distance from each point to the center of the lidar.

[0085] In complex real-world spatial environments, traditional curvature calculations can lead to geometric ambiguities due to physical occlusion and parallel beams. To reduce the impact of spurious features caused by occlusion and parallel beams, this invention introduces a joint depth-angle screening mechanism: if two adjacent points... and The difference in column indices on the image is less than a set threshold, but the absolute depth difference between the two points is significantly amplified (e.g., If the distance is greater than 0.3 m, then the points that are farther away and their neighboring points will be marked as invalid points.

[0086] in, For points The next adjacent laser point; The absolute depth difference between the two points is used to determine whether the two points are adjacent in the distance image.

[0087] A single scan line is divided into several sub-sectors in space. Smoothness is adjusted within each sector. Sort. When And if the point is not marked as invalid, it is considered a corner candidate; when At that time, it was considered as a candidate for pastry.

[0088] in, Threshold for corner point extraction Set the threshold for face point extraction. This completes the extraction of corner and face point features for the current frame.

[0089] The specific process of step 2 is as follows:

[0090] First, the 3D point cloud is projected onto a 2D depth map according to its azimuth and pitch angles, resulting in a map with dimensions of... A two-dimensional depth matrix. Where, This represents the number of rows in the depth map, corresponding to the vertical scan resolution. This represents the number of columns in the depth map, corresponding to the horizontal scan resolution.

[0091] In the 2D depth map, after removing invalid background points, a breadth-first search method is used to cluster the remaining points into connected components. If the neighboring points of the current point in the 2D depth map satisfy the depth continuity condition, that is, the depth difference between adjacent points satisfies... <0.2 m, and elevation If a point is located within a preset physical interval, then that neighboring point will be incorporated into the current connected cluster.

[0092] in, The depth difference between adjacent points; The elevation coordinates of this point are given; the physical interval is used to constrain the height range of the candidate region for the rod-shaped object.

[0093] For each connected cluster, calculate the aspect ratio of its two-dimensional bounding box:

[0094]

[0095] in, The aspect ratio of the current connected cluster; Let be the span of the connected cluster in the vertical direction; Let be the horizontal span of the connected cluster. When If the physical chord width exceeds a set threshold, the connected cluster is determined not to be a rod-shaped object and is removed.

[0096] For the connected clusters that pass the screening, backproject them to a three-dimensional Cartesian coordinate system and then onto the XY plane. Let the projected point set be... To centralize its processing:

[0097]

[0098] in, For the first The coordinates of each projection point in the XY plane; and These are the mean values ​​of all points in the connected cluster in the X and Y directions, respectively. The coordinates are centered planar coordinates. A two-dimensional circle is fitted based on the centered point set to obtain the center offset. And based on this, calculate the center of the cross-section of the rod:

[0099]

[0100] in, This is the offset of the fitted circle center relative to the centralized origin; Let be the coordinates of the center of the cross-section of the rod in a plane coordinate system. Further, let the spatial center point corresponding to the rod be:

[0101] in, The spatial center point of the rod-shaped object after its characteristic parameterization; Here are the vertical coordinates of the center point of the circle.

[0102] Step 2 Extraction of Multidimensional Feature Set The sheer scale of the feature set would cause significant latency if directly fed into the matching network. Therefore, step 3 introduces information entropy theory to perform dimensionality reduction and optimization by evaluating the theoretical contribution of features to localization performance. The optimization process for various features using a greedy algorithm is as follows.

[0103] The specific process of step S3 is as follows:

[0104] (1) Macro-degradation assessment and dynamic constraints

[0105] First, based on the feature set Constructing an information matrix And define the macroscopic degradation factor:

[0106]

[0107] in, For candidate feature set The constructed information matrix; For determinant operations; This represents the macroscopic degradation factor in the current scenario. The larger the value, the stronger the overall constraint of the current feature set on pose estimation.

[0108] When the environment has rich texture, the degradation factor is higher than the safety threshold ( When the system determines that it is in a benign observation state, it only needs to select a smaller number of feature points as the maximum retention base. Conversely, if This indicates that the current scene is at a high level of degradation, and in this case, it is appropriate to increase the maximum retention base. .

[0109] in, The threshold for determining degradation; This is the maximum number of features allowed to proceed to subsequent optimization stages.

[0110] (2) Subset optimization based on random greedy search

[0111] Let the subset of features to be selected be Then the feature selection problem can be expressed as:

[0112]

[0113] in, This is the subset of target features obtained through filtering; For subset The number of features; For the first The observation Jacobian matrix corresponding to each feature; A matrix of historical prior information; For the first The inverse observation covariance matrix corresponding to each feature. For the rod-shaped feature, The residual from the point on the rod to the center of the fitted circle is obtained by differentiating the pose parameters.

[0114] Since the above problem is a combinatorial optimization problem with cardinality constraints, it is impossible to find the absolute optimal solution in polynomial time. However, because its objective function is monotonically increasing and has a sub-model property, i.e., it has diminishing returns, this invention abandons global exhaustive search and uses a stochastic greedy algorithm for approximate solution. In each iteration, a subset of states is randomly sampled from the remaining candidate features. Its size is defined as:

[0115]

[0116] in, The size of the subset of random states; For candidate feature set The total number of characteristics; This is the tolerance attenuation factor. In the subset... Internal retrieval enables the objective function The optimal feature point for generating maximum gain is moved into Iterate multiple times until the maximum base is reached. Upon completion, a preferred set of features is obtained for subsequent degradation assessment and joint optimization.

[0117] To address the lag in degradation feedback in traditional systems, principal component analysis (PCA) is used to pre-quantify the alignment failure risk of the point cloud in six degrees of freedom before the features enter the optimizer. Further, the specific method for step S4 is as follows:

[0118] Perform local geometric analysis on the preferred feature set obtained in step 3. For any feature point... Search for it in the prior map Find the nearest neighbors and construct the covariance matrix of that neighborhood. For the matrix Apply eigenvalue decomposition:

[0119]

[0120] in, For feature vectors; Let the three eigenvalues ​​be eigenvalues; let them be ordered by size as follows: For pastries, the smallest eigenvalue Corresponding feature vector As a plane normal vector; for line points and rod-shaped object points, the maximum eigenvalue is... Corresponding feature vector As the principal direction vector.

[0121] Establish orthogonal basis of global rigid body coordinate system Based on the relationship between the feature point positions and the normal constraints, the spatial moment vector is defined as follows:

[0122]

[0123] in, For the first The spatial moment vector generated by each feature point; This is the normal constraint vector corresponding to the feature point; Project the projections onto the global three axes and sum the projection results to the rotated histogram. In the middle, at the same time, the normal vector The absolute projection results on the global three axes are accumulated into the translation histogram. middle.

[0124] Taking the translational degree of freedom in the X direction as an example, its uncertainty is defined as:

[0125]

[0126] in, This represents the cumulative value in the translation direction corresponding to the X-axis in the translation histogram; This is the cumulative sum of all histogram intervals in the translated histogram.

[0127] The uncertainty of each degree of freedom is denoted as ,and Furthermore, a six-degree-of-freedom prior risk vector is constructed:

[0128] .

[0129] in, This is the six-degree-of-freedom prior risk vector corresponding to the current frame; superscript Indicates transpose; such as Figure 2 As shown, The closer it is to 1, the higher the risk of degradation of the corresponding degree of freedom.

[0130] The specific method for step 5 is as follows:

[0131] A joint factor graph model is constructed, integrating lidar, inertial measurement unit, and prior information. The joint optimization residual objective function is:

[0132]

[0133] in, This is the pre-integrated residual term; For laser observation residuals; For the prior residual term; , , These correspond to the covariance matrices of each residual term. The laser observation residuals in the objective function described above... The Jacobian matrix required for solving is constructed based on the characteristics of the rod-shaped object obtained in step 2.

[0134] The rod-shaped object was extracted and fitted in step 2, and its parameterization was performed using spatial circle center coordinates. Let the point of the rod-shaped object mapped to the global coordinate system in the current frame be... The center of the fitted circle is Then we have:

[0135]

[0136] in, It is a rotation matrix; It is a translation vector; This represents the position of the rod-shaped feature point in the current frame in the global coordinate system. The coordinates of the fitted center of the corresponding rod-shaped object on the map.

[0137] Define the Euclidean distance residual from the rod-shaped point to the center of the map fitting circle as:

[0138]

[0139] in, Matching residuals for rod-shaped objects; for Coordinates in the plane; for The coordinates in the plane. The Jacobian matrix of the residual with respect to pose is solved using the chain rule. First, the residual with respect to the global coordinate points is calculated. The gradient partial derivatives, since the rod is projected onto a two-dimensional cross-section fitting, have a Z-axis derivative of 0. The components in each direction are obtained as follows:

[0140]

[0141]

[0142]

[0143] in, , , These are the partial derivatives of the residuals with respect to the X, Y, and Z directions, respectively.

[0144] Therefore, the biased directional coefficient of the distance residual with respect to the global coordinates is: Then, the distance residual versus rotation amount was calculated separately. With translation The partial derivative. Rotation angle about the X-axis. For example:

[0145]

[0146] Similarly, we can obtain ,

[0147] For translation amount, For example, the derivation process is as follows:

[0148]

[0149] Similarly, we can conclude that: , Finally, the rotational partial derivative component and the translational partial derivative component are concatenated to form the Jacobian vector observed by the rod:

[0150]

[0151] in, , , These are the partial derivatives of the residuals with respect to the rotation about the X, Y, and Z axes, respectively. For the first The observed Jacobian vectors corresponding to each rod-shaped feature are combined into a global Jacobian matrix. And denote the residual vector Then the pose increment Solve using normal equations:

[0152]

[0153] Based on the risk vector obtained in step S4 ,right Adjustments are made per degree of freedom. When the uncertainty in a certain degree of freedom... At that time, the variance of laser observations in the corresponding direction is amplified by the following formula:

[0154]

[0155] in, To adjust the covariance matrix in the th Diagonal elements in one degree of freedom; Based on the observed variance; This is the gain amplification factor; This is a safety threshold. With... As the value increases, the laser observation weights on the corresponding degrees of freedom decrease; when When the value is small, the corresponding degrees of freedom still retain strong laser constraints. Thus, in the degenerate direction, IMU constraints are mainly relied upon, while in the non-degenerate direction, the constraints of laser and rod-like features are still retained.

[0156] The specific method of step S6 is as follows:

[0157] To ensure robustness and map consistency, this invention proposes a point cloud matching error evaluation and dynamic map update mechanism after the LM optimizer converges.

[0158] After optimization, extract the actual update magnitude of the current frame pose. And the final eigenvalue distribution of the Hessian matrix. Among them, To update the translation components, The rotation update components are used; the eigenvalues ​​of the Hessian matrix are used to reflect the strength of constraints on each degree of freedom.

[0159] Compare the actual solution state with the risk vector predicted in step 4. By comparison, when the uncertainty corresponding to a certain degree of freedom in the prior is large, and the eigenvalue of the posterior Hessian on that degree of freedom is also small, and the state update of that degree of freedom is mainly supported by the IMU residual term, it can be determined that the degree of freedom is degraded, and the prior risk assessment result is consistent with the posterior degraded state.

[0160] Based on the above comparison results, map updates are controlled. If the current frame still shows a significant risk of degradation in a certain degree of freedom, such as... Figure 1 As shown, the system will trigger a circuit breaker command, refusing to incorporate the entire current frame into the global map, or only using features corresponding to non-degenerate directions for map updates, to reduce the cumulative impact of mismatches in the global map; only when the current frame meets the set conditions in all degrees of freedom will the current frame's point cloud be incorporated into the global map. This can reduce map ghosting and distortion caused by local misregistration in degenerate scenarios.

[0161] In summary, this invention proposes a rod-assisted laser SLAM method that integrates a random greedy algorithm with six-DOF degradation perception. This method classifies, extracts, and filters corner points, facet points, and rod features from point clouds, introducing rod features as supplementary constraints. Simultaneously, it constructs a unified laser / inertial joint optimization framework by combining random greedy feature optimization, PCA-based observability assessment, directional covariance adjustment, and map update control under posterior evaluation. This method can identify weakly constrained degrees of freedom in degraded scenarios such as long corridors and tunnels, and adjusts the laser observation weights according to the degree of degradation, reducing the computational burden of full feature matching and mitigating the adverse effects of degraded frames on global map updates, thereby improving the stability and consistency of localization and mapping in complex degraded environments.

[0162] Those skilled in the art will recognize that the described embodiments are intended to help readers understand the principles of the invention and should be understood as not limiting the scope of protection of the invention to the described embodiments. Various modifications and variations can be made to the invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the invention should be included within the scope of the claims of the invention.

Claims

1. A lightweight, high-precision laser / inertial SLAM method, characterized in that, Includes the following steps: Step 1: Combine the IMU pre-integration results to perform distortion correction on the current frame point cloud. Based on the depth difference of the local neighborhood and the column index span, identify and remove pseudo feature points caused by parallel beams and physical occlusion, and extract the corner features and surface features of the current frame point cloud. Step 2: Based on the corner and surface features of the current frame point cloud extracted in Step 1, project the 3D point cloud onto a 2D depth panorama; use the breadth-first traversal algorithm and geometric topology feature verification, combined with 2D least squares cross-section fitting, to obtain the center coordinates of the rod-shaped objects in the scene; the rod-shaped objects are parameterized as spatial circle center coordinates as non-degenerate features for subsequent pose optimization. Step 3: Determine the maximum number of features to retain by calculating the degradation factor of the candidate feature set composed of the corner features, surface features extracted in Step 1 and the rod-shaped features extracted in Step 2, and use a random greedy algorithm to select a subset of features that contribute to pose estimation from the candidate features. Step 4: For the feature subset obtained in Step 3, use PCA of local neighborhood to obtain the spatial normal and principal direction; construct a six-degree-of-freedom observability distribution by projecting the position coordinates and normal constraints of the angular features, surface features and rod features, and obtain the prior risk vector corresponding to each independent degree of freedom. Step 5: Construct a laser-inertial joint factor map. Based on the parameterized rod features in Step 2, construct the point-to-point Euclidean distance residual from the center coordinates of the rod in the current frame to the fitted center of the corresponding rod in the prior map, and calculate the observation Jacobian matrix of the residual with respect to the pose parameters to form the laser observation factor of the rod. Based on the observability risk vector obtained in step 4, the covariance matrix of the laser odometry factor is adjusted in different directions. For the degradation direction, the laser observation constraint is weakened, and the pose constraint is provided by the inertial measurement unit. For the non-degradation direction, the constraint effect of rod features and other geometric features on pose is preserved. Finally, the Levenberg-Marquardt algorithm is used to iteratively solve the joint factor graph; Step 6: After the nonlinear optimization converges, extract the pose update amount and the eigenvalue distribution of the Hessian matrix in the current frame, and compare them with the risk vector output in Step 4 to determine the matching reliability of the current frame in each degree of freedom. Based on the judgment result, determine whether to incorporate the current frame point cloud into the global map, thereby achieving dynamic updates of the global map.

2. The lightweight, high-precision laser / inertial SLAM method according to claim 1, characterized in that, The specific method for extracting the rod-shaped object features in step 2 is as follows: First, the 3D point cloud is projected onto a 2D depth map according to its azimuth and pitch angles, resulting in a map with dimensions of... A two-dimensional depth matrix, where, This represents the number of rows in the depth map, corresponding to the vertical scan resolution. This represents the number of columns in the depth map, corresponding to the horizontal scan resolution. In the 2D depth map, after removing invalid background points, the BFS algorithm is used to cluster the remaining points into connected components. If the neighboring points of the current point in the 2D depth map satisfy the depth continuity condition, that is, the depth difference between adjacent points satisfies the condition... <0.2 m, and elevation If a point is located within a preset physical interval, its neighboring point is incorporated into the current connected cluster; where, The depth difference between adjacent points; The elevation coordinates of the neighboring point; the physical interval is used to constrain the height range of the candidate region for the rod-shaped object; For each connected cluster, calculate the aspect ratio of its two-dimensional bounding box: ， in, The aspect ratio of the current connected cluster; Let be the span of the connected cluster in the vertical direction; Let be the horizontal span of the connected cluster; when If the physical chord width exceeds a set threshold, the connected cluster is determined not to be a rod-shaped object and is removed. For the connected clusters that pass the screening, backproject them to a three-dimensional Cartesian coordinate system and then onto the XY plane; let the point set after projection be... To centralize its processing: ， in, For the first The coordinates of each projection point in the XY plane; and These are the mean values ​​of all points in the connected cluster in the X and Y directions, respectively. The coordinates are centered planar coordinates; a two-dimensional circle is fitted based on the centered point set to obtain the center offset. And based on this, calculate the center of the cross-section of the rod: ， in, This is the offset of the fitted circle center relative to the centralized origin; Let be the coordinates of the center of the cross-section of the rod in the plane coordinate system; let the spatial center point of the rod be: ， in, The spatial center point of the rod-shaped object after its characteristic parameterization; Here are the vertical coordinates of the center point of the circle.

3. The lightweight, high-precision laser / inertial SLAM method according to claim 2, characterized in that, The specific method for feature selection based on the random greedy algorithm in step 3 is as follows: First, based on the feature set... Constructing an information matrix And define the macroscopic degradation factor: ， in, For candidate feature set The constructed information matrix; For determinant operations; This represents the macroscopic degradation factor in the current scenario; The larger the value, the stronger the overall constraint of the current feature set on pose estimation; When the degradation factor is higher than the safety threshold ( When the system determines that it is currently in a benign observation state, it only needs to select feature points as the maximum retention base. Conversely, if This indicates that the current scene is at a high level of degradation, and in this case, the maximum retention base should be increased. ; in, The threshold for determining degradation; The maximum number of features allowed to proceed to subsequent optimization stages; Let the subset of features to be selected be Then the feature selection problem can be expressed as: ， in, This is the subset of target features obtained through filtering; For subset The number of features; For the first The observation Jacobian matrix corresponding to each feature; A matrix of historical prior information; For the first The inverse covariance matrix of the observations corresponding to each feature; for the rod-shaped feature... The residual from the point on the rod to the center of the fitted circle is obtained by differentiating the pose parameters. A random greedy algorithm is used for approximate solution. In each iteration, a subset of states is randomly sampled from the remaining candidate features. Its size is defined as: ， in, The size of the subset of random states; Candidate feature set The total number of characteristics; This is the tolerance decay factor; in the state subset Internal retrieval enables the objective function The optimal feature point for generating maximum gain is moved into Iterate multiple times until the maximum base is reached. Upon completion, a preferred set of features is obtained for subsequent degradation assessment and joint optimization.

4. The lightweight, high-precision laser / inertial SLAM method according to claim 3, characterized in that, The specific method for quantitative evaluation of the observability of six-degree-of-freedom features based on PCA in step 4 is as follows: Perform local geometric analysis on the preferred feature set obtained in step 3; for any feature point... Search for it in the prior map Find the nearest neighbors and construct the covariance matrix of that neighborhood. For the matrix Apply eigenvalue decomposition: ， in, For feature vectors; For eigenvalues; Establish orthogonal basis of global rigid body coordinate system Based on the relationship between the feature point positions and the normal constraints, define the spatial moment vector: ， in, For the first The spatial moment vector generated by each feature point; This is the normal constraint vector corresponding to the feature point; Project the projections onto the global three axes and sum the projection results to the rotated histogram. In the middle, at the same time, the normal vector The absolute projection results on the global three axes are accumulated into the translation histogram. middle; The uncertainty of the translational degree of freedom in the X direction is defined as: ， in, This represents the cumulative value in the translation direction corresponding to the X-axis in the translation histogram; This is the cumulative sum of all histogram intervals in the translated histogram; The uncertainty of each degree of freedom is denoted as ,and Construct a six-degree-of-freedom prior risk vector: ， in, This is the six-degree-of-freedom prior risk vector corresponding to the current frame; superscript Indicates transpose; The closer it is to 1, the higher the risk of degradation of the corresponding degree of freedom.

5. A lightweight, high-precision laser / inertial SLAM method according to claim 4, characterized in that, The specific method for joint optimization of orientation adaptive weight adjustment in step 5 is as follows: A factor graph model jointly constructed from the lidar, inertial measurement unit, and prior information is established. The residual objective function for joint optimization is: ， in, This is the pre-integrated residual term; For laser observation residuals; For the prior residual term; , , The covariance matrices corresponding to each residual term; the laser observation residuals in the above objective function. It includes residual terms constructed from angular features, surface features, and rod-shaped features, wherein the residuals and Jacobian matrices corresponding to the rod-shaped features are constructed based on the spatial center coordinates of the rod-shaped features obtained in step 2; In the joint factor graph, the spatial center coordinates of the rod obtained in step 2 are used as part of the laser observation constraint; let the rod point mapped to the global coordinate system in the current frame be... The center of the fitted circle is Then we have: ， in, It is a rotation matrix; It is a translation vector; This represents the position of the rod-shaped feature point in the current frame in the global coordinate system. The coordinates of the fitted center of the corresponding rod-shaped object on the map; Define the Euclidean distance residual from the rod-shaped point to the center of the map fitting circle as: ， in, Matching residuals for rod-shaped objects; for Coordinates in the plane; for The coordinates in the plane; when solving the Jacobian matrix of the residual with respect to pose, calculate the partial derivatives of the Euclidean distance residual with respect to the three-axis rotation and three-axis translation components respectively; concatenate the above partial derivative results according to the six-degree-of-freedom pose parameters to obtain the observation Jacobian vector corresponding to the rod-shaped feature: ， in, , , These are the partial derivatives of the residuals with respect to the rotation about the X, Y, and Z axes, respectively. , , This is the partial derivative of the residual with respect to the translation along the X, Y, and Z directions; For the first The observation Jacobian vectors corresponding to the features of each rod-shaped object are then used for joint optimization. Next, the observation residual factors of the rod-shaped objects are combined with other laser observation residual factors, IMU pre-integration residual factors, and prior residual factors. A global Jacobian matrix is ​​then constructed from the Jacobian matrix and residual terms corresponding to each residual factor. With residual vector Then the pose increment Solve using normal equations: ， Based on the risk vector obtained in step 4 ,right Adjustment is performed per degree of freedom; when the uncertainty in a certain degree of freedom... At that time, the variance of laser observations in the corresponding direction is amplified by the following formula: ， in, To adjust the covariance matrix in the th Diagonal elements in one degree of freedom; Based on the observed variance; This is the gain amplification factor; As a safety threshold; with As the value increases, the laser observation weights on the corresponding degrees of freedom decrease.

Images