Three-dimensional point cloud registration method, mobile device and storage medium

Abstract

The present invention provides a three-dimensional point cloud registration method, a mobile device and a storage medium, comprising: obtaining a posture change according to inertial measurement data and speed data of a mobile device, and obtaining a source feature point cloud and a target feature point cloud respectively according to a source point cloud and a target point cloud; iteratively optimizing the source feature point cloud and the target feature point cloud through a nearest neighbor distance observation function to obtain a first observation matrix; determining the number of degraded enhancements through the first observation matrix; determining a posture change observation function according to the enhancement number and the posture change; obtaining an optimization matrix equation according to the nearest neighbor distance observation function and the posture change observation function; solving the optimization matrix equation to obtain a point cloud registration result. Compared with the prior art, the present invention solves the problem of point cloud registration failure under point cloud degradation through the nearest neighbor distance observation function and the posture change observation function.

Description

Technical Field

[0001] The present invention relates to the field of sensor data processing technology, and in particular to a three-dimensional point cloud registration method, a mobile device and a storage medium. Background Art

[0002] LiDAR has the advantages of strong anti-interference ability, rich perception information, and high measurement accuracy. Therefore, it is widely used in obstacle detection, high-precision positioning, and map construction for autonomous robots and self-driving cars. The data measured by LiDAR is represented by a three-dimensional point cloud composed of a large number of points, each of which contains spatial position and reflection intensity information. Point clouds can represent the three-dimensional world; but since each LiDAR can only scan and obtain data within a limited field of view, in order to obtain a complete three-dimensional scene, it is necessary to combine point clouds from multiple different perspectives.

[0003] Point cloud registration is the problem of calculating the transformation matrix between two frames of point cloud. Point cloud registration is mainly used for: 3D reconstruction, generating a complete 3D scene, such as high-precision 3D map reconstruction in autonomous driving and 3D environment reconstruction in robotics; 3D positioning, such as an autonomous vehicle estimating its position on the map and the distance to the edge of the road; pose estimation, aligning a point cloud A with another point cloud B, can generate the pose information of point cloud A relative to point cloud B, where point cloud A is a 3D real-time view and point cloud B represents the 3D environment.

[0004] However, in environments such as tunnels and wide squares, point clouds are prone to "degradation" due to unclear environmental features, that is, information loss. When point clouds are degraded, point cloud registration will become uncertain and less accurate in some dimensions, causing pose calculation failures and ultimately positioning failures.

[0005] Patent document CN111340862A discloses a point cloud registration method, device and storage medium based on multi-feature fusion, the method comprising: extracting a number of source point cloud feature points and a number of target point cloud feature points from the source point cloud and the target point cloud respectively; extracting the local depth feature, normal angle feature, point cloud density feature and local color feature of each feature point, and then generating a feature descriptor corresponding to each feature point according to the local depth feature, normal angle feature, point cloud density feature and local color feature of each feature point; wherein the feature points include source point cloud feature points and target point cloud feature points; according to the feature descriptors, the source point cloud feature points are paired with the target point cloud feature points to generate feature point pairs; according to the feature point pairs, a transformation matrix is ​​generated, and the source point cloud is transformed according to the transformation matrix to generate a second source point cloud, and then the second source point cloud and the target point cloud are precisely registered. However, this method does not solve the problem of point cloud registration failure under point cloud degradation. Summary of the invention

[0006] In view of the defects in the prior art, an object of the present invention is to provide a three-dimensional point cloud registration method, a mobile device and a storage medium.

[0007] In a first aspect, the present invention provides a three-dimensional point cloud registration method, comprising the following steps:

[0008] Step 1: Obtain the pose change according to the inertial measurement data and speed data of the mobile device, and obtain the source feature point cloud and the target feature point cloud according to the source point cloud and the target point cloud respectively;

[0009] Step 2: Iteratively optimize the source feature point cloud and the target feature point cloud through the nearest neighbor distance observation function to obtain a first observation matrix;

[0010] Step 3: Determine the number of enhancements of the degenerate posture dimension through the first measurement matrix;

[0011] Step 4: Determine a posture change observation function according to the reinforcement quantity and the posture change;

[0012] Step 5: Obtaining an optimization matrix equation according to the nearest neighbor distance observation function and the posture change observation function;

[0013] Step 6: Solve the optimization matrix equation to obtain the point cloud registration result.

[0014] Preferably, step 1 comprises:

[0015] Step 101: using a combined navigation algorithm based on inertial measurement data and velocity data of the mobile device to obtain a position change;

[0016] Step 102: extract features from the source point cloud and the target point cloud to obtain a source feature point cloud and a target feature point cloud, respectively.

[0017] Preferably, step 3 comprises:

[0018] Step 301: Obtain a degradation factor according to the first measurement matrix;

[0019] Step 302: Determine the degenerate posture dimension according to the degradation factor;

[0020] Step 303: Determine the number of enhancements for the degenerate posture dimension.

[0021] Preferably, step 5 comprises:

[0022] Step 501: determining a posture optimization function according to the nearest neighbor distance observation function and the posture change observation function;

[0023] Step 502: Iteratively optimize the posture optimization function to obtain an optimization matrix equation.

[0024] Preferably, the nearest neighbor distance observation function is expressed as:

[0025]

[0026] Where n represents the number of points selected for iterative optimization; a i 、b i 、c i d i 、e i 、f i , g i represents constants related to the source point cloud and the target point cloud, i = 1, 2, ..., n; δx, δy, δz, δφ, δθ, δψ are parameters to be optimized, representing the first pose dimension, the second pose dimension, the third pose dimension, the fourth pose dimension, the fifth pose dimension and the sixth pose dimension, respectively.

[0027] Preferably, the posture change observation function is expressed as:

[0028]

[0029] Among them, dx, dy, dz, dφ, dθ, and dψ represent the pose changes of the first, second, third, fourth, fifth, and sixth pose dimensions, respectively. N 1 、N 2 、N 3 、N 4 、N 5 、N 6 are all positive integers, representing the number of reinforcements in the first, second, third, fourth, fifth and sixth posture dimensions respectively.

[0030] Preferably, the posture optimization function is expressed as:

[0031]

[0032] In a second aspect, the present invention provides a mobile device, comprising:

[0033] A laser radar module, used to scan the environment around the mobile device and generate a source point cloud;

[0034] A high-definition map module, used to store a high-definition map of the mobile device's activity area and provide a target point cloud;

[0035] an inertial measurement unit, configured to provide inertial measurement data of the mobile device;

[0036] A speed sensor, configured to provide speed data of the mobile device;

[0037] The controller module includes a data storage device, a program storage device and a processor, wherein the data storage device stores the source point cloud, the target point cloud, the inertial measurement data and the speed data, the program storage device stores a computer program, and the processor implements the three-dimensional point cloud registration method when executing the computer program.

[0038] In a third aspect, the present invention provides a storage medium storing a computer program, which implements the three-dimensional point cloud registration method when executed.

[0039] Compared with the prior art, the present invention has the following beneficial effects:

[0040] 1. The three-dimensional point cloud registration method provided by the present invention has strong robustness. In scenes such as tunnels, squares, and elevated roads, laser ranging will not be completely or partially unavailable due to the degradation of point clouds. Laser ranging observation can continue to be used for point cloud registration, giving full play to the high-precision characteristics of laser point clouds.

[0041] 2. The present invention can identify the degree of degradation in a non-degraded environment without affecting the laser alignment accuracy in an ideal environment.

[0042] 3. The calculation of the degree of point cloud degradation in the present invention is automatic and does not require manual assignment of environmental parameters, which brings a greater degree of feasibility to the automation of positioning and mapping. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] Other features, objects and advantages of the present invention will become more apparent from the detailed description of non-limiting embodiments made with reference to the following drawings:

[0044] Figure 1 A schematic diagram of the process of the three-dimensional point cloud registration method provided by the present invention;

[0045] Figure 2 This is a flow chart for calculating the enhancement quantity in the three-dimensional point cloud registration method provided by the present invention. DETAILED DESCRIPTION

[0046] The present invention is described in detail below in conjunction with specific embodiments. The following embodiments will help those skilled in the art to further understand the present invention, but are not intended to limit the present invention in any form. It should be noted that, for those of ordinary skill in the art, several changes and improvements can also be made without departing from the concept of the present invention. These all belong to the protection scope of the present invention.

[0047] Figure 1A schematic diagram of the process flow of the three-dimensional point cloud registration method provided by the present invention is shown in FIG. Figure 1 As shown, the present invention provides a three-dimensional point cloud registration method, comprising:

[0048] Step 1: According to the inertial measurement data and speed data of the mobile device, the pose change is obtained, and according to the source point cloud and the target point cloud, the source feature point cloud and the target feature point cloud are obtained respectively.

[0049] Preferably, it includes: using a laser radar to scan the environment around the mobile device and generate a source point cloud; using a high-definition map unit to store a high-definition map of the activity area of ​​the mobile device and provide a target point cloud; using an inertial sensor (Inertial Measurement Unit, IMU) to provide inertial measurement data of the mobile device; using a speed sensor to provide speed data of the mobile device.

[0050] Specifically, it also includes: a controller, including a data storage device, a program storage device and a processor, the data storage device stores source point cloud, target point cloud, inertial measurement data and speed data, the program storage device stores a computer program, and the processor implements the three-dimensional point cloud registration method when executing the computer program.

[0051] Preferably, it includes: step 101: using a combined navigation algorithm to obtain a posture change according to inertial measurement data and speed data of a mobile device; step 102: extracting features from a source point cloud and a target point cloud to obtain a source feature point cloud and a target feature point cloud, respectively.

[0052] It should be noted that the source point cloud and the target point cloud can be used directly without feature extraction. The present invention does not limit the feature extraction of the source point cloud and the target point cloud. If the source point cloud and the target point cloud can be directly input into the nearest neighbor distance observation function for processing, there is no need to perform feature extraction on the source point cloud and the target point cloud.

[0053] Step 2: Iteratively optimize the source feature point cloud and the target feature point cloud through the nearest neighbor distance observation function to obtain a first observation matrix.

[0054] Specifically, the source feature point cloud and the target feature point cloud are fed into a nearest neighbor distance observation function, and the nearest neighbor distance observation function is iteratively optimized to obtain a first observation matrix.

[0055] The nearest neighbor distance observation function in the present invention can be expressed by formula (1), wherein n represents the number of points selected for iterative optimization; a i 、b i 、c i d i 、e i 、f i , gi represents constants related to the source point cloud and the target point cloud, where i = 1, 2, …, n; δx, δy, δz, δφ, δθ, δψ are parameters to be optimized, representing the first pose dimension, the second pose dimension, the third pose dimension, the fourth pose dimension, the fifth pose dimension, and the sixth pose dimension, respectively.

[0056] Furthermore, the optimal result of iterative optimization of the nearest neighbor distance observation function is formula (2).

[0057] Formula (2) can be written in matrix form as Ax=g, where A is the first observation matrix, which can be expressed by formula (3): x=[δx δy δz δφ δθ δψ] T , g=[g 1 g 2 … g n ] T .

[0058] Step 3: Determine the number of enhancements of the degenerate pose dimension through the first observation matrix.

[0059] Preferably, it includes: step 301: obtaining a degradation factor according to the first observation matrix; step 302: determining a degraded posture dimension according to the degradation factor; step 303: determining a reinforcement quantity for the degraded posture dimension.

[0060] Exemplarily, the present invention calculates a degradation factor according to the first observation matrix A, and then determines the degraded pose dimension where degradation occurs according to the degradation factor and calculates the enhancement quantity corresponding to the degraded pose dimension.

[0061] Specifically, Figure 2 The calculation flow chart of the enhancement quantity in the three-dimensional point cloud registration method provided by the present invention is as follows: Figure 2 As shown, first, calculate the matrix A T The eigenvalue λ of A i and the eigenvector v i , where i = 1, 2, ..., 6; then, construct the matrix M = [v 1 v 2 v 3 v 4 v 5 v 6 ], and the matrix N = M; then, for i = 1, 2, ..., 6, determine each eigenvalue λ in turn i Is it less than the first threshold λ th , if all eigenvalues ​​λ i Both are greater than λ th , then it is determined that all posture dimensions are not degraded, and the number of enhancements N corresponding to the posture dimension jare all 0, and the calculation ends, where j = 1, 2, ..., 6; if λ i ≤λ th , then set the i-th column vector in the matrix N to 0 and construct the column vector v a =[1 1 1 1 1 1] T , and then calculate the degradation factor k d =M -1 Nv a ; Further, for j = 1, 2, ..., 6, the degradation factor k is determined in turn d Each element k of dj Is it less than the second threshold k th , if the jth element k dj <k th , then the jth pose dimension is determined to be a degenerate pose dimension, and the number of enhancements corresponding to the degenerate pose dimension is N j =round(mλ th / λ i ), where m is the enhancement factor and round is the rounding function. dj ≥k th , it is determined that the j-th posture dimension has not degenerated, and the corresponding enhancement amount is 0.

[0062] Step 4: Determine a posture change observation function based on the reinforcement quantity and the posture change.

[0063] Specifically, according to the result of step 3, the construction of the posture change observation function can be expressed by formula (4), where dx, dy, dz, dφ, dθ, dψ represent the posture changes of the first posture dimension, the second posture dimension, the third posture dimension, the fourth posture dimension, the fifth posture dimension, and the sixth posture dimension, respectively; N 1 、N 2 、N 3 、N 4 、N 5 、N 6 are all positive integers, representing the number of reinforcements in the first, second, third, fourth, fifth and sixth posture dimensions respectively.

[0064] Step 5: Obtain an optimization matrix equation based on the nearest neighbor distance observation function and the posture change observation function.

[0065] Preferably, it includes: step 501: determining a posture optimization function according to the nearest neighbor distance observation function and the posture change observation function; step 502: iteratively optimizing the posture optimization function to obtain an optimization matrix equation.

[0066] Specifically, the posture optimization function is composed of the nearest neighbor distance observation function and the posture change observation function. The construction of the posture optimization function can be expressed by formula (5).

[0067] Furthermore, the posture optimization function is iteratively optimized, and the optimal result is shown in formula (6). Formula (6) is written in matrix form to obtain the optimization matrix equation: Bx = h.

[0068] Among them, the matrix B represents the second observation matrix, which can be expressed by formula (7); h can be expressed by formula (8).

[0069] Step 6: Solve the optimization matrix equation to obtain the point cloud registration result.

[0070] Specifically, the optimization matrix equation Bx=h is solved to obtain the point cloud registration result.

[0071] The present invention provides a mobile device, comprising: a laser radar module, which is used to scan the environment around the mobile device and generate a source point cloud; a high-definition map module, which is used to store a high-definition map of the activity area of ​​the mobile device and provide a target point cloud; an inertial measurement unit, which is used to provide inertial measurement data of the mobile device; a speed sensor, which is used to provide speed data of the mobile device; and a controller module, which comprises a data storage device, a program storage device and a processor, wherein the data storage device stores the source point cloud, the target point cloud, the inertial measurement data and the speed data, the program storage device stores a computer program, and the processor implements the three-dimensional point cloud registration method when executing the computer program.

[0072] The present invention provides a storage medium, which stores a computer program. When the computer program is executed, the three-dimensional point cloud registration method is implemented.

[0073] Compared with the prior art, the present invention has the following beneficial effects:

[0074] 1. The three-dimensional point cloud registration method of the present invention has strong robustness. In scenes such as tunnels, squares, and elevated roads, laser ranging will not be completely or partially unavailable due to the degradation of point clouds. Laser ranging observation can continue to be used for point cloud registration, giving full play to the high-precision characteristics of laser point clouds.

[0075] 2. The present invention can identify the degree of degradation in a non-degraded environment without affecting the laser alignment accuracy in an ideal environment.

[0076] 3. The calculation of the degree of point cloud degradation of the present invention is automatic and does not require manual assignment of environmental parameters, which brings a greater degree of feasibility to the automation of positioning and mapping.

[0077] Those skilled in the art know that, in addition to implementing the system, device and its various modules provided by the present invention in a purely computer-readable program code, it is entirely possible to implement the same program in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers and embedded microcontrollers by logically programming the method submodule M. Therefore, the system, device and its various modules provided by the present invention can be considered as a hardware component, and the modules included therein for implementing various programs can also be considered as structures within the hardware component; the modules for implementing various functions can also be considered as both software programs for implementing methods and structures within hardware components.

[0078] The above describes the specific embodiments of the present invention. It should be understood that the present invention is not limited to the above specific embodiments, and those skilled in the art can make various changes or modifications within the scope of the claims, which does not affect the essence of the present invention. In the absence of conflict, the embodiments of the present application and the features in the embodiments can be combined with each other arbitrarily.

[0079]

[0080]

[0081]

[0082]

[0083]

[0084]

[0085]

[0086]

[0087]

Claims

1. A 3D point cloud registration method, It is characterized in that include: Step 1: According to the inertial measurement data and speed data of the mobile device, the pose change is obtained, and according to the source point cloud and the target point cloud, the source feature point cloud and the target feature point cloud are obtained respectively; Step 2: Iteratively optimize the source feature point cloud and the target feature point cloud through the nearest neighbor distance observation function to obtain a first observation matrix; Step 3: Determine the number of enhancements of the degenerate posture dimension through the first measurement matrix; Step 4: Determine a posture change observation function according to the reinforcement quantity and the posture change; Step 5: Obtaining an optimization matrix equation according to the nearest neighbor distance observation function and the posture change observation function; Step 6: Solve the optimization matrix equation to obtain the point cloud registration result.

2. The three-dimensional point cloud registration method according to claim 1, It is characterized in that The step 1 comprises: Step 101: obtaining the position change using a combined navigation algorithm according to the inertial measurement data and the speed data of the mobile device; Step 102: extracting features from the source point cloud and the target point cloud to obtain the source feature point cloud and the target feature point cloud respectively.

3. The three-dimensional point cloud registration method according to claim 1, It is characterized in that The step 3 comprises: Step 301: Obtain a degradation factor according to the first measurement matrix; Step 302: Determine the degenerate posture dimension according to the degradation factor; Step 303: Determine the enhancement quantity for the degenerate posture dimension.

4. The three-dimensional point cloud registration method according to claim 1, It is characterized in that The step 5 comprises: Step 501: determining a posture optimization function according to the nearest neighbor distance observation function and the posture change observation function; Step 502: Iteratively optimize the posture optimization function to obtain an optimization matrix equation.

5. The three-dimensional point cloud registration method according to claim 4, It is characterized in that The nearest neighbor distance observation function is expressed as: Where n represents the number of points selected for iterative optimization; a i 、b i 、c i d i 、e i 、f i , g i represents constants related to the source point cloud and the target point cloud, i = 1, 2, ..., n; δx, δy, δz, δɸ, δθ, δψ are parameters to be optimized, representing the first pose dimension, the second pose dimension, the third pose dimension, the fourth pose dimension, the fifth pose dimension and the sixth pose dimension, respectively.

6. The three-dimensional point cloud registration method according to claim 5, It is characterized in that The posture change observation function is expressed as: Among them, dx, dy, dz, dɸ, dθ, and dψ represent the posture changes of the first posture dimension, the second posture dimension, the third posture dimension, the fourth posture dimension, the fifth posture dimension, and the sixth posture dimension, respectively; N 1 、N 2 、N 3 、N 4 、N 5 、N 6 They are all positive integers, representing the number of reinforcements in the first, second, third, fourth, fifth and sixth posture dimensions respectively.

7. The three-dimensional point cloud registration method according to claim 6, It is characterized in that The pose optimization function is expressed as:

8. A mobile device, It is characterized in that The mobile device comprises: A laser radar module, used to scan the environment around the mobile device and generate a source point cloud; A high-definition map module, used to store a high-definition map of the mobile device's activity area and provide a target point cloud; an inertial measurement unit, configured to provide inertial measurement data of the mobile device; A speed sensor, configured to provide speed data of the mobile device; The controller module includes a data storage device, a program storage device and a processor, wherein the data storage device stores the source point cloud, the target point cloud, the inertial measurement data and the speed data, the program storage device stores a computer program, and the processor implements the three-dimensional point cloud registration method as described in any one of claims 1 to 7 when executing the computer program.

9. A storage medium, It is characterized in that The storage medium stores a computer program, and when the computer program is executed, the three-dimensional point cloud registration method as described in any one of claims 1 to 7 is implemented.

Images