A positioning and mapping method, device and storage medium

Abstract

The application relates to the technical field of robot positioning and mapping, and discloses a positioning and mapping method, equipment and a storage medium. The method comprises the following steps: performing motion distortion correction on point cloud data of a current frame collected by a laser radar to obtain corrected point cloud data of the current frame; a local sub-map of the current frame is constructed, the degeneration probability of a unit direction vector of the local sub-map is calculated, a Hessian matrix of an iterative closest point algorithm is modified according to the degeneration probability, a pose estimation result of the corrected point cloud data relative to the local sub-map is determined according to the modified Hessian matrix; the pose estimation result and an IMU prediction value are fused through a Kalman filter to calculate an optimal pose estimation, and the corrected point cloud data is inserted into a global map based on the optimal pose estimation. The problems of point cloud distortion and scene degeneration in a dynamic scene are solved, and high-precision and robust positioning and mapping are realized.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robot positioning and mapping, and particularly relates to a positioning and mapping method, device and storage medium. BACKGROUND

[0002] With the development of automatic driving and mobile robot technology, accurate positioning and environment mapping have become a key technical challenge. Laser radar has become a mainstream sensor due to its high precision and non-lighting affected characteristics, but in the environment with degenerated geometric features (such as long corridors, tunnels, etc.), traditional algorithms are prone to pose estimation drift due to insufficient geometric constraints.

[0003] The related degeneration detection algorithm needs to adjust parameters for specific environments, and is difficult to adapt to the perception degeneration detection needs of different scenes, and the pose estimation effect considering the perception degeneration is poor, resulting in weak accuracy of positioning and mapping. SUMMARY

[0004] The purpose of the embodiments of the present application is to provide a positioning and mapping method, device and storage medium, so as to improve the accuracy of positioning and mapping.

[0005] To solve the above technical problems, the embodiments of the present application provide a positioning and mapping method, which comprises: performing motion distortion correction on point cloud data of a current frame collected by a laser radar to obtain corrected point cloud data of the current frame; constructing a local sub-map of the current frame, calculating a degeneration probability of a unit direction vector of the local sub-map, and modifying a Hessian matrix of an iterative closest point algorithm according to the degeneration probability, determining a pose estimation result of the corrected point cloud data relative to the local sub-map according to the modified Hessian matrix; fusing the pose estimation result and an IMU prediction value through a Kalman filter to calculate an optimal pose estimation, and inserting the corrected point cloud data into a global map based on the optimal pose estimation.

[0006] The embodiments of the present application also provide an electronic device, comprising: at least one processor; and a memory in communication connection with the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor to enable the at least one processor to execute the above-mentioned positioning and mapping method.

[0007] The embodiments of the present application also provide a computer readable storage medium storing a computer program, wherein the computer program is executed by a processor to implement the above-mentioned positioning and mapping method.

[0008] Compared to related technologies, this invention reduces point cloud motion errors through IMU distortion correction, obtaining corrected point cloud data and improving the basic accuracy of point cloud registration. It further constructs a local sub-map composed of keyframes near the current frame, calculates the degradation probability of the unit direction vector, and corrects the eigenvalues ​​of the Hessian matrix based on the degradation probability, effectively suppressing invalid pose update directions. Furthermore, it determines the pose estimation result of the corrected point cloud data relative to the local sub-map based on the corrected Hessian matrix, making the result more accurate and avoiding pose estimation drift. Finally, it uses the pose estimation obtained from ICP registration as a measurement value and fuses it with the IMU state prediction through Kalman filtering to update the state estimation and covariance matrix, obtaining the optimal pose estimation, improving the robustness and accuracy of pose estimation, and achieving continuous global map construction. In summary, this application effectively solves the problem of matching failure caused by point cloud distortion and scene degradation in dynamic scenes by utilizing IMU to compensate for point cloud distortion, enhancing matching stability through degradation detection, and achieving complementary advantages through multi-sensor fusion, thus achieving high-precision and robust localization and mapping. Attached Figure Description

[0009] One or more embodiments are illustrated by way of example with reference numerals in the accompanying drawings. These illustrations do not constitute a limitation on the embodiments. Elements with the same reference numerals in the drawings are denoted as similar elements. Unless otherwise stated, the figures in the drawings are not to be limited by scale.

[0010] Figure 1 This is a flowchart of a positioning and mapping method provided in an embodiment of this application;

[0011] Figure 2 This is a flowchart of a localization and mapping method using probabilistic degradation detection point cloud registration provided in another embodiment of this application;

[0012] Figure 3 This is a schematic diagram of the structure of an electronic device provided in another embodiment of this application. Detailed Implementation

[0013] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the various embodiments of the present invention will be described in detail below with reference to the accompanying drawings. However, those skilled in the art will understand that many technical details are presented in the various embodiments of the present invention to facilitate a better understanding of this application. However, the technical solutions claimed in this application can be implemented even without these technical details and various changes and modifications based on the following embodiments. The division of the various embodiments below is for ease of description and should not constitute any limitation on the specific implementation of the present invention. The various embodiments can be combined with and referenced by each other without contradiction.

[0014] This invention provides a localization mapping method. The implementation program for this localization mapping method can be deployed in an electronic device, such as in the memory and processor of the electronic device; further details are omitted here.

[0015] The localization and mapping method provided in this application improves the accuracy of localization and mapping. Specifically, this application reduces point cloud motion errors through IMU distortion correction to obtain corrected point cloud data, improving the basic accuracy of point cloud registration. It further constructs a local sub-map composed of keyframes near the current frame, calculates the degradation probability of the unit direction vector, and corrects the eigenvalues ​​of the Hessian matrix based on the degradation probability, effectively suppressing invalid pose update directions. Furthermore, it determines the pose estimation result of the corrected point cloud data relative to the local sub-map based on the corrected Hessian matrix, making the result more accurate and avoiding pose estimation drift. Finally, the pose estimate obtained from ICP registration is used as a measurement value and fused with the IMU state prediction through Kalman filtering to update the state estimate and covariance matrix, obtaining the optimal pose estimate, improving the robustness and accuracy of pose estimation, and achieving continuous global map construction. In summary, this application effectively solves the problem of matching failure caused by point cloud distortion and scene degradation in dynamic scenes by utilizing IMU to compensate for point cloud distortion, enhancing matching stability through degradation detection, and achieving complementary advantages through multi-sensor fusion. This results in high-precision and robust localization and mapping. To facilitate understanding of the localization and mapping method provided in the embodiments of this application, the following will describe its different implementation processes.

[0016] In some embodiments, the flowchart of the localization mapping method is as follows: Figure 1 As shown, it includes the following steps:

[0017] Step 101: Perform motion distortion correction on the point cloud data of the current frame acquired by the lidar to obtain the corrected point cloud data of the current frame.

[0018] Step 102: Construct a local sub-map of the current frame, calculate the degradation probability of the unit direction vector of the local sub-map, correct the Hessian matrix of the iterative nearest point algorithm according to the degradation probability, and determine the pose estimation result of the corrected point cloud data relative to the local sub-map according to the corrected Hessian matrix.

[0019] Step 103: The pose estimation result is fused with the IMU prediction value through a Kalman filter to calculate the optimal pose estimation, and the corrected point cloud data is inserted into the global map based on the optimal pose estimation.

[0020] In this way, by reducing point cloud motion errors through IMU distortion correction, corrected point cloud data is obtained, improving the basic accuracy of point cloud registration. Furthermore, a local sub-map composed of keyframes near the current frame is constructed, the degradation probability of the unit direction vector is calculated, and the eigenvalues ​​of the Hessian matrix are corrected based on the degradation probability, effectively suppressing invalid pose update directions. In addition, the pose estimation result of the corrected point cloud data relative to the local sub-map is determined based on the corrected Hessian matrix, making the result more accurate and avoiding pose estimation drift. Finally, the pose estimate obtained from ICP registration is used as a measurement value and fused with the IMU state prediction through Kalman filtering to update the state estimate and covariance matrix, obtaining the optimal pose estimate, improving the robustness and accuracy of pose estimation, and achieving continuous global map construction. In summary, this application effectively solves the problems caused by point cloud distortion and scene degradation in dynamic scenes by utilizing IMU to compensate for point cloud distortion, enhancing matching stability through degradation detection, and achieving complementary advantages through multi-sensor fusion, thus achieving high-precision and robust localization and mapping.

[0021] To facilitate better understanding by those skilled in the art Figure 1 The positioning and mapping method shown below will be further explained in its steps.

[0022] In step 101, motion distortion correction is performed on the point cloud data of the current frame acquired by the lidar to obtain the corrected point cloud data of the current frame. This can be achieved as follows: The laser point acquisition time t0 to t1 is calculated using IMU integration. i The relative motion transformation T(t) of time relative to the scan start time t0 i ;t0); According to the relative motion transformation T(t i ;t0) will change the original point cloud coordinates p i Convert to correction point: p′ i =T(t) i ;t0) -1 p i .

[0023] Specifically, during the scanning process, the LiDAR generates point cloud distortion due to platform movement. High-frequency measurements (typically 100-200Hz) using an IMU are used to estimate the sensor motion during the scan, and motion compensation is applied to each point. Let the LiDAR scanning period be [t0, t1], and the IMU collects k measurements within [t0, t1]. The acquisition time of the i-th laser point is t. i That is, for the i-th point in the scan, its acquisition time is t. i ∈[t0,t1]. The values ​​from t0 to t1 can be obtained through IMU integration. i The relative motion transformation matrix T(t) i ;t0):

[0024]

[0025] Where, ω j and v j These represent the IMU in the j-th time interval [t] j-1 ,t j The average angular velocity and linear velocity over the time interval, Exp(·) is the exponential mapping from the Lie algebra to the Lie group, T(t) i ;t0) represents the relative motion transformation matrix from time t0 to t1, used to transform t i The coordinates of the point cloud collected at time t0 are transformed to the coordinate system at time t0.

[0026] Using this matrix T(t) i ;t0) for the original point cloud p i Perform the transformation to obtain the corrected point cloud p′ i =T(t) i ;t0) -1 ·p i This means unifying the points collected at different times into the coordinate system at time (t_0) to eliminate motion distortion.

[0027] In this way, by accurately compensating for motion through IMU integration, a data foundation with correct geometric structure is provided for subsequent point cloud registration, avoiding positioning errors caused by distortion, and providing key input for subsequent Iterative Closest Point (ICP) registration and Kalman filtering.

[0028] For step 102, the local sub-map is composed of keyframe maps near the current frame. Keyframes are typically selected from frames with significant geometric features or large pose changes to avoid redundant data. Constructing local sub-maps of keyframes narrows the registration search range, improves computational efficiency (without needing to match all points on the global map), and provides local geometric constraints to adapt to changes in local scenes within dynamic environments. In related environmental mapping methods, ICP is typically used for point cloud registration to output prior matching results. This embodiment uses probabilistic degradation detection ICP, utilizing local sub-maps (composed of keyframe maps near the current frame) for alignment and outputting prior matching results.

[0029] Specifically, firstly, let a plane in a local submap be represented by a unit normal vector n. i (Perpendicular to the plane) and the distance d between the planes i (Distance from the origin to the plane) represents the corrected point cloud p′. i After transformation by rotation matrix R and translation vector t to the world coordinate system, we obtain p. i =Rp′ i +t, transforming point pi Substitute into the plane equation (e.g.) The difference between the point equation and the standard plane equation is the distance error from the point to the plane.

[0030]

[0031] This error represents the transformed point p′ i to plane The directed distance is positive when the point is in the positive direction of the plane, and negative otherwise.

[0032] The goal of the ICP algorithm is to minimize the total error. This is typically achieved by minimizing the weighted sum of squared distance errors across all points to find the optimal pose (R,t). Therefore, the error function is constructed as follows:

[0033]

[0034] Among them, w i The weighting factor is used to adjust the influence of different points. Points with more noise (such as distant points) are given smaller weights, while points with obvious features (such as edge points) are given larger weights, thereby improving the registration accuracy.

[0035] Further define the pose perturbation vector Where δr is the rotational perturbation in Lie algebra form, and δt is the translational perturbation. Since the rotational perturbation can be approximated as a vector under the small-angle assumption and satisfies the linearization condition, the problem can be transformed into a standard least-squares form:

[0036]

[0037] Among them, J i Let b be a Jacobian matrix. i This is the residual term.

[0038] In this embodiment, p′ is used. i ×n i Reflecting the rotational coupling relationship between point coordinates and the normal vector, the partial derivative with respect to the translational perturbation is directly set as the normal vector. The Jacobian matrix J i Defined as:

[0039]

[0040] The residual term is defined as follows:

[0041]

[0042] This least squares form can be solved quickly through matrix operations, such as the Gauss-Newton method or singular value decomposition (SVD), which transforms nonlinear optimization into a linear problem, significantly reducing the computational load and making it suitable for real-time robot localization and mapping.

[0043] In related techniques, the Jacobian matrix contributions of all point clouds are typically summed to form the Hessian matrix, which can be expressed as:

[0044]

[0045] Solving the linear equation further yields the pose perturbation x (i.e., the pose estimation result), which is used to update the current pose. The linear equation being solved can be expressed as:

[0046]

[0047] In practical applications, lidar measurements have errors (such as ranging noise), which can cause the point cloud coordinates p′ to be affected. i Inaccurate; furthermore, the plane fitting process introduces the normal vector n. i The estimation error, therefore the point cloud containing noise is The normal vector containing noise is in, This indicates that the corrected point cloud coordinates are affected by zero-mean Gaussian noise, and the covariance matrix... Describe the noise distribution.

[0048] In this embodiment, a contribution vector for each point cloud is constructed using point cloud data and normal vectors. It can be represented as:

[0049]

[0050] in, Point cloud coordinates An antisymmetric matrix used for cross product operations, where I is the identity matrix and w i This is a weighting factor that adjusts the importance of the measurement point. This measurement vector couples the point cloud coordinates, normal vector, and weights, essentially describing the sensitivity of the point-to-plane distance error to attitude perturbations. (Matrix block) This can be understood as the normal vector The transformation matrix mapped to the attitude perturbation space reflects how noise affects the Hessian matrix through the point cloud and normal vectors.

[0051] Further, the Hessian matrix is ​​determined based on this contribution vector:

[0052]

[0053] The contribution vector determines the Hessian matrix. The expectation is in, The covariance of each contribution vector can be specifically expressed as:

[0054]

[0055] Among them, [n i ] × Let n be the normal vector i An antisymmetric matrix.

[0056] When the component of ∑ dominates in a certain direction (i.e., the expectation of the Hessian matrix constructed from the contribution vectors (the expectation can be expressed as...) If the noise contribution in the eigenvalues ​​of a given direction is greater than the signal contribution, that direction is considered degenerate and its pose update needs to be suppressed. However, in geometrically degenerate environments (such as long corridors), the normal vector n... i Approximate parallelism (such as the corridor wall normal vectors being perpendicular to the corridor direction) leads to a significant increase in the covariance of ∑ in that direction, the effective signal of the Hessian matrix is ​​submerged by noise, and pose estimation is prone to divergence.

[0057] Therefore, this application provides a degradation probability determination model. Based on this model, the Hessian matrix is ​​detected and analyzed to identify and suppress the updating of degradation directions. Specifically, the degradation probability of the unit direction vector of the local sub-map is calculated, and the Hessian matrix of the iterative nearest point algorithm is corrected according to the degradation probability. This model can quantitatively analyze the noise impact under different scenarios, providing a theoretical basis for degradation detection, enabling the algorithm to adaptively adjust the pose update strategy and improve robustness in complex environments.

[0058] In some embodiments, the degradation probability of a unit direction vector is determined based on the signal-to-noise ratio (SNR) of the unit direction vector. Specifically, the signal quantity corresponding to a unit direction vector u is defined as: a = u T Hu; The signal quantity of the direction vector in this gear position is a quadratic form composed of the unit direction vector u and the Hessian matrix H, and its value is equal to the projected energy of H on the direction u.

[0059] Due to the influence of noise, the Hessian matrix is ​​perturbed by noise. The corresponding semaphore could be:

[0060]

[0061] In the formula, ξ represents the signal fluctuation caused by noise, which follows a normal distribution. Its expected value and variance are:

[0062]

[0063] In this embodiment, the signal-to-noise ratio of a unit direction vector is defined as... In the formula, a is the signal quantity of the unit direction vector, and ξ is the noise of the unit direction vector.

[0064] In practical applications, if a robot moves through a long corridor, the local sub-map is composed of the two side walls, and the wall normal vector n i Approximately perpendicular to the longitudinal direction of the corridor (denoted as the x-axis), in this scenario, the wall provides strong geometric constraints, the eigenvalue λ of the Hessian matrix is ​​relatively large in this direction, and the semaphore a = u T Hu is large, at which point the degradation probability p u ≈1 indicates non-degeneracy. The corridor lacks longitudinal characteristics; the eigenvalue of the Hessian matrix in this direction is λ≈0, and the semaphore a≈0. In this case, noise dominates, p u ≈0, indicating a direction of degradation.

[0065] Based on the above scenario, this embodiment establishes a degradation probability determination model from a probabilistic statistical perspective: A conservatism factor *s* is defined, requiring the effective signal *a* to be at least *s* times the noise *ξ*, i.e., the signal-to-noise ratio (SNR) ≥ the conservatism factor; in this case, the unit direction vector is considered to have degraded. The degradation probability *p* is... u Represented as:

[0066]

[0067] This probability p u This reflects whether the information in the direction u is sufficient.

[0068] Traditional methods use Singular Value Decomposition (SVD) to find the inverse of the Hessian matrix. If the eigenvalues ​​of the Hessian matrix are close to 0, they are directly set to 0, which may lead to the pose update completely ignoring the direction or introducing errors due to noise amplification. This embodiment further provides an eigenvalue correction method: determining the non-degenerate probability of the unit direction vector based on the degenerate probability; and correcting the eigenvalues ​​of the Hessian matrix based on the following formula: In the formula, λ is the non-degenerate probability of the unit direction vector. k These are the eigenvalues ​​of the Hessian matrix before correction.

[0069] After soft-correcting the eigenvalues ​​of the Hessian matrix based on this probability, the pose perturbation is solved using the corrected pseudo-inverse. The corrected pseudo-inverse is:

[0070]

[0071] in, The eigenvector matrix is ​​diagonalized, and its column vectors are... These are the eigenvectors of the corrected Hessian matrix; This is the corrected eigenvalue diagonal matrix. λ kThese are the eigenvalues ​​of the Hessian matrix before correction. For feature vectors Non-degenerate probability in the corresponding direction

[0072] Where P is a diagonal matrix of degradation probabilities. The non-degenerate probability in each direction is used as a weight and applied directly to the reciprocal of the eigenvalue; for non-degenerate directions (such as the direction of the wall normal vector), then... Normal update; for degradation directions (such as the longitudinal direction of a corridor), there is... Suppress updates.

[0073] in, The sum of the products of the Jacobian matrix and the residual terms of all point clouds corresponds to the right-hand vector b of the linear equation in the standard ICP, representing the driving direction of pose update.

[0074] The degradation probability determination model and correction method provided in this embodiment establish a quantitative standard for degradation detection from a probabilistic statistical perspective, transforming the geometric degradation problem into a statistical comparison of signal and noise, thus avoiding the empirical judgment based on fixed thresholds in traditional methods. The degradation probability determination model and correction method in this embodiment do not require manual pre-setting of degradation scenarios. The algorithm can dynamically adjust the pose update strategy based on the real-time calculated degradation probability, making it suitable for seamless switching from non-degraded to degraded scenarios. Furthermore, the semaphore calculation is based on matrix operations and can be executed in parallel with the ICP registration process, meeting real-time requirements.

[0075] For step 103, LiDAR has the characteristics of high accuracy but low frequency (10-20Hz), while IMU has the characteristics of high frequency (100-200Hz) but has accumulated error. Kalman filtering combines the advantages of both through a state-space model to achieve complementarity. By fusing ICP registration results with IMU predictions, it outputs the optimal pose estimate and uses it for global map updating.

[0076] Specifically, the corrected disturbance x * (i.e., the registration result of ICP), combined with IMU prediction, and then fused using Kalman filtering to obtain the optimal estimate.

[0077] The state prediction and update formulas are as follows:

[0078] State prediction:

[0079] x k|k-1 =F k-1 x k-1|k-1 +B k-1 u k-1 +w k-1

[0080]

[0081] Among them, F k-1 Let B be the state transition matrix. k-1 For the input control matrix, w k-1 For process noise, Q k-1 Let be the process covariance matrix.

[0082] Measurement Update:

[0083]

[0084] x k|k =x k|k-1 +K k (z k -H k x k|k-1 )

[0085] P k|k =(IK k H k )P k|k-1

[0086] Among them, the registration result z output by the input ICP module k Measurement matrix H k Measurement noise covariance matrix R k and Kalman gain K k .

[0087] Among them, the output is the most accurate pose estimate x. k|k Used for global map updates, covariance P k|k This reflects the reliability of the current estimate.

[0088] Through the above filtering, the high-frequency state prediction of the IMU and the prior matching information obtained by ICP can be fully integrated to obtain a more accurate and robust pose estimation. At the same time, the corrected point cloud is updated to the global map to achieve continuous map construction.

[0089] In this way, by reducing point cloud motion errors through IMU distortion correction, corrected point cloud data is obtained, improving the basic accuracy of point cloud registration. Furthermore, a local sub-map composed of keyframes near the current frame is constructed, the degradation probability of the unit direction vector is calculated, and the eigenvalues ​​of the Hessian matrix are corrected based on the degradation probability, effectively suppressing invalid pose update directions. In addition, the pose estimation result of the corrected point cloud data relative to the local sub-map is determined based on the corrected Hessian matrix, making the result more accurate and avoiding pose estimation drift. Finally, the pose estimate obtained from ICP registration is used as a measurement value and fused with the IMU state prediction through Kalman filtering to update the state estimate and covariance matrix, obtaining the optimal pose estimate, improving the robustness and accuracy of pose estimation, and achieving continuous global map construction. In summary, this application effectively solves the problems caused by point cloud distortion and scene degradation in dynamic scenes by utilizing IMU to compensate for point cloud distortion, enhancing matching stability through degradation detection, and achieving complementary advantages through multi-sensor fusion, thus achieving high-precision and robust localization and mapping.

[0090] like Figure 2 As shown, in conjunction with the steps in the above embodiments, another embodiment of this application also provides an overall flow of a localization and mapping method: the IMU module collects high-frequency attitude data and calculates the pose of each point at the time of collection through interpolation; the lidar collects the original point cloud, and outputs a motion-corrected point cloud after IMU-assisted distortion removal; the point cloud registration module uses probabilistic degradation detection (ICP) and uses a local sub-map (composed of key frame maps near the current frame) for alignment, and outputs a priori matching results; the data fusion module uses Kalman filtering to fuse the priori matching results with the IMU state prediction to obtain the optimal pose estimation; the pose estimation results are used to update the global map and also serve as the prior input for the next frame.

[0091] Specifically, in this embodiment, sensor motion is estimated using high-frequency IMU measurement data to compensate for the motion of the laser point cloud and eliminate distortion during the scanning process. The corrected point cloud is registered with a local sub-map, and a probabilistic degradation detection method is used to analyze the Hessian matrix to identify and suppress updates of degradation directions. The optimal pose estimate is obtained by fusing IMU predictions and LiDAR matching results using a Kalman filter. The corrected point cloud is then inserted into the global map based on the optimal pose, and keyframes are managed to construct the local sub-map. The localization and mapping method implemented through the above process exhibits higher robustness and localization accuracy in geometrically degraded environments, making it suitable for fields such as autonomous driving and mobile robotics.

[0092] Another embodiment of the present invention relates to an electronic device, such as... Figure 3 As shown, it includes at least one processor; and,

[0093] A memory communicatively connected to at least one processor; wherein the memory stores instructions executable by at least one processor, the instructions being executed by at least one processor to enable at least one processor to perform the localization mapping method as described above.

[0094] The memory and processor are connected via a bus, which can include any number of interconnecting buses and bridges, connecting various circuits of one or more processors and memories. The bus can also connect various other circuits, such as peripheral devices, voltage regulators, and power management circuits, which are well known in the art and will not be described further herein. The bus interface provides an interface between the bus and the transceiver. The transceiver can be a single element or multiple elements, such as multiple receivers and transmitters, providing a unit for communicating with various other devices over a transmission medium. Data processed by the processor is transmitted over the wireless medium via an antenna, which further receives data and transmits it to the processor.

[0095] The processor manages the bus and general processing, and also provides various functions, including timing, peripheral interfaces, voltage regulation, power management, and other control functions. Memory is used to store data used by the processor during operation.

[0096] Another embodiment of the present invention relates to a computer-readable storage medium storing a computer program. When the computer program is executed by a processor, it implements the method embodiments described above.

[0097] That is, those skilled in the art will understand that all or part of the steps in the methods of the above embodiments can be implemented by a program instructing related hardware. This program is stored in a storage medium and includes several instructions to cause a device (which may be a microcontroller, chip, etc.) or processor to execute all or part of the steps of the methods of the various embodiments of this application. The aforementioned storage medium includes various media capable of storing program code, such as a USB flash drive, a portable hard drive, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk.

[0098] Those skilled in the art will understand that the above embodiments are specific examples of implementing the present invention, and in practical applications, various changes in form and detail may be made without departing from the spirit and scope of the present invention.

Claims

1. A method for positioning and mapping, characterized in that, The method includes: Motion distortion correction is performed on the point cloud data of the current frame acquired by the lidar to obtain the corrected point cloud data of the current frame; Construct a local sub-map of the current frame, calculate the degradation probability of the unit direction vector of the local sub-map, correct the Hessian matrix of the iterative nearest point algorithm according to the degradation probability, and determine the pose estimation result of the corrected point cloud data relative to the local sub-map according to the corrected Hessian matrix. The optimal pose estimate is calculated by fusing the pose estimation result with the IMU prediction value through a Kalman filter, and the corrected point cloud data is inserted into the global map based on the optimal pose estimate. The calculation of the degradation probability of the unit direction vector of the local sub-map includes: The degradation probability of the unit direction vector is determined based on the signal-to-noise ratio (SNR) of the unit direction vector, wherein the SNR of the unit direction vector is defined as... In the formula, The signal quantity of the unit direction vector. The noise of the unit direction vector; if the signal-to-noise ratio is greater than or equal to the conservation factor, then the unit direction vector is considered to be degraded, where the degradation probability represents... In the formula, s is the conservation factor.

2. The positioning and mapping method according to claim 1, characterized in that, The correction of the Hessian matrix of the iterative nearest point algorithm based on the degradation probability includes: The non-degenerate probability of the unit direction vector is determined based on the degradation probability; The eigenvalues ​​of the Hessian matrix are corrected based on the following formula: ; In the formula, Let be the non-degenerate probability of the unit direction vector. These are the eigenvalues ​​of the Hessian matrix before correction.

3. The positioning and mapping method according to claim 1, characterized in that, Before correcting the Hessian matrix of the iterative nearest-point algorithm according to the degradation probability, the method further includes: Determine the normal vector corresponding to the corrected point cloud data in the local sub-map; The Hessian matrix is ​​constructed based on the corrected point cloud data and the normal vector.

4. The positioning and mapping method according to claim 3, characterized in that, The construction of the Hessian matrix based on the corrected point cloud data and the normal vector includes: Based on the corrected point cloud data and the normal vector, the contribution vector corresponding to the corrected point cloud data is determined as follows: ; In the formula, Let be the contribution vector of the i-th corrected point cloud data to the Hessian matrix. The weighting coefficients are those corresponding to the i-th corrected point cloud data. Let be the antisymmetric matrix of the i-th corrected point cloud data. It is the identity matrix. This is the normal vector corresponding to the i-th corrected point cloud data; Determine the Hessian matrix based on the contribution vector: .

5. The positioning and mapping method according to claim 1, characterized in that, The step of performing motion distortion correction on the point cloud data of the current frame acquired by the lidar to obtain the corrected point cloud data of the current frame includes: Laser point acquisition is calculated using IMU integration. Time's up Time relative to the start of the scan Relative motion transformation at time t. ; According to the relative motion transformation Original point cloud coordinates Convert to calibration point: .

6. The positioning and mapping method according to claim 1, characterized in that, The optimal pose estimate is calculated by fusing the pose estimation results with the IMU predictions using a Kalman filter. Based on this optimal pose estimate, the corrected point cloud data is then inserted into the global map, including: The pose estimation result is used as measurement information, combined with the IMU prediction value, and then fused using Kalman filtering to obtain the optimal estimate. The target point cloud is constructed based on the optimal estimate, and the target point cloud is updated to the global map.

7. An electronic device, characterized in that, include: At least one processor; as well as, A memory communicatively connected to the at least one processor; wherein, The memory stores instructions that can be executed by the at least one processor to enable the at least one processor to perform the localization and mapping method as described in any one of claims 1 to 6.

8. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, it implements the localization and mapping method according to any one of claims 1 to 6.

Images