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

CN116399354BCN 116399354 BCN116399354 BCN 116399354BCN-116399354-B

Abstract

The invention provides a high-precision low-drift large-range three-dimensional point cloud map construction and repositioning method, which is characterized in that inter-frame point cloud registration is carried out based on point cloud information and IMU information acquired by a three-dimensional laser radar to obtain relative pose among key frames, GPS information constraint, closed loop detection constraint and laser radar odometer constraint of a robot are considered, optimization is carried out based on the obtained relative pose among the key frames to finish the three-dimensional point cloud map construction of the robot, and key frame description is simultaneously considered to construct key frame description information to carry out inter-frame similarity judgment to finish the repositioning of the robot in a three-dimensional map.

Inventors

LI JING
GUO KEYAN
WANG JUNZHENG
WANG SHOUKUN
ZHAO JIANGBO
MA LILING
SHEN WEI

Assignees

北京理工大学

Dates

Publication Date: 20260508
Application Date: 20230324
Priority Date: 20220526

Claims (4)

1. A high-precision low-drift large-range three-dimensional point cloud map construction and repositioning method is characterized by comprising the following steps: Step1, performing inter-frame point cloud registration based on point cloud information acquired by a three-dimensional laser radar and Inertial Measurement Unit (IMU) information to obtain relative pose between key frames; Step 2, the relative pose among the pose nodes of the key frame is optimized by restraining the GPS information and the closed-loop detection information of the robot, and the three-dimensional point cloud map construction of the robot is completed; Step 3, inquiring the similarity of the current frame and the historical frame according to a descriptor designed by the laser radar point cloud information, and finishing repositioning of the robot under the three-dimensional point cloud map; the step 1 includes: Step 1.1, acquiring motion information by utilizing the inertial measurement unit IMU information, and carrying out distortion correction on point cloud information acquired by a laser radar, wherein the motion information comprises angular speed and linear speed, calculating the time difference of a laser point scanned by the laser radar relative to the acquisition initial moment according to the coordinates of the laser point, then calculating a pose conversion matrix, and converting the laser point according to the conversion matrix; Step 1.2, matching the point clouds of the key frames to obtain relative pose, which specifically comprises the steps of voxelizing the point clouds of the key frames, calculating the mean value and covariance of each voxel, constructing Gaussian distribution, transforming the point clouds of the current frame into a map coordinate system according to the pose initialized by a GPS, calculating the joint probability of all the points, and considering that the two point clouds are successfully matched when the joint probability of all the points is maximum, and outputting the relative pose between the two point clouds; The map optimization method comprises the steps of carrying out map optimization by constructing a pose graph, wherein the pose graph consists of nodes and edges, the nodes represent key frame poses, the edges represent relative pose constraints of two pose nodes, the pose constraints comprise laser radar odometer constraints, loop detection constraints and GPS position observation constraints, all observation and state quantities are put together for optimization during map optimization to construct a residual function, and each residual is distributed with a weight to form an information matrix for nonlinear optimization; the step 3 specifically includes: Dividing a frame of point cloud scanned by the three-dimensional laser radar into mutually independent point clouds, after the point clouds are cut, distributing a real value for each cutting unit by using the point clouds in each cutting unit, and generating the cut point clouds into a matrix with the size of Nr' Ns; and 3.2, performing similarity matching according to the current frame point cloud information descriptors scanned by the robot in the three-dimensional map and the descriptor information stored during map construction, outputting pose information, and finishing repositioning.
2. The method for constructing and repositioning a high-precision low-drift large-scale three-dimensional point cloud map according to claim 1, wherein the step 3.1 comprises (1) cutting the point cloud: Along the direction of increasing the radius of a frame of point cloud, dividing the point cloud space into Nr circular rings, wherein the radial gap between each circular ring is as follows: wherein Lmax is the maximum scanning distance of the laser radar; Dividing each circular ring into Ns equal parts along the rotation scanning direction of the laser radar for one circle, wherein the center angle of each sector is The segmented one-frame point cloud P can be expressed as: wherein pij represents the set of points in the segmentation unit of the jth sector of the ith ring; (2) Generating descriptors by assigning each segmentation unit a real value using the point cloud in each segmentation unit, Generating the cut point cloud into a matrix with the size of Nr-Ns; each row of the matrix represents a circular ring, each column represents a sector, each element represents the maximum value of the heights of all three-dimensional points in each dividing unit, and the encoding function of the dividing unit is as follows: Wherein z (·) represents the coordinate value of the return point P in the z-axis; through the above process, a feature descriptor I, denoted as nr×ns matrix, is finally obtained, which is denoted as: , 。
3. The high-precision low-drift wide-range three-dimensional point cloud map construction and repositioning method according to claim 2, characterized by: The step 3.2 specifically comprises (1) measuring similarity of two frames of descriptors, wherein given a pair of feature descriptors, the similarity between two frames needs to be described by constructing a distance function between the two frames of feature descriptors, and the distance function is as follows: wherein Iq and Ic are feature descriptors of the current frame and the history frame, respectively, And The more similar the corresponding column vectors between the two frames of descriptors, the higher the similarity of the point clouds of the two frames is, and when the distance function is smaller than a certain threshold value, the two frames are considered to be similar; calculating the distance of all possible column shift feature descriptors, and finding the minimum distance; wherein, the Representing the feature description submatrix after n columns of translation of the initial feature description submatrix Ic, wherein D (Iq, ic) represents the minimum distance between two frames, and n represents the translation amount of the corresponding column when the distance between two frames is minimum; (2) Two-layer search matching, namely, after a feature description submatrix between each frame is obtained, the similarity of the two frames is calculated through the similarity of columns corresponding to the matrix, rotation-invariant descriptors extracted from the feature description submatrix are encoded into a single real value through a ring encoding function psi (), the number of points of a ring in the same radius between similar frames is similar, so that a vector k in Nr dimension is generated for the ring of each frame, the first element of the vector k is from the circle nearest to a sensor, the subsequent elements are sequentially from the next ring, and the vector k and the ring encoding function psi () are expressed as follows: Where ri represents each row number of a feature description sub-matrix, ri 0. Representing the number of non-empty partition units in the ring corresponding to ri, wherein ψ (ri) represents a ring coding function and represents the occupancy of the ring; representing the real number domain; a KD-tree can be constructed from vector k, using the vector corresponding to the current frame, looking up in the KD-tree, Finding out n possible similar frames, and accurately finding out the n possible similar frames through a distance function of the feature description submatrix: when D < τ Where C is the candidate frame that is found out of the KD-tree that is likely to be similar, τ is the threshold of the distance function, and C is the index of the identified historical frame.
4. The method for constructing and repositioning a three-dimensional point cloud map with high precision and low drift and large range according to claim 3, wherein if two frames are similar frames, rotation exists between the two frames, the calculated rotation angle is output as a global initial pose, and repositioning is completed.

Description

High-precision low-drift large-range three-dimensional point cloud map construction and repositioning method Technical Field The invention belongs to the field of mobile robot map construction, and particularly relates to a high-precision low-drift large-range three-dimensional point cloud map construction and repositioning method. Background The map construction technology is one of core technologies for realizing the autonomous navigation function by constructing an environment map and providing priori information for positioning and path planning on the premise that the autonomous navigation is realized by the mobile robot. However, in the current three-dimensional point cloud map construction technology, laser radars or cameras are often adopted to conduct point cloud registration between key frames, point cloud information is obtained through the laser radars carried by mobile robots, then the key frame point clouds are matched to obtain relative positions and poses between the key frames, and further position and pose superposition is conducted continuously to achieve map construction. Although the existing method can construct a three-dimensional point cloud map, accumulated errors in the construction of the three-dimensional point cloud map cannot be effectively treated under the condition of outdoor large-range long distance, so that the map drift is caused. In addition, the robot can be repositioned in the three-dimensional map, so that the robustness of the autonomous navigation process of the robot in the map is supported. Therefore, designing a high-precision low-drift large-range three-dimensional point cloud map construction and repositioning method and device is important for autonomous navigation of a mobile robot. Disclosure of Invention In order to solve the technical problems, the invention provides a high-precision low-drift large-range three-dimensional point cloud map construction and repositioning method, which comprises the following steps: Step 1, performing inter-frame point cloud registration based on point cloud information acquired by a three-dimensional laser radar and Inertial Measurement Unit (IMU) information to obtain relative pose between key frames; Step 2, the relative pose among the pose nodes of the key frame is optimized by restraining the GPS information and the closed-loop detection information of the robot, and the three-dimensional point cloud map construction of the robot is completed; and 3, inquiring the similarity of the current frame and the historical frame according to the description of the laser radar point cloud information design, and finishing repositioning of the robot under the three-dimensional point cloud map. In particular, the step 1 includes: Step 1.1, acquiring motion information by utilizing the inertial measurement unit IMU information, and carrying out distortion correction on point cloud information acquired by a laser radar, wherein the motion information comprises angular speed and linear speed, calculating the time difference of a laser point scanned by the laser radar relative to the acquisition initial moment according to the coordinates of the laser point, then calculating a pose conversion matrix, and converting the laser point according to the conversion matrix; Step 1.2, matching the point clouds of the key frames to obtain relative pose, specifically comprising the steps of voxelizing the point clouds of the key frames, calculating the mean value and covariance of each voxel, constructing Gaussian distribution, transforming the point clouds of the current frame into a map coordinate system according to the pose initialized by a GPS, calculating the joint probability of all the points, and considering that the two point clouds are successfully matched when the joint probability of all the points is maximum, and outputting the relative pose between the two point clouds. The map optimization method comprises the steps of carrying out map optimization by constructing a pose graph, wherein the pose graph consists of nodes and edges, the nodes represent key frame poses, the edges represent relative pose constraints of two pose nodes, the pose constraints comprise laser radar odometer constraints, loop detection constraints and GPS position observation constraints, all observation and state quantities are put together for optimization during map optimization, a residual function is constructed, each residual is assigned with a weight to form an information matrix for nonlinear optimization, and the overall consistency map is obtained by correcting odometer errors and track shapes. In particular, the step 3 specifically includes: Dividing a frame of point cloud scanned by the three-dimensional laser radar into mutually independent point clouds, after the point clouds are cut, distributing a real value for each cutting unit by using the point clouds in each cutting unit, and generating the cut point clouds into a matrix with the size of N