Loop closure detection method and device based on laser point cloud intensity information conversion view

Abstract

The present invention provides a loop closure detection method and device based on view conversion using laser point cloud intensity information. The method comprises steps 1 to 5. This invention improves the accuracy and robustness of robot loop closure detection, making the detection process more stable and rotationally invariant, and enhancing mapping results.

Description

Technical Field

[0001] The embodiments of the present invention relate to the technical field of mapping, positioning and navigation of robot platforms, and in particular to a loop detection method and device based on view conversion based on laser point cloud intensity information. Background Art

[0002] As an important module in the SLAM system, loop detection can identify places the robot has been to in order to reduce the cumulative error generated by the robot during the SLAM process. Correct loop detection can provide the SLAM system with effective pose constraints between non-adjacent frames, thereby helping the SLAM system to establish a globally consistent map. Incorrect loop detection will produce erroneous constraint information, affecting the construction of the global map. Therefore, providing accurate loop detection results is crucial to the robot's SLAM process. However, in current related technologies, due to the influence of lighting and the easy loss of detail descriptors in the environment, the detection accuracy still cannot meet actual needs. Therefore, the development of a loop detection method and device based on the conversion view of laser point cloud intensity information can effectively overcome the defects in the above-mentioned related technologies, which has become a technical problem that needs to be solved urgently in the industry. Summary of the Invention

[0003] In response to the above-mentioned problems existing in the prior art, an embodiment of the present invention provides a loop detection method and device based on view conversion of laser point cloud intensity information.

[0004] In the first aspect, an embodiment of the present invention provides a loop detection method based on a laser point cloud intensity information conversion view, comprising: Step 1: A main control machine controls the robot platform to move indoors and outdoors. When the laser radar scans different objects, it will return different intensity information due to the different material properties of the objects. The laser radar sensor collects three-dimensional point clouds containing intensity information in real time and transmits it to the main control machine. When the main control machine receives a frame of three-dimensional point cloud P, it uses the abnormal point removal mechanism in LOAM to remove points that may be blocked in the point cloud P and points parallel to the laser beam, and then projects it onto the intensity image I to obtain a point cloud intensity image; Step 2: The main control machine extracts ORB features from the point cloud intensity image, extracts FAST corner features through the grayscale values ​​of the candidate feature points, and then uses the BRIEF descriptor with rotation invariance to describe the detected feature points to obtain an ORB feature descriptor; Step 3: The main control machine uses DBoW to convert the ORB descriptor into a bag-of-words vector based on the visualization vocabulary proposed by ORB-SLAM, and uses this vector to represent the current frame point cloud. Each new point cloud of the frame is represented by a vector, which is used to construct the DBoW database. When a new frame of 3D point cloud is converted into a vector, the similarity between the new vector and the previous vector is solved to determine whether a loop occurs between the two frames of point cloud, and then the DBoW database is used to query the loop candidate frame; Step 4: The master machine matches the ORB descriptor of the loop candidate frame queried by the DBoW database with the current frame to ensure that the two frames have enough common features to eliminate false detection between the candidate loop frame queried by DBoW and the current frame; Step 5: The master machine uses the PnP algorithm to find the 3D points corresponding to the matching feature points between the two frames, and uses the 3D points and 2D pixel points to construct the PnP problem. Since PnP is sensitive to false matches, in order to improve the stability of the PnP algorithm, random sampling consistent RANSAC is used to delete false matches. When the number of correct matches after random sampling consistent RANSAC deletion is greater than the set threshold, it is determined that the candidate frame queried by DBoW is a correct loop, thereby verifying the matching accuracy between the loop frame queried by DBoW and the current frame.

[0005] On the basis of the content of the above method embodiment, the loop detection method based on the laser point cloud intensity information conversion view provided in the embodiment of the present invention, when the laser radar in step 1 scans different objects, it will return different intensity information due to the different material properties of the objects. The intensity information corresponds to the intensity value, where the intensity value The calculation formula is:

[0006]

[0007] in, is the energy received from the reflected signal, is the emitted laser power, The physical formula is:

[0008]

[0009] in, is the emitted laser power, is the receiver aperture, is the system emission factor, is the atmospheric emission factor, is the incident angle between the object surface and the laser beam, is the surface reflectivity of the object; measuring distance R and incident angle is an external parameter, and It is a commonly used parameter. The calculation formula of the intensity value I is:

[0010]

[0011] in, is a constant, so the surface reflectivity of the object Only with the incident angle and measure distance Regarding the angle of incidence It can be obtained by analyzing the local normal, for each point in the point cloud P , find the two points closest to it , , the calculation formula of the local surface normal n is:

[0012]

[0013] in, represents the cross product, so the angle of incidence It can be expressed as:

[0014]

[0015] In measuring distance and the angle of incidence When the values ​​are the same, objects with different reflectivity will get different intensity values.

[0016] On the basis of the contents of the above-mentioned method embodiments, the loop detection method based on the laser point cloud intensity information conversion view provided in the embodiment of the present invention adopts the abnormal point removal mechanism in LOAM in step 1 to remove the obscured points in the point cloud P and the points parallel to the laser beam, and project them onto the intensity image I; each valid point in P finds a corresponding pixel point in the intensity image I, and the pixel value is determined by the intensity value of the valid point in P; if the size of the intensity image I is m rows and n columns, where m rows correspond to the beam of the laser radar, and n columns correspond to the number of points on each laser beam of the laser radar; in order to ensure the invariance of the environmental structure during projection, the number of rows and columns in the intensity image I are corresponded according to the beam information and horizontal angle information of each point of the laser radar, where the beam information of the valid point is obtained by radar driving, and the horizontal angle is The calculation formula is:

[0017]

[0018] Among them, x and y represent the X and Y axis coordinates of the effective point in the laser radar coordinate system. According to the horizontal angle The value range is [-180°, 180°], so that the horizontal angle The calculation formula for the value range of [0°, n°] is:

[0019]

[0020]

[0021] in, It represents the horizontal angular resolution on the same radar scanning line, n is the number of points on each laser line of the lidar, is the horizontal angle after conversion, and its value range is [0°, n°]. After conversion, each valid point in the point cloud P finds a corresponding pixel in the intensity image I. When there is no corresponding valid point assignment in the intensity image I, it is assigned a zero value.

[0022] Based on the content of the above method embodiment, the loop detection method based on the laser point cloud intensity information conversion view provided in the embodiment of the present invention, in step 2, the FAST corner feature is extracted by the gray value of the candidate feature point The calculation formula is:

[0023]

[0024] in, is the gray value of the candidate feature point, is the grayscale of any point on the circle with the candidate feature point as the center and the radius r is the grayscale difference threshold, if the FAST corner feature If it is greater than a given threshold, it is determined that p is a FAST feature point;

[0025] In step 2, the detected feature points are described using the BRIEF descriptor with rotation invariance. BRIEF is a binary descriptor that defines the binary bits corresponding to each point. The calculation formula is:

[0026]

[0027] in, 、 It represents the grayscale of the p,q point pair, and randomly selects N point pairs from the FAST corner points. , combine the comparison results of these N point pairs as a descriptor , which can be expressed as:

[0028]

[0029] Taking rotation invariance into account, select N pairs of feature points and obtain a 2×N description matrix:

[0030]

[0031] After rotation angle , and get the new description matrix, expressed as:

[0032]

[0033] The size of the point pairs is compared at the new position to form a binary string descriptor, and finally the intensity image ORB feature is obtained.

[0034] Based on the content of the above method embodiment, the loop detection method based on the laser point cloud intensity information conversion view provided in the embodiment of the present invention uses DBoW to convert the ORB descriptor into a word bag vector in step 3, and uses the word bag vector to represent the current frame point cloud. Each new frame of point cloud is represented by a word bag vector, and the word bag vector is used to construct a database for DBoW. When the new frame of three-dimensional point cloud is converted into a vector, the similarity between the new vector and the previous vector is solved to determine whether a loop occurs between the two frames of point cloud. The similarity calculation formula is:

[0035]

[0036] in, N A randomly selected point pair from the FAST corner feature the number of 、 Represented as new vector and previous vector respectively, when and When the similarity is greater than the set threshold λ, it is considered and The corresponding two frames of point cloud are looped.

[0037] Based on the content of the above method embodiments, the loop detection method based on the laser point cloud intensity information conversion view provided in the embodiments of the present invention, in step 4, the ORB descriptor of the loop candidate frame queried by the DBoW database is matched with the current frame to ensure that the two frames have enough identical features to eliminate false detection between the candidate loop frame queried by the DBoW and the current frame; the angular scores of all descriptors in the current frame P1 are sorted in descending order, and the one with the largest angular score is represented as pi, where pi∈P1; for each descriptor in the current frame P1, the best match with can be found in the candidate loop frame q; the Hamming distance is introduced to calculate the distance between the two descriptors, the two descriptors are sorted in ascending order according to the Hamming distance of the matched descriptors, the calculated Hamming distance is compared with twice the minimum Hamming distance of all matches, and the matches less than twice the minimum Hamming distance are retained for verification.

[0038] Based on the content of the above method embodiment, the loop detection method based on the laser point cloud intensity information conversion view provided in the embodiment of the present invention, the main control machine in step 5 verifies the matching accuracy between the loop frame queried by DBoW and the current frame through the PnP algorithm and random sampling consistent RANSAC, and the PnP and random sampling consistent RANSAC screening steps include: randomly extracting 4 groups of data from the PnP point pairs, calculating the transformation matrix H, the size of the matrix is ​​3*3, recorded as model M; calculating the projection error between all PnP point pairs and model M, and if the error is less than a predetermined threshold, adding the internal point set ; If the current interior point set If the number of elements is greater than the optimal interior point set N_best, update N_best = , and update the number of iterations K at the same time; if the number of iterations is greater than K, exit; otherwise, add 1 to the number of iterations and repeat the above steps.

[0039] In the second aspect, an embodiment of the present invention provides a loop detection device based on a laser point cloud intensity information conversion view, including: a first main module, used to implement step 1: the main control machine controls the robot platform to move indoors and outdoors. When the laser radar scans different objects, it will return different intensity information due to the different material properties of the objects. The laser radar sensor collects three-dimensional point clouds containing intensity information in real time and transmits it to the main control machine. When the main control machine receives a frame of three-dimensional point cloud P, it uses the abnormal point removal mechanism in LOAM to remove points that may be blocked in the point cloud P and points parallel to the laser beam, and then projects it onto the intensity image I to obtain a point cloud intensity image; a second main module, used to implement step 2: the main control machine extracts ORB features from the point cloud intensity image, extracts FAST corner features through the grayscale values ​​of the candidate feature points, and then uses the BRIEF descriptor with rotation invariance to describe the detected feature points to obtain an ORB feature descriptor; a third main module, used to implement step 3: the main control machine uses DBoW to convert the ORB descriptor into a bag-of-words vector based on the visualization vocabulary proposed by ORB-SLAM, and uses this vector to represent the current frame point Cloud, each new frame of point cloud is represented by a vector, which is used to build a database for DBoW. When a new frame of 3D point cloud is converted into a vector, the similarity between the new vector and the previous vector is solved to determine whether a loop occurs between the two frames of point cloud, and then the DBoW database is used to query the loop candidate frame; the fourth main module is used to implement step 4: the main control machine matches the ORB descriptor of the loop candidate frame queried by the DBoW database with the current frame to ensure that the two frames have enough common features to eliminate the false detection between the candidate loop frame queried by the DBoW database and the current frame; the fifth main module Block, used to implement step 5: The master machine uses the PnP algorithm to find the 3D points corresponding to the feature points that match between the two frames, and uses the 3D points and 2D pixel points to construct the PnP problem. Since PnP is sensitive to false matches, in order to improve the stability of the PnP algorithm, random sampling consistent RANSAC is used to delete false matches. When the number of correct matches after random sampling consistent RANSAC deletion is greater than the set threshold, it is determined that the candidate frame queried by DBoW is a correct loop, thereby verifying the matching accuracy between the loop frame queried by DBoW and the current frame.

[0040] In a third aspect, an embodiment of the present invention provides an electronic device, including:

[0041] at least one processor; and

[0042] at least one memory communicatively coupled to the processor, wherein:

[0043] The memory stores program instructions that can be executed by the processor, and the processor calls the program instructions to execute the loop detection method based on laser point cloud intensity information conversion view provided by any one of the various implementation methods of the first aspect.

[0044] In a fourth aspect, an embodiment of the present invention provides a non-transitory computer-readable storage medium, which stores computer instructions, and the computer instructions enable a computer to execute the loop detection method based on the laser point cloud intensity information conversion view provided by any one of the various implementation methods of the first aspect.

[0045] The loop detection method and device based on the laser point cloud intensity information conversion view provided in the embodiments of the present invention are rotationally invariant, and in a real environment, when the recall rate is 100%, the accuracy rate can reach 69.57%, thereby improving the accuracy and robustness of robot loop detection, making the detection process more stable and rotationally invariant, and enhancing the mapping effect. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, a brief introduction will be given below to the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0047] Figure 1 A flow chart of a loop closure detection method based on laser point cloud intensity information conversion view provided by an embodiment of the present invention;

[0048] Figure 2 A schematic diagram of the structure of a loop detection device based on laser point cloud intensity information conversion view provided by an embodiment of the present invention;

[0049] Figure 3 A schematic diagram of the physical structure of an electronic device provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0050] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention. In addition, the technical features in the various embodiments or single embodiments provided by the present invention can be arbitrarily combined with each other to form a feasible technical solution. This combination is not restricted by the sequence of steps and / or structural composition mode, but must be based on the ability of ordinary technicians in this field to implement it. When the combination of technical solutions is contradictory or cannot be implemented, it should be deemed that such a combination of technical solutions does not exist and is not within the scope of protection required by the present invention.

[0051] The embodiment of the present invention provides a loop detection method based on the laser point cloud intensity information conversion view, see Figure 1The method includes: step 1: the main control machine controls the robot platform to move indoors and outdoors. When the laser radar scans different objects, it will return different intensity information due to the different material properties of the objects. The laser radar sensor collects the three-dimensional point cloud containing intensity information in real time and transmits it to the main control machine. When the main control machine receives a frame of three-dimensional point cloud P, the abnormal point removal mechanism in LOAM is used to remove the points that may be blocked in the point cloud P and the points parallel to the laser beam, and then project it onto the intensity image I to obtain a point cloud intensity image; step 2: the main control machine extracts ORB features from the point cloud intensity image, extracts FAST corner features through the grayscale values ​​of the candidate feature points, and then uses the BRIEF descriptor with rotation invariance to describe the detected feature points to obtain an ORB feature descriptor; step 3: the main control machine uses DBoW to convert the ORB descriptor into a bag-of-words vector based on the visualization vocabulary proposed by ORB-SLAM, and uses this vector to represent the current frame point cloud. Each new frame of point cloud is represented by a vector, which is used to DBoW builds a database. When a new frame of 3D point cloud is converted into a vector, the similarity between the new vector and the previous vector is solved to determine whether a loop occurs between the two frames of point cloud. The DBoW database is then used to query the loop candidate frame. Step 4: The master machine matches the ORB descriptor of the loop candidate frame queried by the DBoW database with the current frame to ensure that the two frames have enough identical features to eliminate false detections between the candidate loop frame queried by DBoW and the current frame. Step 5: The master machine uses the PnP algorithm to find the 3D points corresponding to the matching feature points between the two frames, and uses the 3D points and 2D pixel points to construct the PnP problem. Since PnP is sensitive to false matches, in order to improve the stability of the PnP algorithm, random sampling consistent RANSAC is used to delete false matches. When the number of correct matches after random sampling consistent RANSAC deletion is greater than the set threshold, it is determined that the candidate frame queried by DBoW is a correct loop, thereby verifying the matching accuracy between the loop frame queried by DBoW and the current frame.

[0052] The robot platform consists of a chassis, a main control unit, a lidar sensor, and an inertial measurement unit (IMU). The chassis is equipped with the main control unit, lidar sensor, and IMU, which are connected to the chassis, lidar sensor, and IMU via wired connections. The main control unit uses an Intel NUC8i7 HVK; the robot platform chassis uses a SCOUT MINI chassis manufactured by AGILE X. The lidar sensor is the stable Velodyne 16-line lidar; and the IMU is an MTi-300 IMU.

[0053] Based on the content of the above method embodiment, as an optional embodiment, the loop detection method based on the laser point cloud intensity information conversion view provided in the embodiment of the present invention, when the laser radar in step 1 scans different objects, it will return different intensity information due to the different material properties of the objects. The intensity information corresponds to the intensity value, where the intensity value The calculation formula is:

[0054]

[0055] in, is the energy received from the reflected signal, is the emitted laser power, The physical formula is:

[0056]

[0057] in, is the emitted laser power, is the receiver aperture, is the system emission factor, is the atmospheric emission factor, is the incident angle between the object surface and the laser beam, is the surface reflectivity of the object; measuring distance R and incident angle is an external parameter, and It is a commonly used parameter. The calculation formula of the intensity value I is:

[0058]

[0059] in, is a constant, so the surface reflectivity of the object Only with the incident angle and measure distance Regarding the angle of incidence It can be obtained by analyzing the local normal, for each point in the point cloud P , find the two points closest to it , , the calculation formula of the local surface normal n is:

[0060]

[0061] in, represents the cross product, so the angle of incidence It can be expressed as:

[0062]

[0063] In measuring distance and the angle of incidence When the values ​​are the same, objects with different reflectivity will get different intensity values.

[0064] Based on the content of the above method embodiment, as an optional embodiment, the loop detection method based on the laser point cloud intensity information conversion view provided in the embodiment of the present invention adopts the abnormal point removal mechanism in LOAM in step 1 to remove the obscured points in the point cloud P and the points parallel to the laser beam, and project them onto the intensity image I; each valid point in P finds a corresponding pixel point in the intensity image I, and the pixel value is determined by the intensity value of the valid point in P; if the size of the intensity image I is m rows and n columns, where m rows correspond to the beam of the laser radar, and n columns correspond to the number of points on each laser beam of the laser radar; in order to ensure the invariance of the environmental structure during projection, the number of rows and columns in the intensity image I are corresponding to the beam information and horizontal angle information of each point of the laser radar, where the beam information of the valid point is obtained by radar driving, and the horizontal angle is The calculation formula is:

[0065]

[0066] Among them, x and y represent the X and Y axis coordinates of the effective point in the laser radar coordinate system. According to the horizontal angle The value range is [-180°, 180°], so that the horizontal angle The calculation formula for the value range of [0°, n°] is:

[0067]

[0068]

[0069] in, It represents the horizontal angular resolution on the same radar scanning line, n is the number of points on each laser line of the lidar, is the horizontal angle after conversion, and its value range is [0°, n°]. After conversion, each valid point in the point cloud P finds a corresponding pixel in the intensity image I. When there is no corresponding valid point assignment in the intensity image I, it is assigned a zero value.

[0070] Based on the content of the above method embodiment, as an optional embodiment, the loop detection method based on the laser point cloud intensity information conversion view provided in the embodiment of the present invention, in step 2, the FAST corner point features are extracted by the gray value of the candidate feature point The calculation formula is:

[0071] in, is the gray value of the candidate feature point, is the grayscale of any point on the circle with the candidate feature point as the center and the radius r is the grayscale difference threshold, if the FAST corner feature If it is greater than a given threshold, it is determined that p is a FAST feature point;

[0072] In step 2, the detected feature points are described using the BRIEF descriptor with rotation invariance. BRIEF is a binary descriptor that defines the binary bits corresponding to each point. The calculation formula is:

[0073]

[0074] in, 、 It represents the grayscale of the p,q point pair, and randomly selects N point pairs from the FAST corner points. , combine the comparison results of these N point pairs as a descriptor , which can be expressed as:

[0075]

[0076] Taking rotation invariance into account, select N pairs of feature points and obtain a 2×N description matrix:

[0077]

[0078] After rotation angle , and get the new description matrix, expressed as:

[0079]

[0080] The size of the point pairs is compared at the new position to form a binary string descriptor, and finally the intensity image ORB feature is obtained.

[0081] Based on the content of the above method embodiment, as an optional embodiment, the loop detection method based on the laser point cloud intensity information conversion view provided in the embodiment of the present invention, in step 3, DBoW is used to convert the ORB descriptor into a word bag vector, and the word bag vector is used to represent the current frame point cloud. Each new frame of point cloud is represented by a word bag vector, and the word bag vector is used to construct a database for DBoW; when a new frame of three-dimensional point cloud is converted into a vector, the similarity between the new vector and the previous vector is solved to determine whether a loop occurs between the two frames of point cloud. The similarity calculation formula is:

[0082]

[0083] in, N A randomly selected point pair from the FAST corner feature the number of 、 Represented as new vector and previous vector respectively, when and When the similarity is greater than the set threshold λ, it is considered and The corresponding two frames of point cloud are looped.

[0084] Based on the content of the above method embodiment, as an optional embodiment, the loop detection method based on the laser point cloud intensity information conversion view provided in the embodiment of the present invention, in step 4, the ORB descriptor of the loop candidate frame queried by the DBoW database is matched with the current frame to ensure that the two frames have enough identical features to eliminate the false detection between the candidate loop frame queried by the DBoW and the current frame; the angular scores of all descriptors in the current frame P1 are sorted in descending order, and the one with the largest angular score is represented as pi, where pi∈P1; for each descriptor in the current frame P1, the best match with can be found in the candidate loop frame q; the Hamming distance is introduced to calculate the distance between the two descriptors, the two descriptors are sorted in ascending order according to the Hamming distance of the matched descriptors, the calculated Hamming distance is compared with twice the minimum Hamming distance of all matches, and the matches less than twice the minimum Hamming distance are retained for verification.

[0085] Based on the content of the above method embodiment, as an optional embodiment, the loop detection method based on the laser point cloud intensity information conversion view provided in the embodiment of the present invention, the main control machine in step 5 verifies the matching accuracy between the loop frame queried by DBoW and the current frame through the PnP algorithm and random sampling consistent RANSAC, and the PnP and random sampling consistent RANSAC screening steps include: randomly extracting 4 groups of data from the PnP point pairs, calculating the transformation matrix H, the size of the matrix is ​​3*3, recorded as model M; calculating the projection error between all PnP point pairs and model M, and if the error is less than a predetermined threshold, adding the internal point set ; If the current interior point set If the number of elements is greater than the optimal interior point set N_best, update N_best = , and update the number of iterations K at the same time; if the number of iterations is greater than K, exit; otherwise, add 1 to the number of iterations and repeat the above steps.

[0086] The loop detection method based on the laser point cloud intensity information conversion view provided in the embodiment of the present invention is rotationally invariant. In a real environment, when the recall rate is 100%, the accuracy rate can reach 69.57%, which improves the accuracy and robustness of robot loop detection, makes the detection process more stable and rotationally invariant, and enhances the mapping effect.

[0087] The implementation basis of each embodiment of the present invention is to implement programmed processing through a device with processor functions. Therefore, in engineering practice, the technical solutions and functions of each embodiment of the present invention can be encapsulated into various modules. Based on this reality, on the basis of the above embodiments, an embodiment of the present invention provides a loop detection device based on the laser point cloud intensity information conversion view, which is used to execute the loop detection method based on the laser point cloud intensity information conversion view in the above method embodiment. Figure 2 The device includes: a first main module, which is used to implement step 1: the main control machine controls the robot platform to move indoors and outdoors. When the laser radar scans different objects, it will return different intensity information due to the different material properties of the objects. The laser radar sensor collects three-dimensional point clouds containing intensity information in real time and transmits it to the main control machine. When the main control machine receives a frame of three-dimensional point cloud P, it uses the abnormal point removal mechanism in LOAM to remove points that may be blocked in the point cloud P and points parallel to the laser beam, and then projects it onto the intensity image I to obtain a point cloud intensity image; the second main module is used to implement step 2: the main control machine extracts ORB features from the point cloud intensity image, extracts FAST corner features through the grayscale values ​​of the candidate feature points, and then uses the BRIEF descriptor with rotation invariance to describe the detected feature points to obtain an ORB feature descriptor; the third main module is used to implement step 3: the main control machine uses DBoW to convert the ORB descriptor into a bag-of-words vector based on the visualization vocabulary proposed by ORB-SLAM, and uses this vector to represent the current frame point cloud. Each new frame of point cloud is represented by a vector. This vector is used to construct the DBoW database. When a new frame of 3D point cloud is converted into a vector, the similarity between the new vector and the previous vector is solved to determine whether a loop occurs between the two frames of point cloud. The DBoW database is then used to query the loop candidate frame. The fourth main module is used to implement step 4: the main control machine matches the ORB descriptor of the loop candidate frame queried by the DBoW database with the current frame to ensure that the two frames have enough common features to eliminate false detection between the candidate loop frame queried by the DBoW and the current frame. The fifth main module is used to implement step 5: the main control machine uses the PnP algorithm to find the 3D points corresponding to the matching feature points between the two frames, and uses the 3D points and 2D pixel points to construct the PnP problem. Since PnP is sensitive to false matches, in order to improve the stability of the PnP algorithm, random sampling consistent RANSAC is used to delete false matches. When the number of correct matches after random sampling consistent RANSAC deletion is greater than the set threshold, it is determined that the candidate frame queried by DBoW is a correct loop, thereby verifying the matching accuracy between the loop frame queried by DBoW and the current frame.

[0088] The loop detection device based on the laser point cloud intensity information conversion view provided by the embodiment of the present invention adopts Figure 2Several modules in the robot are rotation invariant, and in real environments, when the recall rate is 100%, the accuracy rate can reach 69.57%, which improves the accuracy and robustness of robot loop detection, makes the detection process more stable and rotation invariant, and enhances the mapping effect.

[0089] It should be noted that the device in the device embodiment provided by the present invention can be used to implement the method in the above-mentioned method embodiment as well as the method in other method embodiments provided by the present invention. The only difference is that the corresponding functional modules are set. The principle is basically the same as the principle of the above-mentioned device embodiment provided by the present invention. As long as those skilled in the art refer to the specific technical solutions in other method embodiments on the basis of the above-mentioned device embodiment, obtain the corresponding technical means and the technical solutions composed of these technical means by combining technical features, and ensure the practicality of the technical solutions, they can improve the device in the above-mentioned device embodiment to obtain the corresponding device class embodiment, thereby obtaining the corresponding device class embodiment for implementing the methods in other method class embodiments. For example:

[0090] Based on the content of the above device embodiment, as an optional embodiment, the loop detection device based on the laser point cloud intensity information conversion view provided in the embodiment of the present invention further includes: a first submodule for realizing that when the laser radar in step 1 scans different objects, different intensity information will be returned due to different material properties of the objects, and the intensity information corresponds to the intensity value, where the intensity value The calculation formula is:

[0091]

[0092] in, is the energy received from the reflected signal, is the emitted laser power, The physical formula is:

[0093]

[0094] in, is the emitted laser power, is the receiver aperture, is the system emission factor, is the atmospheric emission factor, is the incident angle between the object surface and the laser beam, is the surface reflectivity of the object; measuring distance R and incident angle is an external parameter, and It is a commonly used parameter. The calculation formula of the intensity value I is:

[0095]

[0096] in, is a constant, so the surface reflectivity of the object Only with the incident angle and measure distance Regarding the angle of incidence It can be obtained by analyzing the local normal, for each point in the point cloud P , find the two points closest to it , , the calculation formula of the local surface normal n is:

[0097]

[0098] in, represents the cross product, so the angle of incidence It can be expressed as:

[0099]

[0100] In measuring distance and the angle of incidence When the values ​​are the same, objects with different reflectivity will get different intensity values.

[0101] Based on the content of the above-mentioned device embodiment, as an optional embodiment, the loop detection device based on the laser point cloud intensity information conversion view provided in the embodiment of the present invention further includes: a second submodule, used to implement the abnormal point removal mechanism in LOAM in step 1, remove the obscured points in the point cloud P, and the points parallel to the laser beam, and project them onto the intensity image I; each valid point in P finds a corresponding pixel point in the intensity image I, and the pixel value is determined by the intensity value of the valid point in P; if the size of the intensity image I is m rows and n columns, where m rows correspond to the beam of the laser radar, and n columns correspond to the number of points on each laser beam of the laser radar; in order to ensure the invariance of the environmental structure during projection, the number of rows and columns in the intensity image I are corresponding to the beam information and horizontal angle information of each point of the laser radar, where the beam information of the valid point is obtained by radar driving, and the horizontal angle is The calculation formula is:

[0102]

[0103] Among them, x and y represent the X and Y axis coordinates of the effective point in the laser radar coordinate system. According to the horizontal angle The value range is [-180°, 180°], so that the horizontal angle The calculation formula for the value range of [0°, n°] is:

[0104]

[0105]

[0106] in, It represents the horizontal angular resolution on the same radar scanning line, n is the number of points on each laser line of the lidar, is the horizontal angle after conversion, and its value range is [0°, n°]. After conversion, each valid point in the point cloud P finds a corresponding pixel in the intensity image I. When there is no corresponding valid point assignment in the intensity image I, it is assigned a zero value.

[0107] Based on the content of the above device embodiment, as an optional embodiment, the loop detection device based on the laser point cloud intensity information conversion view provided in the embodiment of the present invention further includes: a third submodule for implementing the extraction of FAST corner point features by the grayscale value of the candidate feature point in step 2 The calculation formula is:

[0108]

[0109] in, is the gray value of the candidate feature point, is the grayscale of any point on the circle with the candidate feature point as the center and the radius r is the grayscale difference threshold. If N is greater than the given threshold, p is determined to be a FAST feature point.

[0110] In step 2, the detected feature points are described using the BRIEF descriptor with rotation invariance. BRIEF is a binary descriptor that defines the binary bits corresponding to each point. The calculation formula is:

[0111]

[0112] in, 、 It represents the grayscale of the p,q point pair, and randomly selects N point pairs from the FAST corner points. , combine the comparison results of these N point pairs as a descriptor , which can be expressed as:

[0113]

[0114] Taking rotation invariance into account, select N pairs of feature points and obtain a 2×N description matrix:

[0115]

[0116] After rotation angle , and get the new description matrix, expressed as:

[0117]

[0118] The size of the point pairs is compared at the new position to form a binary string descriptor, and finally the intensity image ORB feature is obtained.

[0119] Based on the content of the above device embodiment, as an optional embodiment, the loop detection device based on the laser point cloud intensity information conversion view provided in the embodiment of the present invention further includes: a fourth submodule for implementing the use of DBoW to convert the ORB descriptor into a word bag vector as described in step 3, using the word bag vector to represent the current frame point cloud, and each new frame of point cloud is represented by a word bag vector, which is used to construct a database for DBoW; when a new frame of three-dimensional point cloud is converted into a vector, the similarity between the new vector and the previous vector is solved to determine whether a loop occurs between the two frames of point cloud. The similarity calculation formula is:

[0120]

[0121] in, N A randomly selected point pair from the FAST corner feature the number of 、 Represented as new vector and previous vector respectively, when and When the similarity is greater than the set threshold λ, it is considered and The corresponding two frames of point cloud are looped.

[0122] Based on the content of the above-mentioned device embodiment, as an optional embodiment, the loop detection device based on the laser point cloud intensity information conversion view provided in the embodiment of the present invention further includes: a fifth submodule, which is used to implement the ORB descriptor matching of the loop candidate frame queried by the DBoW database with the current frame in step 4, to ensure that the two frames have enough identical features to eliminate false detection between the candidate loop frame queried by the DBoW and the current frame; sorting the angular scores of all descriptors in the current frame P1 in descending order, and the one with the largest angular score is represented as pi, where pi∈P1; for each descriptor in the current frame P1, the best match with can be found in the candidate loop frame q; introducing the Hamming distance to calculate the distance between the two descriptors, sorting the two descriptors in ascending order according to the Hamming distance of the matched descriptors, comparing the calculated Hamming distance with twice the minimum Hamming distance among all matches, and retaining the matches less than twice the minimum Hamming distance for verification.

[0123] Based on the content of the above-mentioned device embodiment, as an optional embodiment, the loop detection device based on the laser point cloud intensity information conversion view provided in the embodiment of the present invention further includes: a sixth submodule, which is used to implement the main control machine in step 5 to verify the matching accuracy between the loop frame queried by DBoW and the current frame through the PnP algorithm and random sampling consistent RANSAC. The PnP and random sampling consistent RANSAC screening steps include: randomly extracting 4 groups of data from the PnP point pairs, calculating the transformation matrix H, the size of the matrix is ​​3*3, recorded as model M; calculating the projection error between all PnP point pairs and the model M, and if the error is less than a predetermined threshold, adding the internal point set ; If the current interior point set If the number of elements is greater than the optimal interior point set N_best, update N_best = , and update the number of iterations K at the same time; if the number of iterations is greater than K, exit; otherwise, add 1 to the number of iterations and repeat the above steps.

[0124] The method of the embodiment of the present invention is implemented by electronic devices, so it is necessary to introduce the relevant electronic devices. Based on this purpose, the embodiment of the present invention provides an electronic device, such as Figure 3 As shown, the electronic device includes: at least one processor, a communications interface, at least one memory, and a communications bus, wherein the at least one processor, the communications interface, and the at least one memory communicate with each other via the communications bus. The at least one processor can call logic instructions in the at least one memory to execute all or part of the steps of the methods provided in the aforementioned method embodiments.

[0125] In addition, the logic instructions in the at least one memory described above can be implemented as a software functional unit and, when sold or used as an independent product, can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the portion that contributes to the prior art, or a portion of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions for causing a computer device (which can be a personal computer, server, or network device, etc.) to execute all or part of the steps of the method described in each method embodiment of the present invention. The aforementioned storage medium includes various media that can store program code, such as a USB flash drive, a mobile hard drive, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk.

[0126] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate, and the components shown as units may or may not be physical units, i.e., they may be located in one location or distributed across multiple network units. Some or all of the modules may be selected based on actual needs to achieve the objectives of the present embodiment. Persons of ordinary skill in the art will be able to understand and implement the present invention without inventive effort.

[0127] Through the description of the above embodiments, those skilled in the art will clearly understand that each embodiment can be implemented using software plus a necessary general-purpose hardware platform, or can also be implemented using hardware. Based on this understanding, the essence of the above technical solution, or the portion that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, a magnetic disk, or an optical disk, and includes a number of instructions for causing a computer device (such as a personal computer, server, or network device) to execute the methods described in various embodiments or portions thereof.

[0128] The flowcharts and block diagrams in the accompanying drawings show the possible architectures, functions and operations of the systems, methods and computer program products according to multiple embodiments of the present invention. Based on this understanding, each box in the flowchart or block diagram can represent a module, program segment or part of the code, and the module, program segment or part of the code contains one or more executable instructions for implementing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in an order different from that marked in the accompanying drawings. For example, two consecutive boxes can actually be executed substantially in parallel, or sometimes in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram and / or flowchart, and the combination of boxes in the block diagram and / or flowchart, can be implemented with a dedicated hardware-based system that performs the specified function or action, or can be implemented with a combination of dedicated hardware and computer instructions.

[0129] It should be noted that the terms "comprise," "include," or any other variations thereof are intended to encompass non-exclusive inclusion, such that a process, method, article, or apparatus comprising a list of elements includes not only those elements but also other elements not explicitly listed, or elements inherent to such process, method, article, or apparatus. In the absence of further limitations, the elements defined by the phrase "comprise..." do not preclude the presence of additional identical elements in the process, method, article, or apparatus comprising the elements.

[0130] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the various embodiments of the present invention.

Claims

1. A loop detection method based on laser point cloud intensity information conversion view, characterized in that: The method includes the following steps: Step 1: The main control computer controls the robot platform to move indoors and outdoors. When the laser radar scans different objects, it returns different intensity information according to the different material properties of the objects. The laser radar sensor collects three-dimensional point clouds containing intensity information in real time and transmits it to the main control computer. When the main control computer receives a frame of three-dimensional point cloud P, it uses the abnormal point removal mechanism in LOAM to remove points that may be blocked in the point cloud P and points parallel to the laser beam. The points are then projected onto the intensity image I to obtain a point cloud intensity image. To ensure the invariance of the environmental structure during projection, the number of rows and columns in the intensity image I are matched according to the beam information and horizontal angle information of each point of the laser radar. Step 2: The main control computer extracts ORB features from the point cloud intensity image, extracts FAST corner features based on the grayscale values ​​of the candidate feature points, and then uses the BRIEF descriptor with rotation invariance to describe the detected feature points to obtain the ORB feature descriptor; Step 3: Based on the visualization vocabulary proposed by ORB-SLAM, the master computer uses DBoW to convert the ORB descriptor into a bag-of-words vector, and uses this vector to represent the current frame point cloud. Each new frame of point cloud is represented by a vector, which is used to build a database for DBoW. When a new frame of 3D point cloud is converted into a vector, the similarity between the new vector and the previous vector is calculated to determine whether a loop occurs between the two frames of point cloud. The DBoW database is then used to query the loop candidate frame. The similarity calculation formula is: in, N A randomly selected point pair from the FAST corner feature the number of 、 Represented as new vector and previous vector respectively, when and When the similarity is greater than the set threshold λ, it is considered and The corresponding two frames of point cloud are looped; Step 4: The master machine matches the ORB descriptors of the loop closure candidate frame queried by the DBoW database with the current frame to ensure that the two frames have enough common features to eliminate false detection between the candidate loop closure frame queried by DBoW and the current frame; sort the corner scores of all descriptors in the current frame P1 in descending order, and the one with the largest corner score is denoted as pi where pi∈P1; for each descriptor in the current frame P1, the best match with the current frame P1 can be found in the candidate loop closure frame q; Step 5: The master computer uses the PnP algorithm to find the 3D points corresponding to the matching feature points between the two frames, and uses the 3D points and 2D pixel points to construct the PnP problem. It uses random sampling consistent RANSAC to delete incorrect matches. When the number of correct matches after random sampling consistent RANSAC deletion is greater than the set threshold, it is determined that the candidate frame queried by DBoW is a correct loop, thereby verifying the matching accuracy between the loop frame queried by DBoW and the current frame.

2. The loop detection method based on laser point cloud intensity information conversion view according to claim 1 is characterized in that: When the laser radar in step 1 scans different objects, it will return different intensity information due to the different material properties of the objects. The intensity information corresponds to the intensity value, where the intensity value is The calculation formula is: in, is the energy received from the reflected signal, is the emitted laser power, The physical formula is: in, is the emitted laser power, is the receiver aperture, is the system emission factor, is the atmospheric emission factor, is the incident angle between the object surface and the laser beam, is the surface reflectivity of the object; measuring distance R and incident angle is an external parameter, and It is a commonly used parameter. The calculation formula of the intensity value I is: in, is a constant, so the surface reflectivity of the object Only with the incident angle and measure distance Regarding the angle of incidence Obtained by analyzing the local normal, for each point in the point cloud P , find the two points closest to it , , the calculation formula of the local surface normal n is: in, represents the cross product, so the angle of incidence Expressed as: In measuring distance and the angle of incidence When the values ​​are the same, objects with different reflectivity will get different intensity values.

3. The loop detection method based on laser point cloud intensity information conversion view according to claim 1 is characterized in that: In step 1, the outlier removal mechanism in LOAM is used to remove the obscured points in the point cloud P and the points parallel to the laser beam, and project them onto the intensity image I. Each valid point in P finds a corresponding pixel in the intensity image I, and the pixel value is determined by the intensity value of the valid point in P. If the size of the intensity image I is m rows and n columns, where m rows correspond to the laser radar beam and n columns correspond to the number of points on each laser beam of the laser radar; the beam information of the valid point is obtained by the radar drive, and the horizontal angle The calculation formula is: Among them, x and y represent the X and Y axis coordinates of the effective point in the laser radar coordinate system. According to the horizontal angle The value range is [-180°, 180°], so that the horizontal angle The calculation formula for the value range of [0°, n°] is: in, It represents the horizontal angular resolution on the same radar scanning line, n is the number of points on each laser line of the lidar, is the horizontal angle after conversion, and its value range is [0°, n°]. After conversion, each valid point in the point cloud P finds a corresponding pixel in the intensity image I. When there is no corresponding valid point assignment in the intensity image I, it is assigned a zero value.

4. The loop detection method based on laser point cloud intensity information conversion view according to claim 3 is characterized in that: In step 2, the FAST corner features are extracted by the grayscale values ​​of the candidate feature points. The calculation formula is: in, is the gray value of the candidate feature point, is the grayscale of any point on the circle with the candidate feature point as the center and the radius r is the grayscale difference threshold, if the FAST corner feature If it is greater than a given threshold, it is determined that p is a FAST feature point; In step 2, the detected feature points are described using the BRIEF descriptor with rotation invariance. BRIEF is a binary descriptor that defines the binary bits corresponding to each point. The calculation formula is: in, 、 It represents the grayscale of the p,q point pair, and randomly selects N point pairs from the FAST corner points. , combine the comparison results of these N point pairs as a descriptor , expressed as: Taking into account the rotation invariance, select N pairs of feature points and obtain a 2×N description matrix: After rotation angle , and get the new description matrix, expressed as: The size of the point pairs is compared at the new position to form a binary string descriptor, and finally the intensity image ORB feature is obtained.

5. The loop detection method based on laser point cloud intensity information conversion view according to claim 1, characterized in that: Step 4 also includes: introducing the Hamming distance to calculate the distance between the two descriptors, sorting the two descriptors in ascending order according to the Hamming distance of the matched descriptors, comparing the calculated Hamming distance with twice the minimum Hamming distance among all matches, and retaining the matches that are less than twice the minimum Hamming distance for verification.

6. The loop detection method based on laser point cloud intensity information conversion view according to claim 5 is characterized in that: The master in step 5 verifies the matching accuracy between the loop frame queried by DBoW and the current frame through the PnP algorithm and random sampling consistent RANSAC. The PnP and random sampling consistent RANSAC screening steps include: randomly extracting 4 groups of data from the PnP point pairs, calculating the transformation matrix H, the size of the matrix is ​​3*3, recorded as model M; calculating the projection error between all PnP point pairs and model M, and if the error is less than a predetermined threshold, adding them to the inliers ; If the current interior point set If the number of elements is greater than the optimal interior point set N_best, update N_best = , and update the number of iterations K at the same time; if the number of iterations is greater than K, exit; otherwise, add 1 to the number of iterations and repeat the above steps.

7. A loop closure detection device based on laser point cloud intensity information conversion view, the loop closure detection device based on laser point cloud intensity information conversion view is used to implement the loop closure detection method based on laser point cloud intensity information conversion view according to claim 1, characterized in that: include: The first main module is used to enable the main control machine to control the robot platform to move indoors and outdoors. When the laser radar scans different objects, it will return different intensity information due to the different material properties of the objects. The laser radar sensor collects the three-dimensional point cloud containing intensity information in real time and transmits it to the main control machine. When the main control machine receives a frame of three-dimensional point cloud P, it uses the abnormal point removal mechanism in LOAM to remove points that may be blocked in the point cloud P and points parallel to the laser beam, and then projects it onto the intensity image I to obtain a point cloud intensity image; the second main module is used to enable the main control machine to extract ORB features from the point cloud intensity image, extract FAST corner features through the grayscale values ​​of the candidate feature points, and then use the BRIEF descriptor with rotation invariance to describe the detected feature points to obtain the ORB feature descriptor; The third main module is used to implement the visualization vocabulary proposed by the master machine based on ORB-SLAM, use DBoW to convert the ORB descriptor into a bag-of-words vector, and use this vector to represent the current frame point cloud. Each new frame of point cloud is represented by a vector, and the vector is used to build a database for DBoW. When a new frame of 3D point cloud is converted into a vector, the similarity between the new vector and the previous vector is solved to determine whether a loop occurs between the two frames of point cloud, and then the DBoW database is used to query the loop candidate frame; the fourth main module is used to implement the master machine to OR the loop candidate frame queried by the DBoW database with the current frame B descriptor matching is used to eliminate false detections between the candidate loop frame queried by DBoW and the current frame; the fifth main module is used to enable the main control machine to find the three-dimensional points corresponding to the matching feature points between the two frames through the PnP algorithm, use the three-dimensional points and two-dimensional pixel points to construct the PnP problem, and use random sampling consistent RANSAC to delete false matches. When the number of correct matches after random sampling consistent RANSAC deletion is greater than the set threshold, it is determined that the candidate frame queried by DBoW is a correct loop, thereby verifying the matching accuracy between the loop frame queried by DBoW and the current frame.

8. An electronic device, characterized in that: include: At least one processor, at least one memory and a communication interface; wherein, The processor, memory and communication interface communicate with each other; The memory stores program instructions that can be executed by the processor, and the processor calls the program instructions to execute the method according to any one of claims 1 to 6.

9. A non-transitory computer-readable storage medium, characterized in that The non-transitory computer-readable storage medium stores computer instructions, which cause the computer to execute the method of any one of claims 1 to 6.

Images