High-precision low-drift large-range three-dimensional point cloud map construction and repositioning method

Abstract

The application provides a high-precision low-drift wide-range three-dimensional point cloud map construction and repositioning method, in which point cloud information and IMU information collected based on a three-dimensional laser radar are used for inter-frame point cloud registration to obtain the relative pose between key frames; considering the robot GPS information constraint, the closed loop detection constraint and the laser radar odometry constraint, the relative pose between the key frames is obtained and optimized to complete the three-dimensional point cloud map construction of the robot; meanwhile, the key frame descriptor is constructed by considering the key frame point cloud feature information, so that the inter-frame similarity is judged and the repositioning of the robot in the three-dimensional map is completed; and the cumulative error caused by the inter-frame registration of the point cloud is fully considered when the three-dimensional point cloud map construction proposition is constructed, the error caused by the non-linear optimization of the laser radar odometry is constrained by the GPS information and the closed loop detection constraint, and the globally consistent map is constructed, so that the quality of the three-dimensional point cloud map construction of the mobile robot can be greatly improved.

Description

Technical Field

[0001] This invention belongs to the field of mobile robot map construction, specifically involving a method for constructing and relocating a high-precision, low-drift, large-area three-dimensional point cloud map. Background Technology

[0002] Map building technology is a prerequisite for autonomous navigation in mobile robots. Constructing an environmental map provides prior information for localization and path planning, and is one of the core technologies for autonomous navigation. However, current 3D point cloud map building techniques often employ LiDAR or cameras for keyframe point cloud registration. Point cloud information is acquired by the LiDAR on the mobile robot, and then the keyframe point clouds are matched to obtain the relative poses between keyframes. These poses are then continuously superimposed to build the map. While existing methods can construct a 3D point cloud map, they cannot effectively handle the accumulated errors during 3D point cloud map construction under large-scale, long-distance outdoor conditions, leading to map drift. Furthermore, the robot's ability to relocalize within the 3D map plays a crucial role in supporting the robustness of autonomous navigation within the map. Therefore, designing a high-precision, low-drift, large-scale 3D point cloud map construction and relocalization method and device is essential for the realization of autonomous navigation in mobile robots. Summary of the Invention

[0003] To address the aforementioned technical problems, this invention proposes a method for constructing and relocating high-precision, low-drift, large-area 3D point cloud maps, comprising:

[0004] Step 1: Perform inter-frame point cloud registration based on the point cloud information acquired by the 3D lidar and the inertial measurement unit (IMU) information to obtain the relative pose between key frames;

[0005] Step 2: By constraining the robot's GPS information and closed-loop detection information, the relative poses between keyframe pose nodes are optimized, and the robot's 3D point cloud map is constructed.

[0006] Step 3: Based on the descriptor designed using the LiDAR point cloud information, perform a similarity query between the current frame and historical frames to complete the robot's relocalization on the 3D point cloud map.

[0007] Specifically, step 1 includes:

[0008] Step 1.1: Obtain motion information using the inertial measurement unit (IMU) information, and perform distortion correction on the point cloud information acquired by the lidar; wherein, the motion information includes angular velocity and linear velocity, calculate the time difference of the laser point relative to the initial acquisition time based on the coordinates of the laser point scanned by the lidar, then calculate the pose transformation matrix, and transform the laser point according to the transformation matrix;

[0009] Step 1.2: Match the keyframe point clouds to obtain the relative pose. Specifically, this includes: voxelizing the keyframe point clouds, calculating the mean and covariance of each voxel, constructing a Gaussian distribution, transforming the current frame point cloud into the map coordinate system based on the GPS-initialized pose, and calculating the joint probability of all points. When the joint probability of all points is maximized, the two frame point clouds can be considered successfully matched. At this time, the relative pose between the two frame point clouds is output.

[0010] Specifically, step 2 includes map optimization by constructing a pose graph, which consists of nodes and edges. Nodes represent keyframe poses, and edges represent relative pose constraints between two pose nodes. The pose constraints include lidar odometry constraints, loop closure detection constraints, and GPS position observation constraints. During map optimization, all observations and state variables are optimized together to construct a residual function. Each residual is assigned a weight to form an information matrix, and nonlinear optimization is performed. Odometry errors are corrected, and trajectory shapes are adjusted to obtain a globally consistent map.

[0011] Specifically, step 3 includes:

[0012] Step 3.1, Point cloud segmentation and descriptor generation: Divide a frame of point cloud scanned by 3D LiDAR into independent point clouds. After the point cloud is segmented, use the point cloud in each segmentation unit to assign a real value to each segmentation unit, and generate the segmented point cloud into a matrix of size Nr*Ns.

[0013] Step 3.2: Perform similarity matching based on the current frame point cloud information descriptor scanned by the robot in the 3D map and the descriptor information stored during map construction, output pose information, and complete relocalization.

[0014] Specifically, step 3.1 includes: (1) cutting the point cloud:

[0015] Divide the point cloud space into N parts along the direction in which the radius of the point cloud in a frame increases. r There are 3 circular rings, and the radial gap between each ring is:

[0016]

[0017] Among them, L max This represents the maximum scanning distance of the lidar.

[0018] Rotate the lidar scanning direction around the circle to divide each ring into N. s Divide into equal parts, with the central angle of each sector being . A segmented point cloud P can be represented as:

[0019]

[0020] Among them, P ij The set of points representing the dividing units of the i-th annulus and the j-th sector;

[0021] (2) Generate descriptors: Assign a real value to each segmentation unit using the point cloud in each segmentation unit. Represents the real number field; generates the segmented point cloud into an Nr*Ns matrix;

[0022]

[0023] Each row of the matrix represents a ring, each column represents a sector, and each element represents the maximum height of all 3D points in each segmentation unit; the encoding function of the segmentation unit is:

[0024]

[0025] Where z(·) represents the coordinate value of the return point P on the z-axis;

[0026] Through the above process, we finally obtain a representation of N. r *N s The characteristic descriptor I of the matrix is ​​expressed as:

[0027]

[0028] Specifically, step 3.2 includes: (1) Two-frame descriptor similarity measurement: Given a pair of feature descriptors, it is necessary to construct a distance function between the two frame feature descriptors to describe the similarity between the two frames. The distance function is as follows:

[0029]

[0030] Among them, I q and I c These are feature descriptors for the current frame and historical frames, respectively. and I respectively q and I c The distance between two column vectors at the same index is calculated using cosine distance for the vector in the j-th column of the descriptor. The more similar all corresponding column vectors are between two frame descriptors, the higher the similarity between the point clouds of the two frames. When the distance function is less than a certain threshold, the two frames are considered similar.

[0031] Calculate the distances to all possible column shift feature descriptors and find the minimum distance;

[0032]

[0033]

[0034] in, Represents the initial feature description submatrix I c The feature descriptor matrix after n-column translation, D(I q ,I c ) represents the minimum distance between two frames, and n* represents the translation amount of the column corresponding to the minimum distance between the two frames;

[0035] (2) Two-layer search matching: After obtaining the feature descriptor matrix between each frame, the similarity between the two frames is calculated by the similarity of the corresponding columns of the matrix; the rotation-invariant descriptors extracted from the feature descriptors are used to encode each row r of the feature descriptor matrix into a single real value through the ring coding function ψ(·); the number of points falling on the rings of equal radius between similar frames should be similar, therefore, an N ring is generated for each frame. r Let there be a vector k, where the first element of vector k comes from the circle closest to the sensor, and the subsequent elements come from the next ring in sequence. The vector k and the ring encoding function ψ(·) are represented as follows:

[0036]

[0037]

[0038] Where, r i ||r represents the row number of each feature description submatrix. i ||0 represents r i The number of non-empty partitioning units in the corresponding annulus, ψ(r) i ) represents the circular coding function, indicating the occupancy rate of the circular code; Represents the real number field.

[0039] A KD-tree can be constructed based on vector k. Using the vector corresponding to the current frame, the KD-tree is searched to find n possible similar frames. Then, a precise search is performed using the distance function of the feature descriptor matrix.

[0040]

[0041] Where C is the potentially similar candidate frame found from the KD tree, τ is the threshold of the distance function, and c * It is the index of a specific historical frame.

[0042] Specifically, if two frames are similar, it means there is rotation between the two frames. The calculated rotation angle is used as the global initial pose output, and the relocalization is completed.

[0043] Beneficial effects:

[0044] (1) This invention provides a high-precision, low-drift, large-range three-dimensional point cloud map construction and relocation method. Based on inertial measurement unit (IMU) information and lidar scanning point cloud information, the relative pose between key frames is obtained by point cloud matching. This process only requires a reasonable point cloud matching result and is an open pose estimation process that can achieve flexibility in details, and is intelligent and efficient.

[0045] (2) In this invention, point cloud motion distortion correction is performed by scanning point cloud information with lidar, thereby improving the application performance of lidar, increasing the coverage of the detected targets, and reducing the loss of target information.

[0046] (3) After obtaining the relative pose between key frames, this invention transmits it to the backend nonlinear optimization module. Combined with lidar odometry constraints, GPS information constraints, and loop closure detection constraints, nonlinear map optimization is performed to complete the construction of the robot's 3D point cloud map. GPS information constraints are particularly considered, enabling the construction of high-quality, low-drift 3D point cloud maps for mobile robots over large outdoor areas and long distances. This provides a high-precision, low-drift, and robust map construction solution, significantly improving the quality of mobile robot map construction technology. Loop closure detection constraints are added during map construction, and scene recognition is performed through loop closure detection. This reduces the cumulative error caused by long-distance operation of the lidar odometry, resulting in stronger global consistency and less map drift in the 3D point cloud map construction.

[0047] (4) This invention constructs feature descriptors for key frames by cutting a point cloud frame, performs inter-frame similarity discrimination, thereby completing the robot's relocation in the three-dimensional point cloud map, and thus providing a guarantee for the robot's subsequent autonomous navigation.

[0048] (5) This invention uses a ring coding function to calculate the similarity of historical frames, which reduces the computational complexity and improves the computational efficiency.

[0049] (6) In this invention, the rotation amount can be used as the initial pose of the lidar, and finally the global initial pose is output to complete the repositioning, which can further improve the accuracy of NDT registration and positioning, and improve the accuracy of lidar. Attached Figure Description

[0050] Figures 1A to 1B This is a schematic diagram of the point cloud motion distortion correction proposed in this invention;

[0051] Figure 2 This is a schematic diagram of the nonlinear optimized pose node relationship proposed in this invention;

[0052] Figure 3 This is a schematic diagram of the layout of the 3D point cloud map construction and relocation module proposed in this invention. Detailed Implementation

[0053] The specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0054] This invention proposes a method for constructing and relocating high-precision, low-drift, large-area 3D point cloud maps, including:

[0055] Step 1: Perform inter-frame point cloud registration based on the point cloud information and IMU information acquired by the 3D LiDAR to obtain the relative pose between key frames.

[0056] First, the motion information obtained from the IMU is used to correct the distortion of the point cloud information acquired by the LiDAR, so that the point cloud data obtained by the LiDAR for a frame is normalized to the same acquisition time. Then, using the point cloud information of each keyframe, a point cloud matching method is used to obtain the relative pose between keyframes, thereby completing the front-end pose estimation and obtaining the relative pose between keyframe nodes. The existing pose information between keyframes and GPS position information are saved as initialization information for subsequent map optimization.

[0057] Step 1.1: Use an IMU to acquire motion information and perform distortion correction on the point cloud information acquired by the lidar.

[0058] The motion information of the carrier, including angular velocity and linear velocity, is obtained from the IMU. The time difference between the laser point and the initial acquisition time is calculated based on the coordinates of the laser point scanned by the lidar. Then, the pose transformation matrix is ​​calculated, and the laser point is transformed according to the transformation matrix.

[0059] Distortion compensation can be described as:

[0060]

[0061] Among them, such as Figure 1A and Figure 1B As shown, Let i be the coordinates of the i-th laser point after distortion compensation. The pose of the lidar at the initial acquisition time in a point cloud frame. Let P be the pose of the lidar when the i-th laser point is acquired. i =[p ix p iy p iz ] T Let T0 be the coordinates of the i-th laser point. When the pose of the lidar at the initial acquisition time is used as the reference coordinate system, T0 is the identity matrix. i This represents the relative pose from time 0 to time i. Therefore, the coordinate transformation relationship can be obtained simply by acquiring the average angular velocity w and average velocity v of the carrier from time 0 to time i.

[0062] At this point, the coordinate transformation relationship can be obtained simply by obtaining the average angular velocity w and average velocity v of the carrier from time 0 to time i.

[0063]

[0064] Motion information can be acquired through an IMU. The time difference between the laser point and the initial acquisition time can be calculated. α is the angle of rotation of the laser point relative to the first laser point, t is the time required for the lidar to rotate by angle α, and f is the frequency of the lidar rotating once.

[0065] Step 1.2: Match the point clouds of keyframes to obtain relative poses.

[0066] The keyframe point cloud is voxelized, the mean and covariance of each voxel are calculated, a Gaussian distribution is constructed, the current frame point cloud is transformed into the map coordinate system according to the pose initialized by GPS, and the joint probability of all points is calculated. When the joint probability of all points is maximized, the two frame point clouds are considered to be successfully matched, and the relative pose between the two frame point clouds is output.

[0067] (1) First, the point cloud of the map coordinate system is gridded, the point cloud space is divided into a set of voxels of a specified size, the mean and covariance of each grid point cloud in the map coordinate system are calculated, and a Gaussian distribution is constructed.

[0068]

[0069]

[0070] in, For the target point cloud point set, x i (i = 1, ..., N) x N represents the coordinates of the point in the target point cloud. x The number of points in the target point cloud.

[0071] (2) Based on the initial pose between the current frame point cloud and the target point cloud, transform the current frame point cloud into the coordinate system of the target point cloud and calculate the joint probability of all points.

[0072] y i =T(p,y) i ) = Ry i +t (5)

[0073]

[0074]

[0075] in, For reference point cloud point set, y i (i = 1, ..., N) y N represents the coordinates of a point in the reference point cloud. yThe number of points in the reference point cloud. T(p,y) i ) represents the coordinate transformation function from the reference point cloud to the target point cloud, y i 'The coordinates of the points transformed from the reference point cloud to the target point cloud coordinate system, p = [t] x ,t y ,t z ,φ x ,φ y ,φ z ] T , t x ,t y ,t z ,φ x ,φ y ,φ z Let f(x,y) represent the translation and rotation along the x, y, and z directions, respectively. Let R be the rotation matrix and t be the translation matrix. i ') represents the joint probability of a single point. Let be the joint probability of all points.

[0076] (3) When the joint probability of all points is maximized, the registration of the two point clouds can be considered successful, and the optimal pose transformation matrix can be obtained at this time. Taking the logarithm of the joint probability function, the original problem is transformed into an optimization problem:

[0077]

[0078]

[0079] Let e i (p)=y i '-u,

[0080] The objective function is:

[0081]

[0082] Iterative optimization means finding the minimum Δp to output the optimal pose.

[0083]

[0084]

[0085] in,

[0086] The objective function changes with the independent variable as follows:

[0087]

[0088] The optimization problem described above is transformed into finding Δp such that ΔF i(p) reaches a local minimum, and its derivative is set to zero:

[0089]

[0090] That is: H i Δp=-b i (15)

[0091] Therefore, we only need to obtain J. i Δp can be obtained from (p).

[0092]

[0093] Step 2: Considering the robot's GPS information constraints and closed-loop detection constraints, optimize the relative poses between keyframe pose nodes to complete the construction of the robot's 3D point cloud map.

[0094] Step 2.1: Based on the key point cloud matching and key frame relative pose obtained in Step 1, further consider GPS information constraints, closed loop detection information constraints, objective function and other information to construct the map construction proposition for the mobile robot.

[0095] (1) GPS information and lidar odometry constraints

[0096] The GPS information constraint can be expressed in Lie algebras as follows:

[0097]

[0098] In the formula: e i Z represents the difference between GPS location information and keyframe pose nodes. i This indicates GPS location observation information. For each element in a Lie algebra, there is a six-dimensional vector, with the first three dimensions representing translation, denoted as ρ, and the last three dimensions representing rotation, denoted as φ. φ represents the four-dimensional matrix corresponding to the six-dimensional vector. φ is the Lie algebra corresponding to the Lie group SO(3), defined in R. 3 Vectors on, Let be the antisymmetric matrix corresponding to the vector. ∨ This indicates that the corresponding vector is obtained from the antisymmetric matrix.

[0099] (2) Closed-loop detection and lidar odometer constraint

[0100] The loop closure detection constraint is mainly the relative pose constraint between the current frame and historical frames. In the Lie group SE(3), it can be expressed as:

[0101] T ij =T i -1 T j (18)

[0102] Among them, T i and T j T represents the absolute pose provided by the lidar odometry in frame i and frame j, respectively. ij This represents the relative pose of the loop closure detection in frame i and frame j.

[0103] In Lie algebras, this is represented as:

[0104] in, For each element in a Lie algebra, there is a six-dimensional vector, with the first three dimensions representing translation, denoted as ρ, and the last three dimensions representing rotation, denoted as φ. φ represents the four-dimensional matrix corresponding to the six-dimensional vector. φ is the Lie algebra corresponding to the Lie group SO(3), defined in R. 3 Vectors on, Let be the antisymmetric matrix corresponding to the vector. Ideally, the two formulas above are equal, but when there is an error in the pose, the residual term e can be calculated using both sides of the equation. ij :

[0105]

[0106] (4) Objective function

[0107] like Figure 2 As shown, map optimization is performed by constructing a pose graph, which consists of nodes and edges. Nodes represent keyframe poses, and edges represent relative pose constraints between two pose nodes, including LiDAR odometry constraints, loop closure detection constraints, and GPS position observation constraints. During optimization, all observations and state variables are optimized together to construct a residual function. In practical applications, each residual is assigned a weight, i.e., an information matrix, which is equivalent to weighting the residuals and performing nonlinear optimization to correct odometry errors and trajectory shapes, thereby obtaining a globally consistent map. The residual function is expressed as:

[0108]

[0109] Where, x i F represents the pose of the pose node. ij For the residual term of a single pose node, Ω ij For the information matrix, e ij Since the residual is zero, the objective function for optimization can be expressed as:

[0110]

[0111] In summary, the problem of map optimization can be described as follows:

[0112] min the objective function Eq.(22)

[0113]

[0114] A new method for constructing a high-precision, low-drift, and large-scale 3D point cloud map simply requires solving formula (23) and minimizing the objective function (22) to obtain a high-quality, low-drift 3D point cloud map.

[0115] Step 2.2: Iteratively solve the objective function obtained in Step 2 to complete the optimized construction of the 3D point cloud map.

[0116] (1) Closed-loop detection constraints and iterative solution of the objective function of lidar odometer

[0117] According to equation (20), perturbations δξ are added to the poses of the i-th and j-th frames respectively. i ,δξ j Solving for the Jacobi, and simplifying using the adjoint property BCH formula, the residual is then expressed as:

[0118]

[0119] The above formula shows that the residual with respect to T i The Jacobian matrix is:

[0120]

[0121] Residual with respect to T j The Jacobian matrix is:

[0122]

[0123] in,

[0124] Optimization is performed using the Gauss-Newton method, and the residuals are expanded using a first-order Taylor series:

[0125]

[0126] J ij Here is the Jacobian matrix of the residual with respect to pose:

[0127]

[0128] For each residual term:

[0129]

[0130] Subsequently, the Gauss-Newton method was used for optimization.

[0131] (2) Iterative solution of the objective function constrained by GPS information and lidar odometer

[0132] Add perturbation δξ to the residual i Furthermore, by simplifying using the adjoint property and the BCH formula, we obtain:

[0133]

[0134] Therefore, the residual with respect to T i The Jacobian matrix is:

[0135]

[0136] in, Subsequently, the Gauss-Newton method was used for optimization.

[0137] Step 3: Based on the descriptor designed using the LiDAR point cloud information, perform a similarity query between the current frame and historical frames to complete the robot's relocalization on the 3D point cloud map.

[0138] Step 3.1: Point cloud segmentation and descriptor generation: Divide a frame of point cloud scanned by 3D LiDAR into independent point clouds. After the point cloud is segmented, assign a real value to each segmentation unit using the point cloud in each segmentation unit, and generate a matrix of size Nr*Ns from the segmented point cloud.

[0139] (1) Cutting point cloud

[0140] (a) Divide the point cloud space into N parts along the direction in which the radius of the point cloud in a frame increases. r There are 3 circular rings, and the radial gap between each ring is:

[0141]

[0142] Among them, L max This is the maximum scanning distance of the lidar.

[0143] (b) Rotate the lidar scanning direction around the circle to divide each ring into N. s Divide into equal parts, with the central angle of each sector being . A segmented point cloud P can be represented as:

[0144]

[0145] Among them, P ij The set of points representing the dividing units of the i-th annulus and j-th sector.

[0146] (2) Generate descriptors: Assign a real value to each segmentation unit using the point cloud in each segmentation unit. Represents the real number field; generates the segmented point cloud into a matrix of size Nr*Ns.

[0147]

[0148] a) Each row of the matrix represents a ring; b) Each column of the matrix represents a sector; c) Each element in the matrix represents the maximum height of all 3D points in each segmentation unit. The encoding function for the segmentation unit is:

[0149]

[0150] Where z(·) represents the coordinate value of the return point P on the z-axis. We assign zeros to empty partition cells.

[0151] Through the above process, we finally obtain a representation of N. r *N s The characteristic descriptor I of the matrix is ​​expressed as:

[0152]

[0153] Step 3.2: Perform similarity matching based on the current frame point cloud information descriptor scanned by the robot in the 3D map and the descriptor information stored during map construction, output pose information, and complete relocalization.

[0154] (1) Two-frame descriptor similarity measurement: Given a pair of feature descriptors, we need to construct a distance function between the two frame feature descriptors to describe the similarity between the two frames. The distance function is as follows:

[0155]

[0156] Among them, I q and I c These are feature descriptors for the current frame and historical frames, respectively. and I respectively q and I c The distance between two column vectors at the same index is calculated using cosine distance, taking the vector in the j-th column of the descriptor. The more similar all corresponding column vectors are between two frame descriptors, the higher the similarity between the point clouds of the two frames. When the distance function is less than a certain threshold, the two frames are considered similar.

[0157] To address the shift in feature descriptor column vectors caused by different sensor viewpoints, we calculate the distances of all possible column-shifted feature descriptors and find the minimum distance.

[0158]

[0159] in, Represents the initial feature description submatrix I c The feature descriptor matrix after n-column translation, D(I q ,Ic ) represents the minimum distance between two frames, and n* represents the translation amount of the column corresponding to the minimum distance between the two frames.

[0160] (2) Two-layer search matching: After obtaining the feature descriptor matrix between each frame, we can calculate the similarity between two frames by the similarity of the corresponding columns of the matrix. However, the similarity calculation is complex by traversing all historical frames. Therefore, we design a search algorithm for screening. We fuse pairwise scoring and nearest neighbor search to achieve a reasonable search time.

[0161] A ring is a rotation-invariant descriptor extracted from a feature descriptor matrix, where each row r of the feature descriptor matrix is ​​encoded into a single real value using the ring encoding function ψ(·). The number of points falling within the ring of equal radius should be similar across similar frames; therefore, an N ring can be generated for each frame. r A vector k is given by dimension k, where the first element of vector k comes from the circle closest to the sensor, and the subsequent elements come from the next circle in sequence.

[0162]

[0163]

[0164] Where, r i This represents the row number of each feature description submatrix. Represents the real number field; ||r i ||0 represents r i The number of non-empty partitioning units in the corresponding annulus, ψ(r) i ) represents the circular coding function, indicating the occupancy rate of the circular code.

[0165] A KD-tree can be constructed based on vector k. Using the vector corresponding to the current frame, the KD-tree is searched to find n possible similar frames. Then, a precise search is performed using the distance function of the feature descriptor matrix.

[0166]

[0167] Where C is the potentially similar candidate frame found from the KD tree, τ is the threshold of the distance function, and c * It is the index of a specific historical frame.

[0168] If two frames are similar, it means there is a rotation between the two frames, and the rotation angle φ is:

[0169]

[0170] This rotation amount can be used as the initial pose for further precise NDT registration and positioning of the lidar, and finally the global initial pose is output, and the repositioning is completed.

[0171] It will be apparent to those skilled in the art that the embodiments of the present invention are not limited to the details of the exemplary embodiments described above, and that the embodiments of the present invention can be implemented in other specific forms without departing from the spirit or essential characteristics of the embodiments of the present invention. Therefore, the embodiments should be considered exemplary and non-limiting in all respects, and the scope of the embodiments of the present invention is defined by the appended claims rather than the foregoing description. Therefore, all variations falling within the meaning and scope of equivalents of the claims are intended to be encompassed within the embodiments of the present invention. No reference numerals in the claims should be construed as limiting the scope of the claims. Furthermore, it is clear that the word "comprising" does not exclude other units or steps, and the singular does not exclude the plural. Multiple units, modules, or devices recited in the system, apparatus, or terminal claims may also be implemented by the same unit, module, or device through software or hardware. The terms "first," "second," etc., are used to indicate names and do not indicate any particular order.

[0172] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the embodiments of the present invention and are not intended to limit them. Although the embodiments of the present invention have been described in detail with reference to the above preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions to the technical solutions of the embodiments of the present invention should not depart from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for constructing and relocating a high-precision, low-drift, large-area 3D point cloud map, characterized in that, include: Step 1: Perform inter-frame point cloud registration based on the point cloud information acquired by the 3D lidar and the inertial measurement unit (IMU) information to obtain the relative pose between key frames; Step 2: By constraining the robot's GPS information and closed-loop detection information, the relative poses between keyframe pose nodes are optimized, and the robot's 3D point cloud map is constructed. Step 3: Based on the descriptor designed using the LiDAR point cloud information, perform a similarity query between the current frame and historical frames to complete the robot's relocalization on the 3D point cloud map; Step 1 includes: Step 1.1: Obtain motion information using the inertial measurement unit (IMU) information, and perform distortion correction on the point cloud information acquired by the lidar; wherein, the motion information includes angular velocity and linear velocity, calculate the time difference of the laser point relative to the initial acquisition time based on the coordinates of the laser point scanned by the lidar, then calculate the pose transformation matrix, and transform the laser point according to the transformation matrix; Step 1.2: Match the keyframe point clouds to obtain the relative pose. Specifically, this includes: voxelizing the keyframe point clouds, calculating the mean and covariance of each voxel, constructing a Gaussian distribution, transforming the current frame point cloud into the map coordinate system based on the GPS-initialized pose, and calculating the joint probability of all points. When the joint probability of all points is maximized, the two frame point clouds can be considered successfully matched. At this time, the relative pose between the two frame point clouds is output. Step 2 includes map optimization by constructing a pose graph, which consists of nodes and edges. Nodes represent keyframe poses, and edges represent relative pose constraints between two pose nodes. The pose constraints include LiDAR odometry constraints, loop closure detection constraints, and GPS position observation constraints. During map optimization, all observations and state variables are optimized together to construct a residual function. Each residual is assigned a weight to form an information matrix, and nonlinear optimization is performed. Odometry errors are corrected, and trajectory shapes are adjusted to obtain a globally consistent map. Step 3 specifically includes: Step 3.1: Point cloud segmentation and descriptor generation: Divide a frame of point cloud scanned by 3D LiDAR into independent point clouds. After the point cloud is segmented, use the point cloud in each segmentation unit to assign a real value to each segmentation unit, and generate the segmented point cloud into a matrix of size Nr*Ns. Step 3.2: Perform similarity matching based on the current frame point cloud information descriptor scanned by the robot in the 3D map and the descriptor information stored during map construction, output pose information, and complete relocalization.

2. The method for constructing and relocating high-precision, low-drift, large-area three-dimensional point cloud maps according to claim 1, characterized in that: Step 3.1 specifically includes: (1) Cutting the point cloud: Divide the point cloud space into Nr annular rings along the direction of increasing radius of a point cloud frame, with the radial gap between each annular ring being: Where Lmax is the maximum scanning distance of the lidar; Rotating the lidar scanning direction for one revolution, each ring is divided into Ns equal parts, with the center angle of each sector being [value missing]. A segmented point cloud P can be represented as: Where pij represents the set of points in the dividing unit of the i-th annulus and the j-th sector; (2) Generate descriptors: Assign a real value to each segmentation unit using the point cloud in each segmentation unit. Represent the real number field; generate the segmented point cloud into a matrix of size Nr*Ns; Each row of the matrix represents a ring, each column represents a sector, and each element represents the maximum height of all 3D points in each segmentation unit; the encoding function of the segmentation unit is: Where z (·) represents the coordinate value of the return point P on the z-axis; Through the above process, we finally obtain an eigenvalue descriptor I, represented as an Nr * Ns matrix, which is expressed as: , .

3. The method for constructing and relocating high-precision, low-drift, large-area three-dimensional point cloud maps according to claim 2, characterized in that: Step 3.2 specifically includes: (1) Two-frame descriptor similarity measurement: Given a pair of feature descriptors, it is necessary to construct a distance function between the two frame feature descriptors to describe the similarity between the two frames. The distance function is as follows: Where Iq and Ic are the feature descriptors of the current frame and historical frames, respectively. and The vectors in the j-th column of Iq and Ic are respectively. The distance between two column vectors at the same index is calculated using cosine distance. The more similar all corresponding column vectors are between two frame descriptors, the higher the similarity of the point clouds between the two frames. When the distance function is less than a certain threshold, the two frames are considered similar. Calculate the distances to all possible column shift feature descriptors and find the minimum distance; in, D(Iq, Ic) represents the feature descriptor matrix after n-column translation of the initial feature descriptor matrix Ic, where D(Iq, Ic) represents the minimum distance between two frames, and n* represents the translation amount of the column corresponding to the minimum distance between the two frames. (2) Two-layer search matching: After obtaining the feature descriptor matrix between each frame, the similarity between the two frames is calculated by the similarity of the corresponding columns of the matrix; the rotation-invariant descriptor extracted from the feature descriptor, each row r of the feature descriptor matrix is ​​encoded into a single real value by the ring encoding function Ψ (.); the number of points falling in the same radius of the rings between similar frames should be similar, so an Nr-dimensional vector k is generated for the rings of each frame. The first element of the vector k comes from the circle closest to the sensor, and the subsequent elements come from the next ring in order. The vector k and the ring encoding function Ψ (.) are represented as follows: Where ri represents the row number of a feature description submatrix, ri 0 represents the number of non-empty segmentation units in the annulus corresponding to ri, and Ψ(ri) represents the annulus encoding function, indicating the occupancy rate of the annulus; Represents the real number field; A KD tree can be constructed based on vector k. Using the vector corresponding to the current frame, a search is performed in the KD tree. Find n possible similar frames, and then perform a precise search using the distance function of the feature descriptor matrix: When D < τ Where C is a candidate frame that is likely to be similar and is found from the KD tree, τ is the threshold of the distance function, and c* is the index of the determined historical frame.

4. The method for constructing and relocating a high-precision, low-drift, large-area three-dimensional point cloud map according to claim 3, characterized in that... If two frames are similar, it means there is rotation between the two frames. The calculated rotation angle is used as the global initial pose output, and the relocalization is completed.

Images