A 3D LiDAR positioning method and system that integrates visual information

Abstract

This invention discloses a 3D LiDAR localization method and system that integrates visual information. It employs feature extraction and feature matching algorithms from LOAM to match point clouds; detects whether similar or missing structural features exist in the current environment; projects points within the camera's field of view onto the image pixel plane to obtain the pixels corresponding to the laser points; after projecting the laser points onto the pixel plane to obtain the pixels, integrates image information into the laser points; finds the best matching point for the current laser point in reference data; calculates the variance of the Hamming distances of all laser points that find the best match using descriptors; for the retained laser point pairs, calculates the Euclidean distance between the two points as the matching error of the laser point with descriptors; combines the pure laser point cloud matching error with the matching error of the laser point with descriptors; and uses a LOAM optimization process to minimize the error and obtain the specific pose. This invention exhibits high robustness and reliability.

Description

Technical Field

[0001] This invention relates to the field of lidar positioning technology, and in particular discloses a 3D lidar positioning method and system that integrates visual information. Background Technology

[0002] Mobile robot localization technology is crucial for robots to achieve autonomous positioning and navigation. Only with accurate and reliable localization can a robot reach its designated location along the correct path to complete various tasks assigned by the system. Currently, mobile robot localization methods are mainly divided into LiDAR-based methods and vision-based methods. LiDAR localization technology is mature and widely used in industry due to its strong stability, but it has localization defects in some open or similar scenarios. Vision-based localization methods can obtain rich environmental data, but due to their poor stability, they are less used in industry. Therefore, the use of vision-assisted LiDAR localization technology has attracted widespread attention.

[0003] Currently, the mainstream laser and vision fusion localization method is a loosely coupled approach. The front-end visual odometry (localization) and laser odometry (localization) operate separately, and then are jointly optimized at the back-end to obtain the final position. In addition, a visual bag-of-words-assisted LiDAR method is used for loop closure detection, providing loop constraints for laser localization and mapping. These laser-vision fusion localization methods are mostly used in online or offline mapping modes. In real-time localization modes, loop closure detection and back-end joint optimization are not applicable.

[0004] Therefore, due to the relatively simple nature of lidar data, lidar positioning technology suffers from significant positioning errors in open environments and areas with similar or missing environmental structural features (i.e., feature degradation areas), which is a technical problem that urgently needs to be solved. Summary of the Invention

[0005] This invention provides a 3D LiDAR positioning method and system that integrates visual information, aiming to solve the technical problem that LiDAR positioning technology has large positioning errors in areas with open environments, similar or missing environmental structural features due to the relatively simple data information of LiDAR.

[0006] One aspect of the present invention relates to a 3D LiDAR positioning method that integrates visual information, comprising the following steps:

[0007] Laser feature extraction and matching: The feature extraction and matching algorithms in LOAM are used to match the point cloud.

[0008] Environmental degradation detection: Detects whether similarities or omissions exist in the current environmental structural features;

[0009] Laser point reprojection: Projecting points within the camera's field of view onto the image pixel plane to obtain the pixel corresponding to the laser point;

[0010] Descriptor extraction: After projecting the laser points onto the pixel plane to obtain the pixels, the image information is incorporated into the laser points;

[0011] Descriptor matching and error calculation: Find the best matching point for the current laser point in the reference data; calculate the variance of the Hamming distance of all laser points that find the best match through descriptors; for the retained laser point pairs, calculate the Euclidean distance between the two points as the matching error of the laser point with descriptors.

[0012] Error fusion: combines the matching error of pure laser point cloud with the matching error of laser points with descriptors;

[0013] Error optimization: The LOAM optimization process is adopted to obtain the specific pose by minimizing the error.

[0014] Furthermore, in the environmental degradation detection step, the equation for solving the laser matching positioning problem is as follows:

[0015]

[0016] For coefficient matrix A T A undergoes eigenvalue decomposition to obtain its eigenvalues ​​and eigenvectors, and the feature threshold is defined as d. min If the coefficient matrix A T A has a feature value λ that is less than the feature threshold d. min When this occurs, it indicates that the current environment is degrading. The direction of degradation is the direction represented by the eigenvector corresponding to the eigenvalue. The ratio of the minimum eigenvalue to the eigenvalue threshold is used. This indicates the current degree of environmental degradation; among which, The mathematical notation represents the value of X when the value of the following expression is minimized; A is the coefficient matrix, x is the variable to be determined, representing the pose transformation matrix; b is the target point value; A T A is the product of the coefficient matrix and its transpose; λ is the eigenvalue, and d min λ is the feature threshold, α is the degree of degradation of the current environment, and λmin is the minimum feature value.

[0017] Furthermore, in the laser point reprojection step, the reprojection formula is as follows:

[0018]

[0019] Where uv represents the pixel coordinates projected onto the image plane, matrix K represents the camera intrinsic parameters, (R, t) represents the rotation and translation matrix from the lidar coordinate system to the camera coordinate system, PL represents the spatial 3D laser point, and T represents the transformation matrix, which consists of R and t.

[0020] Furthermore, in the steps of descriptor matching and error calculation, the retained Euclidean distance between the two laser point pairs is:

[0021]

[0022] Where e is the matching error, xref-i, xl-i, yref-i, yl-i, zref-i, and zl-i represent the XYZ coordinates of the i-th current laser point and its matching reference point in the current matching result, respectively. The subscript ref indicates the reference point, and the subscript l indicates the current laser point.

[0023] The error fusion formula is shown below:

[0024]

[0025] Where eall is the fusion error, elv is the laser point error with image descriptor, and el is the laser point error obtained by pure laser matching; This is the error coefficient.

[0026] Furthermore, in the error optimization step, the optimization function is as follows:

[0027]

[0028] Among them, R l ,t l Let be the rotation and translation matrices of the current laser frame to be determined; arg min is a mathematical notation, which is to find R when the value of the latter expression is minimized. l ,t l value, is the error coefficient, elv is the laser point error with image descriptor, and el is the laser point error with pure laser matching.

[0029] Another aspect of the present invention relates to a 3D LiDAR positioning system that fuses visual information, comprising:

[0030] The laser feature extraction and matching module is used for laser feature extraction and matching: it uses the feature extraction and feature matching algorithms in LOAM to match point clouds;

[0031] The environmental degradation detection module is used for environmental degradation detection: detecting whether there are similarities or missing features in the current environmental structure;

[0032] The laser point reprojection module is used for laser point reprojection: projecting points within the camera's field of view onto the image pixel plane to obtain the pixel points corresponding to the laser points;

[0033] The descriptor extraction module is used to extract descriptors: after projecting the laser points onto the pixel plane to obtain the pixels, the image information is integrated into the laser points;

[0034] The descriptor matching and error calculation module is used for descriptor matching and error calculation: finding the best matching point of the current laser point in the reference data; calculating the variance of the Hamming distance of all laser points that have found the best match through descriptors; and calculating the Euclidean distance between the two retained laser point pairs as the matching error of the laser points with descriptors.

[0035] Error fusion module, used for error fusion: combining the matching error of pure laser point cloud and the matching error of laser points with descriptors;

[0036] The error optimization module is used for error optimization: it adopts the LOAM optimization process to obtain the specific pose by minimizing the error.

[0037] Furthermore, in the environmental degradation detection module, the equation for solving laser matching positioning is as follows:

[0038]

[0039] For coefficient matrix A T A undergoes eigenvalue decomposition to obtain its eigenvalues ​​and eigenvectors, and the feature threshold is defined as d. min If the coefficient matrix A T A has a feature value λ that is less than the feature threshold d. min When this occurs, it indicates that the current environment is degrading. The direction of degradation is the direction represented by the eigenvector corresponding to the eigenvalue. The ratio of the minimum eigenvalue to the eigenvalue threshold is used. This indicates the current degree of environmental degradation; among which, The mathematical notation represents the value of X when the value of the following expression is minimized; A is the coefficient matrix, x is the variable to be determined, representing the pose transformation matrix; b is the target point value; A T A is the product of the coefficient matrix and its transpose; λ is the eigenvalue, and d min λ is the feature threshold, α is the degree of degradation of the current environment, and λmin is the minimum feature value.

[0040] Furthermore, in the laser point reprojection module, the reprojection formula is as follows:

[0041]

[0042] Where uv represents the pixel coordinates projected onto the image plane, matrix K represents the camera intrinsic parameters, (R, t) represents the rotation and translation matrix from the lidar coordinate system to the camera coordinate system, PL represents the spatial 3D laser point, and T represents the transformation matrix, which consists of R and t.

[0043] Furthermore, in the descriptor matching and error calculation module, the retained Euclidean distance between two laser point pairs is:

[0044]

[0045] Where e is the matching error, xref-i, xl-i, yref-i, yl-i, zref-i, and zl-i represent the XYZ coordinates of the i-th current laser point and its matching reference point in the current matching result, respectively. The subscript ref indicates the reference point, and the subscript l indicates the current laser point.

[0046] The error fusion formula is shown below:

[0047]

[0048] Where eall is the fusion error, elv is the laser point error with image descriptor, and el is the laser point error obtained by pure laser matching; This is the error coefficient.

[0049] Furthermore, in the error optimization module, the optimization function is as follows:

[0050]

[0051] Among them, R l ,t l Let be the rotation and translation matrices of the current laser frame to be determined; arg min is a mathematical notation, which is to find R when the value of the latter expression is minimized. l ,t l value, is the error coefficient, elv is the laser point error with image descriptor, and el is the laser point error with pure laser matching.

[0052] The beneficial effects achieved by this invention are as follows:

[0053] This invention provides a 3D LiDAR localization method and system that integrates visual information. It employs feature extraction and matching algorithms from LOAM to match point clouds; detects whether similar or missing structural features exist in the current environment; projects points within the camera's field of view onto the image pixel plane to obtain the pixels corresponding to the laser points; after projecting the laser points onto the pixel plane, image information is integrated into the laser points; the best matching point for the current laser point is found in reference data; the variance of the Hamming distance of all laser points that find the best match using descriptors is calculated; for the retained laser point pairs, the Euclidean distance between the two points is calculated as the matching error of the laser points with descriptors; the pure laser point cloud matching error and the matching error of the laser points with descriptors are combined; and a LOAM optimization process is used to minimize the error and obtain the specific pose. The 3D LiDAR localization method and system that integrates visual information provided by this invention performs pixel-level fusion of image information and 3D LiDAR information, calculating the image pixel descriptors of the 3D laser points, providing more information for laser matching and localization, and improving the robustness and reliability of 3D LiDAR matching and localization. Attached Figure Description

[0054] Figure 1 A flowchart illustrating the first embodiment of the 3D LiDAR positioning method that integrates visual information provided by the present invention;

[0055] Figure 2 A schematic diagram of the main process of the second embodiment of the 3D LiDAR positioning method that integrates visual information provided by the present invention;

[0056] Figure 3 This is a functional block diagram of an embodiment of the 3D LiDAR positioning system that integrates visual information provided by the present invention.

[0057] Explanation of icon numbers:

[0058] 10. Laser feature extraction and matching module; 20. Environmental degradation detection module; 30. Laser point reprojection module; 40. Descriptor extraction module; 50. Descriptor matching and error calculation module; 60. Error fusion module; 70. Error optimization module. Detailed Implementation

[0059] To better understand the above technical solutions, the following will provide a detailed explanation of the technical solutions in conjunction with the accompanying drawings and specific implementation methods.

[0060] like Figure 1 and Figure 2 As shown, this invention proposes a 3D LiDAR positioning method that integrates visual information, comprising the following steps:

[0061] Step S100, Laser Feature Extraction and Matching: The feature extraction and feature matching algorithms in LOAM are used to match the point cloud.

[0062] The feature extraction and feature matching algorithms in LOAM (Lidar Odometry and Mapping) are used to match the point cloud. In LOAM, the feature of each point in the point cloud is extracted by calculating the curvature of each point. Points with larger curvature are called corner feature points, while points with smaller curvature are called surface feature points. Points with curvature in the middle are discarded, which reduces the amount of computation for point cloud data processing.

[0063] During point cloud matching, the distances from corner points to straight lines (composed of corner points) and from corner points to planes are used as the positional errors between the current laser point cloud and the target point cloud.

[0064] Step S200, Environmental degradation detection: Detect whether there are similar or missing structural features in the current environment.

[0065] Environmental degradation detection is used to check for similarities or missing features in the current environmental structure. Degraded environmental features can lead to insufficient constraints in certain directions during laser data matching, resulting in excessive positioning errors. The equations for laser matching positioning are shown below:

[0066]

[0067] In formula (1), for the coefficient matrix A T A undergoes eigenvalue decomposition to obtain its eigenvalues ​​and eigenvectors, and a feature threshold d is defined. min If the coefficient matrix A T A has a feature value λ that is less than the feature threshold d. min When this occurs, it indicates that the current environment is degrading. The direction of degradation is the direction represented by the eigenvector corresponding to the eigenvalue. The ratio of the minimum eigenvalue to the eigenvalue threshold is used. This indicates the current degree of environmental degradation; among which, The mathematical notation represents the value of X when the value of the following expression is minimized; A is the coefficient matrix, x is the variable to be determined, representing the pose transformation matrix; b is the target point value; A T A is the product of the coefficient matrix and its transpose; λ is the eigenvalue, and d min λ is the feature threshold, α is the degree of degradation of the current environment, and λmin is the minimum feature value.

[0068] Step S300, Laser point reprojection: Project the points within the camera's field of view onto the image pixel plane to obtain the pixel points corresponding to the laser points.

[0069] Laser point reprojection projects points within the camera's field of view onto the image pixel plane, obtaining the corresponding pixel point. The reprojection formula is as follows. Since the camera's field of view is limited, only points that fall onto the image plane after reprojection can acquire image information; other points cannot. Therefore, wide-angle cameras or multiple cameras can be used to ensure more laser points can acquire image information, increasing the reliability of positioning.

[0070]

[0071] In formula (2), uv is the pixel coordinate projected onto the image plane, matrix K is the camera intrinsic parameter, (R, t) is the rotation and translation matrix from the lidar coordinate system to the camera coordinate system, PL is the spatial 3D laser point; T is the transformation matrix, which consists of R and t; to ensure the accuracy of fusion positioning, the calibration parameters of the sensor need to be as accurate as possible.

[0072] Step S400: Extract descriptors: After projecting the laser points onto the pixel plane to obtain the pixels, the image information is incorporated into the laser points.

[0073] After projecting the laser points onto the pixel plane to obtain the pixels, image information can be incorporated into the laser points. Considering that images are easily affected by factors such as lighting and viewing angle, the RGB (Red, Green, Blue) information of the pixels is not fused here. Instead, a BRIEF (Binary Robust Independent Elementary Features) descriptor for each pixel is calculated. A BRIEF is a binary vector whose value represents the pixel relationship between the pixels near the pixel. Because of its fast calculation speed and convenient storage, it is often used in image matching.

[0074] Step S500, Descriptor Matching and Error Calculation: Find the best matching point for the current laser point in the reference data; calculate the variance of the Hamming distance of all laser points that have found the best match through descriptors; for the retained laser point pairs, calculate the Euclidean distance between the two points as the matching error of the laser points with descriptors.

[0075] Matching: Descriptor matching finds the best matching point for the current laser point in the reference data. This requires projection of the laser feature matching results. The laser point with descriptors is projected onto the reference frame point cloud (with descriptor part) through the laser feature matching results. Then, the reference laser points within a radius d at the projection position are found. The Hamming distance between the descriptor of each reference point and the descriptor of the current laser point is calculated, and the one with the smallest distance is selected as the best matching point. In subsequent iterations, the search range d will be continuously reduced to ensure matching accuracy.

[0076] Error Calculation: To eliminate the influence of mismatches or outliers, the variance of the Hamming distances of all laser points that found the best match using descriptors is calculated. If the variance is less than a threshold D, all matching results are retained; otherwise, the points are sorted by Hamming distance from smallest to largest, and the last 20% of matching point pairs are discarded. Then, for the retained laser point pairs, the Euclidean distance between the two points is calculated as the matching error of the laser point with descriptors.

[0077]

[0078] In formula (3), e is the matching error, xref-i, xl-i, yref-i, yl-i, zref-i, and zl-i represent the XYZ coordinates of the i-th current laser point and its matching reference point in the current matching result, respectively. The subscript ref represents the reference point, and the subscript l represents the current laser point.

[0079] Step S600, Error Fusion: Combine the matching error of pure laser point cloud with the matching error of laser points with descriptors.

[0080] Error fusion combines the matching error of pure laser point clouds with the matching error of laser points with descriptors for subsequent error optimization. Since pure laser points can exhibit excessively large localization errors in areas with degraded environmental structural features, the fusion method is slightly adjusted based on the degradation detection results during error fusion. This allows the localization to better adapt to environmental changes and increases robustness. `elv` represents the laser point error with image descriptors, and `el` represents the laser point error obtained from pure laser matching. is the error coefficient, and α is the environmental structural feature degradation factor.

[0081]

[0082] In formula (4), eall is the fusion error, elv is the laser point error with image descriptor, and el is the laser point error with pure laser matching. This is the error coefficient.

[0083]

[0084] In formula (5), is the error coefficient, and α is the environmental structural feature degradation factor.

[0085] Step S700, Error Optimization: The LOAM optimization process is adopted to obtain the specific pose by minimizing the error.

[0086] The error optimization framework and algorithm still adopt the optimization process of LOAMLidar Odometry and Mapping (a laser positioning and mapping algorithm). The optimization function is as follows, where the two error functions el and elv are functions of the rotation matrix R and the translation matrix t. The specific pose is obtained by minimizing the error through optimization. The nonlinear optimization method here is the Levenberg-Marquardt algorithm.

[0087]

[0088] In formula (6), R l ,t l Let be the rotation and translation matrices of the current laser frame to be determined; arg min is a mathematical notation, which is to find R when the value of the latter expression is minimized. l ,t l value, is the error coefficient, elv is the laser point error with image descriptor, and el is the laser point error with pure laser matching.

[0089] The entire matching optimization process described above is a continuous iterative process. The initial pose value for the next iteration is the result of the previous calculation. The iteration terminates when the calculation error is less than the threshold or the number of iterations reaches the specified upper limit, and then the final result is output.

[0090] The 3D LiDAR localization method fused with visual information provided in this embodiment, compared with the prior art, uses feature extraction and feature matching algorithms in LOAM to match point clouds; detects whether there are similarities or missing features in the current environmental structure features; projects points within the camera's field of view onto the image pixel plane to obtain the pixels corresponding to the laser points; after projecting the laser points onto the pixel plane to obtain the pixels, integrates image information into the laser points; finds the best matching point for the current laser point in the reference data; calculates the variance of the Hamming distance of all laser points that find the best match through descriptors; calculates the Euclidean distance between the two points for the retained laser point pairs as the matching error of the laser points with descriptors; combines the pure laser point cloud matching error and the matching error of the laser points with descriptors; and uses the LOAM optimization process to obtain the specific pose by minimizing the error. The 3D LiDAR localization method fused with visual information provided in this embodiment performs pixel-level fusion of image information and 3D LiDAR information to calculate the image pixel descriptors of the 3D laser points, providing more information for laser matching and localization, and improving the robustness and reliability of 3D LiDAR matching and localization.

[0091] Please see Figure 3 , Figure 3This is a functional block diagram of an embodiment of a 3D LiDAR positioning system that integrates visual information provided by the present invention. In this embodiment, the 3D LiDAR positioning system that integrates visual information includes a laser feature extraction and matching module 10, an environment degradation detection module 20, a laser point reprojection module 30, a descriptor extraction module 40, a descriptor matching and error calculation module 50, an error fusion module 60, and an error optimization module 70. The laser feature extraction and matching module 10 is used for laser feature extraction and matching: matching point clouds using feature extraction and feature matching algorithms in LOAM. The environment degradation detection module 20 is used for environment degradation detection: detecting whether similar or missing structural features appear in the current environment. The laser point reprojection module 30 is used for laser point reprojection: projecting points within the camera's field of view onto the image pixel plane to obtain the pixels corresponding to the laser points. The descriptor extraction module 40 is used for descriptor extraction: after projecting the laser points onto the pixel plane to obtain the pixels, integrating image information into the laser points. The descriptor matching and error calculation module 50 is used for descriptor matching and error calculation: finding the best matching point for the current laser point in the reference data; calculating the variance of the Hamming distance of all laser points that find the best match through descriptors; and calculating the Euclidean distance between the two retained laser point pairs as the matching error of the laser points with descriptors. The error fusion module 60 is used for error fusion: combining the pure laser point cloud matching error with the matching error of the laser points with descriptors. The error optimization module 70 is used for error optimization: employing the LOAM optimization process to minimize the error and obtain the specific pose.

[0092] In the laser feature extraction and matching module 10, the feature extraction and feature matching algorithm in LOAM is used to match the point cloud. In LOAM, the feature of each point in the point cloud is extracted by calculating the curvature of each point. Points with larger curvature are called corner feature points, while points with smaller curvature are called surface feature points. Points with curvature in the middle are discarded, which reduces the amount of computation for point cloud data processing.

[0093] During point cloud matching, the distances from corner points to straight lines (composed of corner points) and from corner points to planes are used as the positional errors between the current laser point cloud and the target point cloud.

[0094] In the environmental degradation detection module 20, environmental degradation detection is used to detect whether there are similarities or missing features in the current environmental structure. Degradation of environmental structure features can cause insufficient constraints in certain directions during laser data matching, resulting in excessive positioning errors. The equations for solving laser matching positioning are as follows:

[0095]

[0096] In formula (7), for the coefficient matrix A TA undergoes eigenvalue decomposition to obtain its eigenvalues ​​and eigenvectors, and a feature threshold d is defined. min If the coefficient matrix A T A has a feature value λ that is less than the feature threshold d. min When this occurs, it indicates that the current environment is degrading. The direction of degradation is the direction represented by the eigenvector corresponding to the eigenvalue. The ratio of the minimum eigenvalue to the eigenvalue threshold is used. This indicates the current degree of environmental degradation; among which, The mathematical notation represents the value of X when the value of the following expression is minimized; A is the coefficient matrix, x is the variable to be determined, representing the pose transformation matrix; b is the target point value; A T A is the product of the coefficient matrix and its transpose; λ is the eigenvalue, and d min λ is the feature threshold, α is the degree of degradation of the current environment, and λmin is the minimum feature value.

[0097] In the laser point reprojection module 30, laser point reprojection projects points within the camera's field of view onto the image pixel plane to obtain the corresponding pixel points of the laser points. The reprojection formula is as follows. Since the camera's field of view is limited, only points that fall onto the image plane after reprojection can acquire image information; other points cannot. Therefore, a wide-angle camera or multiple cameras can be used in conjunction to allow more laser points to acquire image information, increasing the reliability of positioning.

[0098]

[0099] In formula (8), uv is the pixel coordinate projected onto the image plane, matrix K is the camera intrinsic parameter, (R, t) is the extrinsic parameter transformation matrix from the lidar coordinate system to the camera coordinate system, both of which are obtained through calibration; PL is the spatial 3D laser point; T is the transformation matrix, which is composed of R and t; in order to ensure the accuracy of fusion positioning, the calibration parameters of the sensor need to be as accurate as possible.

[0100] In the descriptor extraction module 40, after projecting the laser points onto the pixel plane to obtain the pixels, image information can be integrated into the laser points. Considering that images are easily affected by factors such as lighting and viewing angle, the RGB information of the pixels is not fused here. Instead, a BRIEF descriptor is calculated for each pixel. BRIEF is a binary vector whose value represents the pixel relationship between pixels near the pixel. Because it is fast to calculate and easy to store, it is often used in image matching.

[0101] In the descriptor matching and error calculation module 50, the matching process involves finding the best matching point for the current laser point in the reference data. This requires projecting the laser feature matching results onto the reference frame point cloud (with descriptor portion) using the laser feature matching results. Then, the reference laser points within a radius d at the projection position are found. The Hamming distance between the descriptor of each reference point and the descriptor of the current laser point is calculated, and the point with the smallest distance is selected as the best matching point. In subsequent iterations, the search range d will be continuously reduced to ensure matching accuracy.

[0102] Error Calculation: To eliminate the influence of mismatches or outliers, the variance of the Hamming distances of all laser points that found the best match using descriptors is calculated. If the variance is less than a threshold D, all matching results are retained; otherwise, the points are sorted by Hamming distance from smallest to largest, and the last 20% of matching point pairs are discarded. Then, for the retained laser point pairs, the Euclidean distance between the two points is calculated as the matching error of the laser point with descriptors.

[0103]

[0104] In formula (9), e is the matching error, xref-i, xl-i, yref-i, yl-i, zref-i, and zl-i represent the XYZ coordinates of the i-th current laser point and its matching reference point in the current matching result, respectively. The subscript ref represents the reference point, and the subscript l represents the current laser point.

[0105] In the error fusion module 60, error fusion combines the matching error of pure laser point clouds with the matching error of laser points with descriptors for subsequent error optimization. Since pure laser points can exhibit excessive positioning errors in areas with degraded environmental structural features, the fusion method is slightly adjusted based on the degradation detection results during error fusion. This allows the positioning to better adapt to environmental changes and increases robustness. `elv` represents the laser point error with image descriptors, and `el` represents the laser point error obtained from pure laser matching. is the error coefficient, and α is the environmental structural feature degradation factor.

[0106]

[0107] In formula (10), eall is the fusion error, elv is the laser point error with image descriptor, and el is the laser point error with pure laser matching. This is the error coefficient.

[0108]

[0109] In formula (11), is the error coefficient, and α is the environmental structural feature degradation factor.

[0110] In the error optimization module 70, the error optimization framework and algorithm still adopt the LOAM optimization process. The optimization function is as follows, where the two error functions el and elv are functions of the rotation matrix R and the translation matrix t. The specific pose is obtained by minimizing the error through optimization. The nonlinear optimization method here is the Levenberg-Marquardt algorithm.

[0111]

[0112] In formula (12), R l ,t l Let be the rotation and translation matrices of the current laser frame to be determined; arg min is a mathematical notation, which is to find R when the value of the latter expression is minimized. l ,t l value, is the error coefficient, elv is the laser point error with image descriptor, and el is the laser point error with pure laser matching.

[0113] The entire matching optimization process described above is a continuous iterative process. The initial pose value for the next iteration is the result of the previous calculation. The iteration terminates when the calculation error is less than the threshold or the number of iterations reaches the specified upper limit, and then the final result is output.

[0114] The 3D LiDAR positioning system fused with visual information provided in this embodiment, compared with the prior art, employs a laser feature extraction and matching module 10, an environment degradation detection module 20, a laser point reprojection module 30, a descriptor extraction module 40, a descriptor matching and error calculation module 50, an error fusion module 60, and an error optimization module 70. It matches point clouds using feature extraction and feature matching algorithms in LOAM; detects whether similar or missing structural features exist in the current environment; projects points within the camera's field of view onto the image pixel plane to obtain the pixels corresponding to the laser points; after projecting the laser points onto the pixel plane to obtain the pixels, integrates image information into the laser points; finds the best matching point for the current laser point in the reference data; calculates the variance of the Hamming distance of all laser points that find the best match through descriptors; calculates the Euclidean distance between the retained laser point pairs as the matching error of the laser points with descriptors; combines the pure laser point cloud matching error with the matching error of the laser points with descriptors; and uses the LOAM optimization process to obtain the specific pose by minimizing the error. The 3D LiDAR positioning system that integrates visual information provided in this embodiment performs pixel-level fusion of image information and 3D LiDAR information to calculate the image pixel descriptor of the 3D laser point, providing more information for laser matching and positioning, and improving the robustness and reliability of 3D LiDAR matching and positioning.

[0115] like Figures 1 to 3 As shown, the 3D LiDAR positioning method and system that fuses visual information provided in this embodiment will be described below with specific examples:

[0116] The main process of this embodiment is as follows: Figure 2 As shown, the sensors used in this embodiment are mainly a camera and a 3D LiDAR. The basic process of 3D LiDAR matching optimization is as follows: Figure 2 As shown on the right, this embodiment incorporates visual image information, as illustrated in the appendix. Figure 2 As shown on the left. After receiving the 3D laser point cloud, the laser point cloud feature points are first extracted. Based on the camera's field of view, feature points within the camera's field of view are obtained. These laser feature points are then incorporated into visual information. The laser matching part on the right performs feature matching and error calculation. After obtaining the error, degradation detection is performed on the current environment to determine the accuracy of laser positioning. Points within the camera's field of view on the left are reprojected to obtain pixels on the corresponding pixel plane. Then, BRIEF descriptors for these pixels are calculated. The BRIEF descriptors are used as initial values ​​based on the laser matching results for descriptor matching. Then, the matching error is calculated. Finally, the two errors are fused, and the error is minimized through nonlinear optimization to obtain the final pose.

[0117] The 3D LiDAR positioning method and system fused with visual information provided in this embodiment offer more dimensional sensor information for laser matching, overcoming the positioning limitations of single-source sensors. The entire process integrates visual image information to assist in laser matching. The correspondence between laser points and image pixels is obtained through intrinsic and extrinsic parameter calibration. During matching, corresponding pixel descriptors are added to the laser points, and these descriptors are used for matching and error calculation. The fused error is then optimized to obtain the pose, improving the robustness and reliability of the entire matching and positioning method.

[0118] Although preferred embodiments of the invention have been described, those skilled in the art, upon learning the basic inventive concept, can make other changes and modifications to these embodiments. Therefore, the appended claims are intended to be interpreted as including both the preferred embodiments and all changes and modifications falling within the scope of the invention. Clearly, those skilled in the art can make various alterations and modifications to the invention without departing from its spirit and scope. Thus, if these modifications and modifications of the invention fall within the scope of the claims and their equivalents, the invention is also intended to include these modifications and modifications.

Claims

1. A 3D LiDAR positioning method that integrates visual information, characterized in that, Includes the following steps: Laser feature extraction and matching: The feature extraction and matching algorithms in LOAM are used to match the point cloud. Environmental degradation detection: Detects whether similarities or omissions exist in the current environmental structural features; Laser point reprojection: Projecting points within the camera's field of view onto the image pixel plane to obtain the pixel corresponding to the laser point; Descriptor extraction: After projecting the laser points onto the pixel plane to obtain the pixels, the image information is incorporated into the laser points; Descriptor matching and error calculation: Find the best matching point for the current laser point in the reference data; calculate the variance of the Hamming distance of all laser points that find the best match through descriptors; for the retained laser point pairs, calculate the Euclidean distance between the two points as the matching error of the laser point with descriptors. Error fusion: combines the matching error of pure laser point cloud with the matching error of laser points with descriptors; Error optimization: The LOAM optimization process is adopted to obtain the specific pose by minimizing the error.

2. The 3D LiDAR positioning method integrating visual information as described in claim 1, characterized in that, In the environmental degradation detection step, the equation for laser matching positioning is solved as follows: For the coefficient matrix Perform eigenvalue decomposition to obtain its eigenvalues ​​and eigenvectors, and define the feature threshold as... If the coefficient matrix a certain feature value Less than the feature threshold When this occurs, it indicates that the current environment is degrading. The direction of degradation is the direction represented by the eigenvector corresponding to the eigenvalue. The ratio of the minimum eigenvalue to the eigenvalue threshold is used. This indicates the current degree of environmental degradation; among which, The mathematical symbol represents finding the value of the expression that minimizes the value of the latter expression. The value of ; A is the coefficient matrix, Let be the variable to be determined, and represent the pose transformation matrix; The target point value; It is the product of the coefficient matrix and its transpose. For eigenvalues, For feature threshold, Given the current degree of environmental degradation, It is the smallest eigenvalue.

3. The 3D LiDAR positioning method integrating visual information as described in claim 1, characterized in that, In the laser point reprojection step, the reprojection formula is as follows: Where u and v are the pixel coordinates projected onto the image plane, matrix K is the camera intrinsic parameter, (R, t) is the rotation and translation matrix from the lidar coordinate system to the camera coordinate system, PL is the spatial 3D laser point, and T is the transformation matrix composed of R and t.

4. The 3D LiDAR positioning method integrating visual information as described in claim 1, characterized in that, In the steps of descriptor matching and error calculation, the retained Euclidean distance between two laser point pairs is: Where e is the matching error, xref-i, x1-i, yref-i, y1-i, zref-i, and z1-i represent the XYZ coordinates of the i-th current laser point and its matching reference point in the current matching result, respectively. The subscript ref in the middle represents the reference point, and the subscript 1 in the middle represents the current laser point. The error fusion formula is shown below: in, For fusion error, For laser point error with image descriptor, The laser point error is obtained for pure laser matching; This is the error coefficient.

5. The 3D LiDAR positioning method integrating visual information as described in claim 1, characterized in that, In the error optimization step, the optimization function is as follows: in, , Let be the rotation and translation matrices of the current laser frame to be determined; The mathematical symbol is used to find the value of the expression that is minimized when the following expression is minimized. , value, The error coefficient, lv For the laser point error with image descriptor, e l The laser point error is obtained for pure laser matching.

6. A 3D lidar positioning system that integrates visual information, characterized in that, include: Laser feature extraction and matching module (10) is used for laser feature extraction and matching: the feature extraction and feature matching algorithm in LOAM is used to match the point cloud; The environmental degradation detection module (20) is used for environmental degradation detection: detecting whether the current environmental structural features are similar or missing; Laser point reprojection module (30) is used for laser point reprojection: projecting points within the camera's field of view onto the image pixel plane to obtain the pixel points corresponding to the laser points; The descriptor extraction module (40) is used to extract descriptors: after projecting the laser points onto the pixel plane to obtain the pixels, the image information is integrated into the laser points; The descriptor matching and error calculation module (50) is used for descriptor matching and error calculation: finding the best matching point of the current laser point in the reference data; calculating the variance of the Hamming distance of all laser points that have found the best match through the descriptor; and calculating the Euclidean distance between the two laser point pairs as the matching error of the laser point with descriptor. Error fusion module (60) is used for error fusion: combining the pure laser point cloud matching error with the matching error of laser points with descriptors; Error optimization module (70) is used for error optimization: the LOAM optimization process is adopted to obtain the specific pose by minimizing the error.

7. The 3D LiDAR positioning system that integrates visual information as described in claim 6, characterized in that, In the environmental degradation detection module (20), the equation for solving the laser matching positioning is as follows: For the coefficient matrix Perform eigenvalue decomposition to obtain its eigenvalues ​​and eigenvectors, and define the feature threshold as... If the coefficient matrix a certain feature value Less than the feature threshold When this occurs, it indicates that the current environment is degrading. The direction of degradation is the direction represented by the eigenvector corresponding to the eigenvalue. The ratio of the minimum eigenvalue to the eigenvalue threshold is used. This indicates the current degree of environmental degradation; among which, The mathematical symbol represents finding the value of the expression that minimizes the value of the latter expression. The value of ; A is the coefficient matrix, Let be the variable to be determined, and represent the pose transformation matrix; The target point value; It is the product of the coefficient matrix and its transpose. For eigenvalues, For feature threshold, Given the current degree of environmental degradation, It is the smallest eigenvalue.

8. The 3D LiDAR positioning system that integrates visual information as described in claim 6, characterized in that, In the laser point reprojection module (30), the reprojection formula is as follows: Where u and v are the pixel coordinates projected onto the image plane, matrix K is the camera intrinsic parameter, (R, t) is the rotation and translation matrix from the lidar coordinate system to the camera coordinate system, PL is the spatial 3D laser point, and T is the transformation matrix composed of R and t.

9. The 3D LiDAR positioning system that integrates visual information as described in claim 6, characterized in that, In the descriptor matching and error calculation module (50), the Euclidean distance between the two laser point pairs is retained as follows: Where e is the matching error, x ref-i x 1-i y ref-i y 1-i z ref-i z 1-i These represent the XYZ coordinates of the i-th current laser point and its matching reference point in the current matching result, respectively. The subscript ref indicates the reference point, and the subscript 1 indicates the current laser point. The error fusion formula is shown below: in, For fusion error, For laser point error with image descriptor, The laser point error is obtained for pure laser matching; This is the error coefficient.

10. The 3D LiDAR positioning system that integrates visual information as described in claim 6, characterized in that, In the error optimization module (70), the optimization function is as follows: in, , Let be the rotation and translation matrices of the current laser frame to be determined; The mathematical symbol is used to find the value of the expression that is minimized when the following expression is minimized. , value, The error coefficient, For laser point error with image descriptor, The laser point error is obtained for pure laser matching.

Images