Laser SLAM loop closure detection method for unstructured orchards

Abstract

This invention provides a laser SLAM loop closure detection method for unstructured orchards, comprising: acquiring point cloud data of a structured orchard and constructing a discriminative global representation of spatial binary patterns; calculating the column contribution of the spatial binary patterns and a low-dimensional attention score map with respect to row factors; constructing a KDTree based on the attention score map; searching for candidate loop closure frames and determining whether the overlap rate of the spatial binary patterns of the candidate frames and the current frame meets a threshold condition; if so, the loop closure detection is successful; otherwise, the loop closure detection fails. Experimental results using a harvesting robot in an actual orchard demonstrate the effectiveness of this method. Furthermore, experiments on the common outdoor dataset KITTI further prove the generalization ability of this method.

Description

Technical Field

[0001] This invention relates to the field of loop closure detection technology, and in particular to a laser SLAM loop closure detection method for unstructured orchards. Background Technology

[0002] In unstructured orchard environments, robots can simultaneously locate themselves and build maps based on radar point cloud data—a core challenge in agricultural robotics, known as laser SLAM (Simultaneous Localization and Mapping). In laser SLAM, the pose of the current frame is calculated from the pose constraints of the previous frame; as time progresses, pose errors accumulate, leading to trajectory drift.

[0003] Incorporating loop closure detection into the SLAM process enables real-time determination of whether the robot has returned to previously visited positions during mapping, establishing pose constraints and reducing localization and mapping discrepancies. The essence of loop closure detection is to determine the similarity registration between two point cloud frames. Currently, three main methods are used for loop closure detection: 1. Using GPS location fusion, which is unsuitable for orchard scenarios with dense canopies that may result in no GPS or inaccurate GPS positioning; 2. Directly using arbitrary keyframe matching, which suffers from high computational cost and low efficiency as the scale of orchard scenarios increases; 3. Extracting histogram features from the point cloud for point cloud registration, which is not only computationally intensive but also limited in highly similar orchard scenarios. Orchards possess unstructured characteristics due to leaf movement, long-term weather changes, and growth variations; therefore, loop closure detection in orchards needs to be invariant to these unstructured characteristics. Summary of the Invention

[0004] The purpose of this invention is to address the shortcomings of existing technologies by proposing a laser SLAM loop closure detection method for unstructured orchards, which can perform laser SLAM loop closure detection in unstructured orchard environments.

[0005] The present invention is achieved through the following technical solution.

[0006] The technical solution of this application is a laser SLAM loop closure detection method for unstructured orchards, which includes the following steps: acquiring point cloud data of the structured orchard; segmenting the current frame point cloud along the tree height direction into N... r ×N s ×N h Each unit block is further projected onto N. r ×N sA matrix is ​​used to construct a discriminative Spatial Binary Pattern (SBP) global representation. The contribution of internal column indices is calculated on the SBP global representation to obtain a low-dimensional attention score map regarding row factors. The two-stage loop closure detection process is as follows: First, a KDTree (K-Dimensional Tree) is constructed from the attention score map to quickly find the k nearest neighbor candidate loop closure frames. Second, the candidate frames are column-shifted, and the SBP overlap rate between all candidate frames and the current frame is calculated. The candidate frame with the largest overlap rate with the current frame is selected as the loop closure target frame. If the maximum overlap rate is greater than a threshold, the loop closure detection is successful, and loop closure correction is performed; otherwise, the loop closure detection fails, and the process proceeds to the next frame for loop closure detection.

[0007] The loop closure detection method of this technical solution can be extended to loop closure detection in conventional outdoor environments, can meet basic accuracy requirements, and has generalizability.

[0008] The laser SLAM loop closure detection method for unstructured orchards in this technical solution specifically includes the following steps:

[0009] Step 11: During its movement, the robot acquires point cloud data of the orchard using the RoboSense 16-line LiDAR.

[0010] Step 12: Using the radar as the origin, plot the point cloud p of the current scan frame. k =(x k ,y k ,z k Divide along the height of the fruit tree to N r ×N s ×N h A unit block, the point cloud is represented in polar coordinates as Γ k =(r k ,θ k ,h k ), h k =z k +p h .

[0011] Where r k It is the scanning radius of the point cloud, θ k It is the point cloud azimuth, h k It is the actual height of the point cloud, determined by the scan height value z. k With platform installation height p h The decision is made jointly. At the same time, the cell block to which the point cloud belongs is also limited by the maximum effective scanning radius R of the radar scan. max Maximum effective height H max And the scanning azimuth angle 2π:

[0012]

[0013] where i∈[1,N] r ],j∈[1,N s ],ξ∈[1,N h ], each unit block P ijξ It can contain multiple point clouds with the same i, j, ξ indices:

[0014]

[0015] Step 13: Project the eight-bit binary unit block data onto N r ×N s A matrix is ​​used to construct a distinctive global representation of the Spatial Binary Pattern (SBP).

[0016]

[0017] Here, δ(P) is a binary value return function, which returns 1 if a point cloud exists within the cell block, and 0 otherwise. The values ​​within the SBP matrix elements are encoded by eight bits, ranging from 0 to 255, revealing the density of the point cloud within the region of interest and the geometric information of the point cloud in the vertical direction.

[0018] Step 21: Calculate the proportion of SBP column indicators in the row factors. The formula is as follows:

[0019]

[0020] Where SBP(i,j) norm It is the pixel-by-pixel normalization of SBP.

[0021] Step 22: Information entropy borrows the concept of entropy from thermodynamics to describe the amount of information in an event. A higher entropy value indicates a more chaotic system (i.e., carrying less information), while a lower entropy value indicates a more ordered system (i.e., carrying more information). The formula for calculating the information entropy of the SBP column index is:

[0022]

[0023] in, It is a constant. The SBP column index information entropy intuitively expresses the point cloud information content at the current radar scan position at column index j and azimuth angle j*60.

[0024] Step 23: Calculate the contribution of the SBP column indicators, represented as a weight matrix:

[0025]

[0026] Step 24: Calculate the low-dimensional attention score map about the internal row factors of SBP from the column index weight matrix. The calculation formula is as follows:

[0027]

[0028] Where S is the attention score map, which is an N r A low-dimensional vector representation of 1×1 dimension.

[0029] Step 31: Construct a KDTree from the attention score map and quickly find the k nearest neighbor candidate loop closure frames from it.

[0030] Step 32: Calculate the SBP overlap rate between the candidate frame and the current frame point cloud.

[0031]

[0032] Among them, SBP q and These represent the spatial binary patterns of the point cloud of the current query frame and the point cloud of the candidate frame shifted by column j, respectively. ε represents the valid threshold for the difference between the two matrices; Φ{a} = 1 if a is true, otherwise 0. valid(SBP) counts the number of non-zero elements in the SBP matrix.

[0033] Step 33: When the candidate frame is shifted n columns, obtain the maximum SBP overlap rate with the current frame, and use it as the final overlap rate value between the candidate frame and the query frame.

[0034]

[0035] Step 34: Determine the candidate frame with the largest overlap with the current frame as the loop closure target frame.

[0036]

[0037] Among them, c k It is a series of indices about candidate frames, c * The candidate frame with the highest overlap with the current frame is used as the loop closure target frame. If the loop closure detection is successful, loop closure correction will be performed; otherwise, proceed to step 11 to proceed to the loop closure detection of the next frame.

[0038] The technical advantages of this invention are as follows: A single-frame radar scan is encoded into a Spatial Binary Pattern (SBP) based on the distribution of point clouds within vertical binary unit blocks. SBP statistically analyzes the distribution of trees and non-tree targets across the overall outline, distinguishes fruit trees from weeds based on tree height, and is unaffected by long-term weather changes and dynamic environments, exhibiting invariance to the unstructured characteristics of orchards. Furthermore, a low-dimensional attention score map is extracted from the SBP to reduce interference from irrelevant features for initial loop closure candidate frame retrieval. This method is less time-consuming than more direct keyframe matching methods and can meet the real-time requirements of robot operations even in large-scale orchards. Determining the final loop closure index from candidate loop closure frames based on overlap rate further ensures loop closure accuracy. Attached Figure Description

[0039] Figure 1 This is a flowchart of the laser SLAM loop closure detection method in the embodiments of this application.

[0040] Figure 2 This is a flowchart illustrating the extraction of the spatial binary pattern in an embodiment of this application.

[0041] Figure 3 This is a comparison diagram of the algorithm trajectory and the true trajectory in an unstructured large-scale orchard scene according to an embodiment of this application.

[0042] Figure 4 This is a comparison diagram of the algorithm trajectory and the true trajectory in the KITTI dataset 00 sequence of the embodiments of this application. Detailed Implementation

[0043] The technical solution of the present invention will be further described below with reference to the accompanying drawings and embodiments. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0044] like Figure 1 and Figure 2 As shown, this embodiment provides a loop closure detection method for laser SLAM in unstructured orchards, which includes radar data acquisition, encoding the orchard radar scan frames into a global scene descriptor (SBP), extracting an attention score map of the SBP for fast k-nearest neighbor candidate frame search, and calculating the overlap rate between the candidate frame and the current frame to ensure the correctness of loop closure detection.

[0045] like Figure 3As shown, this embodiment verifies the feasibility of the technical method of the present invention by implementing the scheme described in the embodiment in an actual unstructured orchard. The figure shows the comparison results of the entire route trajectory of the trajectory (solid line) constructed using this method and the true trajectory value (dashed line) of the sequence. The trajectory comparison figure fully illustrates the rationality and effectiveness of this method, which includes the following implementation steps.

[0046] Step 11: During its movement, the robot acquires point cloud data of the orchard using the RoboSense 16-line LiDAR.

[0047] Step 12: Using the radar as the origin, plot the current frame point cloud p k =(x k ,y k ,z k The point cloud is divided into 20×60×8 unit blocks along the height of the fruit tree. The point cloud is represented in polar coordinates as Γ. k =(r k ,θ k ,h k ), h k =z k +p h .

[0048] Where r k It is the scanning radius of the point cloud, θ k It is the point cloud azimuth, h k It is the actual height of the point cloud, determined by the scan height value z. k With platform installation height p h =1.3 jointly determined. The effective radar scanning range is set to R. max =80m, H max =8m, the azimuth angle of a full radar scan is 2π. Each cell has a resolution of 4×6×1, and each cell can contain multiple point clouds with the same index.

[0049]

[0050] Where i∈[1,20], j∈[1,60], ξ∈[1,8].

[0051] Step 13: Project the eight-bit binary unit block data along the height direction onto a 20×60 matrix to construct a distinctive Spatial Binary Pattern (SBP) global representation.

[0052]

[0053] Here, δ(P) is a binary value return function; it returns 1 if a point cloud exists within the cell block, and 0 otherwise. The values ​​within the SBP matrix elements are encoded using eight bits, ranging from 0 to 255, representing the density of the point cloud within the region of interest and the geometric information in the vertical direction of the point cloud. For example, when the eight-bit binary encoding is [00110011], the corresponding matrix element value is 51.

[0054] Step 21: Calculate the proportion ρ(i,j) of the SBP column index in the row factor, expressed by the following formula:

[0055]

[0056] Where SBP(i,j) norm This is the pixel-by-pixel normalized result of SBP.

[0057] Step 22: Information entropy borrows the concept of entropy from thermodynamics to describe the amount of information in an event. A higher entropy value indicates a more chaotic system (i.e., carrying less information), while a lower entropy value indicates a more ordered system (i.e., carrying more information). The formula for calculating the information entropy of the SBP column index is:

[0058]

[0059] Where K = 0.22 is a constant, and the SBP column index information entropy intuitively expresses the point cloud information content at the current radar scan position at column index j and azimuth angle j*60.

[0060] Step 23: Calculate the contribution of the SBP column indicators, represented as a weight matrix:

[0061]

[0062] Step 24: Calculate the low-dimensional attention score map about the internal row factors of SBP from the column index weight matrix. The calculation formula is as follows:

[0063]

[0064] Where S is the attention score map, which is a 20×1 dimensional compressed vector representation.

[0065] Step 31: Construct a KDTree from the attention score map and quickly find 50 nearest neighbor candidate loop closure frames from it.

[0066] Step 32: Calculate the SBP overlap rate between the candidate frame and the current frame point cloud.

[0067]

[0068] Among them, SBP q and These represent the spatial binary patterns of the point cloud of the current query frame and the point cloud of the candidate frame shifted by column j, respectively. The effective threshold for the difference between the two matrices is ε = 200. When the value of a is true, Φ{a} = 1; otherwise, it is 0. valid(SBP) counts the number of non-zero elements in the SBP matrix.

[0069] Step 33: When the candidate frame is shifted n columns, obtain the maximum SBP overlap rate with the current frame, and use it as the final overlap rate value between the candidate frame and the query frame.

[0070]

[0071] Step 34: Determine the candidate frame with the largest overlap with the current frame as the loop closure target frame.

[0072]

[0073] Where, if c k It is a series of indices about candidate frames, c * The candidate frame with the highest overlap with the current frame is used as the loop closure target frame. If the loop closure detection is successful, loop closure correction will be performed; otherwise, proceed to step 11 to proceed to the loop closure detection of the next frame.

[0074] like Figure 4 As shown, in this embodiment, to verify the feasibility of the present invention in a conventional outdoor environment, the 00 sequence with a relatively large number of loop closures in the KITTI dataset is selected to implement the scheme described in the embodiment. The figure shows the comparison results between the trajectory constructed using this method and the ground truth trajectory of the KITTI 00 sequence. The trajectory comparison figure shows that, within the allowable error range, this method can be used for conventional outdoor loop closure detection, which fully demonstrates the generalization of the method in this paper.

[0075] The above description is merely an embodiment of the present invention and does not limit the patent scope of the present invention. Any equivalent structural or procedural transformations made based on the content of the present invention specification, or direct or indirect applications in other related technical fields, are similarly included within the patent protection scope of the present invention.

Claims

1. A laser SLAM loop closure detection method for unstructured orchards, characterized in that, The steps include the following: First, point cloud data of the structured orchard is acquired, and a distinctive spatial binary pattern global representation is constructed. This process includes the following steps: Step 11: During its movement, the robot acquires point cloud data of the orchard using lidar; Step 12: Using the radar as the origin, plot the point cloud p of the current scan frame. k =(x k ,y k ,z k Divide along the height of the fruit tree to N r ×N s ×N h A unit block, the point cloud is represented in polar coordinates as Γ k =(r k ,θ k ,h k ), h k =z k +p h , Where r k It is the scanning radius of the point cloud, θ k It is the point cloud azimuth, h k It is the actual height of the point cloud, determined by the scan height value z. k With platform installation height p h The decision is made jointly, and the cell block to which the point cloud belongs is also limited by the maximum effective scanning radius R of the radar scan. max Maximum effective height H max And the scanning azimuth angle 2π: where i∈[1,N] r ],j∈[1,N s ],ξ∈[1,N h ], each unit block P ijξ It can contain multiple point clouds with the same i, j, ξ indices: Step 13: Project the eight-bit binary unit block data onto N r ×N s A matrix is ​​used to construct a distinctive spatial binary pattern SBP global representation. Wherein, δ(P) is a binary value return function. It returns 1 when there is a point cloud in the cell block, and 0 otherwise. The values ​​in the SBP matrix elements are encoded by eight bits and the value range is 0-255. Secondly, the column index contribution of the spatial binary pattern and the low-dimensional attention score map with respect to the row factor are calculated, which specifically includes the following steps: Step 21: Calculate the proportion of SBP column indicators in the row factors. The formula is as follows: Where SBP(i,j) norm It is the pixel-wise normalization of SBP; Step 22: Information entropy borrows the concept of entropy from thermodynamics to describe the amount of information in an event. A higher entropy value indicates a more chaotic system, while a lower entropy value indicates a more ordered system. The formula for calculating the information entropy of the SBP column index is: in, It is a constant. The SBP column index information entropy intuitively expresses the point cloud information content at the current radar scan position at column index j and azimuth angle j*60. Step 23: Calculate the contribution of the SBP column indicators, represented as a weight matrix: Step 24: Calculate the low-dimensional attention score map about the internal row factors of SBP from the column index weight matrix. The calculation formula is as follows: Where S is the attention score map, which is an N r A low-dimensional vector representation of 1×1 dimension; Finally, a KDTree is constructed based on the attention score map. Candidate loop closure frames are searched, and it is determined whether the spatial binary pattern overlap rate between the candidate loop closure frame and the current frame meets the threshold condition. If it does, the loop closure detection is successful; otherwise, the loop closure detection fails. The specific steps include the following: Step 31: Construct a KDTree from the attention score map and quickly find the k nearest neighbor candidate loop closure frames from it; Step 32: Calculate the SBP overlap rate between the candidate frame and the current frame point cloud; Among them, SBP q and represents the spatial binary pattern of the current query frame point cloud and the candidate frame point cloud shifted by column j, respectively. ε represents the effective threshold of the difference between the two matrices. Φ{a} = 1 when the value of a is true, otherwise it is 0. valid(SBP) counts the number of non-zero elements in the SBP matrix. Step 33: When the candidate frame is shifted n columns, obtain the maximum SBP overlap rate with the current frame, and use it as the final overlap rate value between the candidate frame and the query frame; Step 34: Determine the candidate frame with the highest overlap rate with the current frame as the loop closure target frame; Among them, c k It is a series of indices about candidate frames, c * The candidate frame with the highest overlap with the current frame is used as the loop closure target frame, when O(SBP) q SBP c* If the value is greater than or equal to 0.7, the loop closure detection is successful and loop closure correction is performed; otherwise, proceed to step 11 to enter the loop closure detection of the next frame.

Images