A target positioning method and related device integrating laser radar and camera

Abstract

The present application discloses a method and related device for target positioning that integrates laser radar and camera, and the method includes: performing rough calibration operations on the selected laser radar and camera to obtain an initial transformation matrix composed of an initial rotation vector and an initial translation vector; performing ICP matching analysis based on the point cloud data between two adjacent frames and the global source point cloud data to obtain the radar rotation vector and radar translation vector with the minimum matching error; triangulating the camera image through the radar rotation vector and the radar translation vector to obtain the camera image depth information; performing relative calculations on the camera image depth information and the preset radar image depth information based on the least squares method to obtain the optimal transformation matrix; using the optimal transformation matrix to perform positioning calculations on the target object to obtain the positioning result. The present application solves the technical problem that the existing technology has limitations to varying degrees, resulting in low accuracy of the positioning result.

Description

Technical Field

[0001] The present application relates to the technical field of object positioning, and in particular to a target positioning method and related devices integrating laser radar and camera. Background Art

[0002] Whether it is a mobile robot or an unmanned vehicle, real-time positioning and mapping are essential and critical parts. Accurate positioning is the primary link for subsequent planning. There are many existing positioning technologies, such as single-point GPS positioning, differential GPS positioning, lidar positioning, and positioning through camera vision.

[0003] However, single-point GPS relies on the number of satellites to measure the quality of positioning; differential GPS requires two stations to maintain decimeter or even centimeter-level positioning; camera vision positioning is highly dependent on the quality of the camera and the limitations of the surrounding environment; and lidar positioning involves a large amount of calculation. All positioning technologies have limitations to varying degrees, which can also lead to low accuracy in the final positioning results. Summary of the invention

[0004] The present application provides a target positioning method and related devices that integrate laser radar and camera, which are used to solve the technical problem that the existing technology has limitations to varying degrees, resulting in low accuracy of positioning results.

[0005] In view of this, the first aspect of the present application provides a target positioning method integrating a laser radar and a camera, comprising:

[0006] Perform a rough calibration operation on the selected laser radar and camera to obtain an initial transformation matrix consisting of an initial rotation vector and an initial translation vector;

[0007] Perform ICP matching analysis on the point cloud data between two adjacent frames and the global source point cloud data to obtain the radar rotation vector and radar translation vector with the minimum matching error;

[0008] Triangulating the camera image by using the radar rotation vector and the radar translation vector to obtain camera image depth information;

[0009] Based on the least square method, relative calculation is performed on the camera image depth information and the preset radar image depth information to obtain an optimal transformation matrix;

[0010] The optimal transformation matrix is ​​used to perform positioning calculation on the target object to obtain a positioning result.

[0011] Preferably, the rough calibration operation on the selected laser radar and camera obtains an initial transformation matrix consisting of an initial rotation vector and an initial translation vector, including:

[0012] A preset measurement method is used to obtain a coarse calibration rotation vector and a coarse calibration translation vector between the selected lidar and the camera;

[0013] Based on the nonlinear least squares method, the coarse calibration rotation vector and the coarse calibration translation vector are optimized and calculated according to preset reference points selected in the laser radar and the camera to obtain an initial transformation matrix composed of an initial rotation vector and an initial translation vector.

[0014] Preferably, the triangulating the camera image by using the radar rotation vector and the radar translation vector to obtain the camera image depth information includes:

[0015] Normalizing the coordinates of the preset feature points in the camera image by using the radar rotation vector and the radar translation vector to obtain a normalization formula;

[0016] The depth information corresponding to the preset feature points is calculated based on the normalization formula to obtain the camera image depth information.

[0017] A second aspect of the present application provides a target positioning device integrating a laser radar and a camera, comprising:

[0018] A coarse calibration module is used to perform coarse calibration operations on the selected lidar and camera to obtain an initial transformation matrix consisting of an initial rotation vector and an initial translation vector;

[0019] A matching analysis module is used to perform ICP matching analysis on the point cloud data between two adjacent frames and the global source point cloud data to obtain the radar rotation vector and radar translation vector with the minimum matching error;

[0020] A triangulation module, used for performing triangulation processing on the camera image by using the radar rotation vector and the radar translation vector to obtain camera image depth information;

[0021] A relative calculation module, used for performing relative calculation on the camera image depth information and the preset radar image depth information based on the least square method to obtain an optimal transformation matrix;

[0022] The positioning calculation module is used to use the optimal transformation matrix to perform positioning calculation on the target object to obtain a positioning result.

[0023] Preferably, the rough calibration module is specifically used for:

[0024] A preset measurement method is used to obtain a coarse calibration rotation vector and a coarse calibration translation vector between the selected lidar and the camera;

[0025] Based on the nonlinear least squares method, the coarse calibration rotation vector and the coarse calibration translation vector are optimized and calculated according to preset reference points selected in the laser radar and the camera to obtain an initial transformation matrix composed of an initial rotation vector and an initial translation vector.

[0026] Preferably, the triangulation module is specifically used for:

[0027] Normalizing the coordinates of the preset feature points in the camera image by using the radar rotation vector and the radar translation vector to obtain a normalization formula;

[0028] The depth information corresponding to the preset feature points is calculated based on the normalization formula to obtain the camera image depth information.

[0029] A third aspect of the present application provides a target positioning device that integrates a laser radar and a camera, the device comprising a processor and a memory;

[0030] The memory is used to store program code and transmit the program code to the processor;

[0031] The processor is used to execute the target positioning method integrating laser radar and camera described in the first aspect according to the instructions in the program code.

[0032] In a fourth aspect, the present application provides a computer-readable storage medium, which is used to store program code, and the program code is used to execute the target positioning method of fusing laser radar and camera described in the first aspect.

[0033] In a fifth aspect, the present application provides a computer program product comprising instructions, which, when executed on a computer, enables the computer to execute the target positioning method of fusing laser radar and camera as described in the first aspect.

[0034] It can be seen from the above technical solutions that the embodiments of the present application have the following advantages:

[0035] In the present application, a target positioning method integrating laser radar and camera is provided, including: performing a rough calibration operation on the selected laser radar and camera to obtain an initial transformation matrix composed of an initial rotation vector and an initial translation vector; performing ICP matching analysis on the point cloud data between two adjacent frames and the global source point cloud data to obtain a radar rotation vector and a radar translation vector with the minimum matching error; triangulating the camera image through the radar rotation vector and the radar translation vector to obtain the camera image depth information; performing relative calculation on the camera image depth information and the preset radar image depth information based on the least squares method to obtain an optimal transformation matrix; and performing positioning calculation on the target object using the optimal transformation matrix to obtain a positioning result.

[0036] The target positioning method integrating laser radar and camera provided in this application integrates the two positioning technologies of laser radar and camera, optimizes the camera image through the radar rotation vector and radar translation vector generated by the point cloud matching of the laser radar, obtains more accurate camera image depth information, makes the optimal transformation matrix calculated based on the camera image depth information and the preset radar image depth information more reliable, and thus improves the accuracy of the positioning result. Therefore, this application can solve the technical problem that the existing technology has limitations to varying degrees, resulting in low accuracy of the positioning result. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] Figure 1 A schematic diagram of a flow chart of a target positioning method integrating a laser radar and a camera provided in an embodiment of the present application;

[0038] Figure 2 A schematic diagram of the structure of a target positioning device that integrates a laser radar and a camera provided in an embodiment of the present application. DETAILED DESCRIPTION

[0039] In order to enable those skilled in the art to better understand the solution of the present application, the technical solution in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0040] For easier understanding, see Figure 1 , an embodiment of a target positioning method integrating a laser radar and a camera provided in the present application includes:

[0041] Step 101: perform a rough calibration operation on the selected laser radar and camera to obtain an initial transformation matrix consisting of an initial rotation vector and an initial translation vector.

[0042] Furthermore, step 101 includes:

[0043] A preset measurement method is used to obtain a coarse calibration rotation vector and a coarse calibration translation vector between the selected lidar and the camera;

[0044] Based on the nonlinear least squares method, the coarse calibration rotation vector and the coarse calibration translation vector are optimized and calculated according to the preset reference points selected in the lidar and the camera, and an initial transformation matrix consisting of the initial rotation vector and the initial translation vector is obtained.

[0045] The corresponding coarse calibration rotation vector and coarse calibration translation vector can be obtained between the laser radar and the camera through the preset measurement method, and the preset measurement method can also be a measurement tool in actual operation. The number of coarse calibration rotation vectors and coarse calibration translation vectors is 3 and 1. The preset reference point generally refers to the corresponding position point in the laser radar and the camera. The optimization calculation based on the reference point at the same position can ensure the accuracy of the initial rotation vector and the initial translation vector, making the initial transformation matrix more reliable. The initial transformation matrix can achieve preliminary positioning and obtain the preliminary positioning result of the target, but the positioning calculation still needs to be further optimized.

[0046] The nonlinear least squares method is a parameter estimation method that uses the minimum sum of squares of errors as the criterion to estimate the parameters of nonlinear static models. Since this method is nonlinear, it cannot obtain parameter estimates by finding the extreme value of a multivariate function like the linear least squares method, but requires a complex optimization algorithm to solve; there are two commonly used algorithms, one is the search algorithm and the other is the iterative algorithm. In addition to being used directly to estimate the parameters of static nonlinear models, the nonlinear least squares method is also used in time series modeling and parameter estimation of continuous dynamic models.

[0047] The specific optimization calculation formula is:

[0048]

[0049] Where n is the number of preset reference points selected, x i ,y i are the coordinates of the i-th preset reference point in the laser radar coordinate system and the camera coordinate system, R 标 , T 标 are the coarse calibration rotation vector and coarse calibration translation vector with the smallest error respectively.

[0050] Step 102: Perform ICP matching analysis on the point cloud data between two adjacent frames and the global source point cloud data to obtain a radar rotation vector and a radar translation vector with the minimum matching error.

[0051] The global source point cloud data is all the point cloud data corresponding to the global map, and the point cloud data between two laser radar frames can reflect a certain position change of the target, that is, rotation and translation. The main principle of the ICP matching algorithm is to use the nearest point search algorithm to solve the problem based on free-form surfaces. The specific processing flow can be divided into: select the first point set in the target point cloud data, select the second point set in the source point cloud data and minimize the distance between the first point set and the second point set, which can be specifically the Euclidean distance; then calculate the rotation vector and translation vector between the two point sets to minimize the error function value; the new first point set can be obtained by calculating the rotation vector and translation vector; then calculate the average distance between the new first point set and the second point set. If the average distance is less than the preset distance value or the iterative optimization exceeds the preset number of times, stop the iteration and determine the radar rotation vector and radar translation vector at this time. Otherwise, return to the step of selecting the second point set in the source point cloud data and continue to iterate until convergence.

[0052] The specific ICP matching calculation formula is:

[0053]

[0054] Among them, E(R 1 ,T 1 ), p i is the point cloud data between two adjacent frames, q i is the global source point cloud data, R 1 ,t 1 are the rotation vector and translation vector between the point cloud data respectively, and n is the number of selected point cloud data.

[0055] Step 103: triangulate the camera image using the radar rotation vector and the radar translation vector to obtain camera image depth information.

[0056] Furthermore, step 103 includes:

[0057] The coordinates of the preset feature points in the camera image are normalized by using the radar rotation vector and the radar translation vector to obtain a normalization formula;

[0058] The depth information corresponding to the preset feature points is calculated based on the normalization formula to obtain the camera image depth information.

[0059] Optimization is the process of triangulating each point of the image data obtained by the camera and combining them to obtain the depth information of the camera image. Using the pose estimation calculated by the lidar to optimize the camera pose estimation can improve the accuracy of positioning and compensate for the impact of the environment on the camera image. The essence of triangulation is to find the depth value and use the triangular relationship to calculate the depth information of the three-dimensional point.

[0060] Let the two selected preset feature points be x 1 and x 2 , then we have:

[0061] s 1 x 1 =s 2 R 1 x 2 +t 1 ;

[0062] Among them, s 1 、s 2 are the depth information corresponding to two preset feature points. According to this formula, the following formula can be further obtained:

[0063]

[0064] in For x 1 The antisymmetric matrix of x 1 、x 2 Both represent two-dimensional projection points.

[0065] Step 104: perform relative calculation on the camera image depth information and the preset radar image depth information based on the least square method to obtain an optimal transformation matrix.

[0066] The least squares method finds the best function match for the data by minimizing the sum of squares of errors. The least squares method can be used to easily obtain unknown data and minimize the sum of squares of errors between the obtained data and the actual data. In this embodiment, the optimal transformation matrix is ​​obtained based on this principle. The specific calculation formula is:

[0067]

[0068] Among them, R and t are the optimal rotation vector and optimal translation vector between the laser radar and the camera, respectively, and S 1 , S 2 They are the camera image depth information and the preset radar image depth information respectively, and n is the number of selected feature points.

[0069] Step 105: Use the optimal transformation matrix to perform positioning calculation on the target object to obtain a positioning result.

[0070] The target positioning method integrating laser radar and camera provided in the embodiment of the present application integrates the two positioning technologies of laser radar and camera, optimizes the camera image through the radar rotation vector and radar translation vector generated by the point cloud matching of the laser radar, obtains more accurate camera image depth information, makes the optimal transformation matrix calculated based on the camera image depth information and the preset radar image depth information more reliable, and thus improves the accuracy of the positioning result. Therefore, the embodiment of the present application can solve the technical problem that the existing technology has limitations to varying degrees, resulting in low accuracy of the positioning result.

[0071] For easier understanding, see Figure 2 The present application provides an embodiment of a target positioning device integrating a laser radar and a camera, including:

[0072] A coarse calibration module 201 is used to perform a coarse calibration operation on the selected laser radar and camera to obtain an initial transformation matrix consisting of an initial rotation vector and an initial translation vector;

[0073] A matching analysis module 202 is used to perform ICP matching analysis on the point cloud data between two adjacent frames and the global source point cloud data to obtain a radar rotation vector and a radar translation vector with the minimum matching error;

[0074] A triangulation module 203 is used to triangulate the camera image through the radar rotation vector and the radar translation vector to obtain the camera image depth information;

[0075] A relative calculation module 204 is used to perform relative calculation on the camera image depth information and the preset radar image depth information based on the least square method to obtain an optimal transformation matrix;

[0076] The positioning calculation module 205 is used to perform positioning calculation on the target object using the optimal transformation matrix to obtain a positioning result.

[0077] Furthermore, the rough calibration module 201 is specifically used for:

[0078] A preset measurement method is used to obtain a coarse calibration rotation vector and a coarse calibration translation vector between the selected lidar and the camera;

[0079] Based on the nonlinear least squares method, the coarse calibration rotation vector and the coarse calibration translation vector are optimized and calculated according to the preset reference points selected in the lidar and the camera, and an initial transformation matrix consisting of the initial rotation vector and the initial translation vector is obtained.

[0080] Furthermore, the triangulation module 203 is specifically used for:

[0081] The coordinates of the preset feature points in the camera image are normalized by using the radar rotation vector and the radar translation vector to obtain a normalization formula;

[0082] The depth information corresponding to the preset feature points is calculated based on the normalization formula to obtain the camera image depth information.

[0083] For ease of understanding, the present application also provides a target positioning device that integrates a laser radar and a camera, the device including a processor and a memory;

[0084] The memory is used to store the program code and transmit the program code to the processor;

[0085] The processor is used to execute the target positioning method integrating the laser radar and the camera in the above method embodiment according to the instructions in the program code.

[0086] The present application also provides a computer-readable storage medium, which is used to store program code, and the program code is used to execute the target positioning method of fusing laser radar and camera in the above method embodiment.

[0087] The present application also provides a computer program product including instructions, which, when executed on a computer, enables the computer to execute the target positioning method of fusing laser radar and camera in the above method embodiment.

[0088] In the several embodiments provided in the present application, it should be understood that the disclosed devices and methods can be implemented in other ways. For example, the device embodiments described above are only schematic. For example, the division of the units is only a logical function division. There may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or units, which can be electrical, mechanical or other forms.

[0089] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0090] In addition, each functional unit in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0091] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application is essentially or the part that contributes to the prior art or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including a number of instructions for executing all or part of the steps of the method described in each embodiment of the present application through a computer device (which can be a personal computer, a server, or a network device, etc.). The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (full name in English: Read-Only Memory, English abbreviation: ROM), random access memory (full name in English: Random Access Memory, English abbreviation: RAM), disk or optical disk and other media that can store program codes.

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

Claims

1. A target positioning method integrating laser radar and camera, characterized in that: include: Perform a rough calibration operation on the selected laser radar and camera to obtain an initial transformation matrix consisting of an initial rotation vector and an initial translation vector; According to the ICP matching analysis between the point cloud data of two adjacent frames and the global source point cloud data, the radar rotation vector and radar translation vector with the minimum matching error are obtained. The ICP matching calculation formula is: Among them, E(R1,T1) is the average distance between point clouds, p i is the point cloud data between two adjacent frames, q i is the global source point cloud data, R1 and t1 are the rotation vector and translation vector between the point cloud data, respectively, and n is the number of selected point cloud data; The camera image is triangulated by the radar rotation vector and the radar translation vector to obtain the camera image depth information. The specific process includes: Normalizing the coordinates of the preset feature points in the camera image by using the radar rotation vector and the radar translation vector to obtain a normalization formula; Calculating the depth information corresponding to the preset feature points based on the normalization formula to obtain the camera image depth information; Based on the least square method, relative calculation is performed on the camera image depth information and the preset radar image depth information to obtain an optimal transformation matrix; The optimal transformation matrix is ​​used to perform positioning calculation on the target object to obtain a positioning result.

2. The target positioning method integrating laser radar and camera according to claim 1, characterized in that: The rough calibration operation on the selected laser radar and camera obtains an initial transformation matrix consisting of an initial rotation vector and an initial translation vector, including: A preset measurement method is used to obtain a coarse calibration rotation vector and a coarse calibration translation vector between the selected lidar and the camera; Based on the nonlinear least squares method, the coarse calibration rotation vector and the coarse calibration translation vector are optimized and calculated according to preset reference points selected in the laser radar and the camera to obtain an initial transformation matrix composed of an initial rotation vector and an initial translation vector.

3. A target positioning device integrating laser radar and camera, characterized in that: include: A coarse calibration module is used to perform coarse calibration operations on the selected lidar and camera to obtain an initial transformation matrix consisting of an initial rotation vector and an initial translation vector; The matching analysis module is used to perform ICP matching analysis based on the point cloud data between two adjacent frames and the global source point cloud data to obtain the radar rotation vector and radar translation vector with the minimum matching error. The ICP matching calculation formula is: Among them, E(R1,T1) is the average distance between point clouds, p i is the point cloud data between two adjacent frames, q i is the global source point cloud data, R1 and t1 are the rotation vector and translation vector between the point cloud data, respectively, and n is the number of selected point cloud data; A triangulation module is used to triangulate the camera image through the radar rotation vector and the radar translation vector to obtain camera image depth information. The triangulation module is specifically used to: Normalizing the coordinates of the preset feature points in the camera image by using the radar rotation vector and the radar translation vector to obtain a normalization formula; Calculating the depth information corresponding to the preset feature points based on the normalization formula to obtain the camera image depth information; A relative calculation module, used for performing relative calculation on the camera image depth information and the preset radar image depth information based on the least square method to obtain an optimal transformation matrix; The positioning calculation module is used to use the optimal transformation matrix to perform positioning calculation on the target object to obtain a positioning result.

4. The target positioning device integrating laser radar and camera according to claim 3, characterized in that: The rough calibration module is specifically used for: A preset measurement method is used to obtain a coarse calibration rotation vector and a coarse calibration translation vector between the selected lidar and the camera; Based on the nonlinear least squares method, the coarse calibration rotation vector and the coarse calibration translation vector are optimized and calculated according to preset reference points selected in the laser radar and the camera to obtain an initial transformation matrix composed of an initial rotation vector and an initial translation vector.

5. A target positioning device integrating laser radar and camera, characterized in that: The device comprises a processor and a memory; The memory is used to store program code and transmit the program code to the processor; The processor is used to execute the target positioning method of integrating laser radar and camera as described in any one of claims 1-2 according to the instructions in the program code.

6. A computer-readable storage medium, characterized in that: The computer-readable storage medium is used to store program code, and the program code is used to execute the target positioning method integrating laser radar and camera as described in any one of claims 1-2.

7. A computer program product comprising instructions, characterized in that When it runs on a computer, the computer executes the target positioning method integrating laser radar and camera as described in any one of claims 1-2.

Images