A solid-state lidar loop closure detection method based on point cloud fiducial markers

Abstract

The invention relates to the technical field of loop detection, and in particular to a solid-state laser radar loop detection method based on point cloud reference markers. The method comprises the following steps: making a point cloud reference marker and precalculating a continuous function in a regenerated Hilbert space of the point cloud reference marker to construct a function dictionary; the method comprises the following steps: performing frame overlap processing on an input point cloud by using a front-end odometer, and performing intensity value compensation; the method comprises the following steps: projecting the compensated point cloud based on the intensity value, and extracting the point cloud reference marker by using a ray method; the method comprises the following steps: calculating an ID and a six-degree-of-freedom posture of the point cloud reference marker after extracting the point cloud reference marker, and saving the ID and the posture to a database; the method comprises the following steps: retrieving the same ID in the database, calculating the relative posture of the corresponding point cloud according to the saved posture, constructing a loop detection factor and adding it to a back-end graph optimization to reduce the accumulated error of the map construction; the method is applicable to a solid-state laser radar with a small field of view angle, and solves the problem that most algorithms of the solid-state laser radar are not applicable in reverse loop detection of SLAM.

Description

Technical Field

[0001] The present invention relates to the technical field of loop closure detection, and in particular to a solid-state laser radar loop closure detection method based on point cloud reference marking. Background Art

[0002] With the rapid development of autonomous driving technology, including the large-scale application of unmanned vehicles in cleaning, warehousing logistics, medical distribution and other fields, it has greatly reduced labor costs, liberated productivity, and provided more convenience for people. Among them, using loop detection methods to build more accurate and realistic three-dimensional point cloud maps for these unmanned vehicles is a key technology in research.

[0003] Loop detection is to determine whether a position has been passed before based on some features in the recognized environment. By judging the similarity between two positions, the relative pose constraints of the two positions are established in the SLAM framework, thereby achieving the purpose of reducing the cumulative error of mapping. It is very important for improving the consistency and accuracy of global mapping.

[0004] Some common laser SLAM loop detection algorithms, such as ScanContext and BoW3D, have relatively good loop detection performance on mechanical rotating radars with a 360° field of view. However, the performance of solid-state laser radars with a small field of view is greatly reduced in reverse loop detection, and the algorithm may even fail. The reason is that when a solid-state laser radar with a small field of view scans in reverse at the same position, the two scanned frames of point clouds to be detected do not contain most of the common point cloud features or even no common features, so the similarity cannot be detected, and the overlap between the two frames of point clouds is very small. Point cloud matching methods such as ICP and GICP cannot correctly solve the pose, and thus cannot construct loop detection constraints and achieve global optimization.

[0005] Therefore, this application proposes a solid-state laser radar loop closure detection method based on point cloud benchmark markers. Summary of the invention

[0006] In order to make up for the deficiencies of the prior art and solve the technical problems existing in the background technology, the present invention proposes a solid-state laser radar loop detection method based on point cloud reference markers.

[0007] The present invention is achieved through the following technical solutions:

[0008] A solid-state laser radar loop closure detection method based on point cloud reference markers comprises the following steps:

[0009] Step 1: Create point cloud fiducial markers and pre-calculate the continuous function in the regenerated Hilbert space of each point cloud fiducial marker to build a function dictionary;

[0010] Step 2: Use the front-end odometer to perform frame stacking on the input point cloud and perform intensity value compensation on the point cloud;

[0011] Step 3: The compensated point cloud is projected based on the intensity value, and the point cloud fiducial markers are extracted using the ray method;

[0012] Step 4: After successfully extracting the point cloud fiducial marker, calculate its ID and six-degree-of-freedom pose, and save the ID and six-degree-of-freedom pose to the database;

[0013] Step 5: Retrieve the same ID number in the database and calculate the relative pose of the corresponding point cloud based on the saved pose, construct a loop detection factor and add it to the back-end graph optimization to achieve the purpose of reducing the cumulative error of map construction.

[0014] Preferably, the specific process of step one is as follows:

[0015] Use high-reflectivity traffic reflective stickers to create point cloud fiducial markers;

[0016] The reference tag is the Tag16h5 family of ApriTag3, and the continuous functions of each tag in the Tag16h5 family on its regenerated Hilbert space are calculated in advance to form a function dictionary.

[0017] Preferably, the specific process of step 2 is as follows:

[0018] The input point cloud is overlaid based on the posture of the front-end odometer to improve the density of the point cloud, and the distance of the point cloud from the radar origin and the cosine value of the incident angle are calculated. The point cloud intensity value is compensated by the intensity compensation formula, which is beneficial to improve the success rate of extracting point cloud benchmark markers under different viewing angles.

[0019] Preferably, the intensity compensation formula is as follows:

[0020]

[0021] In the above formula, , They are the original intensity value of the point cloud and the intensity value after compensation. is the coefficient factor, is the Euclidean distance from the point cloud to the radar origin, is the cosine of the incident angle of the point cloud.

[0022] Preferably, the specific process of step three is as follows:

[0023] The overlapping frame point cloud is projected into a two-dimensional intensity image according to the projection formula, and the noise of the intensity image is reduced and the image brightness is enhanced through filtering and pixel filtering.

[0024] Calculate the pixel gradient of the intensity image and select the interest points that meet the requirements;

[0025] The four corner points of the point cloud fiducial marker are extracted by Euclidean clustering of the interest points, and then the point cloud on the point cloud fiducial marker is extracted according to the ray method, and the plane fitting is performed to select the point cloud that meets the requirements.

[0026] Preferably, the specific process of step 4 is as follows:

[0027] The point cloud and four corner points on the point cloud reference marker are projected onto the YZ plane of the radar origin using the relative pose relationship, and two nonlinear optimization problems are constructed to solve the optimal pose.

[0028] The inner product of the continuous function of the regenerated Hilbert space of the point cloud projected onto the radar plane by the point cloud fiducial marker and the continuous function of the function dictionary is calculated to obtain the ID, and the ID and pose are saved in the database.

[0029] Preferably, the specific process of step five is as follows:

[0030] The latest ID passed in is searched for the same ID in the database. After successfully retrieving the same ID, the relative pose of the two frames of point clouds is calculated using the saved corresponding poses, and the loop detection factor is constructed and added to the backend graph optimization to reduce the cumulative error during map construction.

[0031] The beneficial effects of the present invention are:

[0032] The present invention is suitable for solid-state laser radars with small field of view, which solves the problem that most algorithms are not applicable to solid-state laser radars with small field of view in the field of reverse loop detection of SLAM; in addition, reflective stickers with high reflection intensity are used to make point cloud benchmark markers and accurately extract the point cloud benchmark markers to construct loop detection factors to solve the problem of cumulative posture drift in map construction. Compared with existing algorithms, the present invention can realize the reverse loop detection algorithm of solid-state laser radars with small field of view, and establish a more accurate three-dimensional point cloud map for autonomous driving. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] Figure 1 is a flow chart of the method of the present invention;

[0034] Figure 2 A flowchart of extracting point cloud reference markers in the present invention;

[0035] Figure 3 is a schematic diagram of a point cloud reference marker in the present invention;

[0036] Figure 4 It is a schematic diagram of projecting a three-dimensional point cloud into an intensity image in the present invention;

[0037] Figure 5 Schematic diagram of the pose estimation of the point cloud reference marker in the present invention;

[0038] Figure 6 Schematic diagram of the relative pose of two frames of similar point clouds calculated in the present invention. DETAILED DESCRIPTION

[0039] The present invention will be further described below in conjunction with specific examples. It should be understood that these examples are only used to illustrate the present invention and are not intended to limit the scope of the present invention. The experimental methods in the following examples without specifying specific conditions are usually carried out under conventional conditions or under conditions recommended by the manufacturer.

[0040] Unless otherwise defined, all professional and scientific terms used herein have the same meanings as those familiar to those skilled in the art. The reagents or raw materials used in the present invention can be purchased through conventional channels. Unless otherwise specified, the reagents or raw materials used in the present invention are used in a conventional manner in the art or in accordance with the product instructions. In addition, any method and material similar to or equivalent to the described content can be applied to the method of the present invention. The present invention is further described according to the accompanying drawings and specific embodiments of the present invention, and the preferred implementation methods and materials described in the present invention are only for demonstration purposes. Example

[0041] like Figure 1 and Figure 2 As shown, this embodiment provides a solid-state laser radar loop detection method based on point cloud reference markers:

[0042] First, make point cloud fiducial markers and calculate the continuous function composition function dictionary of each fiducial marker;

[0043] Then the front-end odometer is used to stack the point cloud to increase the point cloud density and perform intensity compensation on the point cloud;

[0044] The stacked frame point cloud is then projected into a two-dimensional intensity image, and the noise of the intensity image is reduced and the brightness is enhanced through filtering, pixel filtering and other processing methods;

[0045] Then calculate the pixel gradient of the intensity image and select the interest points; then perform Euclidean clustering of the interest points to extract the four corner points of the point cloud fiducial markers, and then extract the point cloud on the point cloud fiducial markers according to the ray method; and then perform plane fitting to select potential fiducial markers;

[0046] Then, the point cloud and four corner points on the point cloud fiducial marker are projected onto the YZ plane of the radar origin using the relative pose relationship, and two nonlinear optimization problems are constructed to solve the optimal pose. Then, the ID is obtained based on the point cloud on the extracted point cloud fiducial marker, and the ID and pose are saved in the database.

[0047] Finally, we find two frames of point cloud with the same ID number, use the saved pose to calculate the relative pose of the two frames of point cloud, add them to the corresponding pose graph, and perform global optimization to achieve loop detection of lidar with a small field of view and improve the accuracy and quality of map construction.

[0048] Specifically, this embodiment includes the following steps:

[0049] Step 1: Create point cloud fiducial markers and pre-calculate the continuous functions in the regenerated Hilbert space (RKHS) of each point cloud fiducial marker, and build a function dictionary, including:

[0050] The reference mark uses the Tag16h5 family of ApriTag3. A reflective sticker with high reflection intensity is pasted on the white area of ​​the point cloud reference mark, and the black part is made of KD board raw material, thus forming a point cloud reference mark with distinct reflection intensity, such as Figure 3 As shown in Figure 1, the continuous functions of the point cloud fiducial markers are constructed by converting each pixel of the label image into a 3D point and then constructing it in the Regenerated Hilbert Space (RKHS) of 3D points.

[0051] Step 2: Use the front-end odometer to perform frame stacking on the input point cloud and perform intensity value compensation on the point cloud, including:

[0052] First, the point cloud is stacked using the SLAM front-end odometer, with twenty frames as a key frame to increase the density of the point cloud. Since the point cloud intensity value is affected by the point cloud distance and the incident angle, the following formula is used to compensate the intensity value of the key frame point cloud:

[0053]

[0054] in , They are the original intensity value of the point cloud and the intensity value after compensation. is the coefficient factor, is the Euclidean distance from the point cloud to the radar origin, is the cosine of the incident angle of the point cloud.

[0055] Step 3: The compensated point cloud is projected based on the intensity value, and the point cloud fiducial markers are extracted using the ray method. The specific process is as follows: Figure 2 As shown, specifically including the following:

[0056] The intensity compensated 3D point cloud The intensity image projected into the pixel coordinate system is shown in the following figure: Figure 4 As shown, the coordinate formula is as follows:

[0057]

[0058] in , is the two-dimensional coordinate after projection, , , , , and and the horizontal and vertical resolution of the radar, and is the size of the radar's horizontal and vertical field of view angles, and the pixels of the coordinates are rendered with the intensity values ​​of the point cloud. After obtaining the two-dimensional intensity image, a finite impulse response filter is constructed using the opencv library to filter out the horizontal artifacts of the intensity image to make the image clearer. After filtering out the horizontal line artifacts, in order to further remove the influence of the incident angle on the point cloud intensity value, a sliding window based on the average intensity brightness map is constructed using the blur function of the opencv library. , use the brightness map value to calculate the filtered pixel value, the calculation formula is as follows:

[0059]

[0060] in , They are the filtered pixel values ​​and the original pixel values ​​respectively. Through the above steps, it can be fully guaranteed that the intensity value of the point cloud will not be affected by the incident angle and distance, so as to better extract the interest points in the subsequent process.

[0061] The OpenCV library is used to calculate the pixel gradient of the intensity image after brightness enhancement, a threshold is set, and the pixels greater than the threshold are selected. The three-dimensional coordinate points corresponding to these pixels are the points of interest to be selected, and then these three-dimensional points of interest are Euclidean clustered to form a point cloud cluster.

[0062] like Figure 5 As shown, the four corner points defining the point cloud fiducial marker are , find the six points with the maximum and minimum values ​​of the X, Y and Z coordinates of the point cloud in the clustered point cloud cluster Since we know the side length d of the point cloud fiducial marker, we can calculate the diagonal distance (approximately equal to 1.414d) to filter out the four corner points of the point cloud fiducial marker. The screening formula is as follows:

[0063]

[0064] The above formula indicates that the distances of the maximum and minimum points in the x, y and z directions must be within the acceptable diagonal distance range of the point cloud fiducial marker, which may be the four corner points of the point cloud fiducial marker. After obtaining the four corner points, the candidate point cloud of the point cloud fiducial marker is extracted from the key frame point cloud. , The x, y, and z coordinates of the point cloud satisfy the following conditions:

[0065]

[0066] The advantage of using the ray method is that it can quickly calculate whether a point is inside or outside a polygon. Extract the point cloud on the point cloud reference marker and record it as , a polygon is a quadrilateral surrounded by four corner points, is a point inside the quadrilateral, such as Figure 5 shown.

[0067] The RANSAC algorithm is used to fit the plane to eliminate erroneous point cloud reference markers and remove noise points outside the plane.

[0068] Step 4: After successfully extracting the point cloud fiducial marker, calculate its ID and six-degree-of-freedom pose, and save the ID and six-degree-of-freedom pose to the database, including:

[0069] Two nonlinear optimization problems are constructed to solve the optimal relative pose between the point cloud fiducial marker and the radar. ,like Figure 5 Shown: The first one is the four corner points of the point cloud fiducial marker and the template corner points of the radar YZ plane In relative position

[0070] A nonlinear optimization problem with unknown variables:

[0071]

[0072] The initial relative pose can be solved by the SVD method .

[0073] The second is the point cloud on the point cloud fiducial marker using the initial relative pose For the nonlinear optimization problem of the YZ plane projected to the radar origin, first define a heuristic function:

[0074]

[0075] The points on the point cloud fiducial marker are projected onto the YZ plane of the radar origin using the following formula, where is the projected point, For points on the point cloud fiducial marker:

[0076]

[0077] The above heuristic function is used to define the fitting error function of the projected point cloud. The error function is as follows:

[0078]

[0079] Where d represents the side length of the point cloud fiducial marker, are the coordinates of the projected 3D point, is a parameter that takes into account the depth error of the projected point cloud, so the nonlinear optimization problem can be expressed as:

[0080]

[0081] Solving the above problem using a nonlinear optimization method can obtain the optimal relative pose between the point cloud benchmark and the radar. .

[0082] By constructing a continuous function in the regenerated Hilbert space (RKHS) of the projected point cloud, and doing an inner product with the pre-built function dictionary, the maximum value of the inner product obtained is the ID of the point cloud fiducial marker, and the recognized ID and the solved pose are compared. Stored in the database.

[0083] Step 5: Retrieve the same ID number in the database and calculate the relative pose of the corresponding point cloud based on the saved pose, construct a loop detection factor and add it to the backend graph optimization to create a more accurate point cloud map. The specific process includes:

[0084] When the ID of the same point cloud fiducial marker is identified, the relative pose of the two frames of point clouds is calculated through the geometric pose relationship, such as Figure 6 As shown, assuming that the pose saved in the current frame point cloud is , the pose saved in the historical frame point cloud is , then the relative pose of the two frame point clouds It is expressed as:

[0085]

[0086] After obtaining the relative pose of the two frames of point cloud, the loop detection factor is constructed to add the corresponding constraints to the back-end pose graph optimization to realize solid-state lidar loop detection based on point cloud benchmark markers.

[0087] This embodiment designs a method for accurately extracting point cloud reference markers in an environment, constructs a loop detection problem based on the saved ID and pose, realizes loop detection of solid-state lidar based on point cloud reference markers, and improves the accuracy and quality of autonomous driving mapping.

[0088] Any matters not described in the present invention are applicable to the prior art.

Claims

1. A solid-state laser radar loop detection method based on point cloud fiducial markers, characterized in that: The following steps are involved: Step 1: Create point cloud fiducial markers and pre-calculate the continuous function in the regenerated Hilbert space of each point cloud fiducial marker to build a function dictionary; Step 2: Use the front-end odometer to perform frame stacking on the input point cloud and perform intensity value compensation on the point cloud; Step 3: The compensated point cloud is projected based on the intensity value, and the point cloud fiducial markers are extracted using the ray method; Step 4: After successfully extracting the point cloud fiducial marker, calculate its ID and six-degree-of-freedom pose, and save the ID and six-degree-of-freedom pose to the database; Step 5: Retrieve the same ID number in the database and calculate the relative pose of the corresponding point cloud based on the saved pose, construct a loop detection factor and add it to the backend graph optimization to reduce the cumulative error of map construction; The specific process of step three is as follows: The overlapping frame point cloud is projected into a two-dimensional intensity image according to the projection formula, and the noise of the intensity image is reduced and the image brightness is enhanced through filtering and pixel filtering. Calculate the pixel gradient of the intensity image and select the interest points that meet the requirements; The four corner points of the point cloud fiducial marker are extracted by Euclidean clustering of the interest points, and then the point cloud on the point cloud fiducial marker is extracted according to the ray method, and the point cloud that meets the requirements is selected by plane fitting; The specific process of step 4 is as follows: The point cloud and four corner points on the point cloud reference marker are projected onto the YZ plane of the radar origin using the relative pose relationship, and two nonlinear optimization problems are constructed to solve the optimal pose. The inner product of the continuous function of the regenerated Hilbert space of the point cloud projected onto the radar plane by the point cloud fiducial marker and the continuous function of the function dictionary is calculated to obtain the ID, and the ID and pose are saved in the database.

2. The solid-state laser radar loop detection method based on point cloud reference markers according to claim 1, characterized in that: The specific process of step one is as follows: Use high-reflectivity traffic reflective stickers to create point cloud fiducial markers; The reference tag is the Tag16h5 family of ApriTag3, and the continuous functions of each tag in the Tag16h5 family on its regenerated Hilbert space are calculated in advance to form a function dictionary.

3. The solid-state laser radar loop detection method based on point cloud reference markers according to claim 1, characterized in that: The specific process of step 2 is as follows: The input point cloud is overlaid based on the posture of the front-end odometer to improve the density of the point cloud, and the distance of the point cloud from the radar origin and the cosine value of the incident angle are calculated. The point cloud intensity value is compensated by the intensity compensation formula, which is beneficial to improve the success rate of extracting point cloud benchmark markers under different viewing angles.

4. The solid-state laser radar loop detection method based on point cloud reference markers according to claim 3 is characterized in that: The intensity compensation formula is as follows: In the above formula, , They are the original intensity value of the point cloud and the intensity value after compensation. is the coefficient factor, is the Euclidean distance from the point cloud to the radar origin, is the cosine of the incident angle of the point cloud.

5. The solid-state laser radar loop detection method based on point cloud reference markers according to claim 1, characterized in that: The specific process of step five is as follows: The latest ID passed in is searched for the same ID in the database. After successfully retrieving the same ID, the relative pose of the two frames of point clouds is calculated using the saved corresponding poses, and the loop detection factor is constructed and added to the backend graph optimization to reduce the cumulative error during map construction.

Images