Closed-loop detection method suitable for laser radar

By generating intensity maps, matching feature points and calculating rotation and translation information, and calculating the eigenma using the five-point method, the problems of inaccurate judgment and poor real-time performance in lidar closed-loop detection are solved, and high-precision closed-loop detection and positioning are achieved Calibration, suitable for fields such as autonomous driving.

CN119044938AInactive Publication Date: 2024-11-29ZIBO XUFENG CRAFTS CO LTD
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202411109679.8
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-14
Publication Date
2024-11-29
Estimated Expiration
Not applicable · inactive patent

AI Technical Summary

Technical Problem

The existing lidar closed-loop detection technology has problems such as inaccurate closed-loop judgment, difficult to correct positioning errors, large data demands, poor real-time performance, and noise sensitivity, especially in the field of autonomous driving, it is difficult to meet the needs of precise environmental perception.

Method used

By generating intensity maps, extracting and matching feature points, calculating rotation and translation information, and calculating the eigenma using the five-point method, closed-loop detection and correction are achieved, which avoids the problem of large amount of calculations for direct point cloud matching and sensitivity to initial values. Processing is performed on the image plane to improve robustness.

Benefits of technology

Accurate closed-loop detection and positioning correction are realized, the robot positioning accuracy and map construction accuracy are improved, the system's real-time response ability and anti-interference ability to noise are enhanced, and errors are reduced, and suitable for complex environments.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN119044938A_ABST
    Figure CN119044938A_ABST
Patent Text Reader

Abstract

The invention discloses a closed-loop detection method suitable for a laser radar, and relates to the technical field of computer vision, the closed-loop detection method suitable for the laser radar comprises the following specific steps: step 1, generating an intensity diagram.The closed-loop detection method suitable for the laser radar has the advantages that when the number of matching point pairs is smaller than a threshold value, it is considered that the matching point pairs are not the same place; a closed loop cannot be formed, otherwise a closed loop may be formed, and transformations between closed-loop frames are calculated. According to the judgment mode, a track closed loop can be accurately detected, so that positioning errors caused by long-time operation and sensor error accumulation are corrected, an intrinsic matrix between two frames is calculated by utilizing a five-point method, rotation and normalized translation vectors are obtained through decomposition from the intrinsic matrix, and then by recovering the translation scale, positioning errors caused by long-time operation and sensor error accumulation are corrected. According to the method, transformation between closed-loop frames can be accurately calculated, so that the robot can correct positioning errors in time according to rotation and translation information obtained through calculation, and the positioning precision and the map construction accuracy are further improved.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the field of computer vision technology, and in particular to a closed-loop detection method suitable for laser radar. Background Art

[0002] LiDAR is an advanced technology that measures distance and obtains target information by emitting laser beams. It plays an important role in many fields such as autonomous driving, surveying and mapping, and weather monitoring. It provides vehicles with accurate environmental perception during autonomous driving.

[0003] The existing lidar closed-loop detection technology has certain defects. First, it is inaccurate to judge whether a closed loop is formed at the same location, and the positioning error is difficult to correct. It also requires a large amount of data and training resources like deep learning algorithms. Direct point cloud matching is susceptible to interference, without multi-sensor fusion, and is not updated in time. In addition, the ICP calculation is large and has poor real-time performance. It is sensitive to initial values ​​and has poor matching effects. The NDT method rasterizes the original point cloud. If the point cloud in the same grid does not satisfy the Gaussian distribution, the matching effect is poor. For this reason, we propose a closed-loop detection method suitable for lidar. Summary of the invention

[0004] The purpose of the present invention is to provide a closed-loop detection method suitable for laser radar.

[0005] In order to solve the problems raised in the above background technology, the present invention provides the following technical solutions: a closed-loop detection method applicable to laser radar, the closed-loop detection method applicable to laser radar comprising the following specific steps:

[0006] Step 1: Generate intensity map: Generate intensity map based on the reflection intensity of laser point cloud, specifically the position of known laser 3D point P = [x, y, z] T , using the virtual camera internal parameters Project it onto the image plane where f x and f y is the focal length of the virtual camera, c x and c y The main point of the virtual camera is used to convert the three-dimensional position information of the laser point into the two-dimensional coordinate information on the image plane. Then, multiple frames of data are directly accumulated for the solid-state laser and dynamically accumulated for the mechanical laser radar. Through the above-mentioned projection method, the pixel point corresponding to the laser point on the imaging plane of the virtual camera is found, and the reflection intensity of the laser point is assigned to the pixel point to obtain the intensity map.

[0007] Step 2: Determine whether it is the same location: After obtaining the intensity maps of the two frames at different times through step 1, extract the feature points on the intensity maps of the two frames at different times, and then match the feature points between the two frames of intensity maps to obtain the matching point pair (xi ,x' i ), i∈N, N represents the number of matching point pairs. If the number of matching point pairs N is less than the threshold, it is considered that they are not in the same location and cannot form a closed loop. Otherwise, they are in the same location and can form a closed loop. The transformation between closed loop frames is calculated according to the subsequent steps.

[0008] Step 3: Calculate rotation and normalized translation: Based on the matching point pairs, use the five-point method to calculate the intrinsic matrix E between the two frames. The intrinsic matrix satisfies The rotation R and normalized translation vector decomposed from the eigenmatrix

[0009] Step 4: Restore the scale of the translation: Using the projection method in step 1, find the back-projected 3D point pair (P i , P' i ), i∈N,P i and P' i Represents the back-projected 3D point pair of the matching point pair, which satisfies a transformation. The specific formula for satisfying the transformation is as follows:

[0010] Where: λ represents the scale of translation. According to the above transformation, the scale of translation can be calculated. The specific calculation formula is as follows:

[0011]

[0012] The scale of the translation can be calculated using the above calculation formula.

[0013] Step 5: Closed-loop detection and correction: Determine whether a closed loop is formed through the above steps. If a closed loop is formed, correct the positioning error caused by long-term operation and sensor error accumulation based on the calculated rotation and translation information to improve the robot's positioning accuracy and map construction accuracy.

[0014] As a further solution of the present invention: in the step one, the specific method of accumulating multiple frames of data for the solid-state laser is to continuously accumulate a preset number of frame data to increase the density of the laser point cloud, and to monitor and analyze the accumulated data in real time to ensure the quality and accuracy of the data. When anomalies and noise are found in the data, a filtering method is used to process the data to improve the generation quality of the intensity map.

[0015] As a further solution of the present invention: In the step one, the method for dynamically accumulating the mechanical laser radar includes real-time monitoring of the scanning angle and speed of the laser radar, dynamically selecting appropriate frames for accumulation according to preset rules to improve the quality and stability of the laser point cloud, obtaining the scanning angle and speed information of the laser radar through a sensor, and comparing it with a preset threshold value, and when the scanning angle and speed meet specific conditions, selecting the corresponding frame for accumulation.

[0016] As a further solution of the present invention: In the step 2, when determining whether it is the same location, when extracting feature points, it specifically includes performing a convolution operation on the intensity map to obtain feature information of the image, and then screening out representative feature points according to preset thresholds and rules. In the convolution operation, multiple convolution kernels of different sizes and shapes are used to extract features of different scales and directions. The size and number of the convolution kernels can be adjusted as needed to obtain the best feature extraction effect. When screening feature points, the response strength, stability and uniqueness of the feature points are evaluated to ensure that the screened feature points can accurately reflect the characteristics of the image.

[0017] As a further solution of the present invention: in the step 2, the method for matching feature points includes calculating a similarity matrix of feature points in two-frame intensity images, finding the best matching point pair according to the similarity matrix, and removing the mismatched point pairs through preset screening conditions. When calculating the similarity matrix, a variety of Euclidean distance similarity measurement methods are used to improve the matching accuracy. After finding the best matching point pair, the RANSAC algorithm is used to screen the matching point pairs, remove the mismatched point pairs, and improve the matching reliability.

[0018] As a further solution of the present invention: In the step three, the specific process of calculating the eigenmatrix E using the five-point method is to select five matching point pairs, construct a set of equations based on their coordinate relationship, and obtain the eigenmatrix E by solving the set of equations. When constructing the set of equations, the coordinate error and noise of the matching point pairs are taken into consideration, and the least squares estimation method is used to improve the accuracy of the calculation.

[0019] As a further solution of the present invention: in the step 4, the specific method for finding the back-projected 3D point pairs of the matching point pairs is to obtain the corresponding 3D point coordinates by reverse projection calculation based on the virtual camera intrinsic parameters and the pixel coordinates of the matching point pairs. In the reverse projection calculation, the distortion and error of the camera are taken into consideration, and a radial distortion correction method is adopted to improve the accuracy of the 3D point coordinates. The calculated 3D point coordinates are verified and screened, and unreasonable point pairs are removed to ensure the accuracy of subsequent calculations.

[0020] As a further solution of the present invention: in step five, during closed-loop detection and correction, the robot's position information and map are updated in real time according to the results of the closed-loop detection to ensure the accuracy of the robot's navigation and positioning. When a closed loop is detected, the system will promptly correct the robot's position according to the calculated rotation and translation information, and update the map accordingly.

[0021] As a further solution of the present invention: in the step 4, when restoring the translation scale, it is necessary to consider the influence of the installation height and angle of the laser radar on the translation scale. Different installation heights and angles will cause the projection method of the laser point cloud to change, thereby affecting the calculation of the translation scale. After the translation scale is calculated, it needs to be calibrated and compensated accordingly.

[0022] By adopting the above technical solution, compared with the prior art, the beneficial effects of the present invention are:

[0023] 1. The present invention can effectively determine whether a closed loop is formed by extracting and matching the feature points on the intensity maps of the two frames when judging whether they are at the same location. When the number of matching point pairs is less than a threshold, it is considered that they are not at the same location and a closed loop cannot be formed. Otherwise, a closed loop can be formed and the transformation between closed-loop frames can be calculated. This judgment method can accurately detect the trajectory closed loop, thereby correcting the positioning error caused by long-term operation and sensor error accumulation, improving the positioning accuracy of the robot and the accuracy of map construction, using the five-point method to calculate the intrinsic matrix E between the two frames, and decomposing the rotation and normalized translation vectors from the intrinsic matrix, and then by restoring the scale of the translation, the transformation between closed-loop frames can be accurately calculated, which enables the robot to correct the positioning error in a timely manner according to the calculated rotation and translation information, further improving the positioning accuracy and the accuracy of map construction;

[0024] 2. The present invention solves the closed-loop detection problem based on LiDAR point cloud by using the epipolar geometry method based on image. Compared with the direct point cloud matching method, it is more robust and can better cope with noise and interference in the environment, thus improving the accuracy and stability of closed-loop detection. In addition, the epipolar geometry processing method based on image is used, so that the method can be easily extended to construct the same processing method and shared feature point word bag in multi-sensor fusion with camera, which can make full use of the advantages of different sensors, improve the perception ability and decision-making accuracy of the system, and update the robot's position information and map in real time according to the detection results during closed-loop detection and correction, thus ensuring the accuracy of robot navigation and positioning, and timely correcting positioning errors, thus improving the robot's work efficiency and safety.

[0025] 3. Unlike the ICP in the classical laser registration method, which directly processes the original point cloud, the present invention avoids the problem of poor real-time performance caused by the increase in the amount of calculation as the number of point clouds increases. At the same time, compared with the ICP method which is sensitive to the initial position, the present method does not require an initial value and has high robustness to the initial value. Even when the initial position difference is large, a good matching effect can be achieved. This enables the method to obtain accurate detection results more quickly when processing large-scale data, thereby improving the real-time response capability of the system.

[0026] 4. The present invention does not require rasterization processing and does not rely on the condition that the local area satisfies the Gaussian distribution. Unlike the NDT method, which performs rasterization processing on the original point cloud and requires the point cloud in the same grid to satisfy the Gaussian distribution, the present invention can maintain stable performance in various complex environments. In addition, compared with the key point and descriptor methods that are greatly affected by noise and the optimization solution is greatly affected by the initial value, the features extracted by this method do not need to be optimized and calculated, and the analytical solution can be directly obtained. The calculation is simpler, thereby improving the accuracy and reliability of detection. This stability and accuracy enables the robot to accurately perform closed-loop detection in different scenarios, reduce errors, and improve positioning accuracy and the quality of map construction. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] Figure 1 A schematic diagram of a method flow in an embodiment of the present invention;

[0028] Figure 2 Schematic diagram of the process of generating intensity map from laser point cloud in an embodiment of the present invention;

[0029] Figure 3 This is an intensity map generated by the laser point cloud in an embodiment of the present invention. DETAILED DESCRIPTION

[0030] The specific embodiments of the present invention will be further described below in conjunction with the accompanying drawings. It should be noted that the description of these embodiments is used to help understand the present invention, but does not constitute a limitation of the present invention.

[0031] In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0032] Please see attached Figure 1 The present invention provides a closed-loop detection method applicable to a laser radar, and the closed-loop detection method applicable to a laser radar comprises the following specific steps:

[0033] Step 1: Generate intensity map: Generate intensity map based on the reflection intensity of laser point cloud, specifically the position of known laser 3D point P = [x, y, z] T , using the virtual camera internal parameters Project it onto the image plane where f x and f y is the focal length of the virtual camera, c x and c y The main point of the virtual camera is used to convert the three-dimensional position information of the laser point into the two-dimensional coordinate information on the image plane. Then, multiple frames of data are directly accumulated for the solid-state laser and dynamically accumulated for the mechanical laser radar. Through the above-mentioned projection method, the pixel point corresponding to the laser point on the imaging plane of the virtual camera is found, and the reflection intensity of the laser point is assigned to the pixel point to obtain the intensity map.

[0034] Step 2: Determine whether it is the same location: After obtaining the intensity maps of the two frames at different times through step 1, extract the feature points on the intensity maps of the two frames at different times, and then match the feature points between the two frames of intensity maps to obtain the matching point pair (x i ,x' i ), i∈N, N represents the number of matching point pairs. If the number of matching point pairs N is less than the threshold, it is considered that they are not in the same location and cannot form a closed loop. Otherwise, they are in the same location and can form a closed loop. The transformation between closed loop frames is calculated according to the subsequent steps.

[0035] Step 3: Calculate rotation and normalized translation: Based on the matching point pairs, use the five-point method to calculate the intrinsic matrix E between the two frames. The intrinsic matrix satisfies The rotation R and normalized translation vector decomposed from the eigenmatrix

[0036] Step 4: Restore the scale of the translation: Using the projection method in step 1, find the back-projected 3D point pair (P i , P' i ), i∈N,P i and P' i Represents the back-projected 3D point pair of the matching point pair, which satisfies a transformation. The specific formula for satisfying the transformation is as follows:

[0037]

[0038] Where: λ represents the scale of translation. According to the above transformation, the scale of translation can be calculated. The specific calculation formula is as follows:

[0039]

[0040] The scale of the translation can be calculated using the above calculation formula.

[0041] Step 5: Closed-loop detection and correction: Determine whether a closed loop is formed through the above steps. If a closed loop is formed, correct the positioning error caused by long-term operation and sensor error accumulation based on the calculated rotation and translation information to improve the robot's positioning accuracy and map construction accuracy.

[0042] In one embodiment of the present invention: in step one, the specific method of accumulating multiple frames of data for the solid-state laser is to continuously accumulate a preset number of frame data to increase the density of the laser point cloud, and to monitor and analyze the accumulated data in real time to ensure the quality and accuracy of the data. When anomalies and noise are found in the data, a filtering method is used to process the data to improve the quality of the intensity map generation.

[0043] In one embodiment of the present invention: In step one, the method for dynamically accumulating mechanical laser radar includes real-time monitoring of the scanning angle and speed of the laser radar, dynamically selecting appropriate frames for accumulation according to preset rules to improve the quality and stability of the laser point cloud, obtaining the scanning angle and speed information of the laser radar through a sensor, and comparing it with a preset threshold value, and when the scanning angle and speed meet specific conditions, selecting the corresponding frame for accumulation.

[0044] In one embodiment of the present invention: In step 2, when determining whether it is the same location, when extracting feature points, it specifically includes performing a convolution operation on the intensity map to obtain feature information of the image, and then screening out representative feature points according to preset thresholds and rules. In the convolution operation, multiple convolution kernels of different sizes and shapes are used to extract features of different scales and directions. The size and number of convolution kernels can be adjusted as needed to obtain the best feature extraction effect. When screening feature points, the response strength, stability and uniqueness of the feature points are evaluated to ensure that the screened feature points can accurately reflect the characteristics of the image.

[0045] In one embodiment of the present invention: in step 2, the method for matching feature points includes calculating a similarity matrix of feature points in two-frame intensity images, finding the best matching point pair based on the similarity matrix, and removing the mismatched point pairs through preset screening conditions. When calculating the similarity matrix, a variety of Euclidean distance similarity measurement methods are used to improve the accuracy of matching. After finding the best matching point pair, the RANSAC algorithm is used to screen the matching point pairs, remove the mismatched point pairs, and improve the reliability of matching.

[0046] In one embodiment of the present invention: In step three, the specific process of calculating the eigenmatrix E using the five-point method is to select five matching point pairs, construct a set of equations based on their coordinate relationship, and obtain the eigenmatrix E by solving the set of equations. When constructing the set of equations, the coordinate error and noise of the matching point pairs are taken into consideration, and the least squares estimation method is used to improve the accuracy of the calculation.

[0047] In one embodiment of the present invention: in step 4, the specific method for finding the back-projected 3D point pairs of the matching point pairs is to obtain the corresponding 3D point coordinates by reverse projection calculation based on the virtual camera intrinsic parameters and the pixel coordinates of the matching point pairs. In the reverse projection calculation, the distortion and error of the camera are taken into account, and a radial distortion correction method is used to improve the accuracy of the 3D point coordinates. The calculated 3D point coordinates are verified and screened, and unreasonable point pairs are removed to ensure the accuracy of subsequent calculations.

[0048] In one embodiment of the present invention: In step five, during closed-loop detection and correction, the robot's position information and map are updated in real time based on the results of the closed-loop detection to ensure the accuracy of the robot's navigation and positioning. When a closed loop is detected, the system will promptly correct the robot's position based on the calculated rotation and translation information, and update the map accordingly.

[0049] In one embodiment of the present invention: In step four, when restoring the translation scale, it is necessary to consider the influence of the installation height and angle of the laser radar on the translation scale. Different installation heights and angles will cause the projection method of the laser point cloud to change, thereby affecting the calculation of the translation scale. After the translation scale is calculated, it needs to be calibrated and compensated accordingly.

[0050] Example 1, please refer to the attached Figure 1 In the process of judging whether it is the same location, when extracting feature points, it specifically includes performing a convolution operation on the intensity map to obtain the feature information of the image, and then screening out representative feature points according to the preset threshold and rules. In the convolution operation, it is very critical to use multiple convolution kernels of different sizes and shapes. Convolution kernels of different sizes can extract features of different scales, and convolution kernels of different shapes can extract features of different directions. For example, convolution kernels of sizes such as 3x3 and 5x5, as well as convolution kernels of shapes such as square and circle can be used. By combining these convolution kernels, the feature information of the image can be fully extracted. When screening feature points, it is necessary to evaluate them based on the response strength, stability and uniqueness of the feature points. Feature points with higher response strength are usually more representative of the important features of the image. Feature points with good stability can maintain good consistency between different frames, and feature points with strong uniqueness are easier to distinguish from other feature points.

[0051] Example 2, please refer to the attached Figure 1The specific process of calculating the eigenmatrix E using the five-point method is to select five matching point pairs, construct a set of equations according to their coordinate relationship, and obtain the eigenmatrix E by solving the set of equations. When constructing the set of equations, the coordinate error and noise of the matching point pairs are taken into account. The least squares estimation method can improve the accuracy of the calculation. The least squares method solves the set of equations by minimizing the sum of squares of the errors, thereby obtaining a more accurate eigenmatrix E. In addition, other optimization algorithms can be used to solve the set of equations, such as the gradient descent method. These algorithms can be selected and used according to specific circumstances to improve the calculation efficiency and accuracy. In practical applications, multiple sets of equations may need to be solved, so choosing a suitable algorithm can greatly improve the calculation speed.

[0052] Example 3, please refer to the attached Figure 1 In step five, during closed-loop detection and correction, the robot's position information and map are updated in real time according to the results of closed-loop detection to ensure the accuracy of the robot's navigation and positioning. When a closed loop is detected, the system will promptly correct the robot's position based on the calculated rotation and translation information. This can be achieved by using some positioning and navigation algorithms. When updating the map, new environmental information will be taken into account to optimize and improve the map. For example, new obstacle information can be added, the topological structure of the map can be updated, etc. By updating the robot's position information and map in real time, the robot can better adapt to environmental changes and avoid collisions and getting lost.

[0053] Specifically, when judging whether it is the same location, by extracting and matching the feature points on the intensity map of the two frames, it can effectively determine whether a closed loop is formed. When the number of matching point pairs is less than the threshold, it is determined that it is not the same location and a closed loop cannot be formed. Otherwise, a closed loop can be formed and the transformation between closed-loop frames is calculated. This judgment method can accurately detect the trajectory closed loop, thereby correcting the positioning error caused by long-term operation and sensor error accumulation, greatly improving the robot's positioning accuracy and map construction accuracy. The five-point method is used to calculate the eigenmatrix E between the two frames, and then the rotation and normalized translation vectors are decomposed from the eigenmatrix, and then the scale of the translation is restored, so that the transformation between the closed-loop frames can be accurately calculated. This enables the robot to correct the positioning error in a timely manner based on the obtained rotation and translation information, further enhancing the positioning accuracy and map construction accuracy.

[0054] Specifically, the problem of closed-loop detection based on lidar point cloud is solved by using image-based epipolar geometry. Compared with direct point cloud matching, this method is more robust and can better cope with noise and interference in the environment, thereby improving the accuracy and stability of closed-loop detection. In addition, the image-based epipolar geometry processing method is used, so that this method can be easily extended to multi-sensor fusion with cameras, constructing the same processing method and shared feature point word bags, thereby giving full play to the advantages of different sensors and enhancing the system's perception ability and decision-making accuracy. During the closed-loop detection and correction process, the robot's position information and map can be updated in real time according to the detection results, ensuring the accuracy of the robot's navigation and positioning, and timely correcting positioning errors to improve the robot's work efficiency and safety.

[0055] Specifically, unlike the ICP in the classic laser registration method that directly processes the original point cloud, this method avoids the problem of poor real-time performance caused by the increase in the amount of calculation as the number of point clouds increases. At the same time, compared with the ICP method that is sensitive to the initial position, this method does not require an initial value and has strong robustness to the initial value. Even when the initial position difference is large, it can still obtain a good matching effect. This enables the method to obtain accurate detection results more quickly when processing large-scale data, and enhances the real-time response capability of the system.

[0056] Specifically, there is no need for rasterization processing, nor does it rely on the local area satisfying the Gaussian distribution condition. This is different from the NDT method, which rasterizes the original point cloud and requires the point cloud in the same grid to satisfy the Gaussian distribution. Therefore, it can maintain stable performance in various complex environments. In addition, compared with the key point and descriptor methods that are greatly affected by noise and the optimization solution is greatly affected by the initial value, the features extracted by this method do not need to be optimized and calculated, and the analytical solution can be directly obtained. The operation is simpler, thereby improving the accuracy and reliability of detection. This stability and accuracy enable the robot to accurately perform closed-loop detection in different scenarios, reduce errors, and improve positioning accuracy and the quality of map construction.

[0057] Working principle:

[0058] First, an intensity map is generated based on the reflection intensity of the laser point cloud. The position of the laser 3D point is projected onto the image plane using the virtual camera internal parameters, multiple frames of data are directly accumulated for the solid-state laser, and the mechanical laser radar is dynamically accumulated. The reflection intensity of the pixel points corresponding to the laser point in the virtual camera imaging plane is assigned to obtain the intensity map. Then, the feature points of the intensity maps of the two frames at different times are extracted and matched. The number of matching point pairs is used to determine whether they are at the same location. If a closed loop can be formed, the transformation between the closed-loop frames is calculated, including using the five-point method to calculate the intrinsic matrix E to obtain the rotation R and the normalized translation vector, finding the back-projected 3D point pairs of the matching point pairs and calculating the scale of the translation. Finally, based on the closed-loop detection results, if a closed loop is formed, the positioning error is corrected to improve the positioning accuracy of the robot and the accuracy of map construction. At the same time, the robot's position information and map are updated in real time. At this point, the entire workflow ends.

[0059] At the same time, the present application uses specific words to describe the embodiments of the present application. For example, "one embodiment", "an embodiment", and / or "some embodiments" refer to a certain feature, structure or characteristic related to at least one embodiment of the present application. Therefore, it should be emphasized and noted that "one embodiment" or "an embodiment" or "an alternative embodiment" mentioned twice or more in different positions in this specification does not necessarily refer to the same embodiment. In addition, certain features, structures or characteristics in one or more embodiments of the present application can be appropriately combined.

[0060] Some aspects of the present application may be performed entirely by hardware, entirely by software (including firmware, resident software, microcode, etc.), or by a combination of hardware and software. The above hardware or software may be referred to as "data blocks", "modules", "engines", "units", "components" or "systems". The processor may be one or more application specific integrated circuits (ASICs), digital signal processors (DSPs), digital signal processing devices (DAPDs), programmable logic devices (PLDs), field programmable gate arrays (FPGAs), processors, controllers, microcontrollers, microprocessors, or combinations thereof. In addition, various aspects of the present application may be expressed as computer products located in one or more computer-readable media, which include computer-readable program codes. For example, computer-readable media may include, but are not limited to, magnetic storage devices (e.g., hard disks, floppy disks, tapes ...), optical disks (e.g., compact disks CDs, digital versatile disks DVDs ...), smart cards, and flash memory devices (e.g., cards, sticks, key drives ...).

[0061] A computer-readable medium may include a propagated data signal containing computer program code, such as in baseband or as part of a carrier wave. The propagated signal may have a variety of manifestations, including electromagnetic, optical, etc., or a suitable combination. A computer-readable medium may be any computer-readable medium other than a computer-readable storage medium, which may be connected to an instruction execution system, device or apparatus to communicate, propagate or transmit a program for use. The program code on the computer-readable medium may be propagated through any suitable medium, including radio, cable, fiber optic cable, radio frequency signal, or similar medium, or a combination of any of the above mediums.

[0062] Similarly, it should be noted that in order to simplify the description disclosed in this application, so as to help understand one or more embodiments of the invention, in the above description of the embodiments of the present application, sometimes multiple features are merged into one embodiment, drawings or descriptions thereof. However, this disclosure method does not mean that the features required by the object of the present application are more than the features mentioned in the claims. In fact, the features of the embodiment are less than all the features of the single embodiment disclosed above. In some embodiments, numbers describing the number of components and attributes are used. It should be understood that such numbers used in the description of the embodiments are modified by the modifiers "approximately", "approximately" or "substantially" in some examples. Unless otherwise specified, "approximately", "approximately" or "substantially" indicate that the number allows a variation of ±20%. Accordingly, in some embodiments, the numerical parameters used in the specification and claims are approximate values, which may change according to the characteristics required by individual embodiments. In some embodiments, the numerical parameters should take into account the specified significant digits and adopt the general method of retaining the digits. Although the numerical domains and parameters used to confirm the breadth of their range in some embodiments of the present application are approximate values, in specific embodiments, such numerical settings are as accurate as possible within the feasible range.

[0063] Although the present invention is disclosed as above in terms of preferred embodiments, it is not intended to limit the present invention. Any person skilled in the art may make possible changes and modifications without departing from the spirit and scope of the present invention. Therefore, any modification, equivalent change and modification made to the above embodiments according to the technical essence of the present invention without departing from the content of the technical solution of the present invention shall fall within the scope of protection defined by the claims of the present invention.

Claims

1. A closed-loop detection method suitable for laser radar, characterized in that: The closed-loop detection method applicable to laser radar includes the following specific steps: Step 1: Generate intensity map: Generate intensity map based on the reflection intensity of laser point cloud, specifically the position of known laser 3D point P = [x, y, z] T , using the virtual camera internal parameters Project it onto the image plane where f x and f y is the focal length of the virtual camera, c x and c y The main point of the virtual camera is used to convert the three-dimensional position information of the laser point into the two-dimensional coordinate information on the image plane. Then, multiple frames of data are directly accumulated for the solid-state laser and dynamically accumulated for the mechanical laser radar. Through the above-mentioned projection method, the pixel point corresponding to the laser point on the imaging plane of the virtual camera is found, and the reflection intensity of the laser point is assigned to the pixel point to obtain the intensity map. Step 2: Determine whether it is the same location: After obtaining the intensity maps of the two frames at different times through step 1, extract the feature points on the intensity maps of the two frames at different times, and then match the feature points between the two frames of intensity maps to obtain the matching point pair (x i ,x' i ), i∈N, N represents the number of matching point pairs. If the number of matching point pairs N is less than the threshold, it is considered that they are not in the same location and cannot form a closed loop. Otherwise, they are in the same location and can form a closed loop. The transformation between closed loop frames is calculated according to the subsequent steps. Step 3: Calculate rotation and normalized translation: Based on the matching point pairs, use the five-point method to calculate the intrinsic matrix E between the two frames. The intrinsic matrix satisfies The rotation R and normalized translation vector decomposed from the eigenmatrix Step 4: Restore the scale of the translation: Using the projection method in step 1, find the back-projected 3D point pair (P i , P' i ), i∈N,P i and P' i Represents the back-projected 3D point pair of the matching point pair, which satisfies a transformation. The specific formula for satisfying the transformation is as follows: Where: λ represents the scale of translation. According to the above transformation, the scale of translation can be calculated. The specific calculation formula is as follows: The scale of the translation can be calculated using the above calculation formula. Step 5: Closed-loop detection and correction: Determine whether a closed loop is formed through the above steps. If a closed loop is formed, correct the positioning error caused by long-term operation and sensor error accumulation based on the calculated rotation and translation information to improve the robot's positioning accuracy and map construction accuracy.

2. A closed-loop detection method suitable for laser radar according to claim 1, characterized in that: In the step 1, the specific method of accumulating multiple frames of data for the solid-state laser is to continuously accumulate a preset number of frame data to increase the density of the laser point cloud, and to monitor and analyze the accumulated data in real time to ensure the quality and accuracy of the data. When anomalies and noise are found in the data, a filtering method is used to process the data to improve the quality of the intensity map generation.

3. The closed-loop detection method applicable to laser radar according to claim 1, characterized in that: In step one, the method for dynamically accumulating the mechanical laser radar includes real-time monitoring of the scanning angle and speed of the laser radar, dynamically selecting appropriate frames for accumulation according to preset rules to improve the quality and stability of the laser point cloud, obtaining the scanning angle and speed information of the laser radar through a sensor, and comparing it with a preset threshold value, and when the scanning angle and speed meet specific conditions, selecting the corresponding frame for accumulation.

4. The closed-loop detection method applicable to laser radar according to claim 1, characterized in that: In the step 2, when determining whether it is the same location, when extracting feature points, it specifically includes performing a convolution operation on the intensity map to obtain feature information of the image, and then screening out representative feature points according to preset thresholds and rules. In the convolution operation, multiple convolution kernels of different sizes and shapes are used to extract features of different scales and directions. The size and number of convolution kernels can be adjusted as needed to obtain the best feature extraction effect. When screening feature points, the response strength, stability and uniqueness of the feature points are evaluated to ensure that the screened feature points can accurately reflect the characteristics of the image.

5. The closed-loop detection method applicable to laser radar according to claim 1, characterized in that: In the step 2, the method for matching feature points includes calculating a similarity matrix of feature points in two-frame intensity images, finding the best matching point pair based on the similarity matrix, and removing the mismatched point pair through preset screening conditions. When calculating the similarity matrix, a variety of Euclidean distance similarity measurement methods are used to improve the accuracy of matching. After finding the best matching point pair, the RANSAC algorithm is used to screen the matching point pairs, remove the mismatched point pairs, and improve the reliability of matching.

6. The closed-loop detection method applicable to laser radar according to claim 1, characterized in that: In the step three, the specific process of calculating the eigenmatrix E using the five-point method is to select five matching point pairs, construct a set of equations based on their coordinate relationship, and obtain the eigenmatrix E by solving the set of equations. When constructing the set of equations, the coordinate error and noise of the matching point pairs are taken into account, and the least squares estimation method is used to improve the accuracy of the calculation.

7. The closed-loop detection method applicable to laser radar according to claim 1, characterized in that: In the step 4, the specific method for finding the back-projected 3D point pairs of the matching point pairs is to obtain the corresponding 3D point coordinates by reverse projection calculation based on the virtual camera intrinsic parameters and the pixel coordinates of the matching point pairs. In the reverse projection calculation, the distortion and error of the camera are taken into account, and a radial distortion correction method is used to improve the accuracy of the 3D point coordinates. The calculated 3D point coordinates are verified and screened, and unreasonable point pairs are removed to ensure the accuracy of subsequent calculations.

8. The closed-loop detection method applicable to laser radar according to claim 1, characterized in that: In step five, during closed-loop detection and correction, the robot's position information and map are updated in real time based on the results of the closed-loop detection to ensure the accuracy of the robot's navigation and positioning. When a closed loop is detected, the system will promptly correct the robot's position based on the calculated rotation and translation information, and update the map accordingly.

9. The closed-loop detection method applicable to laser radar according to claim 1, characterized in that: In step 4, when restoring the translation scale, it is necessary to consider the influence of the installation height and angle of the laser radar on the translation scale. Different installation heights and angles will cause the projection method of the laser point cloud to change, thereby affecting the calculation of the translation scale. After calculating the translation scale, it needs to be calibrated and compensated accordingly.