Lidar odometry method and system with reliability evaluation and precision increment association

Abstract

The application discloses a laser radar odometer method and system for reliability evaluation and accurate incremental data association, which performs scanning internal motion compensation according to the relative sampling phase of scanning points and the relative motion of the last two effective poses; a coarse pose is obtained through first round scanning-map registration, geometric residual is calculated at the pose, and a cross-frame residual history is updated; bounded information weights are constructed by combining map point stability, local geometric quality, measurement uncertainty and constraint direction distribution. The lower bound of the distance of the candidate set outside the query position is updated, and the maximum distance of the preset number of neighbors in the cache candidate is compared to determine whether the cache contains accurate neighbors; when it cannot be determined, full-amount accurate neighbor search is performed within the association radius. The local geometry is reused when the same frame has fixed and ordered neighbor identification keys. After the final pose is obtained, the map point support and conflict state are updated, the local map is updated, and the trajectory is output, so that the positioning accuracy and processing efficiency are considered.

Description

Technical Field

[0001] This invention relates to the fields of mobile robot localization, 3D point cloud processing, and lidar perception technology, specifically to a lidar odometry method and system based on reliability assessment and precise incremental data correlation. The method is applicable to ground vehicles, unmanned transport platforms, inspection robots, and mobile mapping equipment, and can estimate the sensor's six-degree-of-freedom pose and incrementally construct a local map based on continuous lidar scanning without relying on pre-built maps. Background Technology

[0002] LiDAR odometry uses continuous point clouds to estimate sensor motion and is fundamental for autonomous mobile systems to achieve localization, mapping, and environmental perception. Feature-based LiDAR odometry typically extracts edge and planar features from the point cloud, establishes a point-to-line or point-to-area correspondence between the current scan and historical local maps, and calculates the current pose through nonlinear optimization. This type of method has the advantages of clear structure and good real-time performance.

[0003] However, a single frame scan of a rotating lidar system consists of echoes from different orientations acquired at different times. When the platform translates or rotates during a single frame scan, treating all scan points as acquired simultaneously can cause bending of line and surface structures, leading to systematic errors in subsequent feature extraction and scan-map registration. Continuous-time trajectory models can handle this problem, but their state variables and computational load are large, which is not conducive to lightweight real-time odometry.

[0004] Current scan-map registration methods typically assign similar weights to all correspondences that pass distance thresholds and geometric fit tests. However, small residuals do not necessarily indicate reliable observations: moving objects or transient structures may form clusters of unusually large residuals with consistent orientation within the same spatial grid cell; historical map points may accumulate conflicting observations; even if neighboring points are approximately collinear or coplanar, they may have weak geometric constraints on some degrees of freedom; and the measurement uncertainty of plane echoes under long-distance echo and grazing incidence conditions also increases significantly. A single residual threshold or a fixed robust loss function is insufficient to simultaneously distinguish these situations.

[0005] A more complete evaluation of correspondences increases the overhead of nearest neighbor search, local line or plane fitting, and weight calculation. The query position in adjacent nonlinear iterations usually only changes slightly, but if a full accurate nearest neighbor search is performed within the association radius and the eigenvalue decomposition of the covariance matrix is ​​recalculated each time, it will consume real-time computing budget and reduce real-time processing margin. If approximate nearest neighbors are used or the cache is reused without accuracy verification, it may change the actual set of factors entering the optimizer, causing the optimization objective to be inconsistent with the design objective.

[0006] Therefore, a lidar odometry scheme is needed that can jointly evaluate the current residual, map history, local geometry, measurement uncertainty and constraint direction, while also being able to use verifiable sufficient conditions to verify whether the cached candidate set contains exact nearest neighbors, and automatically perform a full exact nearest neighbor search within the associated radius when the criterion cannot be determined. Summary of the Invention

[0007] The purpose of this invention is to provide a lidar odometry method and system based on reliability assessment and precise incremental data correlation, so as to reduce the impact of intra-scan motion distortion, unreliable correspondence, local geometric degradation and repeated nearest neighbor calculation on positioning accuracy and real-time performance.

[0008] To achieve the above objectives, this invention first performs motion compensation on the point cloud based on the relative sampling phase of the scanned points and the relative motion between the two most recent valid poses in each frame processing. Then, it performs a first-round scan-map registration optimization on the compensated edge features and planar features to obtain a coarse pose estimate. Under a fixed coarse pose estimate, it recalculates the geometric residuals of each valid correspondence, updates the cross-frame residual history of the polar coordinate grid cells, and determines the residual consistency weight, orientation balance weight, and robust scale. In the final round, during each reconstruction of the correspondence, it determines the map stability weight and local geometric quality weight based on the current precise nearest neighbor, the edge distance weight based on the measured distance of the edge feature points, and the planar normal projection uncertainty weight based on the current rotation. Finally, it obtains the final pose using a weighted robust objective function.

[0009] This invention limits the update time of different cross-frame states to the pose stage they depend on. The residual history of polar coordinate mesh cells is updated once per frame based on the current cell residual inconsistency under a fixed coarse pose, and is used to generate the residual consistency weights for the final round. This history is not rewritten in intermediate iterations of the final round. Map point count increments and conflict count increments are temporarily stored in the final round and only written to the cross-frame map point state after the final pose solution is completed; current features judged to be inconsistent are not inserted into the map. This avoids the repeated writing of intermediate states from nonlinear optimization into the cross-frame history or map state.

[0010] This invention also maintains incremental voxel indexes for edge feature maps and planar feature maps respectively. The intra-frame cache for each current feature includes the last query position, a finite number of nearest candidate map points, a lower bound of distances to map points outside the candidate set, and an ordered nearest neighbor identifier key. The lower bound of distances to map points outside the candidate set at the new query position is updated according to the triangle inequality; the distance lower bound is compared with the cached candidate... Comparison between small distances verifies whether the cached candidate set contains the current one. Find the exact nearest neighbors and determine if the number of neighbors within the association radius is insufficient; if this cannot be used to make a determination, perform a full exact nearest neighbor search within the association radius.

[0011] With the local map remaining fixed across optimization rounds within the same frame, the unchanged ordered nearest neighbor identifier key means that the centroid determined by the map, the eigenvalue decomposition results of the covariance matrix, geometric validity, geometric quality, and plane bias all remain unchanged, thus allowing the reuse of these local geometric quantities. In each reconstruction of the correspondence in the final round, the current point transformation, residual, Jacobian, and weights dependent on the current pose are all recalculated. The robustness scale is determined based on statistical results under a fixed coarse pose, and the cache does not span map updates.

[0012] The technical solution of this invention is a lidar odometry method for reliability assessment and precise incremental data correlation, comprising: Acquire the current frame of lidar scanning data and scan motion compensation based on relative sampling time; Edge and planar features are extracted from the motion-compensated scan, and a local map composed of historical map points is obtained. The first round of scanning and map registration optimization is performed to obtain coarse pose estimation. Geometric residuals are calculated under coarse pose and cross-frame residual history is updated. In the final round, when reconstructing the correspondence, bounded information weights are constructed by combining map stability, local geometric quality, projection uncertainty and orientation balance weights. Perform precise incremental data association, update the lower bound of the distance between map points outside the candidate set according to the change of the query location, compare it with the maximum distance of a preset number of nearest neighbors in the cached candidates, and determine whether the cache contains precise nearest neighbors; if it cannot be determined, perform a full precise nearest neighbor search within the association radius. The unique identifiers of the precise nearest neighbor map points are sorted to form an ordered nearest neighbor identifier key. The map-related local geometric model is reused only when the local map in the same frame is fixed and the ordered nearest neighbor identifier key remains unchanged. In the final round of scanning-map registration optimization, the bounded information weights are used to obtain the final pose, and only after the final pose is obtained are the intra-frame increments of map point support records and conflict records written into the cross-frame map point state and the local map updated.

[0013] Furthermore, the relative sampling phase of the scan point within one frame is recorded, and intra-scan motion compensation is performed on the scan point based on the relative motion between the two most recent valid poses. The intra-scan motion compensation includes: adjusting the azimuth difference of the scan point relative to the first valid echo according to... Modulo and mapping to interval Divide by The normalized sampling phase is obtained; the six-dimensional motion torque is obtained from the relative transformation of the effective poses of the two most recent frames; the rotation and translation components of the motion torque are multiplied by the rotation scaling factor and translation scaling factor constrained by preset upper and lower limits, respectively; according to the difference between the normalized sampling phase and the unified reference phase of the scan point, the scan point is transformed to the unified reference phase through three-dimensional special Euclidean group exponent mapping.

[0014] Furthermore, based on the intra-frame robust statistics of the geometric residuals, the cross-frame residual history aggregated by polar coordinate grid cells is updated, and the residual consistency weight is determined. The determination of the residual consistency weight includes: calculating the median and median absolute deviation of each geometric residual in the current frame under a fixed coarse pose estimation, thereby determining the residual center, robust scale, large residual judgment threshold, and the turning point scale of the Huber loss function; dividing the effective correspondence into polar coordinate grid cells centered on the lidar and discrete by azimuth and radial distance, and combining the normalized average excess amount of residuals exceeding the judgment threshold, the proportion of normalized large residuals, and the consistency of residual displacement vector direction in each polar coordinate grid cell to obtain the cell residual inconsistency; performing exponential smoothing update on cells with a sample number not less than a preset lower limit and the current cell residual inconsistency not less than a preset trigger threshold, performing decay update on cells that do not simultaneously meet the above conditions, and reducing the residual consistency weight of the cell after the cross-frame history reaches the confirmation threshold.

[0015] Furthermore, the construction of bounded information weights by combining map stability, local geometric quality, projection uncertainty, and directional balance weights includes: determining map stability weights based on cross-frame support records and conflict records of the current precise nearest neighbor map points; determining local geometric quality weights based on the covariance eigenvalues ​​of the current precise nearest neighbors; determining edge distance weights based on the measured distances of edge feature points; and determining the planar normal projection uncertainty weights based on the measured distances of planar feature points and the absolute value of the cosine of the angle between the planar normal and the unit observation ray after the current rotation transformation; and statistically calculating directional balance based on edge type and planar type. The determination of the map stability weights includes: storing support counts and conflict counts for each map point; calculating the stability probability of the map point using a Beta distribution prior; limiting the stability probability of each precise nearest neighbor to a preset weight range after affine transformation; and robustly aggregating the obtained nearest neighbor stability weights using the median; in the final round of scan-map registration optimization, first forming an intra-frame support count increment or conflict count increment; and then writing the intra-frame count increment into the cross-frame map point state after the final pose solution is completed; and not inserting current features that are determined to be inconsistent according to the residual consistency evaluation results into the local map.

[0016] Furthermore, the determination of the local geometric quality weight, projection uncertainty weight, and directional balance weight includes: calculating linearity and flatness based on the ascending eigenvalues ​​of the nearest neighbor covariance matrix, and mapping them to edge geometric weight and planar geometric weight, respectively; determining the edge distance weight based on the measured distance of the feature points after motion compensation; determining the planar normal projection uncertainty weight based on the absolute value of the cosine of the angle between the planar normal and the unit observation ray after the current rotation transformation, and the measured distance; dividing the effective straight line direction and the planar normal into directional bins, and determining the bounded inverse frequency directional balance weight based on the ratio of the average number of constraints in the non-empty directional bins to the number of constraints in each directional bin.

[0017] Furthermore, in the first and final rounds of scan-map registration optimization, incremental spatial indexes are maintained for both the edge feature map and the planar feature map; for each feature query, a preset positive integer target nearest neighbor number is set. The system caches the previous query location, the set of candidate map points, and the lower bound of distances to map points outside the candidate set. It updates the lower bound of distances based on changes in the query location. The updated lower bound of distances is then compared with the cached candidate point's distance. Comparison between small distances verifies whether the cached candidate set contains the current one. A precise nearest neighbor is determined; when determining if a feature is included, the precise nearest neighbor is determined from the cached candidate set; if the number of nearest neighbors within the association radius is insufficient, the feature query is rejected; if the determination cannot be completed based on this, a full precise nearest neighbor search is performed within the association radius using the incremental spatial index; specifically: set up For feature query index, and Let be the current map coordinate system 3D position vector for feature query and the cached reference 3D position vector, both in meters; let the target nearest neighbors be... and cache capacity All are positive integers, and Not less than ;set up A positive correlation radius, The distance comparison margin is positive, and both are in meters; the precise incremental data association includes: in Stored in a location not exceeding A set of the nearest candidate map points And the lower bound of the effective distance to non-negative candidate map points outside the candidate set. , The unit is meters; the map points obtained from the cached candidate and the full exact nearest neighbor search are all ordered according to their nearest neighbor. Sort by Euclidean distance in ascending order; if distances are the same, sort by unique map point identifier in ascending order; minus and The Euclidean distance between them yields the map points outside the candidate set. The lower bound of the distance at that point , For real numbers, the unit is meters; when The number of candidates in the pool is not less than non-negative first Small candidate distance and The sum is less than ,and Less than Accept with certainty at that time. The unit is meters; when The number of candidates in is less than and Not less than ,or The front of the middle The nearest neighbors have been identified as exact neighbors and Not less than A deterministic rejection is made only when neither the deterministic acceptance nor the deterministic rejection condition is met. With the center of the ball, Perform a full exact nearest neighbor search within a spherical neighborhood of radius excluding the boundary and refresh the cache.

[0018] Furthermore, for queries that meet the association acceptance criteria, the ordered nearest neighbor identifier key is determined by... The unique identifiers of each precise nearest neighbor map point are arranged in a preset order; the local map is fixed in each optimization round of the same frame, and when the ordered nearest neighbor identifier keys are the same, the corresponding map-related quantities are reused, including centroid, eigenvalues ​​and eigenvectors of the covariance matrix, geometric validity flag, geometric quality, plane bias, and fitting failure state. When the ordered nearest neighbor identifier keys are different, the above map-related quantities are recalculated; feature point transformation, residual, Jacobian, and pose-related weights are not included in the local geometry cache and are recalculated with pose updates; the robustness scale is determined according to the statistical results under a fixed coarse pose; the cache does not span local map updates.

[0019] The present invention also provides a lidar odometry system based on reliability assessment and precise incremental data correlation, comprising: The data acquisition and motion compensation module is used to acquire LiDAR scans containing the relative sampling phase of the scan points, and to perform intra-scan motion compensation based on the relative motion between the two most recent valid poses. The Feature and Local Map module is used to extract edge features and planar features, and maintain edge feature maps and planar feature maps with map point support records and conflict records; The phased optimization and reliability assessment module is used to perform the first round of scan-map registration optimization to obtain coarse pose estimation, calculate geometric residuals under coarse pose and update cross-frame residual history; in the final round, when rebuilding the correspondence, it constructs bounded information weights by combining map stability, local geometric quality, projection uncertainty and orientation balance weights. The precise incremental data association module is used to perform precise incremental data association. It updates the lower bound of the distance between map points outside the candidate set according to the change of the query location, compares it with the maximum distance of a preset number of nearest neighbors in the cached candidates, and determines whether the cache contains precise nearest neighbors. If it cannot be determined, it performs a full precise nearest neighbor search within the association radius. The local geometry reuse module is used to sort the unique identifiers of precise nearest neighbor map points to form an ordered nearest neighbor identifier key, and reuse the map-related local geometry model only when the local map in the same frame is fixed and the ordered nearest neighbor identifier key remains unchanged. The final optimization and map state update module is used to obtain the final pose by using the bounded information weights in the final round scan-map registration optimization, and only after the final pose is obtained, write the intra-frame increment of map point support records and conflict records into the cross-frame map point state and update the local map. The results output module is used to output the final pose, LiDAR trajectory, and local map.

[0020] The present invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and capable of running on the processor, wherein the processor executes the computer program to implement the method described above.

[0021] The present invention also provides a computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the method described in the above technical solution.

[0022] Compared with existing technologies, the present invention has at least the following advantages: First, by utilizing the relative sampling phase and the relative motion torque determined by the two most recent valid poses, the motion distortion within the scan is reduced with lower computational cost; Second, by combining multiple types of reliability evidence under a fixed coarse pose estimation, the impact of moving objects, transient structures, local geometric constraint degradation, and high-noise observations on pose estimation is reduced; Third, the residual history is updated only once per frame under a fixed coarse pose, and the map point state and local map are updated only after the final pose is obtained, avoiding the influence of intermediate iteration states on subsequent frames; Fourth, by combining the nearest neighbor accuracy criterion with a full accurate nearest neighbor search within the association radius, the overhead of repeated search and fitting is reduced, while maintaining the accurate nearest neighbor set and weighted optimization objective unchanged.

[0023] The present invention also provides a lidar odometry system, electronic device, and computer-readable storage medium for performing the above-described methods. Each module can be implemented by a software thread within the same processor, or distributed across multiple processors, graphics processors, programmable logic devices, or dedicated hardware acceleration units. Attached Figure Description

[0024] Figure 1 This is a schematic diagram of the overall processing flow of the lidar odometer according to an embodiment of the present invention; Figure 2 This is a schematic diagram of the structure and data flow of a lidar odometer system according to an embodiment of the present invention; Figure 3 This is a comparison chart of positioning accuracy and processing efficiency on the KITTI 00 to 10 sequences in an embodiment of the present invention.

[0025] Figure labeling: 100 is the lidar odometry system; 110 is the scanning and relative time acquisition unit; 120 is the point-to-point motion compensation unit; 130 is the edge and planar feature extraction unit; 140 is the local map and incremental indexing unit; 150 is the first-round scan-map registration optimization unit; 160 is the reliability assessment unit; 161 is the residual recalculation unit under fixed coarse pose; 162 is the residual history update and consistency weight unit; 163 is the map stability assessment unit; 164 is the local geometric and measurement uncertainty unit. The system includes: a constraint direction distribution balance evaluation unit; a bounded information weight generation unit; a precise incremental data association unit; a candidate cache reading unit; a candidate set map point distance lower bound update unit; a nearest neighbor accuracy determination unit; a precise nearest neighbor determination or full precise nearest neighbor search unit; an ordered nearest neighbor identification key and local geometric reuse unit; a final weighted optimization unit; a map point status and map update unit; and a trajectory and map output unit. Detailed Implementation

[0026] The present invention will be further described below with reference to the accompanying drawings and specific embodiments. The following embodiments are used to illustrate the technical solutions of the present invention, and not to limit the scope of protection of the present invention. Without changing the following technical relationships, each step can be implemented by software, hardware or a combination of both: calculating reliability indicators such as residual consistency that do not depend on the current pose of the final round under a fixed coarse pose estimation and updating the residual history; calculating pose-related weights under the current pose of the final round; determining nearest neighbors through a full accurate nearest neighbor search within a verifiable sufficient condition or associated radius; determining whether to reuse map-related local geometric quantities based on the ordered nearest neighbor identifier key; writing map point states and updating the local map only after the final pose solution is completed.

[0027] like Figure 1 and Figure 2 As shown, the lidar odometry system 100 includes a scanning and relative time acquisition unit 110, a point-by-point motion compensation unit 120, an edge and planar feature extraction unit 130, a local map and incremental indexing unit 140, a first-round scan-map registration optimization unit 150, a reliability assessment unit 160, a precise incremental data association unit 170, a final weighted optimization unit 180, a map point status and map update unit 190, and a trajectory and map output unit 200.

[0028] Record No. Frame LiDAR scanning is The first one in the scan The original points are To avoid ambiguity, this embodiment uniformly stipulates: For scan frame index, For the original scan point index, For marginal factor index, For plane factor index, It serves as a general feature query index and also as a factor index formed by this query. For map point indexing, For polar coordinate grid cell indexing, For directional bin indexing, This is the nearest neighbor number. Belongs to the three-dimensional special Euclidean group , indicating from the first Transforming the frame lidar coordinate system to the local map coordinate system Homogeneous rigid body transformation; Belongs to the three-dimensional special orthogonal group ,for Rotation matrix; for Translation vector, in meters.

[0029] Step 1: Scan data acquisition and motion compensation based on relative sampling time. The scanning and relative time acquisition unit 110 receives the lidar point cloud and time information, and the point-by-point motion compensation unit 120 determines the scanning phase and sampling time offset of each point according to the following relationship.

[0030] No. The azimuth angle, normalized sampling phase, and physical sampling time offset of each scan point are expressed as:

[0031]

[0032]

[0033] In the formula, For the first The three-dimensional coordinate vectors of the original scan points in the lidar coordinate system, in meters; , and These are its three coordinate components; symbols Indicates transpose; For the first The counterclockwise azimuth angle of each point The azimuth angle of the first valid echo in this frame is taken as the principal value interval of the arctangent function in the fourth quadrant. And the unit is radians; It is the arctangent function in the four quadrants; This indicates that the azimuth difference is calculated as follows: Modulo and mapping to interval ; Pi; The sampling phase is dimensionless and normalized. A positive single-frame scan period satisfies The unit is seconds; For the first The sampling time offset of each point relative to the start time of this frame satisfies The unit is seconds.

[0034] The relative motion under constant velocity approximation is calculated from the poses corresponding to the two most recent valid frames. To distinguish between the scalar azimuth angle and the rotation vector, this embodiment uses... Representing the rotation vector, using Represents the translation vector, and defines the six-dimensional torsion, the scaled torsion, and their matrix forms.

[0035] The relative motion torque, torque matrix, and motion compensation point are expressed as follows:

[0036]

[0037]

[0038]

[0039] In the formula, and The lidar poses corresponding to the two most recent valid frames are given. For an earlier position, Both are later poses. Homogeneous rigid body transformation matrix; superscript Indicates the inverse of a matrix; and The above are respectively Logarithmic and exponential mappings; The torque of six-dimensional relative motion; This is a three-dimensional rotation vector, with units in radians; This is a three-dimensional translation vector, with units in meters; The rotation and translation components are respectively processed by and Scaled six-dimensional torque, superscript This is a scaled-down identifier; superscript symbol This represents the Lie algebra matrix mapping (hat operator), the result of which is Special Euclidean Algebra matrix; and Let be non-negative and dimensionless rotation scaling factors and translation scaling factors, respectively, satisfying... and And subject to preset upper and lower bounds; This represents converting a three-dimensional vector into... Antisymmetric cross product matrix; for The zero vector, where the 0 at the bottom right corner of the matrix is ​​the scalar zero; To unify the reference phase, the value belongs to ; To compensate for the homogeneous coordinates of the previous point; Homogeneous coordinates after compensation to the reference phase; superscript This is a motion compensation identifier.

[0040] In one embodiment, Take 0.10 seconds. Take 0.50. For straight-line driving conditions. Take 0.25, Take 0.00, under the condition of sharp turn. Take 0.10, A value of 0.20 is used; continuous interpolation is performed between the two operating conditions based on the most recent relative rotation angle, and the interpolation results are truncated to the above value range. Adjacent phase intervals can share the pre-calculated exponential mapping, thereby reducing point-by-point compensation overhead.

[0041] Step 2: Feature extraction, local map construction, and geometric residual definition. Edge and planar feature extraction unit 130 extracts edge feature sets from the compensated point cloud. and planar feature set Local map and incremental index unit 140 in local map The system stores edge map points and planar map points separately, and stores cross-frame support counts and collision counts for each map point. Let... The number of nearest neighbors of the target is a positive integer. For each feature query, the following is used: The nearest neighbors are fitted to local straight lines or local planes; the nearest neighbors are first sorted by Euclidean distance from smallest to largest, and if the distances are the same, they are then sorted by the unique identifier of the map point. The weighted refinement rounds in the first round, the initial stage, and each round in the final round... For nearest neighbor queries, the nearest neighbors are determined according to the nearest neighbor accuracy criterion or the full accurate nearest neighbor search rule within the association radius described in step 5.

[0042] The point-to-line residual of the edge factor and the point-to-surface residual of the plane factor are expressed as follows:

[0043]

[0044] In the formula, , and Same meaning as before; and The first The edge feature point and the first The three-dimensional coordinates of a planar feature point after motion compensation, in meters; For the first The centroid of a local straight line in the map coordinate system, in meters; This is the unit direction vector of the line in the map coordinate system; For the first The unit normal vector of a local plane in the map coordinate system; To meet The plane offset, in meters, is where is any planar point in the map coordinate system, with units of meters; Represents the cross product of vectors; Represents the Euclidean norm; The distance from the non-negative point to the line. The distances are from a signed point to a surface, both in meters; superscript. and These are the edge feature type identifier and the planar feature type identifier, respectively.

[0045] Step 3: First-round scan – map registration optimization and reliability index calculation under fixed coarse pose. In the first round, precise nearest neighbor search and geometric validity verification are performed as in Step 5. Directly calculable geometric quality terms and measurement uncertainty terms are used in the solution. Residual consistency, map stability, and orientation balance terms, which still require intra-frame or cross-frame statistical analysis, are temporarily set to a neutral value of 1, resulting in the coarse pose estimate. In a fixed The geometric residuals are recalculated, and the residual consistency weight, orientation balance weight, and robustness scale are determined. The cross-frame residual history is also updated. Map stability weights and local geometric quality weights are determined based on the current precise nearest neighbors during each reconstruction of the correspondence in the final round. Edge distance weights are directly determined by the measured distance, and the plane normal projection uncertainty weight is based on the current... Recalculate. Steady-state optimization uses two rounds: the first round and the final round. During the startup phase, several weighted refinement rounds can be added, with their number decreasing frame by frame until the steady-state two-round optimization is achieved.

[0046] The final pose is obtained through the following weighted Huber objective:

[0047]

[0048]

[0049]

[0050] In the formula, For the first The final optimized pose of the frame is shown, and the asterisk indicates the solution that minimizes the objective function; This represents the variable that minimizes the objective function; and These are the edge factor target item and the planar factor target item under the current pose, respectively, both in square meters; and These are the effective edge factor index set and the planar factor index set formed after the current frame completes the association and geometric verification, respectively; and These are the non-negative and dimensionless marginal factor information weights and planar factor information weights, respectively; and The meaning is the same as the previous one; For The Huber loss function is a transition scale, and its output unit is square meters. The non-negative weighted squared residuals input to the Huber loss function, in square meters; express The non-negative square root; The residual scale is positive, and the unit is meters; For the first Frame adaptive Huber transition scale and satisfy The summation symbol iterates through all valid edge factors and plane factors respectively.

[0051] Under a fixed coarse pose estimation, the frame-by-frame residual center, robust scale, large residual decision threshold, and transition scale of the Huber loss function are expressed as follows:

[0052]

[0053]

[0054]

[0055] In the formula, For the first The set of all valid correspondences obtained by recalculating the frame under a fixed coarse pose estimation; For correspondence The shortest displacement vector from the feature point to the fitted line or plane after the transformation, in meters; Let be the Euclidean length of the displacement vector; Indicates taking the median of the set indicated by the subscript; vertical bar Represents absolute value; This indicates taking the maximum value of the quantities within the parentheses; This is the center of the residual for this frame; This is the robust residual scale for this frame; This is the lower limit of the scale; The median absolute deviation (MAD) is a positive and dimensionless standard normality correction factor. The threshold for determining large residuals; This is the positive lower limit of the threshold for determining large residuals; The scale coefficient is a positive and dimensionless threshold for determining large residuals. The Huber transition scale coefficient is positive and dimensionless; Indicates to Truncation to numerical range ,in and This function only represents the lower and upper bounds of the values; and These are the lower and upper bounds of the Huber transition scale, respectively, satisfying... ;In this formula , , , , , , and The unit for all values ​​is meters.

[0056] To avoid the same symbol representing different dimensions, this embodiment denotes the lower bound of the denominator for the consistency of the residual displacement vector direction, the lower bound of the denominator for the covariance eigenvalue, and the precise distance comparison margin as follows: , and All three are positive numbers and satisfy the following conditions: , and Its dimensions are meter, square meter and meter, respectively.

[0057] The normalized average excess amount of residuals exceeding the judgment threshold in each polar coordinate grid cell, the proportion of normalized large residuals, the consistency of residual displacement vector direction, and the inconsistency of cell residuals are expressed as follows:

[0058]

[0059]

[0060]

[0061] In the formula, Polar coordinate grid cells The set of valid correspondences within; the summation symbol represents traversal. All correspondences in the table; for The number of correspondences in the data; The residual amplitude is Exceed The number of correspondences, superscript This serves as a category identifier for large residual counts; Indicates taking The main part; The normalized average excess amount when the residual exceeds the judgment threshold; The lower limit of the normalized denominator is positive, and the unit is meters; To normalize the proportion of large residuals; To reference the proportion of large residuals, and to meet the following requirements: ; The consistency of the direction of the residual displacement vector; For unit residual inconsistency; belong , representing the minimum contribution of the directional consistency term to the element residual inconsistency; , , and All are dimensionless quantities and their values ​​belong to This formula is only applicable when... When not less than 1, calculate according to the listed fractions; when At that time, it was stipulated .

[0062] The cross-frame unit history, the degree of inconsistency after confirmation, and the residual consistency weights are represented as follows:

[0063]

[0064]

[0065] In the formula, For unit As of the Frame-to-frame exponential smoothing history, initial value ; For the first The historical value of the frame updated based on the current unit residual inconsistency under a fixed coarse pose; A valid indication value of 0 or 1 when the unit The value is set to 1 when the number of samples is not less than the preset lower limit and the inconsistency of the current unit residual is not less than the preset trigger threshold; otherwise, it is set to 0. belong And it is the history preservation coefficient of the unit that meets the update conditions; belong And it is the attenuation coefficient of the unit that does not meet the update conditions; The degree of inconsistency confirmed and normalized based on updated historical values; The historical confirmation threshold; As a normalization benchmark for inconsistency, both satisfy... ; belong And this is the maximum attenuation amplitude; This is the residual consistency weight; This is the positive lower bound of the weight, satisfying... ; , and All belong to , belong All of the above quantities are dimensionless. The cross-frame residual history is updated once per frame under a fixed coarse pose and used for the final round of optimization. The history is no longer updated in the intermediate iterations of the final round.

[0066] Step 4: Evaluation of map point stability, local geometric quality, measurement uncertainty, and constraint direction distribution uniformity. For each map point... Store non-negative integers to support counting and conflict count superscript and This indicates the category identifier for support counts and collision counts, rather than the numerical positive or negative sign. In the final round, each association that passes the geometric validity test first generates an intra-frame support count increment or collision count increment, and then applies a preset information weight threshold. Only factors with information weights not lower than the threshold are included in the optimization; the intra-frame count increment is only stored in a temporary storage area before the final pose is obtained. The periodic free space visibility test is only used to confirm existing collisions and does not generate a collision count increment separately.

[0067] The map point stability probability, stability probability mapping, and map stability weights are represented as follows:

[0068]

[0069]

[0070] In the formula, and Same meaning as before; and For the positive pseudo-count parameter of the Beta distribution prior, when the prior is symmetric... ; Let be the posterior mean of the stability probability of map points, satisfying ; This indicates that the input probability will be used. After performing an affine transformation, the amplitude is limited to a preset weighted range, where belong ; Let be the slope of the positive and dimensionless affine transformation. Let be the intercept of the dimensionless affine transformation belonging to the set of real numbers; and These are the lower and upper bounds of the dimensionless map point weights, respectively, satisfying... ; For query Adopted A set of nearest neighbors on a map; for Neighboring The median of the values ​​satisfies And it is a dimensionless quantity. When a map point Cumulative valid observations When the value is below a preset threshold, the following is specified: Take the neutral value of 1.

[0071] The nearest neighbor covariance, linearity, flatness, and a geometric quality weighting mapping are represented as follows:

[0072]

[0073]

[0074]

[0075]

[0076]

[0077] In the formula, For query The The three-dimensional coordinates of the nearest neighbor in the map coordinate system, in meters, with superscript. For map coordinate system identification; The nearest neighbor number is mentioned above; for Nearest centroid, in meters; for Nearest neighbor covariance matrix, in square meters; , and for The three ascending non-negative eigenvalues, in square meters; This is the lower bound of the denominator for the aforementioned positive covariance eigenvalues; and These are dimensionless linearity and flatness, respectively, both belonging to... ; belong And for query The feature type has the same meaning as before; Dimensionless geometric mass selected by type and belonging to ; For the corresponding type of geometric effective threshold, satisfying ; It is a dimensionless geometric weight positive lower bound, satisfying ; Let be a bounded and dimensionless geometric mass weight, satisfying The listed weight mappings apply only to... The associated evaluation has been accepted; if If the geometric relationship is not found, the fraction is rejected and the equation is not evaluated.

[0078] The weights for measured distance, absolute value of the cosine of the incident angle on the plane, standard deviation of the normal projection, and uncertainty weights for the normal projection on the plane are expressed as follows:

[0079]

[0080]

[0081]

[0082]

[0083]

[0084] In the formula, For query The three-dimensional coordinates of the motion compensation point in the lidar coordinate system, in meters; Measure the distance to meet the requirements. The unit is meters; For positive edge distance scales, satisfying The unit is meters; Weighted by edge distance; The lower bound of the edge distance weight satisfies ; , for the first The positive distance measurement of a planar feature point satisfies The unit is meters; This is the unit observation ray of the feature point in the plane in the lidar coordinate system; , and Same meaning as before; For the plane normal and the meridian The absolute value of the cosine of the angle between the rotated observed rays satisfies ; This represents the standard deviation of the projection of the normal to the lower edge of the rotating plane, in meters. The standard deviation of the baseline radial measurement noise is positive, and it also serves as the lower limit of the denominator of the standard deviation of the normal projection. The unit is meters. is a non-negative and dimensionless proportionality coefficient for the standard deviation of tangential noise; The positive reference normal noise standard deviation, in meters; The uncertainty weight for the plane normal projection under the current rotation; The lower limit of the weights satisfies . belong , belong Both are dimensionless quantities. The distance weight is independent of pose and can be reused within a frame; in each reconstruction of the correspondence in the final round, the absolute value of the incident angle cosine, the standard deviation of the normal projection, and the uncertainty weights of the plane normal projection are all based on the current... Recalculate.

[0085] The directional balance weights for edge type and planar type statistics are expressed as follows:

[0086]

[0087] In the formula, For characteristic type variables, the meaning of their values ​​is the same as before; The bins are determined by the combination of coordinate axes corresponding to the two components with the largest and second largest absolute values ​​in the effective straight line direction vector or plane normal vector, and the corresponding directional bins are represented. For type Directional compartment The number of effective constraints in the data; For type The number of non-empty direction compartments; The total number of valid constraints in these non-empty directional bins; superscript For effective statistical identification; , and All are non-negative integers; The average constraint number of the non-empty direction bins, only when Calculate according to the listed fractions; A positive and dimensionless equilibrium index; and These are the lower and upper bounds of the dimensionless directional weights, respectively, satisfying... ; Let the inverse frequency direction weights be bounded and dimensionless, satisfying .when or At that time, the corresponding fraction is not calculated, and the direction weight is set to a neutral value of 1. The edge direction and the plane normal are counted independently, and directions with opposite positive and negative values ​​but the same axis combination are merged into the same direction bin.

[0088] Query The general reliability combination term and the final information weight determined by feature type are expressed as follows:

[0089]

[0090]

[0091]

[0092] In the formula, For query The corresponding polar coordinate grid cell; For query The container belongs to the direction of the container; and , , , , and The meaning is the same as the aforementioned formulas; The superscript is a dimensionless product of residuals, map data, and directional evidence. This is used to identify the reliability combination item, and its value range is as shown in the above formula; For query The final bounded and dimensionless information weight of the forming factor takes values ​​as shown in the above formula. The marginal factor weights are obtained by taking the values. ;right ,definition To obtain the plane factor weights The uncertainty weight of the plane normal projection is currently... The factor residuals and corresponding Jacobians are evaluated at the appropriate level before entering the Huber loss function. ; If the information weight is below a preset information weight threshold, the factor is rejected. Each weight is within its own preset upper and lower bounds, and the upper bound of the directional weight can be greater than 1.

[0093] Step 5, Precise Incremental Data Association. The precise incremental data association unit 170 maintains uniform voxel hash tables for both the edge map and the planar map. Association radius. It is a positive number, and the unit is meters; the side length of the voxel can be taken as the associated radius. This doubles the index, allowing radius queries to access only a finite number of voxels intersecting the spherical neighborhood centered on the query point. Newly inserted map points are added to the index incrementally using their voxel keys; the index is only rebuilt if map clipping or voxel filtering changes the set of map points or unique identifiers.

[0094] like Figure 1 As shown, the candidate cache reading unit 171 receives the current feature query. It continues to use the aforementioned positive integer target nearest neighbor number. Set the cache capacity are positive integers and satisfy Not less than The cached records include the baseline query location. No more than The set of the most recent candidates and the lower bound of the effective distance from the reference location to any map point outside the candidate set. Candidate map points are first sorted by Euclidean distance in ascending order; if the distances are the same, they are then sorted by their unique map point identifier. The lower bound of the distance can be taken as the [missing value]. The minimum of the available options among nearest neighbor distance, the distance to the nearest map point excluded due to a distance not less than the associated radius in the radius search, and the minimum distance from the query location to the boundary of the unvisited voxel region.

[0095] The lower bound of the distance between the current map coordinate system query location and the candidate set outside the map point is represented as follows:

[0096]

[0097] In the formula, For query In the current position The map coordinate system below shows the three-dimensional position, in meters; , and Same meaning as before; For the feature within the same frame, query the map coordinate system position when the last full exact nearest neighbor search or cache refresh was performed, in meters. The superscript 0 indicates the cache reference rather than the exponentiation. In order to be in to candidate set The effective distance lower bound of any map point outside the map is a non-negative number, in meters; In the new location The lower bound of the effective distance from the location to any map point outside the candidate set, in meters; This represents the Euclidean distance between the old and new query positions. This relationship is derived from the triangle inequality; when the query displacement is greater than... hour, It can be a negative number. Although this value is still a valid lower bound in a mathematical sense, it is not sufficient to determine acceptance or rejection on its own.

[0098] The sufficient conditions for the cache candidate set to include exact nearest neighbors, the sufficient condition for insufficient number of nearest neighbors within the association radius, the deterministic acceptance condition, and the deterministic rejection condition are expressed as follows:

[0099]

[0100]

[0101]

[0102] In the formula, , , and Same meaning as before; Number of candidates; only in Not less than Time calculation This quantity is used to move cached candidates to a new location. The distances were reordered to obtain the first Small distances are measured in meters, and when distances are the same, they are sorted by unique map point identifiers; this embodiment will use... Center of the sphere, with the associated radius A spherical neighborhood with radius excluding the boundary is defined as an associated open sphere neighborhood, with a distance equal to 0.5. The map point does not belong to the associated opening neighborhood; the square brackets in the formula indicate that the condition is true if it is true and false otherwise; logical symbols and These represent "AND" and "OR" respectively; The cache candidate set includes the current position. A sufficient condition for a precise nearest neighbor; For the number of nearest neighbors within the associated opening neighborhood is less than Sufficient condition; For the condition of certainty; This is a deterministic rejection condition. The condition is met if any map point outside the candidate set cannot enter the previous position at the current location. A close neighbor.

[0103] The precise data correlation output is determined according to the following three mutually exclusive and complete branches:

[0104] In the formula, For query In position The exact correlation output at the location, the asterisk indicates that the output is consistent with the result of the full exact nearest neighbor search within the correlation radius; This means first move from the candidate point to the current position. Sort the Euclidean distances from smallest to largest; if the distances are the same, sort them by the unique identifier of the map point, then return to the previous order. A map point; the empty set symbol indicates that the query is rejected; Indicated by Centered on the sphere, check each element on the incremental voxel index whose distance is less than [a certain value]. All map points, sorted according to the same rules, when the number of map points in the associated opening neighborhood is not less than Return to the previous time. If the candidate set is found to be the closest point, otherwise an empty set is returned. At the same time, the candidate set, the lower bound of the distance between map points outside the candidate set and the cached reference position are refreshed. and The meaning is the same as before; other situations refer to and None of these are true. The outputs of the three branches are within the associated radius of the same pose; directly performing a full, precise nearest neighbor search yields the previous results. The nearest neighbor or reject result is consistent. For the accepted query, let As used for the aforementioned geometric fitting and map stability aggregation Nearest neighbor set.

[0105] In one embodiment, Take 5. Take 6. Take 1.0 meter as the unit and 2.0 meters as the side length of the voxel. Pick Meters. The first frame after map initialization can use a global kd-tree for precise lookups, while subsequent frames use an incremental voxel hash index.

[0106] Step 6, Local geometric reuse controlled by ordered nearest neighbor identifier key. After the cached nearest neighbors are determined by the nearest neighbor accuracy criterion or by a full accurate nearest neighbor search within the association radius, the ordered nearest neighbor identifier key and the local geometric reuse unit 175 will... The unique identifiers of map points that remain unchanged within the current frame are arranged in ascending order to form an ordered nearest neighbor identifier key. The query cache is cleared before the start of the next frame, thus not spanning map updates.

[0107] The local geometric reuse relationship under a fixed local map is represented as follows:

[0108] In the formula, and Queries within the same frame In the Second and third The ordered nearest neighbor identifier key during secondary association queries; and These are two rounds of indexes for related queries within the same frame, both of which are positive integers; the unique identifier of each map point remains unchanged within the frame, and the coordinates of the corresponding map point also remain unchanged within the frame; For the first All frames are optimized and used in the same round while maintaining a fixed local map; A deterministic local geometric fitting function performed on a given nearest neighbor identifier and fixed map coordinates; To in the local map The top is identified by ordered nearest neighbor keys. A uniquely determined local geometric model includes nearest-neighbor centroids, covariance matrix and its eigenvalues ​​and eigenvectors, line direction or plane normal, plane bias, geometric validity indicators, geometric quality, and fitting failure states; the implied symbol represents that the consequent holds when the antecedent holds. The model is recalculated when the ordered nearest-neighbor identifier key changes. ; only reuse when the key remains unchanged The current feature point coordinate transformation, residuals, Jacobian and all pose-related quantities are still updated or recalculated according to each custom, and the Huber loss function transition scale is determined according to the statistical results under fixed coarse pose.

[0109] Step 7: Final weighted optimization, map point state update, and map update. In the final round, the final weighted optimization unit 180 re-establishes precise associations, calculates the final pose based on the non-negative square root scaling residuals and corresponding Jacobians of the final information weights determined by feature type, as described above. The map point state and map update unit 190 then writes the map point support count increment and conflict count increment into the cross-frame map point state; for consistent current features that meet the map point insertion conditions, the support count and conflict count of the new map point are initialized based on the stability state of its nearest map points, and the new map point is inserted into the local map; inconsistent features are not inserted into the local map. The trajectory and map output unit 200 outputs the final pose, cumulative trajectory, and updated local map.

[0110] In a specific implementation, the standard normality correction coefficient of MAD The Huber transition scale coefficient is set to 1.4826. Take 1.5, Huber's turning point scale Limited to 0.055 meters to 0.15 meters; directional balance index The value is set to 0.50, and the directional weight is limited to 0.65 to 1.55. The above values ​​are only examples and can be calibrated according to the lidar measurement noise, scanning frequency, and operating environment scale.

[0111] This embodiment conducts limited comparative experiments on the KITTI dataset to illustrate the feasibility and technical effects of the present invention; the experimental conditions described below do not constitute a limitation on the scope of protection of the present invention.

[0112] The experiment used sequences 00 to 10 of the KITTI odometry benchmark dataset, comprising 23,201 frames of Velodyne LiDAR point clouds. The software environment consisted of WSL2 and ROS Noetic, with an Intel Core i5-14600K processor and 31.8 GB of memory; the nonlinear optimization solver was configured for single-CPU thread operation. The point clouds were acquired by a 64-line LiDAR, with effective measurement distances ranging from 3 to 90 meters. Each sequence was replayed at 1.0x real-time rate according to the original timestamp; the ground truth poses were standardized to the Velodyne coordinate system conforming to ROS coordinate system conventions after calibrating extrinsic parameters and transforming the coordinate axes. After rigid body alignment of each estimated trajectory with the ground truth trajectory, the root mean square (RMSE) of the translational absolute pose error (APE) for each sequence was calculated, and the arithmetic mean of the 00 to 10 sequences was used as the index in Table 1. The number of missing output poses was defined as the number of input scan frames minus the number of output poses successfully matched according to the input timestamps.

[0113] Table 1 Comparison of absolute pose error during translation of KITTI 00 to 10 sequences

[0114] As shown in Table 1, the arithmetic mean of the sequence translation APE RMSE of the method of the present invention (denoted as RST-LO) is 3.299 meters, which is the lowest among the listed methods. PFilter completes all frame processing under the above real-time operating conditions without missing output poses, and its arithmetic mean is 3.938 meters; compared to PFilter, the arithmetic mean of the method of the present invention is reduced by 16.23%, and there is no missing output poses. The arithmetic mean of GenZ-ICP is 3.851 meters, but under the same real-time operating conditions, 168 output poses are missing, resulting in an incomplete output trajectory. This comparison only retains a small number of representative methods from KITTI. Figure 3 Subgraphs in Solid marks indicate that the output pose is complete, while hollow marks indicate that the output pose is missing.

[0115] To compare processing efficiency, while maintaining the same operating environment, input data, parameter configuration, reliability weighted model, local covariance eigenvalue decomposition method, pose solver, optimization iteration budget, and 1.0x real-time playback conditions, only the precise data association and the local geometry reuse implementation controlled by it were replaced: the global search variant re-executes the kd-tree nearest neighbor query and refits the local geometry each time the correspondence is reconstructed, while the variant of this invention uses precise incremental data association and reuses the local geometry while keeping the ordered nearest neighbor identifier key unchanged. Both implementations fully process 23,201 frames of KITTI 00 to 10 sequences and output 23,201 poses. Statistics were compiled for 23,190 pose updates, excluding the initialization of the first frame of each sequence. The internal processing latency in Table 2 is the complete internal update latency for a single frame of odometry, including downsampling, data association, local geometry calculation, pose optimization, and map maintenance, and does not refer only to the nearest neighbor search latency; this latency does not include data reading, real-time playback waiting, message conversion, and result publishing. The frame-weighted average is weighted by the number of valid timing frames in each sequence, and the equivalent processing frequency is 1000 divided by the average delay.

[0116] Table 2 Comparison of odometer processing efficiency under different data association implementations.

[0117] As shown in Table 2, compared to the full model variant that re-executes global kd-tree query and local geometry fitting for each pose update, the full model of this invention, employing precise incremental data association and local geometry reuse, reduces the frame-weighted average internal processing latency from 30.314 ms / frame to 21.520 ms / frame, a reduction of 29.01%; the equivalent processing frequency calculated from this average latency increases from 32.99 Hz to 46.47 Hz, an improvement of 40.87%. The precise incremental data association variant has a lower average internal processing latency across all 11 sequences than the global search variant, and neither variant has missing output poses. In the controlled control, the arithmetic mean of the translational APE RMSE for each sequence of the global search variant and the precise incremental data association variant are 3.3296 m and 3.2988 m, respectively; this result only indicates that the latter's cross-sequence arithmetic mean index was not higher than the former in this run, and the numerical difference between the two is not solely attributed to the data association implementation. Figure 3 Subgraphs in The frame weighted average internal processing delay and the corresponding equivalent processing frequency are shown.

[0118] This invention also provides a lidar odometry system based on reliability assessment and precise incremental data correlation, comprising: The data acquisition and motion compensation module is used to acquire LiDAR scans containing the relative sampling phase of the scan points, and to perform intra-scan motion compensation based on the relative motion between the two most recent valid poses. The Feature and Local Map module is used to extract edge features and planar features, and maintain edge feature maps and planar feature maps with map point support records and conflict records; The phased optimization and reliability assessment module is used to perform the first round of scan-map registration optimization to obtain coarse pose estimation, calculate geometric residuals under coarse pose and update cross-frame residual history; in the final round, when rebuilding the correspondence, it constructs bounded information weights by combining map stability, local geometric quality, projection uncertainty and orientation balance weights. The precise incremental data association module is used to perform precise incremental data association. It updates the lower bound of the distance between map points outside the candidate set according to the change of the query location, compares it with the maximum distance of a preset number of nearest neighbors in the cached candidates, and determines whether the cache contains precise nearest neighbors. If it cannot be determined, it performs a full precise nearest neighbor search within the association radius. The local geometry reuse module is used to sort the unique identifiers of precise nearest neighbor map points to form an ordered nearest neighbor identifier key, and reuse the map-related local geometry model only when the local map in the same frame is fixed and the ordered nearest neighbor identifier key remains unchanged. The final optimization and map state update module is used to obtain the final pose by using the bounded information weights in the final round scan-map registration optimization, and only after the final pose is obtained, write the intra-frame increment of map point support records and conflict records into the cross-frame map point state and update the local map. The results output module is used to output the final pose, LiDAR trajectory, and local map.

[0119] The specific implementation of each module and each step are not described in this invention.

[0120] This invention also provides an electronic device, including a processor, a memory, a communication interface, and a bus connecting the processor, the memory, and the communication interface. The communication interface is used to receive LiDAR scans; the memory is used to store a computer program, a local map, cross-frame residual history of polar coordinate grid cells, map point support and conflict status, and a query cache; the processor executes the computer program to complete steps 1 to 7. The processor may be a central processing unit, a graphics processing unit, a digital signal processor, a programmable logic device, or a combination thereof.

[0121] This invention also provides a computer-readable storage medium storing a computer program thereon. When the computer program is executed by a processor, it implements the aforementioned lidar odometer method. The storage medium may be a read-only memory, random access memory, flash memory, solid-state drive, magnetic disk, optical disk, or a combination thereof.

[0122] This invention is not limited to the specific embodiments described above. While maintaining the cross-frame residual history updated once per frame under a fixed coarse pose estimation, determining neighbors using the nearest neighbor accuracy criterion or full accurate nearest neighbor search within the associated radius, controlling local geometric reuse based on ordered nearest neighbor identifier keys, recalculating pose-related quantities, and updating cross-frame map point states and local maps only after the final pose solution is completed, equivalent substitutions can be made for feature types, index structures, robust loss functions, threshold acquisition methods, processor deployment, and data interfaces. The above embodiments are not intended to limit this invention; the scope of protection of this invention is defined by the claims.

Claims

1. A lidar odometry method for reliability assessment and correlation of accurate incremental data, characterized in that, include: Acquire the current frame of lidar scanning data and scan motion compensation based on relative sampling time; Edge and planar features are extracted from the motion-compensated scan, and a local map composed of historical map points is obtained. The first round of scanning and map registration optimization is performed to obtain coarse pose estimation. Geometric residuals are calculated under coarse pose and cross-frame residual history is updated. In the final round, when reconstructing the correspondence, bounded information weights are constructed by combining map stability, local geometric quality, projection uncertainty and orientation balance weights. Perform precise incremental data association, update the lower bound of the distance between map points outside the candidate set according to the change of the query location, compare it with the maximum distance of a preset number of nearest neighbors in the cached candidates, and determine whether the cache contains precise nearest neighbors; if it cannot be determined, perform a full precise nearest neighbor search within the association radius. The unique identifiers of the precise nearest neighbor map points are sorted to form an ordered nearest neighbor identifier key. The map-related local geometric model is reused only when the local map in the same frame is fixed and the ordered nearest neighbor identifier key remains unchanged. In the final round of scanning-map registration optimization, the bounded information weights are used to obtain the final pose, and only after the final pose is obtained are the intra-frame increments of map point support records and conflict records written into the cross-frame map point state and the local map updated.

2. The method according to claim 1, characterized in that: Record the relative sampling phase of the scan point within one frame of scanning, and perform intra-scan motion compensation on the scan point based on the relative motion between the two most recent valid poses. The intra-scan motion compensation includes: adjusting the azimuth difference of the scan point relative to the first valid echo according to... Modulo and mapping to interval Divide by The normalized sampling phase is obtained; the six-dimensional motion torque is obtained from the relative transformation of the effective poses of the two most recent frames; the rotation and translation components of the motion torque are multiplied by the rotation scaling factor and translation scaling factor constrained by preset upper and lower limits, respectively; according to the difference between the normalized sampling phase and the unified reference phase of the scan point, the scan point is transformed to the unified reference phase through three-dimensional special Euclidean group exponent mapping.

3. The method according to claim 1, characterized in that: The residual consistency weight is determined by updating the cross-frame residual history aggregated by polar coordinate grid cells based on the intra-frame robust statistics of the geometric residuals. The determination of the residual consistency weight includes: calculating the median and median absolute deviation of each geometric residual in the current frame under a fixed coarse pose estimation, thereby determining the residual center, robust scale, large residual judgment threshold, and the turning point scale of the Huber loss function; dividing the effective correspondence into polar coordinate grid cells centered on the lidar and discrete by azimuth and radial distance, and combining the normalized average excess amount of residuals exceeding the judgment threshold, the proportion of normalized large residuals, and the consistency of residual displacement vector direction in each polar coordinate grid cell to obtain the cell residual inconsistency; performing exponential smoothing update on cells with a sample number not less than a preset lower limit and a current cell residual inconsistency not less than a preset trigger threshold, performing decay update on cells that do not simultaneously meet the above conditions, and reducing the residual consistency weight of the cell after the cross-frame history reaches the confirmation threshold.

4. The method according to claim 1, characterized in that: The bounded information weights constructed by combining map stability, local geometric quality, projection uncertainty, and directional balance weights include: determining map stability weights based on cross-frame support records and conflict records of the current precise nearest neighbor map points; determining local geometric quality weights based on the covariance eigenvalues ​​of the current precise nearest neighbors; determining edge distance weights based on the measured distances of edge feature points; determining planar projection uncertainty weights based on the measured distances of planar feature points and the absolute value of the cosine of the angle between the planar normal and the unit observation ray after the current rotation transformation; and calculating directional balance weights based on edge type and planar type. The determination of the map stability weights includes: storing support counts and conflict counts for each map point; calculating the stability probability of the map point using a Beta distribution prior; limiting the stability probability of each precise nearest neighbor to a preset weight range after affine transformation; and robustly aggregating the obtained nearest neighbor stability weights using the median; in the final round of scan-map registration optimization, first forming an intra-frame support count increment or conflict count increment; and then writing the intra-frame count increment into the cross-frame map point state after the final pose solution is completed; and not inserting current features that are determined to be inconsistent according to the residual consistency evaluation results into the local map.

5. The method according to claim 4, characterized in that: The determination of the local geometric quality weight, projection uncertainty weight, and directional balance weight includes: calculating linearity and flatness based on the ascending eigenvalues ​​of the nearest neighbor covariance matrix, and mapping them to edge geometric weight and planar geometric weight, respectively; determining the edge distance weight based on the measured distance of the feature points after motion compensation; determining the planar normal projection uncertainty weight based on the absolute value of the cosine of the angle between the planar normal and the unit observation ray after the current rotation transformation, as well as the measured distance; dividing the effective straight line direction and the planar normal into directional bins, and determining the bounded inverse frequency directional balance weight based on the ratio of the average number of constraints in the non-empty directional bins to the number of constraints in each directional bin.

6. The method according to claim 1, characterized in that: In the first and final rounds of scan-map registration optimization, incremental spatial indexes are maintained for both the edge feature map and the planar feature map; for each feature query, a preset positive integer target nearest neighbor number is set. The system caches the previous query location, the set of candidate map points, and the lower bound of distances to map points outside the candidate set. It updates the lower bound of distances based on changes in the query location. The updated lower bound of distances is then compared with the cached candidate point's distance. Comparison between small distances verifies whether the cached candidate set contains the current one. A precise nearest neighbor is determined; when determining if a feature is included, the precise nearest neighbor is determined from the cached candidate set; if the number of nearest neighbors within the association radius is insufficient, the feature query is rejected; if the determination cannot be completed based on this, a full precise nearest neighbor search is performed within the association radius using the incremental spatial index; specifically: set up For feature query index, and Let be the current map coordinate system 3D position vector for feature query and the cached reference 3D position vector, both in meters; let the target nearest neighbors be... and cache capacity All are positive integers, and Not less than ;set up A positive correlation radius, The positive distance margin is used for comparison, and both are in meters; The precise incremental data association includes: in Stored in a location not exceeding A set of the nearest candidate map points And the lower bound of the effective distance to non-negative candidate map points outside the candidate set. , The unit is meters; the map points obtained from the cached candidate and the full exact nearest neighbor search are all ordered according to their nearest neighbor. Sort by Euclidean distance in ascending order; if distances are the same, sort by unique map point identifier in ascending order; minus and The Euclidean distance between them yields the map points outside the candidate set. The lower bound of the distance at that point , For real numbers, the unit is meters; when The number of candidates in the pool is not less than non-negative first Small candidate distance and The sum is less than ,and Less than Accept with certainty at that time. The unit is meters; when The number of candidates in is less than and Not less than ,or The front of the middle The nearest neighbors have been identified as exact neighbors and Not less than A deterministic rejection is made only when neither the deterministic acceptance nor the deterministic rejection condition is met. With the center of the ball, Perform a full exact nearest neighbor search within a spherical neighborhood of radius excluding the boundary and refresh the cache.

7. The method according to claim 6, characterized in that: For queries that meet the association acceptance criteria, the ordered nearest neighbor identifier key is determined by... The unique identifiers of each precise nearest neighbor map point are arranged in a preset order; the local map is fixed in each optimization round of the same frame, and the corresponding map correlation quantities are reused when the ordered nearest neighbor identifier keys are the same, including centroid, eigenvalues ​​and eigenvectors of the covariance matrix, geometric validity flag, geometric quality, plane bias and fitting failure state; the above map correlation quantities are recalculated when the ordered nearest neighbor identifier keys are different. Feature point transformation, residuals, Jacobian, and pose-related weights are not included in the local geometry cache and are recalculated with pose updates; the robustness scale is determined based on statistical results under a fixed coarse pose; the cache does not span local map updates.

8. A lidar odometry system based on reliability assessment and precise incremental data correlation, characterized in that, include: The data acquisition and motion compensation module is used to acquire LiDAR scans containing the relative sampling phase of the scan points, and to perform intra-scan motion compensation based on the relative motion between the two most recent valid poses. The Feature and Local Map module is used to extract edge features and planar features, and maintain edge feature maps and planar feature maps with map point support records and conflict records; The phased optimization and reliability assessment module is used to perform the first round of scan-map registration optimization to obtain coarse pose estimation, calculate geometric residuals under coarse pose and update cross-frame residual history; in the final round, when rebuilding the correspondence, it constructs bounded information weights by combining map stability, local geometric quality, projection uncertainty and orientation balance weights. The precise incremental data association module is used to perform precise incremental data association. It updates the lower bound of the distance between map points outside the candidate set according to the change of the query location, compares it with the maximum distance of a preset number of nearest neighbors in the cached candidates, and determines whether the cache contains precise nearest neighbors. If it cannot be determined, it performs a full precise nearest neighbor search within the association radius. The local geometry reuse module is used to sort the unique identifiers of precise nearest neighbor map points to form an ordered nearest neighbor identifier key, and reuse the map-related local geometry model only when the local map in the same frame is fixed and the ordered nearest neighbor identifier key remains unchanged. The final optimization and map state update module is used to obtain the final pose by using the bounded information weights in the final round scan-map registration optimization, and only after the final pose is obtained, write the intra-frame increment of map point support records and conflict records into the cross-frame map point state and update the local map. The results output module is used to output the final pose, LiDAR trajectory, and local map.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and capable of running on the processor, characterized in that, When the processor executes the computer program, it implements the method according to any one of claims 1 to 7.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the method described in any one of claims 1 to 7.

Images