A method for calibrating external parameters of radar and vision fusion

Abstract

The invention relates to the technical field of sensor external parameter calibration, and discloses a radar-vision fusion external parameter calibration method, which includes obtaining a point cloud set, obtaining a minimum outer bounding box of an area where a target object is located in an image of a single camera, segmenting the point cloud, extracting point cloud boundary points, obtaining edges in the image, initializing external parameters to be calibrated between a camera and a laser radar, an algorithm entering iteration, obtaining the minimum outer bounding box of all areas where the iteration is located, calculating a residual, calculating a cost value Cost, a Jacobian matrix, updating a parameter vector, calculating a probability value P of accepting a worse solution, reaching a specified number of iterations Iter, and finally obtaining a parameter vector; by using the method of the invention to calibrate the external parameters of a camera and a laser radar, it does not need to rely on a specific calibration target and can be applied to most scenarios.

Description

Technical Field

[0001] The present invention relates to the technical field of sensor external parameter calibration, and in particular to an external parameter calibration method for radar-vision fusion, namely, camera and laser radar. Background Art

[0002] With the rapid development of LiDAR sensors, LiDAR point cloud data and camera image data are effectively fused in many application scenarios to improve the sensor's ability to perceive the environment. The premise of data fusion of different sensors is the calibration of sensor extrinsic parameters, so camera and LiDAR extrinsic parameter calibration is a very critical step in the process of realizing camera and LiDAR data fusion.

[0003] Currently, many scholars have proposed different methods for extrinsic calibration of cameras and lidars, among which the technical solutions closest to the present invention are:

[0004] Patent application number: 202211138055.X, name: Camera and LiDAR extrinsic parameter precise calibration method and system, proposes to reproject the calibration points in the calibration plate obtained by the LiDAR to the pixel plane, and establish a residual relationship with the calibration plate data obtained by the camera to optimize the solution of the extrinsic parameters. This method relies on a specific calibration plate as the calibration target;

[0005] Patent application number: 202311244551.8, title: Camera joint external parameter calibration method, system and storage medium using laser radar, proposes to obtain the camera's pose information according to the 2D and 3D positions of the checkerboard feature points to achieve external parameter calibration, and this method also relies on the checkerboard as a specific calibration target;

[0006] The literature (Zhou Yuanyuan, Yi Peng, Ma Li. Online calibration of camera and laser extrinsic parameters based on pose interpolation [J]. Electronic Design Engineering, 2023, 31(16): 43-46.) proposed a construction projection residual loss function between the camera image and the natural planar target in the lidar point cloud, and obtained the extrinsic parameter calibration result by optimization. This method is only applicable to controlled scenes with obvious planar features;

[0007] The paper (Duan Hangqi, Peng Gang, Zhou Yicheng. Self-calibration of camera-lidar extrinsic parameters based on line feature matching and particle swarm optimization [C] / / 42nd Chinese Control Conference, 2023:9123-9129.) proposed to obtain accurate extrinsic parameters of camera and lidar by minimizing the reprojection error of 3D line features of point cloud and 2D line features of image through particle swarm algorithm. This method only uses geometric features such as line segments in point cloud and image. The extraction of geometric features is very unstable, which affects the calibration effect. In addition, the optimization time complexity based on particle swarm algorithm is high.

[0008] The paper (Yufeng Zhu, Chenghui Li, Yubo Zhang. Online Camera-LiDAR Calibration with Sensor Semantic Information [C] / / IEEE International Conference on Robotics and Automation, 2020: 4970-4976.) proposes to optimize the camera lidar extrinsic parameters based on semantic object features in images and point clouds. However, semantic features do not have the details of geometric features, and the calibration results directly based on semantic features are relatively rough, resulting in large errors in the calibration results. In addition, the use of gradient method to optimize extrinsic parameters is prone to fall into local extreme values.

[0009] In summary, the current camera and lidar extrinsic calibration methods have the following shortcomings: (1) they rely on specific calibration targets; (2) they are only applicable to specific controlled scenarios; (3) they only use a single feature, resulting in large errors in the calibration results; and (4) extrinsic parameter optimization is prone to falling into local extremes or has high time complexity. Summary of the invention

[0010] In view of the above problems existing in the existing radar-vision fusion external parameter calibration method, the present invention proposes a new radar-vision fusion external parameter calibration method.

[0011] The technical solution of the present invention is as follows:

[0012] A method for calibrating external parameters of radar-vision fusion includes the following steps:

[0013] Step 1: The point cloud set obtained by the laser radar is recorded as Point, and the existing point cloud instance segmentation method is used to process Point to obtain the target object in the point cloud, and the point clouds of all target objects are retained to form a new point cloud set Object = {p i |i=1,2,…,N}, where p i Represents the i-th point in the point cloud set Object, and N represents the number of points in Object;

[0014] Step 2: The image acquired by a single camera is recorded as Picture, and the existing image instance segmentation method is used to process the Picture to obtain the target object in the image. The area where the target object is located in the Picture is retained, and the pixel values ​​of other areas are set to 0. The minimum outer bounding box of the area where the target object is located in the Picture is obtained, recorded as Rect[v1,v2,v3,v4], where v1, v2, v3, and v4 represent the upper left vertex, upper right vertex, lower left vertex, and lower right vertex of the Rect in order;

[0015] Step 3: Use the existing region growing algorithm to perform region growing segmentation on all point clouds in the point cloud set Object, and record the result of point cloud segmentation as the set Cluster = {c j |j=1,2,…,M}, where c j represents the jth point cloud subset after point cloud segmentation, and M represents the number of point cloud subsets after point cloud segmentation;

[0016] Step 4: For all point cloud subsets c in Cluster j Using the existing point cloud boundary point extraction algorithm, each point cloud subset c j The boundary points in the point cloud are extracted and put into the point cloud boundary point set A = {a s |s=1,2,…,S}, where a s represents the sth boundary point in the boundary point set A, and S represents the number of boundary points in the set A;

[0017] Step 5: Grayscale the image processed in step 2, and use Canny edge detection to obtain the edges in the image based on the grayscale image. Put all the edge points detected into the set B = {b k |k=1,2,…,K}, where b k represents the kth edge point in the edge point set B, and K represents the number of edge points in the set B;

[0018] Step 6: Initialize the external parameters to be calibrated between the camera and the lidar, including the rotation vector R = (r1, r2, r3) and the translation vector t = (t1, t2, t3), where r1, r2, r3 represent the three elements in the vector R, and t1, t2, t3 represent the three elements in the vector t; initialize the total number of iterations of the optimization algorithm Iter, and set the random strategy start symbol flag ran = false, accept the worse solution times Count = 0, initialize the damping factor λ, the maximum value of the damping factor λ max , simulated annealing initial temperature T, cooling coefficient a, cooling threshold Tc;

[0019] Step 7: The algorithm enters the iteration and all the boundary points a in set A are s According to the current external parameter vector R and t, projection is performed according to formula (1), and all points p in the set Object are projected. i Project according to the current external parameter vector R and t according to formula (2);

[0020]

[0021]

[0022] in for a s The corresponding projection point in the image Picture, For p i The corresponding projection point in the image Picture, C is the camera internal parameter, which is preset;

[0023] Step 8: For each of the Traverse the set B to find its nearest neighbor point, denoted as b s , and get all the pictures obtained in step 7 The minimum outer bounding box of the area is denoted as Rect′[v1′,v′2,v′3,v′4], where v1′, v′2, v′3, and v′4 represent the upper left vertex, upper right vertex, lower left vertex, and lower right vertex of Rect′ in order;

[0024] Step 9: Calculate each The corresponding b s The residual between where 2s-1 and 2s Respectively represent the 2s-1th and 2sth elements in the RE vector, and They are The x and y coordinates of b s .x and b s .y are b s The x and y coordinates of

[0025]

[0026] Step 10: Calculate each v′ according to formula (4) u The corresponding v u The residual between where v′ u and v u Rect′ and the u-th vertex in Rect, rb 2u-1 and rb 2u Respectively represent the 2u-1th and 2uth elements in the vector RB, v′ u .x and v′ u .y are respectively v′ u The x and y coordinates of v u .x and v u .y are v u The x and y coordinates of

[0027]

[0028] Step 11: Calculate the cost value Cost according to formula (5) and determine whether Cost is less than or equal to the optimization end condition threshold T end , T end By presetting, if Cost≤T end , then the calibration method iteration ends, and the current external parameter vectors R and t are the calibration results. If Cost>T end , then continue to step 12;

[0029]

[0030] Where w1 and w2 are predefined weight coefficients, satisfying w1+w2=1, and U is the number of vertices in Rect or Rect′;

[0031] Step 12: Approximate using numerical differentiation methods The calculation function of the Jacobian matrix of the current external parameters R and t is recorded as And calculate the update vector of external parameters R and t according to formula (6) Where I is the identity matrix of dimension 6×6;

[0032] DE=(JE T JE+λI) -1 JE T RE (6)

[0033] Step 13: Approximate v′ using numerical differentiation methods u The calculation function of the Jacobian matrix of the current external parameters R and t is recorded as And calculate the update vector of external parameters R and t according to formula (7)

[0034]

[0035] DB=(JB T JB+λI) -1 JB T RB (7)

[0036] Step 14: Update a set of new parameter vectors Rtmp1 and ttmp1 according to formula (8), and update a set of new parameter vectors Rtmp2 and ttmp2 according to formula (9), where DE(1:3) and DE(4:6) respectively represent new vectors formed by taking 1 to 3 elements and 4 to 6 elements in the DE vector, and DB(1:3) and DB(4:6) respectively represent new vectors formed by taking 1 to 3 elements and 4 to 6 elements in the DB vector;

[0037]

[0038]

[0039] Step 15: Calculate the corresponding cost value Cost1 based on Rtmp1 and ttmp1, and calculate the corresponding cost value Cost2 based on Rtmp2 and ttmp2;

[0040] Step 16: If Cost1 < Cost2 is satisfied or Cost1 = Cost2 and w1 > w2 are satisfied, then let Delta = DE, Rtmp = Rtmp1, ttmp = ttmp1, Cost tmp = Cost1, otherwise, let Delta = DB, Rtmp = Rtmp2, ttmp = ttmp2, Cost tmp = Cost2, where Delta is the current iteration parameter update vector, Rtmp is the rotation vector obtained by the current iteration update, ttmp is the translation vector obtained by the current iteration update, and Cost tmp is the cost value obtained after the current iteration update;

[0041] Step 17: If flag ran = true or ||Delta||0 = 0 or Cost tmp = Cost, then regenerate all parameter update values in Delta according to Equation (10), and then execute Step 18, otherwise, execute Step 19, where d m represents the m-th parameter update value in Delta, and random[-0.1,0.1] represents generating a random number between -0.1 and 0.1;

[0042] d m = random[-0.1, 0.1] (10)

[0043] Step 18: Update to obtain new parameter vectors Rtmp and ttmp according to Equation (11), and calculate the corresponding cost value Cost tmp where Delta(1:3) and Delta(4:6) respectively represent new vectors formed by taking the 1st to 3rd elements and the 4th to 6th elements in the Delta vector;

[0044]

[0045] Step 19: Calculate the probability value P of accepting a worse solution according to Equation (12);

[0046] P = exp((Cost - Cost tmp ) / T) (12)

[0047] Step 20: If Cost tmpIf <Cost or P> is random[0,1], then let R = Rtmp, t = ttmp, and flag ran = false, and then update λ and T. Otherwise, determine whether λ is less than λ max . If λ < λ max , let λ = λ × 10. If λ ≥ λ max , let flag ran = true, where random[0,1] represents generating a random number between 0 and 1;

[0048] Step 21: Return to Step 7 and execute until the specified number of iterations Iter is reached. The finally obtained parameter vectors R and t are the optimized parameter vectors.

[0049] Furthermore, the steps of using the numerical differentiation method to approximate the computation function for the Jacobian matrix of the current external parameters R and t are as follows:

[0050] 12.1): Let R′ = R, t′ = t, and add a small perturbation value δ to the first element in R′;

[0051] 12.2): Project all the boundary points a in set A s onto the image Picture according to the external parameters R′ and t′ processed in Step 12.1) using Equation (13), where is the projection point corresponding to a s in the image Picture;

[0052]

[0053] 12.3): Calculate the element je (2s-1)1 in the (2s - 1)-th row and the first column and the element je (2s)1 in the 2s-th row and the first column of the Jacobian matrix JE according to Equation (14);

[0054]

[0055] 12.4): For the second and third elements in R′, calculate all the elements in the second column and the third column of the Jacobian matrix JE respectively according to the steps of 12.1)-12.3);

[0056] 12.5): For the first, second, and third elements in t′, calculate all the elements in the fourth column, the fifth column, and the sixth column of the Jacobian matrix JE respectively according to the steps of 12.1)-12.3).

[0057] Furthermore, in Step 13, the numerical differentiation method is used to approximate v′u The steps of calculating the Jacobian matrix of the current external parameters R and t are as follows:

[0058] 13.1): Let R″=R, t″=t, and add a small perturbation value δ to the first element in R″;

[0059] 13.2): All points p in the set Object i The external parameters R″ and t″ processed in step 13.1) are projected into the image Picture according to formula (15), where For p i The corresponding projection point in the image Picture;

[0060]

[0061] 13.3): Get all the pictures obtained in step 13.2) The minimum outer bounding box of the area is denoted as Rect″[v1″,v′2′,v′3′,v′4′], where v1″, v′2′, v′3′, and v′4′ represent the upper left vertex, upper right vertex, lower left vertex, and lower right vertex of Rect″ in order.

[0062] 13.4): Calculate all the elements of the first column in the Jacobian matrix JB according to formula (16);

[0063]

[0064] 13.5): For the second and third elements in R″, calculate all the elements in the second and third columns of the Jacobian matrix JB according to steps 13.1)-13.4);

[0065] 13.6): For the first, second and third elements in needle t″, calculate all the elements in the fourth column, the fifth column and the sixth column of the Jacobian matrix JB according to steps 13.1)-13.4) respectively.

[0066] Furthermore, the steps for calculating the corresponding cost value Cost1 according to Rtmp1 and ttmp1 in step 15 are as follows:

[0067] 15.1): Use Rtmp1 and ttmp1 to replace R and t in step 7 respectively, and then execute steps 7 to 10;

[0068] 15.2): Calculate the cost value Cost1 according to formula (17).

[0069]

[0070] Furthermore, the steps of calculating the corresponding cost value Cost2 based on Rtmp2 and ttmp2 in step 15 are as follows:

[0071] 15.3): Use Rtmp2 and ttmp2 to replace R and t in step 7 respectively, and execute steps 7 - 10;

[0072] 15.4): Calculate the cost value Cost2 according to formula (18).

[0073]

[0074] Furthermore, the steps of calculating the corresponding cost value Cost based on Rtmp and ttmp in step 18 tmp are as follows:

[0075] 18.1): Use Rtmp and ttmp to replace R and t in step 7 respectively, and execute steps 7 - 10;

[0076] 18.2): Calculate the cost value Cost according to formula (19). tmp .

[0077]

[0078] Furthermore, the steps of updating λ and T in step 20 are as follows:

[0079] 20.1): Judge whether Cost tmp is less than Cost. If Cost tmp < Cost, then let λ = λ / 10;

[0080] 20.2): If Cost tmp ≥ Cost, let λ = λ × 10, Count = Count + 1, and then judge whether Count % Tc is equal to 0. If Count % Tc = 0, then let T = T × a.

[0081] The beneficial effects of the present invention are as follows:

[0082] By using the method of the present invention to calibrate the external parameters of the camera and lidar, it does not need to rely on a specific calibration target, can be applied to most scenarios, and uses the edge point matching of instance segmentation objects and the object projection boundary point matching to jointly construct an optimization objective function, combining semantic features and geometric features to improve the accuracy of the calibration result. Moreover, in the process of parameter optimization iteration, the idea of guiding optimization by the Jacobian matrix and the randomness strategy of the simulated annealing algorithm are combined to quickly find a globally optimal solution. Description of the Drawings

[0083] Figure 1is a point cloud set Point selected in the embodiment of the present invention;

[0084] Figure 2 The point clouds of all target objects retained after point cloud instance segmentation in the embodiment of the present invention;

[0085] Figure 3 is a Picture selected in the embodiment of the present invention;

[0086] Figure 4 Schematic diagram of the minimum outer bounding box of the area where the target object is located in the Picture in an embodiment of the present invention;

[0087] Figure 5 The result of performing region growing segmentation on all point clouds in Object in the embodiment of the present invention;

[0088] Figure 6 is a point cloud boundary point set A obtained in an embodiment of the present invention;

[0089] Figure 7 is the edge point set B obtained in the embodiment of the present invention;

[0090] Figure 8 This is the effect of projecting part of the point cloud in Point into the image Picture according to the calibrated R and t in the embodiment of the present invention. DETAILED DESCRIPTION

[0091] The following is a detailed description of a specific implementation of a radar-vision fusion external parameter calibration method of the present invention in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0092] The present invention provides a method for calibrating external parameters of radar and vision fusion, and the specific steps are as follows:

[0093] Step 1: The point cloud set obtained by the laser radar is recorded as Point, and the existing point cloud instance segmentation method is used to process Point to obtain the target object in the point cloud, and the point clouds of all target objects are retained to form a new point cloud set Object = {p i |i=1,2,…,N}, where p i represents the i-th point in the point cloud set Object, N represents the number of points in the Object, and the point cloud set Point selected in this embodiment is as follows Figure 1 As shown in the figure, after point cloud instance segmentation, the point clouds of all target objects are retained to form a new point cloud set Object. Figure 2 As shown;

[0094] Step 2: The image acquired by a single camera is recorded as Picture, and the existing image instance segmentation method is used to process the Picture to obtain the target object in the image. The area where the target object is located in the Picture is retained, and the pixel values ​​of other areas are set to 0. The minimum outer bounding box of the area where the target object is located in the Picture is obtained, which is recorded as Rect[v1,v2,v3,v4], where v1, v2, v3, and v4 represent the upper left corner vertex, the upper right corner vertex, the lower left corner vertex, and the lower right corner vertex of the Rect in order. The Picture selected in this embodiment is as follows Figure 3 As shown, get the minimum outer bounding box of the area where the target object is located in the picture. Figure 4 As shown;

[0095] Step 3: Use the existing region growing algorithm to perform region growing segmentation on all point clouds in the point cloud set Object, and record the result of point cloud segmentation as the set Cluster = {c j |j=1,2,…,M}, where c j represents the jth point cloud subset after point cloud segmentation, M represents the number of point cloud subsets after point cloud segmentation, and the result after performing region growing segmentation on all point clouds in the point cloud set Object in this embodiment is as follows Figure 5 As shown;

[0096] Step 4: For all point cloud subsets c in Cluster j Using the existing point cloud boundary point extraction algorithm, each point cloud subset c j The boundary points in the point cloud are extracted and put into the point cloud boundary point set A = {a s |s=1,2,…,S}, where a s represents the sth boundary point in the boundary point set A, S represents the number of boundary points in the set A, and in this embodiment, the point cloud boundary point set A is as follows: Figure 6 As shown;

[0097] Step 5: Grayscale the image processed in step 2, and use Canny edge detection to obtain the edges in the image based on the grayscale image. Put all the edge points detected into the set B = {b k |k=1,2,…,K}, where b k represents the kth edge point in the edge point set B, K represents the number of edge points in the set B. In this embodiment, the edge point set B is as follows: Figure 7 As shown;

[0098] Step 6: Initialize the external parameters to be calibrated between the camera and the lidar, including the rotation vector R = (r1, r2, r3) and the translation vector t = (t1, t2, t3), where r1, r2, r3 represent the three elements in the vector R, and t1, t2, t3 represent the three elements in the vector t. Initialize the total number of iterations of the optimization algorithm Iter, and set the random strategy start symbol flag ran = false, accept the worse solution times Count = 0, initialize the damping factor λ, the maximum value of the damping factor λ max , simulated annealing initial temperature T, cooling coefficient a, cooling threshold Tc, in this embodiment, initialize Iter = 3 × 10 4 ,λ=1×10 -8 ,λ max =1×10 307 , T = 100, a = 0.99, Tc = 10, R and t are initialized as:

[0099]

[0100]

[0101] Step 7: The algorithm enters the iteration and all the boundary points a in set A are s According to the current external parameter vector R and t, projection is performed according to formula (1), and all points p in the set Object are projected. i Project according to the current external parameter vector R and t according to formula (2);

[0102]

[0103]

[0104] in for a s The corresponding projection point in the image Picture, For p i The corresponding projection point in the image Picture, C is the camera internal parameter, which is preset. In this embodiment, C is set to:

[0105]

[0106] Step 8: For each of the Traverse the set B to find its nearest neighbor point, denoted as b s , and get all the pictures obtained in step 7 The minimum outer bounding box of the area is denoted as Rect′[v1′,v′2,v′3,v′4], where v1′, v′2, v′3, and v′4 represent the upper left vertex, upper right vertex, lower left vertex, and lower right vertex of Rect′ in order;

[0107] Step 9: Calculate each The corresponding b s The residual between where 2s-1 and 2s Respectively represent the 2s-1th and 2sth elements in the RE vector, and They are The x and y coordinates of b s .x and b s .y are b s The x and y coordinates of

[0108]

[0109] Step 10: Calculate each v′ according to formula (4) u The corresponding v u The residual between where v′ u and v u Rect′ and the u-th vertex in Rect, rb 2u-1 and rb 2u Respectively represent the 2u-1th and 2uth elements in the vector RB, v′ u .x and v′ u .y are respectively v′ u The x and y coordinates of v u .x and v u .y are v u The x and y coordinates of

[0110]

[0111] Step 11: Calculate the cost value Cost according to formula (5) and determine whether Cost is less than or equal to the optimization end condition threshold T end , T end By presetting, if Cost≤T end , then the calibration method iteration ends, and the current external parameter vectors R and t are the calibration results. If Cost>T end , then continue to step 12. In this embodiment, T end Set to 10;

[0112]

[0113] Where w1 and w2 are predefined weight coefficients, satisfying w1+w2=1, and U is the number of vertices in Rect or Rect′;

[0114] Step 12: Approximate using numerical differentiation methods The calculation function of the Jacobian matrix of the current external parameters R and t is recorded as And calculate the update vector of external parameters R and t according to formula (6) Where I is the identity matrix of dimension 6×6;

[0115] DE=(JE T JE+λI) -1 JE T RE (6)

[0116] In step 12, we use numerical differentiation to approximate The steps of calculating the Jacobian matrix of the current external parameters R and t are as follows:

[0117] 12.1): Let R′ = R, t′ = t, and add a small perturbation value δ to the first element in R′;

[0118] 12.2): All boundary points a in set A s The external parameters R′ and t′ processed in step 12.1) are projected into the image Picture according to formula (13), where for a s The corresponding projection point in the image Picture;

[0119]

[0120] 12.3): According to formula (14), calculate the element je in the 2s-1th row and 1st column of the Jacobian matrix JE (2s-1)1 and the element je at row 2s and column 1 (2s)1 ;

[0121]

[0122] 12.4): For the second and third elements in R′, calculate all the elements in the second and third columns of the Jacobian matrix JE according to steps 12.1)-12.3);

[0123] 12.5): For the first, second and third elements in t′, calculate all the elements in the fourth, fifth and sixth columns of the Jacobian matrix JE according to steps 12.1)-12.3) respectively.

[0124] Step 13: Approximate v′ using numerical differentiation methods u The calculation function of the Jacobian matrix of the current external parameters R and t is recorded as And calculate the update vector of external parameters R and t according to formula (7)

[0125]

[0126] DB=(JB T JB+λI) -1 JB T RB (7)

[0127] In step 13, we use numerical differentiation to approximate v′ u The steps of calculating the Jacobian matrix of the current external parameters R and t are as follows:

[0128] 13.1): Let R″=R, t″=t, and add a small perturbation value δ to the first element in R″;

[0129] 13.2): All points p in the set Object i The external parameters R″ and t″ processed in step 13.1) are projected into the image Picture according to formula (15), where For p i The corresponding projection point in the image Picture;

[0130]

[0131] 13.3): Get all the pictures obtained in step 13.2) The minimum outer bounding box of the area is denoted as Rect″[v1″,v′2′,v′3′,v′4′], where v1″, v′2′, v′3′, and v′4′ represent the upper left vertex, upper right vertex, lower left vertex, and lower right vertex of Rect″ in order.

[0132] 13.4): Calculate all the elements of the first column in the Jacobian matrix JB according to formula (16);

[0133]

[0134] 13.5): For the second and third elements in R″, calculate all the elements in the second and third columns of the Jacobian matrix JB according to steps 13.1)-13.4);

[0135] 13.6): The first, second, and third elements in the needle t″ are used to calculate all the elements in the fourth, fifth, and sixth columns of the Jacobian matrix JB according to the steps in 13.1)-13.4), respectively.

[0136] Step 14: Update a new set of parameter vectors Rtmp1 and ttmp1 according to Equation (8), and update a new set of parameter vectors Rtmp2 and ttmp2 according to Equation (9), where DE(1:3) and DE(4:6) respectively represent new vectors formed by taking the first to third elements and the fourth to sixth elements in the DE vector, and DB(1:3) and DB(4:6) respectively represent new vectors formed by taking the first to third elements and the fourth to sixth elements in the DB vector;

[0137]

[0138]

[0139] Step 15: Calculate the corresponding cost value Cost1 according to Rtmp1 and ttmp1, and calculate the corresponding cost value Cost2 according to Rtmp2 and ttmp2;

[0140] The steps for calculating the corresponding cost value Cost1 according to Rtmp1 and ttmp1 in Step 15 are as follows:

[0141] 15.1): Use Rtmp1 and ttmp1 to replace R and t in Step 7 respectively, and then execute Steps 7 - 10;

[0142] 15.2): Calculate the cost value Cost1 according to Equation (17).

[0143]

[0144] The steps for calculating the corresponding cost value Cost2 according to Rtmp2 and ttmp2 are as follows:

[0145] 15.3): Use Rtmp2 and ttmp2 to replace R and t in Step 7 respectively, and execute Steps 7 - 10;

[0146] 15.4): Calculate the cost value Cost2 according to Equation (18).

[0147]

[0148] Step 16: If Cost1 < Cost2 is satisfied, or Cost1 = Cost2 and w1 > w2 are satisfied, then let Delta = DE, Rtmp = Rtmp1, ttmp = ttmp1, Cost tmp= Cost1, otherwise, let Delta = DB, Rtmp = Rtmp2, ttmp = ttmp2, Cost tmp = Cost2, where Delta is the parameter update vector for the current iteration, Rtmp is the rotation vector obtained by the current iteration update, ttmp is the translation vector obtained by the current iteration update, and Cost tmp The cost value obtained after the current iteration update;

[0149] Step 17: If flag ran =true or ||Delta||0=0 or Cost tmp = Cost, then all parameter update values ​​in Delta are regenerated according to formula (10), and then step 18 is executed. Otherwise, step 19 is executed, where d m Indicates the mth parameter update value in Delta, random[-0.1,0.1] means generating a random number between -0.1 and 0.1;

[0150] d m =random[-0.1, 0.1] (10)

[0151] Step 18: Update the new parameter vectors Rtmp and ttmp according to formula (11), and calculate the corresponding cost value Cost according to Rtmp and ttmp tmp , where Delta(1:3) and Delta(4:6) represent new vectors formed by taking 1 to 3 elements and 4 to 6 elements of the Delta vector respectively;

[0152]

[0153] In step 18, the corresponding cost value Cost is calculated based on Rtmp and ttmp tmp The steps are as follows:

[0154] 18.1): Use Rtmp and ttmp to replace R and t in step 7 respectively, and execute steps 7 to 10;

[0155] 18.2): Calculate the cost Cost according to formula (19) tmp .

[0156]

[0157] Step 19: Calculate the probability value P of accepting a worse solution according to formula (12);

[0158] P = exp((Cost-Cost tmp ) / T) (12)

[0159] Step 20: If Cost tmp < Cost or P> random[0,1], then let R = Rtmp, t = ttmp, flag ran = false, and then update λ and T. Otherwise, determine whether λ is less than λ max If λ < λ max , let λ = λ × 10. If λ ≥ λ max , let flag ran = true, where random[0,1] represents generating a random number between 0 and 1;

[0160] The steps to update λ and T in Step 20 are as follows:

[0161] 20.1): Determine whether Cost tmp is less than Cost. If Cost tmp < Cost, then let λ = λ / 10;

[0162] 20.2): If Cost tmp ≥ Cost, let λ = λ × 10, Count = Count + 1, and then determine whether Count % Tc is equal to 0. If Count % Tc = 0, then let T = T × a.

[0163] Step 21: Return to Step 7 and execute until the specified number of iterations Iter is reached. The finally obtained parameter vectors R and t are the optimized parameter vectors.

[0164] In this embodiment, the effect of projecting some point clouds in the point cloud set Point onto the image Picture according to the calibrated R and t is as Figure 8 shown. It can be seen that all target point clouds have a high degree of coincidence with the targets in the image Picture.

Claims

1. A method for calibrating external parameters of radar and vision fusion, characterized in that: The steps include: Step 1: Record the point cloud set obtained by the laser radar as Point, process Point to obtain the target object in the point cloud, and retain the point clouds of all target objects to form a new point cloud set Object = {p i |i=1,2,…,N}, where p i Represents the i-th point in the point cloud set Object, and N represents the number of points in Object; Step 2: The image acquired by a single camera is recorded as Picture, and the Picture is processed to obtain the target object in the image. The area where the target object is located in the Picture is retained, and the pixel values ​​of other areas are set to 0. The minimum outer bounding box of the area where the target object is located in the Picture is obtained, recorded as Rect[v1,v2,v3,v4], where v1, v2, v3, and v4 represent the upper left vertex, upper right vertex, lower left vertex, and lower right vertex of the Rect in order; Step 3: Perform region growing segmentation on all point clouds in the point cloud set Object, and record the result of point cloud segmentation as the set Cluster = {c j |j=1,2,…,M}, where c j represents the jth point cloud subset after point cloud segmentation, and M represents the number of point cloud subsets after point cloud segmentation; Step 4: For all point cloud subsets c in Cluster j , each point cloud subset c j The boundary points in the point cloud are extracted and put into the point cloud boundary point set A = {a s |s=1,2,…,S}, where a s represents the sth boundary point in the boundary point set A, and S represents the number of boundary points in the set A; Step 5: Grayscale the image processed in step 2, and then use Canny edge detection to obtain the edges in the image. Put all the detected edge points into the set B = {b k |k=1,2,…,K}, where b k represents the kth edge point in the edge point set B, and K represents the number of edge points in the set B; Step 6: Initialize the external parameters to be calibrated between the camera and the lidar, including the rotation vector R = (r1, r2, r3) and the translation vector t = (t1, t2, t3), where r1, r2, r3 represent the three elements in the vector R, and t1, t2, t3 represent the three elements in the vector t; initialize the total number of iterations of the optimization algorithm Iter, and set the random strategy start symbol flag ran = false, accept the worse solution times Count = 0, initialize the damping factor λ, the maximum value of the damping factor λ max , simulated annealing initial temperature T, cooling coefficient a, cooling threshold Tc; Step 7: The algorithm enters the iteration and all the boundary points a in set A are s According to the current external parameter vector R and t, projection is performed according to formula (1), and all points p in the set Object are projected. i Project according to the current external parameter vector R and t according to formula (2); in for a s The corresponding projection point in the image Picture, For p i The corresponding projection point in the image Picture, C is the camera internal parameter; Step 8: For each of the Traverse the set B to find its nearest neighbor point, denoted as b s , and get all the pictures obtained in step 7 The minimum outer bounding box of the area is denoted as Rect′[v1′,v′2,v′3,v′4], where v1′, v′2, v′3, and v′4 represent the upper left vertex, upper right vertex, lower left vertex, and lower right vertex of Rect′ in order; Step 9: Calculate each The corresponding b s The residual between where 2s-1 and 2s Respectively represent the 2s-1th and 2sth elements in the RE vector, and They are The x and y coordinates of b s .x and b s .y are b s The x and y coordinates of Step 10: Calculate each v′ according to formula (4) u The corresponding v u The residual between where v′ u and v u Rect′ and the u-th vertex in Rect, rb 2u-1 and rb 2u Respectively represent the 2u-1th and 2uth elements in the vector RB, v′ u .x and v′ u .y are v′ u The x and y coordinates of v u .x and v u .y are v u The x and y coordinates of Step 11: Calculate the cost value Cost according to formula (5) and determine whether Cost is less than or equal to the optimization end condition threshold T end , if Cost≤T end , then the calibration method iteration ends, and the current external parameter vectors R and t are the calibration results. If Cost>T end , then continue to step 12; Where w1 and w2 are predefined weight coefficients, satisfying w1+w2=1, and U is the number of vertices in Rect or Rect′; Step 12: Approximate using numerical differentiation methods The calculation function of the Jacobian matrix of the current external parameters R and t is recorded as And calculate the update vector of external parameters R and t according to formula (6) THE=(YOU T JE+λI) -1 YOU T RE (6) Where I is the identity matrix of dimension 6×6; Step 13: Approximate v′ using numerical differentiation methods u The calculation function of the Jacobian matrix of the current external parameters R and t is recorded as And calculate the update vector of external parameters R and t according to formula (7) DB=(JB T JB+λI) -1 JB T RB (7); Step 14: Update a new set of parameter vectors Rtmp1 and ttmp1 according to formula (8), and update a new set of parameter vectors Rtmp2 and ttmp2 according to formula (9). DE(1:3) and DE(4:6) represent new vectors formed by taking 1 to 3 elements and 4 to 6 elements of the DE vector, respectively; DB(1:3) and DB(4:6) represent new vectors formed by taking 1 to 3 elements and 4 to 6 elements of the DB vector, respectively; Step 15: Calculate the corresponding cost value Cost1 according to Rtmp1 and ttmp1, and calculate the corresponding cost value Cost2 according to Rtmp2 and ttmp2; Step 16: If Cost1 < Cost2 is satisfied or Cost1 = Cost2 is satisfied and w1 > w2, then let Delta = DE, Rtmp = Rtmp1, ttmp = ttmp1, and Cost tmp = Cost1; otherwise, let Delta = DB, Rtmp = Rtmp2, ttmp = ttmp2, and Cost tmp = Cost2, where Delta is the current iteration parameter update vector, Rtmp is the rotation vector obtained by the current iteration update, ttmp is the translation vector obtained by the current iteration update, and Cost tmp is the cost value obtained after the current iteration update; Step 17: If flag ran =true or ||Delta||0=0 or Cost tmp = Cost, then all parameter update values ​​in Delta are regenerated according to formula (10), and then step 18 is executed. Otherwise, step 19 is executed. m =random[-0.1,0.1] (10) where d m represents the mth parameter update value in Delta, random[-0.1,0.1] represents the generation of a random number between -0.1 and 0.1; ||*||0 is the symbol for the vector to take the 0 normal form; Step 18: Update the new parameter vectors Rtmp and ttmp according to formula (11), and calculate the corresponding cost value Cost according to Rtmp and ttmp tmp , Delta(1:3) and Delta(4:6) represent new vectors formed by taking 1 to 3 elements and 4 to 6 elements of the Delta vector respectively; Step 19: Calculate the probability value P of accepting a worse solution according to formula (12); P=exp((Cost-Cost tmp ) / T) (12); Step 20: If Cost tmp < Cost or P>random[0,1], then let R = Rtmp, t = ttmp, flag ran = false, and then update λ and T. Otherwise, judge whether λ is less than λ max , if λ < λ max , let λ = λ × 10. If λ ≥ λ max , let flag ran = true, where random[0,1] represents generating a random number between 0 and 1; Step 21: Return to step 7 and execute until the specified number of iterations Iter is reached. The parameter vectors R and t finally obtained are the optimized parameter vectors.

2. The method for calibrating external parameters of radar-visual fusion according to claim 1, characterized in that In step 12, we use numerical differentiation to approximate The steps of calculating the Jacobian matrix of the current external parameters R and t are as follows: 12.1): Let R′ = R, t′ = t, and add a small perturbation value δ to the first element in R′; 12.2): All boundary points a in set A s The external parameters R′ and t′ processed in step 12.1) are projected into the image Picture according to formula (13), where for a s The corresponding projection point in the image Picture; 12.3): According to formula (14), calculate the element je in the 2s-1th row and 1st column of the Jacobian matrix JE (2s-1)1 and the element je at row 2s and column 1 (2s)1 ; 12.4): For the second and third elements in R′, calculate all the elements in the second and third columns of the Jacobian matrix JE according to steps 12.1)-12.3); 12.5): For the first, second and third elements in t′, calculate all the elements in the fourth, fifth and sixth columns of the Jacobian matrix JE according to steps 12.1)-12.3) respectively.

3. The method for calibrating external parameters of radar-visual fusion according to claim 1, characterized in that In step 13, we use numerical differentiation to approximate v′ u The steps of calculating the Jacobian matrix of the current external parameters R and t are as follows: 13.1): Let R″=R, t″=t, and add a small perturbation value δ to the first element in R″; 13.2): All points p in the set Object i The external parameters R″ and t″ processed in step 13.1) are projected into the image Picture according to formula (15), where For p i The corresponding projection point in the image Picture; 13.3): Get all the pictures obtained in step 13.2) The minimum outer bounding box of the area is denoted as Rect″[v1″,v′2′,v′3′,v′4′], where v1″, v′2′, v′3′, and v′4′ represent the upper left vertex, upper right vertex, lower left vertex, and lower right vertex of Rect″ in order; 13.4): Calculate all the elements of the first column in the Jacobian matrix JB according to formula (16); 13.5): For the second and third elements in R″, calculate all the elements in the second and third columns of the Jacobian matrix JB according to steps 13.1)-13.4); 13.6): For the first, second and third elements in needle t″, calculate all the elements in the fourth column, the fifth column and the sixth column of the Jacobian matrix JB according to steps 13.1)-13.4) respectively.

4. The method for calibrating external parameters of radar-visual fusion according to claim 1, characterized in that In step 15, the steps for calculating the corresponding cost value Cost1 according to Rtmp1 and ttmp1 are as follows: 15.1): Use Rtmp1 and ttmp1 to replace R and t in step 7 respectively, and then execute steps 7 to 10; 15.2): Calculate the cost Cost1 according to formula (17) 5. The method for calibrating external parameters of radar and visual fusion according to claim 1 is characterized in that In step 15, the steps for calculating the corresponding cost value Cost2 according to Rtmp2 and ttmp2 are as follows: 15.3): Use Rtmp2 and ttmp2 to replace R and t in step 7 respectively, and execute steps 7 to 10; 15.4): Calculate the cost Cost2 according to formula (18) 6. The method for calibrating external parameters of radar-visual fusion according to claim 1, characterized in that In step 18, the corresponding cost value Cost is calculated based on Rtmp and ttmp tmp The steps are as follows: 18.1): Use Rtmp and ttmp to replace R and t in step 7 respectively, and execute steps 7 to 10; 18.2): Calculate the cost Cost according to formula (19) tmp 7. The method for calibrating external parameters of radar-visual fusion according to claim 1, characterized in that The steps for updating λ and T in step 20 are as follows: 20.1): Determine Cost tmp Whether it is less than Cost. If Cost tmp < Cost, then let λ = λ / 10; 20.2): If Cost tmp ≥Cost, let λ=λ×10, Count=Count+1, and then determine whether Count%Tc is equal to 0. If Count%Tc=0, then let T=T×a; Count%Tc means the remainder of Count divided by Tc.

Images