Adaptive weight-based laser radar and camera joint calibration optimization method, program, equipment and storage medium

Abstract

The invention belongs to the technical field of multi-sensor external parameter calibration, and particularly relates to a laser radar and camera joint calibration optimization method based on adaptive weight, a program, equipment and a storage medium. According to the method, the calibration optimization process is improved, a self-adaptive weight optimization strategy is provided, in each optimization iteration process, the weight is adjusted based on the current error value, and the point with the large error is endowed with the small weight, so that the interference of the point with the large error on the optimization result is reduced, the robustness of the calibration process is improved, and the calibration precision is improved. Even under the condition that noise is large or data quality is poor, accurate external parameter estimation can still be obtained. And when the error is reduced to a preset threshold value, the optimization process is automatically stopped, the convergence and efficiency of optimization are ensured, an error function is iteratively optimized by adopting a self-adaptive weight optimization method, and an optimal rotation matrix and an optimal translation vector are solved. The method can be widely applied to multi-sensor fusion tasks in the fields of automatic driving, robot navigation and the like.

Description

Technical Field

[0001] The present invention belongs to the technical field of multi-sensor extrinsic parameter calibration, and specifically relates to a laser radar and camera joint calibration optimization method, program, device and storage medium based on adaptive weights. Background Art

[0002] With the rapid development of multi-sensor fusion technology, autonomous driving, robot navigation and intelligent perception systems have put forward increasingly stringent requirements for high-precision sensor calibration.

[0003] LiDAR can accurately provide information about the target's position in three-dimensional space, but its point cloud data is sparse and lacks semantic information. Cameras can provide rich color and texture information, but their depth information is less precise. Joint calibration of LiDAR and cameras determines their extrinsic parameters (rotation matrix and translation vector), enabling data fusion and fully leveraging the complementary strengths of the two sensors.

[0004] However, due to hardware manufacturing errors and uncertainties in installation positions, different sensors exhibit inherent coordinate system deviations. Existing calibration techniques face numerous challenges. First, the data characteristics of different sensors vary significantly, making the construction of effective error metrics a key difficulty in the calibration process. Second, existing algorithms are often sensitive to initial parameters; even small parameter perturbations can significantly alter the calibration results, significantly limiting the robustness and applicability of calibration methods. Summary of the Invention

[0005] The purpose of the present invention is to solve the problems of insufficient accuracy, poor adaptability and low optimization efficiency in the existing joint calibration technology of lidar and camera, and to provide a lidar and camera joint calibration optimization method, program, equipment and storage medium based on adaptive weights. By improving the error function and optimization strategy in the traditional calibration method, the accuracy of the joint calibration can be effectively improved, and its adaptability to different sensor configurations and environmental changes can be enhanced.

[0006] A joint calibration optimization method for lidar and camera based on adaptive weights includes the following steps:

[0007] Establish a world coordinate system on the calibration plate, establish a lidar coordinate system in the lidar, and establish a camera coordinate system in the camera;

[0008] Fix the positions of the lidar and camera, move the calibration plate to different positions, and scan the calibration plate plane with the lidar each time after moving the calibration plate to obtain the coordinates of each scanning point in the lidar coordinate system, obtain multiple sets of point cloud data, and obtain the rotation matrix and translation matrix from the world coordinate system on the calibration plate to the camera coordinate system;

[0009] Using geometric relationships and point cloud data, a linear least squares problem is constructed, and the singular value decomposition method is used to calculate the initial estimates of the rotation matrix and translation matrix from the camera coordinate system to the lidar coordinate system;

[0010] The Euler angles of the rotation matrix from the camera coordinate system to the lidar coordinate system and the elements of the translation matrix are constructed as a state vector. The error function is constructed according to the distance from each scanning point in the point cloud data to the calibration plate plane. With the minimization of the error function as the goal, an adaptive weight optimization method is used to iteratively solve the state vector with the minimum value of the error function. The rotation matrix and translation matrix from the camera coordinate system to the lidar coordinate system corresponding to the state vector are output to complete the joint calibration of the lidar and camera.

[0011] Furthermore, the world coordinate system (X w ,Y w ,Z w ), establish the laser radar coordinate system (X l ,Y l ,Z l ), establish the camera coordinate system (X c ,Y c ,Z c ); Fix the laser radar and camera positions, move the calibration plate to different positions, and scan the calibration plate plane through the laser radar each time after moving the calibration plate to obtain the coordinates P of each scanning point in the laser radar coordinate system lij =(x lij ,y lij ,0), get N groups of point clouds, and get the rotation matrix R from the world coordinate system on the calibration plate to the camera coordinate system wci and the translation matrix T wci ;

[0012] Where i represents the calibration plate at the i-th position, i = 1, 2, ..., N, N is the total number of calibration plate positions; P lij Indicates the jth scanning point scanned by the lidar when the calibration plate is at the i-th position, j = 1, 2, ..., m i , m i is the total number of scanning points scanned by the lidar when the calibration plate is at the i-th position.

[0013] Furthermore, the construction of the linear least squares problem AH=B is specifically as follows:

[0014] A=[a1,...,a N ] T ,B=[b1,...,b N ] T

[0015]

[0016] a i,j =[r wci31 x lij ,r wci32 x lij ,r wci33 x lij ,r wci31 y lij ,r wci32 y lij ,r wci33 y lij ,r wci31 ,r wci32 ,r wci33 ]

[0017] Among them, R wci3 Represents the rotation matrix R wci The third column vector is a 3×1 vector, R wci3 =[r wci31 ,r wci32 ,r wci33 ] T ; Translation matrix T wci is a 3×1 column vector;

[0018] Matrix A is a G×9 matrix, Perform singular value decomposition on matrix A:

[0019]

[0020] Where Σ is the diagonal matrix consisting of all singular values ​​of matrix A; U and V are the decomposed unitary matrices;

[0021] Solve for the matrix H:

[0022]

[0023] Among them, U9 is the matrix composed of the first 9 columns of the unitary matrix U;

[0024] The solved matrix H is a 9×1 column vector, H=[h 11 ,h 21 ,h 31 ,h 12 ,h 22 ,h 32 ,h 13 ,h 23 ,h 33 ] T , reorganize the elements of the matrix H into the matrix

[0025] According to the matrix H2, get the initial estimated value R of the rotation matrix from the camera coordinate system to the lidar coordinate system cl (0) and the initial estimate of the translation matrix T cl (0);

[0026] R cl (0)=[R cl1 (0),R cl2 (0),R cl3 (0)] T , R cl1 (0), R cl2 (0), R cl3 (0) R cl The column vector formed by transposing the elements of the 1st, 2nd and 3rd rows of (0);

[0027] R cl1 (0)=[h 11 ,h 21 ,h 31 ] T , R cl2 (0)=[h 12 ,h 22 ,h 32 ] T ,

[0028] Since the matrix R cl (0) is an orthogonal matrix, so according to R cl1 (0) and R cl2 (0) Solve for R cl3 (0), we get the matrix R cl (0), and then solve to get the matrix T cl (0).

[0029] Furthermore, the Euler angles of the rotation matrix from the camera coordinate system to the lidar coordinate system and the elements of the translation matrix are constructed as a state vector, specifically:

[0030] D(t)=[ψ(t),θ(t),φ(t),T clx (t),T cly (t),T clz (t)]

[0031] The rotation matrix R from the camera coordinate system to the lidar coordinate system is represented by the state vector D(t) cl (t) and the translation matrix T cl (t);

[0032] R cl (t) = R z (ψ(t))·R y (θ(t))·Rx (φ(t)), T cl (t)=[T clx (t),T cly (t),T clz (t)] T

[0033]

[0034] Furthermore, the error function is constructed based on the distance between each scanning point in the point cloud data and the calibration plate plane, specifically:

[0035]

[0036] Among them, d i,j (t) represents the distance from the jth scanning point in the laser radar coordinate system to the calibration plate plane when the calibration plate is located at the i-th position, which is calculated according to D(t):

[0037]

[0038] O wli (t) = R cl (t)T wci +T cl (t).

[0039] Furthermore, the minimization of the error function is taken as the goal, and an adaptive weight optimization method is adopted to iteratively solve the state vector corresponding to the minimum error function value, which specifically includes the following steps:

[0040] Step 1: Initialize the number of iterations t = 1 and the error function weight Point cloud index set K1 = {1, 2, ..., N};

[0041] According to the initial estimate of the rotation matrix R from the camera coordinate system to the lidar coordinate system cl (0) and the initial estimate of the translation matrix T cl (0), construct the initial state vector D(0);

[0042] Step 2: Optimize the error function L(D(t)) and obtain the state vector D(t) with the minimum value of the L(D(t)) function.

[0043] Step 3: Calculate the error of each point cloud based on the state vector D(t) obtained by optimization C that is greater than the threshold E1 i (t) The corresponding point cloud index is from the point cloud index set K t Eliminate them and get the updated index set K t+1, the number of point cloud indexes in the updated index set is N t+1 The threshold E1 is from all C at t=1 i (1) Select;

[0044] Step 4: For the remaining N t+1 Group point clouds and calculate the average error Update the error function weight ω of each group of point clouds i (t+1);

[0045]

[0046] Step 5: If If the error is less than the set error threshold E2, the iteration is stopped and the rotation matrix R from the camera coordinate system to the lidar coordinate system corresponding to the state vector D(t) is output. cl (t) and the translation matrix T cl (t), complete the joint calibration of the lidar and camera; otherwise, set t = t + 1 and return to step 2.

[0047] Furthermore, the rotation matrix R from the world coordinate system to the camera coordinate system on the calibration plate is wci and the translation matrix T wci It was obtained using Zhang Zhengyou calibration method;

[0048] The laser radar scans the calibration plate plane and uses the random sampling consensus algorithm RANSAC to perform straight line fitting. According to the distance between each scanning point and the fitting line, the scanning points far from the fitting line are eliminated. i The total number of scan points retained after culling;

[0049] The error function L(D(t)) is optimized by using the NM algorithm.

[0050] A computer device / equipment / system includes a memory, a processor, and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the above-mentioned adaptive weight-based joint calibration optimization method for lidar and camera.

[0051] A computer-readable storage medium having a computer program / instruction stored thereon, which, when executed by a processor, implements the steps of the above-mentioned adaptive weight-based joint calibration optimization method for lidar and camera.

[0052] A computer program product includes a computer program / instruction, which, when executed by a processor, implements the steps of the above-mentioned adaptive weight-based joint calibration optimization method for lidar and camera.

[0053] The beneficial effects of the present invention are:

[0054] The present invention improves the calibration optimization process and proposes an adaptive weight optimization strategy. During each optimization iteration, the weights are adjusted based on the current error value. Points with larger errors are assigned smaller weights, thereby reducing their interference with the optimization results. This strategy significantly improves the robustness of the calibration process, allowing the system to obtain accurate external parameter estimates even in the presence of large noise or poor data quality. When the error drops to a preset threshold, the optimization process automatically stops, ensuring the convergence and efficiency of the optimization. Finally, the error function is iteratively optimized using the adaptive weight optimization method to obtain the optimal rotation matrix and translation vector. The present invention can be widely applied to multi-sensor fusion tasks in fields such as autonomous driving and robot navigation. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] Figure 1 It is the overall flow chart of the present invention.

[0056] Figure 2 This is a flowchart for implementing the adaptive weight optimization algorithm in the present invention.

[0057] Figure 3 This is a diagram of the self-assembled radar and camera experimental equipment in an embodiment of the present invention.

[0058] Figure 4 This is a joint calibration point cloud projection diagram in an embodiment of the present invention.

[0059] Figure 5 This is a diagram showing the radar point cloud and image fusion effect in an embodiment of the present invention. DETAILED DESCRIPTION

[0060] The present invention will be further described below with reference to the accompanying drawings.

[0061] The present invention designs a joint calibration optimization method for lidar and camera based on adaptive weights. First, a world coordinate system is defined on the calibration board, and coordinate systems are established in the lidar and camera respectively. Then, the conversion parameters from the world coordinate system to the camera coordinate system and the camera intrinsic parameters are obtained through the Zhang Zhengyou calibration method. Then, the initial extrinsic parameters are calculated using geometric relationships and point cloud data in the camera coordinate system.

[0062] The camera coordinate system is (X c ,Y c ,Z c ), the three central axes are oriented to the right, downward, and forward relative to the radar itself; the radar coordinate system is (X l ,Y l ,Z l ), the three axes are oriented forward, left and upward relative to themselves; a world coordinate system (X w ,Y w ,Zw ), calibration plate at X w O w Y w On the plane, Z w Pointing camera radar system.

[0063] The rotation matrix from the world coordinate system to the camera coordinate system is R wc , the translation matrix is ​​T wc The normal vector of the calibration plate plane is And pass through the world coordinate system origin O w , the two correspond to in the camera coordinate system O wc , the corresponding relationship is as follows:

[0064]

[0065] O wc =R wc O w +T wc =T wc

[0066] The point on the radar scanning calibration plate plane is represented as P in the radar coordinate system. l =[x l ,y l ,0] T , in the camera coordinate system is P c , the corresponding relationship is as follows:

[0067]

[0068] P c Located on the calibration plate plane, there are the following constraints:

[0069]

[0070] From formulas (1) and (2), we can get:

[0071]

[0072] remember P1′=[x1,y1,1] T , we can get:

[0073]

[0074] Equation (3) can be restated as follows:

[0075]

[0076] and O w In the radar coordinate system, it is expressed as and O wl , the conversion relationship is as follows:

[0077]

[0078] O wl =R cl O wc +T cl =R cl T wc +T cl (8)

[0079] The rotation matrix R and the Euler angles (θ, φ, ψ) can be converted to each other. The conversion process is as follows:

[0080] The rotation matrix is ​​converted to Euler angle as follows:

[0081] make Pitch angle θ = arcsin(-r 31 ), where θ∈[-90°,90°].

[0082] When θ≠±90°, the roll angle φ=arctan2(r 32 ,r 33 ), where φ∈[-180°,180°].

[0083] When θ≠±90°, the yaw angle ψ=arctan2(r 21 ,r 11 ), where ψ∈[-180°,180°].

[0084] When θ = ±90°, there is a universal lock problem, but this situation generally does not occur here and is not considered.

[0085] The Euler angle rotation matrix is ​​as follows (in ZYX order):

[0086] Rotate around the Z axis (yaw angle ψ), the rotation matrix is ​​as follows:

[0087]

[0088] Rotate around the Y axis (pitch angle θ), the rotation matrix is ​​as follows:

[0089]

[0090] Rotate around the Z axis (roll angle φ), the rotation matrix is ​​as follows:

[0091]

[0092] Combining the rotation matrices yields R = R z (ψ)·Ry (θ)·R x (φ).

[0093] Based on the above content, the present invention provides a joint calibration optimization method of lidar and camera based on adaptive weights, including the following steps:

[0094] Step 1: Establish the world coordinate system (X w ,Y w ,Z w ), establish the laser radar coordinate system (X l ,Y l ,Z l ), establish the camera coordinate system (X c ,Y c ,Z c );

[0095] Step 2: The laser radar and camera are fixed in position, and the calibration plate is moved to different positions. Each time the calibration plate is moved, the laser radar is used to scan the calibration plate plane to obtain the coordinates P of each scanning point in the laser radar coordinate system. lij =(x lij ,y lij ,0), get N groups of point clouds, and get the rotation matrix R from the world coordinate system on the calibration plate to the camera coordinate system wci =[R wci1 ,R wci2 ,R wci3 ] and the translation matrix T wci ;

[0096] Rotation matrix R wci and the translation matrix T wci It was obtained using Zhang Zhengyou calibration method;

[0097] Where i represents the calibration plate at the i-th position, i = 1, 2, ..., N, N is the total number of calibration plate positions; P lij Indicates the jth scanning point scanned by the lidar when the calibration plate is at the i-th position, j = 1, 2, ..., m i ;

[0098] After the lidar scan, the random sampling consensus algorithm RANSAC is used to perform straight line fitting. According to the distance from each scanning point to the fitting line, the scanning points far from the fitting line are eliminated. i The total number of scan points retained after culling;

[0099] By filtering the point cloud data through RANSAC in the radar coordinate system and constructing the error function, noise points and irregular points can be effectively removed, reducing the impact of abnormal points on the calibration results, and thus obtaining a more accurate point cloud dataset.

[0100] R wci1 、R wci2 、R wci3 Represents the rotation matrix R wci The first, second, and third column vectors of , and they are all 3×1 vectors; the translation matrix T wci is a 3×1 column vector;

[0101] Step 3: Construct a linear least squares problem AH=B, perform singular value decomposition on matrix A, and solve matrix H;

[0102] A=[a1,...,a N ] T ,B=[b1,...,b N ] T

[0103]

[0104] a i,j =[r wci31 x lij ,r wci32 x lij ,r wci33 x lij ,r wci31 y lij ,r wci32 y lij ,r wci33 y lij ,r wci31 ,r wci32 ,r wci33 ]

[0105] Among them, the calibration plate is installed at the i-th position, and the rotation matrix R wci The third column vector R wci3 =[r wci31 ,r wci32 ,r wci33 ] T ;

[0106] Matrix A is a G×9 matrix, Perform singular value decomposition on matrix A:

[0107]

[0108] Where Σ is the diagonal matrix consisting of all singular values ​​of matrix A; U and V are the decomposed unitary matrices;

[0109] Solve for the matrix H:

[0110]

[0111] Among them, U9 is the matrix composed of the first 9 columns of the unitary matrix U;

[0112] The solved matrix H is a 9×1 column vector, H=[h 11 ,h 21 ,h 31 ,h 12 ,h 22 ,h 32 ,h 13 ,h 23 ,h 33 ] T , reorganize the elements of the matrix H into the matrix

[0113] Step 4: Based on the matrix H2, obtain the preliminary estimated value R of the rotation matrix from the camera coordinate system to the lidar coordinate system cl (0) and the initial estimate of the translation matrix T cl (0);

[0114] R cl (0)=[R cl1 (0),R cl2 (0),R cl3 (0)] T , R cl1 (0), R cl2 (0), R cl3 (0) R cl The column vector formed by transposing the elements of the 1st, 2nd and 3rd rows of (0);

[0115] According to the matrix

[0116] R cl1 (0)=[h 11 ,h 21 ,h 31 ] T , R cl2 (0)=[h 12 ,h 22 ,h 32 ] T ,

[0117] Since the matrix R cl (0) is an orthogonal matrix, so according to R cl1 (0) and R cl2 (0) Solve for R cl3 (0), we get the matrix R cl (0), and then solve to get the matrix T cl (0);

[0118] Step 5: Initialize the number of iterations t = 1 and the error function weight Point cloud index set K1 = {1, 2, ..., N};

[0119] Construct the state vector D(t) = [ψ(t),θ(t),φ(t),T clx (t),T cly (t),T clz (t)], D(t) represents the rotation matrix R from the camera coordinate system to the lidar coordinate system cl (t) and the translation matrix T cl (t);

[0120] R cl (t) = R z (ψ(t))·R y (θ(t))·R x (φ(t)), T cl (t)=[T clx (t),T cly (t),T clz (t)] T

[0121]

[0122] According to the initial estimate of the rotation matrix R from the camera coordinate system to the lidar coordinate system cl (0) and the initial estimate of the translation matrix T cl (0), construct the initial state vector D(0);

[0123] Step 6: Construct the error function L(D(t)) based on the distance between each scanning point in each group of point clouds and the calibration plate plane; use the NM algorithm to optimize the error function L(D(t)) and obtain the state vector D(t) corresponding to the minimum value of the L(D(t)) function;

[0124]

[0125] Among them, d i,j (t) represents the distance from the jth scanning point in the laser radar coordinate system to the calibration plate plane when the calibration plate is located at the i-th position, which is calculated according to D(t):

[0126]

[0127] O wli (t) = R cl (t)T wci +T cl (t);

[0128] The NM algorithm uses L(D(t)) as the objective function for optimization. It initializes an initial simplex containing 7 vertices (each vertex represents a set of parameter combinations) in 6-dimensional space, calculates the objective function value corresponding to each vertex and sorts it. It adjusts the simplex through iterative reflection, expansion, contraction, or reduction operations: first, the worst vertex is reflected to generate a new parameter point. If the reflected point is better than the current optimal one, it is further expanded. Otherwise, the quality of the reflected point determines whether to replace the second-worst point; if the reflection fails, it is contracted to generate a parameter point closer to the center; if the contraction is still ineffective, the simplex is contracted toward the optimal vertex to enhance local search. The above process is repeated until the parameter space converges or the termination condition is reached (the function value is stable or the number of iterations is exhausted), and finally the optimal parameter combination and its objective function value are output;

[0129] Step 7: From all C at t=1 i (1) Select one as the threshold E1; Calculate the error of each group of point clouds based on the state vector D(t) obtained by optimization C that is greater than the threshold E1 i (t) The corresponding point cloud index is from the point cloud index set K t Eliminate them and get the updated index set K t+1 , the number of point cloud indexes in the updated index set is N t+1 ;

[0130] Step 8: For the remaining N t+1 Group point clouds and calculate the average error Update the error function weight ω of each group of point clouds i (t+1);

[0131]

[0132] Step 9: If Less than the set error threshold E2, E2 can be 10 -4 , then stop the iteration and output the rotation matrix R from the camera coordinate system to the lidar coordinate system corresponding to the state vector D(t) cl (t) and the translation matrix T cl (t), complete the joint calibration of the lidar and camera; otherwise, set t = t + 1 and return to step 6.

[0133] Example 1:

[0134] Based on the existing single-line radar LRS3000 and ZED-2i binocular camera fixed connection, such as Figure 3As shown. A checkerboard calibration plate with a square length of 10 cm and a corner point specification of "11×9" was used to calibrate multiple sets of data in the improved Autoware calibration toolbox in the ROS environment to obtain the camera's intrinsic parameters, and then optimize to obtain the extrinsic parameters. The internal algorithm has been improved according to the previous principles, and the extrinsic parameters can be calibrated with one click. The calibration results are displayed, and then all the point clouds outside the calibration plate are projected onto the image to verify the joint calibration effect. The results are shown as follows: Figure 4 、 5 shown.

[0135] The foregoing description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Those skilled in the art will readily appreciate that various modifications and variations of the present invention are possible. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the present invention are intended to be within the scope of protection of the present invention.

Claims

1. A joint calibration optimization method for lidar and camera based on adaptive weights, characterized by: Establish a world coordinate system on the calibration plate, establish a lidar coordinate system in the lidar, and establish a camera coordinate system in the camera; Fix the positions of the lidar and camera, move the calibration plate to different positions, and scan the calibration plate plane with the lidar each time after moving the calibration plate to obtain the coordinates of each scanning point in the lidar coordinate system, obtain multiple sets of point cloud data, and obtain the rotation matrix and translation matrix from the world coordinate system on the calibration plate to the camera coordinate system; Using geometric relationships and point cloud data, a linear least squares problem is constructed, and the singular value decomposition method is used to calculate the initial estimates of the rotation matrix and translation matrix from the camera coordinate system to the lidar coordinate system; The Euler angles of the rotation matrix from the camera coordinate system to the lidar coordinate system and the elements of the translation matrix are constructed as a state vector. The error function is constructed according to the distance from each scanning point in the point cloud data to the calibration plate plane. With the minimization of the error function as the goal, an adaptive weight optimization method is used to iteratively solve the state vector with the minimum value of the error function. The rotation matrix and translation matrix from the camera coordinate system to the lidar coordinate system corresponding to the state vector are output to complete the joint calibration of the lidar and camera.

2. The method for optimizing joint calibration of a laser radar and a camera based on adaptive weights according to claim 1, characterized in that: The world coordinate system (X w ,Y w ,Z w ), establish the laser radar coordinate system (X l ,Y l ,Z l ), establish the camera coordinate system (X c ,Y c ,Z c ); Fix the laser radar and camera positions, move the calibration plate to different positions, and scan the calibration plate plane through the laser radar each time after moving the calibration plate to obtain the coordinates P of each scanning point in the laser radar coordinate system lij =(x lij ,y lij ,0), get N groups of point clouds, and get the rotation matrix R from the world coordinate system on the calibration plate to the camera coordinate system wci and the translation matrix T wci ; Where i represents the calibration plate at the i-th position, i = 1, 2, ..., N, N is the total number of calibration plate positions; P lij Indicates the jth scanning point scanned by the lidar when the calibration plate is at the i-th position, j = 1, 2, ..., m i , m i is the total number of scanning points scanned by the lidar when the calibration plate is at the i-th position.

3. The method for optimizing joint calibration of a laser radar and a camera based on adaptive weights according to claim 2, characterized in that: The construction of the linear least squares problem AH=B is specifically as follows: A=[a1,...,a N ] T ,B=[b1,...,b N ] T a i,j =[r wci31 x lij ,r wci32 x lij ,r wci33 x lij ,r wci31 y lij ,r wci32 y lij ,r wci33 y lij ,r wci31 ,r wci32 ,r wci33 ] Among them, R wci3 Represents the rotation matrix R wci The third column vector is a 3×1 vector, R wci3 =[r wci31 ,r wci32 ,r wci33 ] T ; Translation matrix T wci is a 3×1 column vector; Matrix A is a G×9 matrix, Perform singular value decomposition on matrix A: Where Σ is the diagonal matrix consisting of all singular values ​​of matrix A; U and V are the decomposed unitary matrices; Solve for the matrix H: Among them, U9 is the matrix composed of the first 9 columns of the unitary matrix U; The solved matrix H is a 9×1 column vector, H=[h 11 ,h 21 ,h 31 ,h 12 ,h 22 ,h 32 ,h 13 ,h 23 ,h 33 ] T , reorganize the elements of the matrix H into the matrix According to the matrix H2, get the initial estimated value R of the rotation matrix from the camera coordinate system to the lidar coordinate system cl (0) and the initial estimate of the translation matrix T cl (0); R cl (0)=[R cl1 (0),R cl2 (0),R cl3 (0)] T , R cl1 (0), R cl2 (0), R cl3 (0) R cl The column vector formed by transposing the elements of the 1st, 2nd and 3rd rows of (0); R cl1 (0)=[h 11 ,h 21 ,h 31 ] T ,R cl2 (0)=[h 12 ,h 22 ,h 32 ] T , Since the matrix R cl (0) is an orthogonal matrix, so according to R cl1 (0) and R cl2 (0) Solve for R cl3 (0), we get the matrix R cl (0), and then solve to get the matrix T cl (0).

4. The method for optimizing joint calibration of a laser radar and a camera based on adaptive weights according to claim 3, characterized in that: The Euler angles of the rotation matrix from the camera coordinate system to the lidar coordinate system and the elements of the translation matrix are constructed as a state vector, specifically: D(t)=[ψ(t),θ(t),φ(t),T clx (t),T cly (t),T clz (t)] The rotation matrix R from the camera coordinate system to the lidar coordinate system is represented by the state vector D(t) cl (t) and the translation matrix T cl (t); R cl (t)=R z (ψ(t))·R y (θ(t))·R x (φ(t)),T cl (t)=[T clx (t),T cly (t),T clz (t)] T 5. The method for joint calibration and optimization of a laser radar and a camera based on adaptive weights according to claim 4, characterized in that: The error function is constructed based on the distance from each scanning point in the point cloud data to the calibration plate plane, specifically: Among them, d i,j (t) represents the distance from the jth scanning point in the laser radar coordinate system to the calibration plate plane when the calibration plate is located at the i-th position, which is calculated according to D(t):

6. The method for joint calibration and optimization of a laser radar and a camera based on adaptive weights according to claim 5, characterized in that: The objective is to minimize the error function, and an adaptive weight optimization method is used to iteratively solve the state vector corresponding to the minimum error function value, specifically including the following steps: Step 1: Initialize the number of iterations t = 1 and the error function weight Point cloud index set K1 = {1, 2, ..., N}; According to the initial estimate of the rotation matrix R from the camera coordinate system to the lidar coordinate system cl (0) and the initial estimate of the translation matrix T cl (0), construct the initial state vector D(0); Step 2: Optimize the error function L(D(t)) and obtain the state vector D(t) with the minimum value of the L(D(t)) function. Step 3: Calculate the error of each point cloud based on the state vector D(t) obtained by optimization C that is greater than the threshold E1 i (t) The corresponding point cloud index is from the point cloud index set K t Eliminate from the set and get the updated index set K t+1 , the number of point cloud indexes in the updated index set is N t+1 The threshold E1 is from all C at t=1 i (1) Select; Step 4: For the remaining N t+1 Group point clouds and calculate the average error Update the error function weight ω of each group of point clouds i (t+1); Step 5: If If the error is less than the set error threshold E2, the iteration is stopped and the rotation matrix R from the camera coordinate system to the lidar coordinate system corresponding to the state vector D(t) is output. cl (t) and the translation matrix T cl (t), complete the joint calibration of the lidar and camera; otherwise, set t = t + 1 and return to step 2.

7. The method for optimizing joint calibration of a laser radar and a camera based on adaptive weights according to claim 6, characterized in that: The rotation matrix R from the world coordinate system on the calibration plate to the camera coordinate system wci and the translation matrix T wci It was obtained using Zhang Zhengyou calibration method; The laser radar scans the calibration plate plane and uses the random sampling consensus algorithm RANSAC to perform straight line fitting. According to the distance between each scanning point and the fitting line, the scanning points far from the fitting line are eliminated. i The total number of scan points retained after culling; The error function L(D(t)) is optimized by using the NM algorithm.

8. A computer device / apparatus / system comprising a memory, a processor, and a computer program stored in the memory, characterized in that: The processor executes the computer program to implement the steps of the method according to any one of claims 1 to 7.

9. A computer-readable storage medium having a computer program / instruction stored thereon, characterized in that: When the computer program / instructions are executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

10. A computer program product comprising a computer program / instructions, characterized in that: When the computer program / instructions are executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

Images