Laser radar pose estimation method based on multilayer iterative extended Kalman filtering

Abstract

The invention discloses a laser radar pose estimation method based on multilayer iterative extended Kalman filtering, and belongs to the technical field of radar navigation. The method aims at solving the problems that an existing laser radar pose estimation method is low in operation efficiency and poor in estimation precision. Comprising the following steps: calculating a state predicted value # imgabs0 # and a covariance matrix predicted value # imgabs1 # according to a state value xk-1 and a covariance matrix Pk-1, obtaining an observation matrix # imgabs2 # by adopting a scan-to-submap matching method insensitive to an initial value, updating the predicted value to obtain updated # imgabs3 # and # imgabs4 #, and performing iterative distortion correction on preprocessed point cloud data by combining a High-level motion compensation method; performing iterative updating on the updated state value # imgabs5 # by combining a low-level state updating method, and obtaining a current state optimal value xk after convergence; and then the updated covariance matrix # imgabs6 # is updated, and the current covariance matrix Pk is obtained. The method is used for pose estimation.

Description

Technical Field

[0001] The invention relates to a laser radar posture estimation method based on multi-layer iterative extended Kalman filtering, and belongs to the technical field of radar navigation. Background Art

[0002] In the field of intelligent transportation and mobile robots, the state estimation technology of LiDAR is crucial to achieve high-precision environmental perception and positioning. Existing LiDAR odometer methods face problems such as sparse sensor data, high noise, and non-uniformity, making it difficult to operate stably in diverse environments. In addition, odometer methods that rely on inertial measurement units (IMUs) have the problem of reduced accuracy under fast motion conditions.

[0003] There are three main methods for existing laser odometers: ICP-based method, LOAM-based method and continuous time-based method.

[0004] Laser odometry based on ICP:

[0005] Point cloud registration is an important component of radar odometry, and the pose transformation between point clouds is obtained through point cloud observations. The iterative closest point (ICP) algorithm first proposed a comprehensive method for free surface registration, laying the foundation for contemporary point cloud registration technology. Since then, many variants have been proposed by researchers, using novel data association metrics such as point-to-line and point-to-plane distances, or integrating robust kernels such as the Huber loss function. For example, FR-ICP introduces the Welsch function to model the registration error and transforms the ICP problem into a fixed point iteration problem, using the Anderson acceleration method to speed up the convergence. In contrast, Kiss-ICP proposes a simplified point-to-point ICP method that can achieve efficient motion estimation in different scenarios without a lot of parameter tuning, although its performance decreases with the increase of sparse point clouds. Therefore, in order to address these limitations, hybrid methods such as Voxelized ICP have emerged, which utilize the point distribution within the voxel to bypass the computationally intensive nearest neighbor search while maintaining accuracy and speed. In addition, VoxelMap integrates an adaptive voxel map within the filtering framework, focusing on LiDAR observation uncertainty. However, VoxelMap focuses more on the structure of the point cloud map and ignores the motion distortion caused by the movement of the radar point cloud.

[0006] Laser odometry based on LOAM:

[0007] LOAM proposed the first real-time radar odometry framework, which uses point-to-line and point-to-plane optimization residuals for accurate pose estimation. This work is extremely groundbreaking and laid a theoretical foundation for the subsequent development of laser odometry and even the development of multi-sensor fusion algorithms. For example, F-LOAM bypasses the scan-to-scan matching method by directly implementing the scan-to-map method for pose estimation, improving computational efficiency while maintaining accuracy. However, it is still susceptible to motion distortion, which limits its practical performance. LeGO-LOAM is tailored for ground vehicles. Under significant ground constraints, a novel two-stage LM optimization method is introduced, which accelerates the convergence of optimization without affecting accuracy, and can be deployed even in embedded systems with limited computing resources. In addition, it innovatively clusters the input point cloud and segments the ground points, which not only reduces the noise generated by vegetation during motion that affects motion estimation, but also adds additional constraints through the segmented ground points to reduce the error in the z-axis direction. LLOAM introduces a loop detection mechanism based on LOAM to improve robustness and long-term performance. The emergence of new LiDAR sensors such as non-repetitive scanning radars has overcome the problem of sparse point clouds of mechanical LiDARs, further diversifying the research direction of LiDAR SLAM. LOAM-Livox extends LOAM to solid-state LiDAR, overcoming the field of view (FOV) limitation, but its effectiveness is still limited under extreme conditions.

[0008] Continuous-time laser odometry:

[0009] Considering the continuity of radar data stream, researchers add additional continuity trajectory constraints to pose estimation. Linear interpolation is a simple but effective way, but it cannot effectively describe the real motion situation for fast-moving low-frequency laser SLAM systems, resulting in serious distortion of point clouds. CT-ICP solves this problem by considering scanning discontinuity errors and introducing additional motion constraints, although its performance will degrade in dynamic scenes, such as handheld sensors or aircraft. B-spline is a weighted sum of basis functions, providing a parametric representation of trajectories that balances accuracy and efficiency. This approach has been verified in various scenarios, including aerial applications. On this basis, MARS adopts the surfel-based BSpline method, which has demonstrated robustness and accuracy in land and air environments. CLINS uses IMU data for trajectory estimation, but it is difficult to meet real-time requirements, while its successor CLIC extends the BSpline model to include LiDAR, inertial and camera systems, thereby enhancing trajectory estimation. Traj-LO divides the trajectory into multiple intervals and approximates continuous motion by simple linear interpolation. FAST-LIO introduces a discrete motion model integrated with IMU for state prediction and accurate correction of motion distortion in point clouds. State updates are performed via IEKF and LiDAR observations. FAST-LIO2 further improves efficiency by performing mapping maintenance via incremental kd-trees. Summary of the invention

[0010] In view of the problems of low computational efficiency and poor estimation accuracy in existing laser radar pose estimation methods, the present invention provides a laser radar pose estimation method based on multi-layer iterative extended Kalman filtering.

[0011] A laser radar pose estimation method based on multi-layer iterative extended Kalman filtering of the present invention comprises:

[0012] According to the state value x at time k-1 k-1 and the covariance matrix P at k-1 time k-1 Calculate the state prediction value at time k And the covariance matrix prediction value at time k

[0013] The observation matrix at time k is obtained by using a scan-to-submap matching method that is insensitive to the initial value. Prediction value of state and the covariance matrix predicted values Update and get the updated status value And the updated covariance matrix

[0014] Preprocessing the point cloud data of the mechanical LiDAR sensor to obtain preprocessed point cloud data;

[0015] High-level motion compensation method is used to use the updated state value And the updated covariance matrix The pre-processed point cloud data is iteratively corrected for distortion to obtain the final distortion-corrected point cloud data; the low-level state update method is then used to update the updated state value using the final distortion-corrected point cloud data. Perform iterative updates and obtain the optimal value x of the current state after convergence k ;

[0016] Then update the covariance matrix Update and get the current covariance matrix P k .

[0017] According to the laser radar pose estimation method based on multi-layer iterative extended Kalman filtering of the present invention, the state prediction value at time k is And the covariance matrix prediction value at time k The calculation method is:

[0018]

[0019] In the formula is a custom operator, indicating the addition of disturbance, Δt is the duration of adjacent moments, f(x k-1 ,w k-1 ) is the discretized motion model, is the Jacobian matrix at time k-1, representing the error state δx k-1 Relative to the error state δx at the previous moment k-2 The partial derivative of is the error state δx k-1 Relative to the noise w k-1 The partial derivative of k-1 is the nonlinear covariance matrix at k-1 moment;

[0020] The forward propagation of the error state is:

[0021]

[0022] f(x k-1 ,w k-1 ) is defined as follows:

[0023]

[0024] Where ω k-1 is the angular velocity at time k-1, v k-1 is the speed at time k-1, n v is the velocity noise, n ωis the angular velocity noise, G represents the global coordinate system, and T represents the transposition;

[0025]

[0026] R k-1 Customize observation confidence for k-1 moment;

[0027] and Both are sparse matrices:

[0028]

[0029] According to the laser radar pose estimation method based on multi-layer iterative extended Kalman filtering of the present invention, the observation matrix for:

[0030]

[0031] In the formula The rotation matrix is ​​obtained by using the scan-to-submap matching method based on the corresponding points with matching relationship. It is the translation vector obtained by using the scan-to-submap matching method based on the corresponding points with matching relationship.

[0032] According to the laser radar pose estimation method based on multi-layer iterative extended Kalman filtering of the present invention, the updated state value And the updated covariance matrix The calculation method is:

[0033]

[0034] Where Kalman gain at time k is for:

[0035]

[0036] In the formula is a custom operator, indicating disturbance reduction, I is the unit matrix, is the Kalman gain at time k, H is the Jacobian matrix of the observation equation, is the observation covariance at time k;

[0037]

[0038] In the formula is the Hessian matrix obtained using the scan-to-submap matching method, is the set of real numbers, is the noise characteristic parameter determined by experiment.

[0039] According to the laser radar pose estimation method based on multi-layer iterative extended Kalman filtering of the present invention, the symbol of the disturbance is added and the sign of the disturbance reduction The calculation method is:

[0040]

[0041] Where A, B, a, b, A1 and A2 are all set variables.

[0042] According to the laser radar pose estimation method based on multi-layer iterative extended Kalman filtering of the present invention, the preprocessed point cloud data is represented as

[0043] After preprocessing, the point cloud p j Perform distortion correction to obtain the corresponding current frame L k Point Cloud

[0044]

[0045] In the formula The status updated for the low-level status, Where R is the updated rotation matrix, p is the updated translation matrix, k = 0, ..., M, M is the maximum number of iterations, κ is the number of high-level iterations;

[0046]

[0047] Where SO is the Lie group;

[0048]

[0049] Where T k-1 is the corresponding state value x k-1 The transformation matrix composed of the rotation matrix and translation matrix;

[0050]

[0051] Where t is time, t j P j The corresponding time, t k represents time k, t k is the k-1 moment;

[0052] Then the final distortion-corrected point cloud data is obtained

[0053] According to the laser radar pose estimation method based on multi-layer iterative extended Kalman filtering of the present invention, a low-level state update method is used to update the state value after the final distortion correction point cloud data. The method for iterative updating is:

[0054]

[0055] In the formula is the point-surface residual measurement matrix, is the point-surface Kalman gain, H k is the point-surface residual Jacobian resignation, R k is the noise covariance matrix;

[0056] After low-level convergence, the final As the optimal value of the current state x k .

[0057] According to the laser radar pose estimation method based on multi-layer iterative extended Kalman filtering of the present invention, the current covariance matrix P k for:

[0058]

[0059] In the formula is the updated covariance matrix The left matrix obtained by Cholseky decomposition is, is the noise vector e k The weight matrix of

[0060] Noise vector e k for:

[0061]

[0062] Where diag(·) means generating a diagonal matrix, is each element of the noise vector; φ(e k,i ) is the weight factor, ρ(e k,i ) is the weight function, and the Huber-Loss function is selected as the weight function.

[0063] Beneficial effects of the invention: The method of the invention provides a real-time lightweight laser odometer that can realize positioning and mapping based on the point cloud perception of the laser radar to achieve more robust and accurate pose estimation. The method of the invention can realize high-precision and high-efficiency laser radar positioning and mapping across multiple scenes.

[0064] The method proposed in this paper proposes a new coarse-to-fine lidar odometry framework, which achieves high-precision state estimation through multi-level iterative extended Kalman filtering. It is built on the basis of FAST-LIO, combines the advantages of FAST-LIO, and adapts to the unique challenges of LiDAR observation.

[0065] A coarse-to-fine LiDAR odometry framework based on multi-layer iterative extended Kalman filtering (C2F-LO) is proposed. First, C2F-LO preprocesses each LiDAR frame using neighbor filters and voxel filters. Then, a coarse-to-fine strategy (C2FS) is introduced, which starts with a constant velocity model for coarse prediction and then refines it using a distribution-based scan-to-submap approach, which is less sensitive to initial values. C2FS incrementally integrates information from the previous frame, keyframe-based local maps, and global maps, thereby improving the accuracy and robustness during the optimization process. In addition, C2FLO introduces a new submap surgery technique to reduce redundant information in the local map, which would otherwise consume computational resources without making meaningful contributions to the results. For optimization, the proposed method proposes a powerful hierarchical iterative extended Kalman filter (RH-IEKF), which includes two iterative processes: low-level state update and high-level motion compensation. Different from existing methods, RH-IEKF only focuses on reapplying motion compensation, thereby balancing convergence speed and resource efficiency. The low-level state update iteratively refines the estimate using robust observations, mitigating the impact of outliers by reweighting the update steps.

[0066] This paper presents a coarse-to-fine LiDAR odometry algorithm, which is experimentally verified to achieve state-of-the-art performance on both public and self-collected benchmarks. The proposed method exploits asymptotic information to provide accurate initialization and state estimates. The proposed robust hierarchical iterative extended Kalman (RH-IEKF) has high-level motion compensation and low-level state updates using robust observations. Its innovative sub-graph generation criteria effectively reduces redundant information. BRIEF DESCRIPTION OF THE DRAWINGS

[0067] Figure 1 It is a flow chart of the laser radar pose estimation method based on multi-layer iterative extended Kalman filtering described in the present invention. DETAILED DESCRIPTION

[0068] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0069] It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments may be combined with each other.

[0070] The present invention will be further described below in conjunction with the accompanying drawings and specific embodiments, but they are not intended to limit the present invention.

[0071] Specific implementation method 1. Combination Figure 1 As shown, the present invention provides a laser radar pose estimation method based on multi-layer iterative extended Kalman filtering, comprising:

[0072] According to the state value x at time k-1 k-1 and the covariance matrix P at k-1 time k-1 Calculate the state prediction value at time k And the covariance matrix prediction value at time k

[0073] The observation matrix at time k is obtained by using a scan-to-submap matching method that is insensitive to the initial value. Prediction value of state and the covariance matrix predicted values Update and get the updated status value And the updated covariance matrix

[0074] Preprocessing the point cloud data of the mechanical LiDAR sensor to obtain preprocessed point cloud data;

[0075] High-level motion compensation method is used to use the updated state value And the updated covariance matrix The pre-processed point cloud data is iteratively corrected for distortion to obtain the final distortion-corrected point cloud data; the low-level state update method is then used to update the updated state value using the final distortion-corrected point cloud data. Perform iterative updates and obtain the optimal value x of the current state after convergence k ;

[0076] Then update the covariance matrix Update and get the current covariance matrix P k .

[0077] Furthermore, for continuous motion LiDAR systems, the state is predicted by discrete motion propagation. C2F-LO uses the constant velocity assumption to achieve state prediction.

[0078] State prediction value at time k And the covariance matrix prediction value at time k The calculation of is realized by discrete propagation equation:

[0079]

[0080] In the formula is a custom operator, indicating the addition of disturbance, Δt is the duration of adjacent moments, f(x k-1 ,w k-1 ) is the discretized motion model, is the Jacobian matrix at time k-1, representing the error state δx k-1 Relative to the error state δx at the previous moment k-2 The partial derivative of is the error state δx k-1 Relative to the noise w k-1 The partial derivative of k-1 is the nonlinear covariance matrix at k-1 moment;

[0081] The forward propagation of the error state is:

[0082]

[0083] In general motion scenes, such as handheld devices, rotations and translations in different directions are usually independent. Therefore, it is assumed that the linear velocity and angular velocity are in the time interval t of the i-th frame. i Under this condition, f(x k-1 ,w k-1 ) is defined as follows:

[0084]

[0085] Where ω k-1 is the angular velocity at time k-1, v k-1 is the speed at time k-1, n v- is the velocity noise, n ω is the angular velocity noise, G represents the global coordinate system, and T represents the transposition;

[0086] For ground mobile robots, there are strong coupling constraints between rotation and translation under the assumption of uniform motion. In this case, the function f(x k-1 ,w k-1 ) is expressed as:

[0087]

[0088] R k-1 Customize observation confidence for k-1 moment;

[0089] and Both are sparse matrices:

[0090]

[0091] Scan-to-Submap matches:

[0092] Considering that the constant velocity assumption is not completely consistent with the true motion condition without additional sensors, the forward propagation alone leads to a coarse prediction estimate. To address this limitation, C2FS incorporates a scan-to-submap matching process that is insensitive to the initial value and refines the initial state through scan-to-submap.

[0093] In this embodiment, the observation matrix for:

[0094]

[0095] In the formula The rotation matrix is ​​obtained by using the scan-to-submap matching method based on the corresponding points with matching relationship. It is the translation vector obtained by using the scan-to-submap matching method based on the corresponding points with matching relationship.

[0096] For the observed value The rotation and translation components are extracted directly from the scan-to-submap matching results. Then, the covariance matrix and state vector are obtained according to the forward propagation and updated through the Kalman filter (using the constant velocity model).

[0097] Updated status value And the updated covariance matrix The calculation method is:

[0098]

[0099] Where Kalman gain at time k is for:

[0100]

[0101] In the formula is a custom operator, indicating disturbance reduction, I is the unit matrix, is the Kalman gain at time k, H is the Jacobian matrix of the observation equation, is the observation covariance at time k;

[0102]

[0103] Observation covariance Use the Hessian matrix derived from the scan-to-submap result to approximate;

[0104] In the formula is the Hessian matrix obtained using the scan-to-submap matching method, is the set of real numbers, is the noise characteristic parameter determined by experiment.

[0105] Considering the problem of processing on the manifold, this implementation defines two operators: as well as To characterize the tangent space of the manifold and manifold The relationship between:

[0106] As an example, add the perturbation symbol and the sign of the disturbance reduction The calculation method is:

[0107]

[0108] Where A, B, a, b, A1 and A2 are all set variables.

[0109] The following multi-layer iterative extended Kalman filter is performed:

[0110] Stage 1: High-level motion compensation:

[0111] Given the raw LiDAR point cloud, it is critical to account for motion-induced distortions since the sensor is not stationary during data acquisition. In the prediction phase, the raw point cloud is not directly dedistorted. In order to improve the accuracy of state estimation within the RH-IEKF framework, the updated point cloud state is used for distortion correction in the first stage of the multi-layer iterative extended Kalman filter. The RH-IEKF method uses linear interpolation to transform points from the kth raw point cloud to the current frame L k The end time t k .

[0112] Specifically, for the relative time t j Radar point p j , given the state from the low-level update (When κ=0 ) and the posterior estimate from the k-1th frame

[0113] Going further, the preprocessed point cloud data is represented as

[0114] After preprocessing, the point cloud p j Perform distortion correction to obtain the corresponding current frame Lk Point Cloud

[0115]

[0116] In the formula The status updated for the low-level status, Where R is the updated rotation matrix, p is the updated translation matrix, κ=0,…,M, M is the maximum number of iterations, and κ is the number of high-level iterations;

[0117]

[0118] Where SO is the Lie group;

[0119]

[0120] Where T k-1 is the corresponding state value x k-1 The transformation matrix composed of the rotation matrix and translation matrix;

[0121]

[0122] Where t is time, t j For p j The corresponding time, t k represents time k, t k is the k-1 moment;

[0123] Then the final distortion-corrected point cloud data is obtained

[0124] This embodiment may apply two filtering strategies to the raw point cloud data before state estimation, aiming to reduce the point cloud density while retaining key spatial details. First, a neighboring point filter may be applied to eliminate points within a 1-meter radius of the LiDAR sensor, thereby minimizing interference from the platform (e.g., unmanned ground vehicles, unmanned aerial vehicles) or operators. Next, a 3D voxel grid filter is used for downsampling, retaining key geometric structures. For indoor settings, the voxel resolution is set to 0.3 meters, and for outdoor environments, the voxel resolution is set to 0.5 meters. It is worth noting that this embodiment omits traditional feature extraction and chooses to process the raw point cloud data directly. After preprocessing, approximately 10,000 points are retained for subsequent operations, which is conducive to the generation of dense maps.

[0125] The coarse-to-fine strategy (C2FS) of C2F-LO has two components: forward propagation and scan-to-Submap matching.

[0126] Phase 2: low-level status update

[0127] The low-level state update method is used to update the state value using the final distortion-corrected point cloud data. The method for iterative updating is:

[0128]

[0129] In the formula is the point-surface residual measurement matrix, is the point-surface Kalman gain, H k is the point-surface residual Jacobian resignation, R k is the noise covariance matrix;

[0130] After low-level convergence, the final As the optimal value of the current state x k .

[0131] Current covariance matrix P k for:

[0132]

[0133] In the formula is the updated covariance matrix The left matrix obtained by Cholseky decomposition is, is the noise vector e k The weight matrix of

[0134] Noise vector e k for:

[0135]

[0136] Where diag(·) means generating a diagonal matrix, is each element of the noise vector; φ(e k,i ) is the weight factor, ρ(e k,i ) is the weight function, and the Huber-Loss function is selected as the weight function.

[0137] Keyframe selection and local map update:

[0138] Furthermore, the present invention is also used for updating local maps:

[0139] When updating the system state, keyframe selection is performed based on the updated pose.

[0140] First, based on the current state optimal value x k and the current covariance matrix P kTo make a keyframe selection:

[0141] If the current frame L k If the displacement or rotation relative to the previous key frame exceeds the corresponding set threshold, the current frame L k As the current key frame, add it to the key frame database The common center threshold can be pre-defined or adaptively adjusted according to the current scene, following the relevant principles in DLO. This embodiment uses C2FS, which enhances the initial state estimation by utilizing scan-to-submap matching. Effectively managing and generating local maps is crucial for reliable scan-to-submap matching. The Kiss-ICP method introduces a radius-based method to construct a local map, in which operations are performed on each point while excluding those points outside the search radius. In contrast, the keyframe-based local map construction method only processes relevant keyframes. This method not only reduces the computational complexity, but also retains more point cloud data.

[0142] Therefore, the method for updating the local map is:

[0143] Representing key frame database based on incremental kd-tree structure The key frame data in each node is used to store the optimal value x of the current state k and the current covariance matrix P k , point cloud data, timestamp and feature descriptor; the feature descriptor is obtained according to ScanContext;

[0144] In the keyframe database In the method, E nearest neighbor keyframes are selected based on the distance, and g nearest neighbor convex hull keyframes are selected based on the nearest neighbor convex hull; the point cloud descriptors are calculated for the E nearest neighbor keyframes and the g nearest neighbor convex hull keyframes, and the keyframes whose similarity is lower than the predefined threshold are removed according to the similarity between the point cloud descriptors; the remaining nearest neighbor keyframes after pruning are combined into point cloud A k , the remaining nearest neighbor convex hull keyframes after pruning form point cloud B k ;

[0145] A k and B k The point cloud composed of is downsampled to obtain the updated local map S k :

[0146]

[0147] Where DS(·) represents weighted downsampling; Represents the merging of point clouds.

[0148] However, selecting keyframes based solely on spatial distance may introduce redundant frames that contribute little to the matching quality. To address this issue, this embodiment proposes an improved local map construction criterion. It includes using ScanContext to generate point cloud descriptors for keyframes and the current frame, and evaluating the similarity between point clouds based on these descriptors. For the E most recent keyframes, only those keyframes with similarity scores above a defined threshold are retained to build a local map. In addition, since the confidence of the LiDAR odometry decreases over time, point clouds from keyframes with earlier timestamps are preferentially retained during downsampling.

[0149] Although the present invention is described herein with reference to specific embodiments, it should be understood that these embodiments are merely examples of the principles and applications of the present invention. It should therefore be understood that many modifications may be made to the exemplary embodiments and that other arrangements may be devised without departing from the spirit and scope of the present invention as defined by the appended claims. It should be understood that the various dependent claims and features described herein may be combined in a manner different from that described in the original claims. It should also be understood that the features described in conjunction with a single embodiment may be used in other described embodiments.

Claims

1. A laser radar pose estimation method based on multi-layer iterative extended Kalman filtering, characterized in that: include: According to the state value x at time k-1 k-1 and the covariance matrix P at k-1 time k-1 Calculate the state prediction value at time k And the covariance matrix prediction value at time k The observation matrix at time k is obtained by using a scan-to-submap matching method that is insensitive to the initial value. Prediction value of state and the covariance matrix predicted values Update and get the updated status value And the updated covariance matrix Preprocessing the point cloud data of the mechanical LiDAR sensor to obtain preprocessed point cloud data; High-level motion compensation method is used to use the updated state value And the updated covariance matrix The pre-processed point cloud data is iteratively corrected for distortion to obtain the final distortion-corrected point cloud data; the low-level state update method is then used to update the updated state value using the final distortion-corrected point cloud data. Perform iterative updates and obtain the optimal value x of the current state after convergence k ; Then update the covariance matrix Update and get the current covariance matrix P k .

2. The method for estimating a laser radar pose based on multi-layer iterative extended Kalman filtering according to claim 1, characterized in that: State prediction value at time k And the covariance matrix prediction value at time k The calculation method is: In the formula is a custom operator, indicating the addition of disturbance, Δt is the duration of adjacent moments, f(x k-1 ,w k-1 ) is the discretized motion model, is the Jacobian matrix at time k-1, representing the error state δx k-1 Relative to the error state δx at the previous moment k-2 The partial derivative of is the error state δx k-1 Relative to the noise w k-1 The partial derivative of k-1 is the nonlinear covariance matrix at k-1 moment; The forward propagation of the error state is: f(x k-1 ,w k-1 ) is defined as follows: Where ω k-1 is the angular velocity at time k-1, v k-1 is the speed at time k-1, n v is the velocity noise, n ω is the angular velocity noise, G represents the global coordinate system, and T represents the transposition; R k-1 Customize observation confidence for k-1 moment; and Both are sparse matrices:

3. The laser radar pose estimation method based on multi-layer iterative extended Kalman filtering according to claim 2 is characterized in that: Observation Matrix for: In the formula The rotation matrix is ​​obtained by using the scan-to-submap matching method based on the corresponding points with matching relationship. It is the translation vector obtained by using the scan-to-submap matching method based on the corresponding points with matching relationship.

4. The method for estimating a laser radar pose based on multi-layer iterative extended Kalman filtering according to claim 3, characterized in that: Updated status value And the updated covariance matrix The calculation method is: Where Kalman gain at time k is for: In the formula, Θ is a custom operator representing subtracting perturbations, I is the identity matrix, is the Kalman gain at time k, and H is the Jacobian matrix of the observation equation, is the observation covariance at time k; In the formula is the Hessian matrix obtained using the scan-to-submap matching method, is the set of real numbers, is the noise characteristic parameter determined by experiment.

5. The method for estimating a laser radar pose based on multi-layer iterative extended Kalman filtering according to claim 4, characterized in that: Add perturbation symbol and the sign of the disturbance reduction The calculation method is: Where A, B, a, b, A1 and A2 are all set variables.

6. The method for estimating a laser radar pose based on multi-layer iterative extended Kalman filtering according to claim 5, characterized in that: The preprocessed point cloud data is represented as After preprocessing, the point cloud p j Perform distortion correction to obtain the corresponding current frame L k Point Cloud In the formula The status updated for the low-level status, Where R is the updated rotation matrix, p is the updated translation matrix, κ=0,…,M, M is the maximum number of iterations, and κ is the number of high-level iterations; Where SO is the Lie group; Where T k-1 is the corresponding state value x k-1 The transformation matrix composed of the rotation matrix and translation matrix; Where t is time, t j For p j The corresponding time, t k represents time k, t k is the k-1 moment; Then the final distortion-corrected point cloud data is obtained 7. The method for estimating a laser radar pose based on multi-layer iterative extended Kalman filtering according to claim 6, characterized in that: The low-level state update method is used to update the state value using the final distortion-corrected point cloud data. The method for iterative updating is: In the formula is the point-surface residual measurement matrix, is the point-surface Kalman gain, H k is the point-surface residual Jacobian resignation, R k is the noise covariance matrix; After low-level convergence, the final As the optimal value x of the current state k .

8. The method for estimating a laser radar pose based on multi-layer iterative extended Kalman filtering according to claim 7, characterized in that: Current covariance matrix P k for: In the formula is the updated covariance matrix The left matrix obtained by Cholseky decomposition is, is the noise vector e k The weight matrix of Noise vector e k for: Where diag(·) means generating a diagonal matrix, is each element of the noise vector; φ(e k,i ) is the weight factor, ρ(e k,i ) is the weight function, and the Huber-Loss function is selected as the weight function.

9. The method for estimating a laser radar pose based on multi-layer iterative extended Kalman filtering according to claim 8, characterized in that: Based on the current state optimal value x k and the current covariance matrix P k The method for key frame selection is: If the current frame L k If the displacement or rotation relative to the previous key frame exceeds the corresponding set threshold, the current frame L k As the current key frame, add it to the key frame database 10. The method for estimating a laser radar pose based on multi-layer iterative extended Kalman filtering according to claim 9, characterized in that: The method for performing a local map update is: Representing key frame database based on incremental kd-tree structure The key frame data in each node is used to store the optimal value x of the current state k and the current covariance matrix P k , point cloud data, timestamps and feature descriptors; In the keyframe database In the method, E nearest neighbor keyframes are selected based on the distance, and g nearest neighbor convex hull keyframes are selected based on the nearest neighbor convex hull; the point cloud descriptors are calculated for the E nearest neighbor keyframes and the g nearest neighbor convex hull keyframes, and the keyframes whose similarity is lower than the predefined threshold are removed according to the similarity between the point cloud descriptors; the remaining nearest neighbor keyframes after pruning are combined into point cloud A k , the remaining nearest neighbor convex hull keyframes after pruning form point cloud B k ; A k and B k The point cloud composed of is downsampled to obtain the updated local map S k : Where DS(·) represents weighted downsampling; Represents the merging of point clouds.

Images