An External Parameter Calibration Method for a Rotating LiDAR

Abstract

The present invention discloses an external parameter calibration method for a rotating lidar, including: obtaining a first half-scan point cloud and a second half-scan point cloud scanned by the rotating lidar; determining whether the two scanned point clouds completely overlap. If so, output the current external parameters of the rotating lidar as the final external parameters. Otherwise, perform grid division on the first half-scan point cloud and the second half-scan point cloud respectively to obtain a number of divided first half-scan grids and a number of second half-scan grids; use a grid filtering method to obtain the approximate probability of the planes contained in all half-scan grids; then perform screening to obtain the corresponding optimal half-scan grids; use a plane model fitting method to extract the plane point clouds in all optimal half-scan grids; perform a plane matching operation on the plane point clouds to obtain a matching result; use the matching result to optimize the current external parameters of the rotating lidar to obtain new external parameters of the rotating lidar; take the new external parameters as the current external parameters and return to the judgment step.

Description

Technical Field

[0001] The present invention relates to the technical field of lidar, and particularly relates to an external parameter calibration method for a rotating lidar. Background Art

[0002] In the past decade, three-dimensional lidar has been widely used in the fields of robotics, autonomous driving, and geographic mapping due to its advantages of wide sensing range, high measurement accuracy, and strong anti-interference ability. However, the defects of most three-dimensional lidars, such as small field of view and low resolution in the vertical direction, limit their application in the fields of indoor closed environment for unmanned aerial vehicles or unmanned vehicle autonomous driving. Due to technical bottlenecks and high costs, simply increasing the laser scanning beam to expand the vertical field of view is a challenging task. Therefore, rotating the entire lidar around a fixed axis to obtain a larger scanning range in the vertical direction is a more economical and effective solution. The external parameters of a rotating lidar are the relative pose between the lidar center and the rotating device, and their accuracy will greatly affect the quality of lidar data. However, due to the inevitable deviations in the processing and installation of the rotating device, as well as the influence of material thermal expansion and contraction characteristics and mechanical wear, the initial external parameters of the rotating lidar during design can only be used as reference values, and its external parameters need to be recalibrated before actual operation. Therefore, it is crucial to efficiently and conveniently calibrate the external parameters of a rotating lidar in an unprepared environment. Although existing calibration methods have solved the problem of ensuring the accuracy and robustness of external parameter calibration in an unprepared environment, due to their large amount of operation data, the probability of misjudgment of GRSC is relatively large. Summary of the Invention

[0003] The purpose of the present invention is to provide an external parameter calibration method for a rotating lidar, which can reduce the amount of operation data of GRSC while reducing the probability of its misjudgment, thereby improving the accuracy of external parameter calibration of the rotating lidar.

[0004] The technical solution of the present invention to solve the above technical problems is as follows:

[0005] The present invention provides an external parameter calibration method for a rotating lidar, and the external parameter calibration method for the rotating lidar includes:

[0006] S1: Obtain the first half-scan point cloud and the second half-scan point cloud scanned by the rotating lidar;

[0007] S2: Determine whether the first half-scan point cloud and the second half-scan point cloud completely overlap. If so, output the current external parameters of the rotating lidar as the final external parameters; otherwise, proceed to step S3;

[0008] S3: Respectively perform mesh division on the first half-scan point cloud and the second half-scan point cloud to obtain a number of divided first half-scan meshes and a number of second half-scan meshes;

[0009] S4: Use a mesh filtering method to obtain the approximate probability of a plane contained in each of the first half-scan meshes and each of the second half-scan meshes;

[0010] S5: According to the approximate probability, screen a number of the first half-scan meshes and a number of the second half-scan meshes to obtain a number of first optimal half-scan meshes and a number of second optimal half-scan meshes;

[0011] S6: Use a plane model fitting method to extract the plane point cloud in each of the first optimal half-scan meshes and the second optimal half-scan meshes;

[0012] S7: Perform a plane matching operation on the plane point cloud in the first optimal half-scan meshes and the second optimal half-scan meshes to obtain a matching result;

[0013] S8: Use the matching result to optimize the current external parameters of the rotating lidar to obtain new external parameters of the rotating lidar;

[0014] S9: Take the new external parameters as the current external parameters and return to step S2.

[0015] Optionally, before the step S1, the external parameter calibration method of the rotating lidar further includes:

[0016] Obtain the rotation angle of the rotating lidar;

[0017] Convert the coordinate system where the rotating lidar is located into a fixed coordinate system;

[0018] According to the rotation angle and the origin of the fixed coordinate system, construct a first half-scan model and a second half-scan model;

[0019] Obtain the first half-scan point cloud according to the first half-scan model and / or obtain the second half-scan point cloud according to the second half-scan model.

[0020] Optionally, the first half-scan model is:

[0021]

[0022] The second half-scan model is:

[0023]

[0024] Wherein, represents the coordinates of the i-th point cloud in the fixed coordinate system, represents the rotation angle.

[0025] Optionally, the step S4 includes:

[0026] Convert the independent scan frame point clouds in each of the first half-scan grids and each of the second half-scan grids into ordered depth maps respectively;

[0027] Obtain two adjacent points in the first direction of the target point in the depth map;

[0028] Perform curvature calculation on the target point and two adjacent points in the first direction where it is located to obtain a first curvature calculation result;

[0029] If the first curvature result is a first preset result, mark the target point and two adjacent points in the first direction where it is located, and perform curvature calculation on two adjacent points in the second direction of the target point to obtain a second curvature calculation result;

[0030] If the second curvature calculation result is a second preset result, mark the target point, two adjacent points in the first direction of the target point, and two adjacent points in the second direction of the target point as preset plane points;

[0031] Perform coordinate transformation on all the preset plane points and convert them into the first half-scan grid and the second half-scan grid respectively to form a new first half-scan grid and a new second half-scan grid respectively;

[0032] Perform downsampling operations on the first half-scan grid, the second half-scan grid, the new first half-scan grid, and the new second half-scan grid;

[0033] Perform grid division on the downsampled first half-scan grid, second half-scan grid, new first half-scan grid, and new second half-scan grid to obtain a number of first half-scan sub-grids, second half-scan sub-grids, new first half-scan sub-grids, and new second half-scan sub-grids;

[0034] Obtain the approximate probability of the first half-scan grid according to the first half-scan sub-grid and the corresponding new first half-scan sub-grid; and / or obtain the approximate probability of the second half-scan grid according to the second scan sub-grid and the corresponding new second half-scan sub-grid.

[0035] Optionally, the first curvature calculation result is obtained by the following method:

[0036]

[0037] Among them, c represents the first direction, represents the angle between the i-th point and two adjacent points in the first direction where it is located, and respectively represent the points two adjacent points in the first direction;

[0038] The second curvature calculation result is obtained in the following way:

[0039]

[0040] Among them, r represents the second direction, represents the curvature calculation result of the i-th point in the second direction, n is a constant, represents the target point, k represents the number of adjacent points of the target point, represents the coordinates of the adjacent points of the target point;

[0041] The approximate probability is obtained in the following way:

[0042]

[0043] Among them, represents the approximate probability of the n i th grid, n i represents the number of preset plane points in the i-th grid, ||g i || weight represents the normalization parameter of the number of preset plane points and ||g i || represents the distance between the target grid and the rotating lidar, r max represents the maximum measurement distance of the lidar, r min represents the minimum measurement distance of the lidar.

[0044] Optionally, the step S7 includes:

[0045] S71: Take one of the plane point clouds in the first optimal semi-scanned grid point cloud data and the plane point cloud in the second optimal semi-scanned grid point cloud data as the current plane point cloud, and the other as the paired plane point cloud that matches the current plane point cloud for preliminary matching to obtain a candidate plane pair;

[0046] S72: Obtain the plane parameter pair of the current candidate plane pair;

[0047] S73: Obtain the associated plane judgment parameter according to the plane parameter pair;

[0048] S74: Determine whether the current candidate plane pair is an associated plane according to the associated plane judgment parameter. If it is, proceed to step S75; otherwise, proceed to step S76;

[0049] S75: Determine whether the current candidate plane pair is the last group of candidate plane pairs. If it is, output the associated plane as the matching result and proceed to step S8; otherwise, proceed to step S76;

[0050] S76: Re-obtain a candidate plane pair and return to step S71.

[0051] Optionally, in step S72, the plane parameter pair includes:

[0052] The centroid coordinates of the current plane point cloud and / or the paired plane point cloud, and the plane unit normal vector of the current plane point cloud and / or the paired plane point cloud;

[0053] In step S73, the associated plane judgment parameter includes:

[0054] The centroid distance between the centroid of the current plane point cloud and the paired plane point cloud, the projection centroid distance between the projection of the centroid coordinate of one of the current plane point cloud and / or the paired plane point cloud on the other and the centroid coordinate of the other, and the angle between the unit normal vectors of the current plane point cloud and the paired plane point cloud.

[0055] Optionally, step S74 includes:

[0056] S741: Determine whether the centroid distance is less than a first preset threshold. If it is, proceed to step S742; otherwise, proceed to step S76;

[0057] S742: Determine whether the projection centroid distance is less than a second preset threshold. If it is, proceed to step S743; otherwise, proceed to step S76;

[0058] S743: Determine whether the angle between the unit normal vectors is less than a third preset threshold. If it is, proceed to step S75; otherwise, return to step S76.

[0059] Optionally, the centroid distance Δct ij is calculated by the following method:

[0060] Δct ij = ct f,i - ct l,j

[0061] where, ct f,i and ct l,jrespectively represent the centroid of the current plane point cloud and the centroid of the paired plane point cloud;

[0062] The projected centroid distance is calculated in the following way:

[0063]

[0064] where, Δct ij represents the centroid distance, nv l,j represents the unit normal vector of the current plane point cloud or the paired plane point cloud;

[0065] The included angle Δθ between the unit normal vectors ij is calculated in the following way:

[0066] Δθ ij = cos -1 (nv f,i · nv l,j )

[0067] where, nv f,i and nv l,j respectively represent the unit normal vector of the current plane point cloud and the unit normal vector of the paired plane point cloud.

[0068] Optionally, the extrinsic parameters of the rotating lidar include estimated extrinsic parameters;

[0069] The calculation method of the estimated extrinsic parameters is:

[0070]

[0071] where, W* represents the set of estimated extrinsic parameters, F(·) represents the cost function of the similarity between the current plane point cloud and the paired plane point cloud and F f (·) and F l (·) respectively represent the objective function of the point cloud from the current plane to the midpoint of the paired plane, and the objective function of the point cloud from the paired plane to the midpoint of the current plane and (*) represents represents point is the initial point in the coordinate system of the rotating lidar, f f (*) and f l (*) are the functions of the distance between point and plane p f,i and p l,j and w f and w l are the weights of the distance and and represent the weight of the planar observation accuracy and

[0072] ct f,i and ct l,j represent the centroid of the current planar point cloud and the centroid of the paired planar point cloud respectively, s d 、s i represent the distance between the rotating lidar and the target plane and the weight ratio of the incident angle respectively, and s d +s i =1, p f,i and p l,j represent the current planar point cloud and the paired planar point cloud respectively, is the coordinate of the estimated parameter and represents the estimated parameter, r max represents the maximum measurement distance of the lidar; nv f,i and nv l,j represent the unit normal vector of the current planar point cloud and the unit normal vector of the paired planar point cloud respectively.

[0073] The present invention has the following beneficial effects:

[0074] 1) On the one hand, the curvature calculation is only carried out during the interval time of continuous scanning, without occupying the calibration running time; on the other hand, the approximate probability provided by the FG strategy can help avoid some meaningless calculations in advance.

[0075] 2) The GRSC method first extracts the optimal plane in the grid where the probability provided by the FG strategy reaches the threshold, can make full use of more trivial planes in an unprepared environment, and then ensures reliable plane association between the first half scan and the second half scan according to the coarse-to-fine matching scheme. The FG strategy can correctly guide the GRSC method to extract reliable planes according to the approximate probability, achieving better external parameter calibration accuracy and robustness. BRIEF DESCRIPTION OF THE DRAWINGS

[0076] Figure 1 is the flowchart of the external parameter calibration method for the rotating lidar provided by the present invention;

[0077] Figure 2 is the schematic diagram of the process of target point curvature calculation;

[0078] Figure 3 is the schematic diagram of the process of calculating the approximate probability of the existence of a plane in the target grid;

[0079] Figure 4 is Figure 1 the sub-step flowchart of step S7 in

[0080] Figure 5 It is a sub - step flowchart of step S74;

[0081] Figure 6 It is a schematic diagram of the plane matching process of the present invention;

[0082] Figure 7 It is a schematic diagram of the coordinate conversion relationship of the rotating lidar of the present invention. Specific embodiments

[0083] The principles and features of the present invention will be described below with reference to the accompanying drawings. The examples given are only for explaining the present invention and are not intended to limit the scope of the present invention.

[0084] Embodiment

[0085] The present invention provides an external parameter calibration method for a rotating lidar. Referring to Figure 1 as shown, the external parameter calibration method for the rotating lidar includes:

[0086] S1: Obtain the first half - scan point cloud and the second half - scan point cloud scanned by the rotating lidar;

[0087] Since the scanning of the rotating lidar is symmetric, when the rotating lidar completes a scanning cycle, it actually scans the environment twice. We represent the two - scan point clouds of the entire environment as the first half - scan model S f and the second half - scan model S l . If the external parameters are correct, these two half - scans will completely overlap. With the predicted external transformation T ml and the transformation T obtained from the rotation angle bm , the origin p in the radar coordinate system F l can be transformed into p l in the fixed coordinate system F b . Then, according to the rotation angle , the point p b is accumulated to construct the two half - scans.

[0088] Optionally, the first half - scan model is:

[0089]

[0090] The second half - scan model is:

[0091]

[0092] where denotes the coordinates of the $i$-th point cloud in the fixed coordinate system, denotes the rotation angle.

[0093] S2: Determine whether the first half-scanned point cloud and the second half-scanned point cloud completely overlap. If so, output the current external parameters of the rotating lidar as the final external parameters. Otherwise, proceed to step S3;

[0094] S3: Perform grid division on the first half-scanned point cloud and the second half-scanned point cloud respectively to obtain a number of divided first half-scanned grids and a number of second half-scanned grids;

[0095] To overcome the influence of environmental point cloud distortion, small planes can be fully utilized. In the present invention, the entire environmental point cloud is divided into small grids. All half-scans, such as the first half-scan model S f and the second half-scan model S l , will be divided into grids of the same size. In addition, the grids of each half-scan should be completely aligned to ensure that GRSC can reliably perform planar association on the corresponding grids. Specifically, first divide the first half-scan S f into small grids. In this process, a set of parameters for dividing the grids will be formed, including the grid size and the ranges of the x, y, and z coordinates. To ensure the consistency of the grids from different half-scans, the division parameters of the first half-scan will be used to divide the second half-scan S l .

[0096] S4: Use the grid filtering method to obtain the approximate probability of each first half-scanned grid and each second half-scanned grid containing a plane;

[0097] Optionally, the present invention provides an FG strategy to obtain the approximate probability, which includes two processes of calculating point curvature and normalizing plane confidence, specifically including:

[0098] Convert the point cloud of the independent scan frame in each first half-scanned grid and each second half-scanned grid into an ordered depth map respectively;

[0099] Obtain two adjacent points in the first direction of the target point in the depth map;

[0100] Perform curvature calculation on the target point and the two adjacent points in the first direction where it is located to obtain the first curvature calculation result;

[0101] If the first curvature result is the first preset result, mark the target point and the two adjacent points in the first direction where it is located, and perform curvature calculation on multiple adjacent points in the second direction of the target point to obtain the second curvature calculation result;

[0102] If the second curvature calculation result is the second preset result, then mark the target point, two adjacent points in the first direction where the target point is located, and multiple adjacent points in the second direction where the target point is located as preset plane points;

[0103] Perform coordinate transformation on all the preset plane points and transform them into the first half-scanning grid and the second half-scanning grid respectively to form a new first half-scanning grid and a new second half-scanning grid respectively;

[0104] Perform downsampling operations on the first half-scanning grid, the second half-scanning grid, the new first half-scanning grid, and the new second half-scanning grid;

[0105] Perform grid division on the downsampled first half-scanning grid, second half-scanning grid, new first half-scanning grid, and new second half-scanning grid to obtain a number of first half-scanning sub-grids, second half-scanning sub-grids, new first half-scanning sub-grids, and new second half-scanning sub-grids;

[0106] Obtain the approximate probability of the first half-scanning grid based on the first half-scanning sub-grid and the corresponding new first half-scanning sub-grid; and / or obtain the approximate probability of the second half-scanning grid based on the second scanning sub-grid and the corresponding new second half-scanning sub-grid.

[0107] Specifically, since the point cloud scanned by the lidar is disordered, the present invention first utilizes the ordered properties of the azimuth angle α and elevation β of the points to convert the disordered point cloud into an ordered depth map, searches for adjacent points of the target point in the same row or the same column, and calculates the curvature of the target point in the first direction and the second direction in turn. In the present invention, since the points in the column direction are relatively sparse, the first direction is selected as the column direction and the second direction is selected as the row direction, and the description is carried out based on this:

[0108] Suppose is the i-th point in a single-frame point cloud. Therefore, the present invention can derive the azimuth angle α and elevation angle β through the coordinate values of .

[0109]

[0110] Then, in combination with the resolution specification of the lidar, the point can be assigned to the row-column pair (c, l). In fact, the points in each column are sparse, so two adjacent points and in the same column are selected to calculate the nominal curvature of the point and their included angle Such asFigure 2 as shown in (a).

[0111]

[0112] where c represents the column. If ( is an adjustable threshold), the point and its adjacent points will be marked as preset plane points. After calculating all columns, only the points marked as preset plane points will participate in the calculation of each subsequent row.

[0113] As Figure 2 shown in (b), the point groups and are respectively the n closest consecutive points in the same row. The curvature of the point can be calculated by the following formula:

[0114]

[0115] where r represents the row. Similarly, if is less than an adjustable threshold then the point and its adjacent points will be marked as preset plane points.

[0116] All the points marked as plane points will be transformed into points b in the coordinate system F and respectively aggregated into the corresponding first half-scan and second half-scan .

[0117] To ensure consistent point cloud density, first perform the same downsampling operation on all half-scans S f , S l , and , and then divide all half-scans using the same grid settings.

[0118] Intuitively, the sum of the plane points in the grid n i (where i represents the i-th grid) can be considered as the probability P i that there is a plane in the grid, but strictly speaking, this is not a probability in the mathematical sense. However, due to the divergence of the laser beam, the densities of different grid points at different distances from the lidar are different. As Figure 3 shown, when the grid Gi Closer to the lidar than G j When closer to the lidar, grid G i The probability P i is greater than that of grid G j The probability P j is larger, which can be expressed mathematically as ||g i || < ||g j ||, although grids G i and G j both belong to the same plane.

[0119] Assuming that the range of the scanning points is much larger than the distance between two consecutive scanning points, we can assume that the shape of O l p1p2 is an approximate sector, where p1 and p2 are consecutive points in the grid, γ is the turning angle between p1 and p2. When γ is small, the arc length p1p2 can be approximately replaced by the straight-line distance.

[0120]

[0121] If the size of the grid is very small and it is far from the lidar, then g j can be regarded as the center of all points in grid G j . If the size of the grid is l, then the density of the planar points in grid G j can be defined as:

[0122]

[0123] Similarly, the density of the planar points in grid G i is:

[0124]

[0125] Therefore, the ratio of the density of grid G i to G j is:

[0126]

[0127] According to the formula it can be considered that the distance ||g i || can be used to normalize p i . ||g i || actually has a finite distance ||g i || ∈ [r min , r max , which is determined by the inherent properties of the lidar. Therefore, a weighted ||g i || can be defined as:

[0128]

[0129] Furthermore, the grid G can be deduced i The three-dimensional expression of the approximate probability of

[0130]

[0131] Wherein, represents the approximate probability of the nth i grid, n i represents the number of preset plane points in the ith grid, ||g i || weight represents the normalization parameter of the number of preset plane points and ||g i || represents the distance between the target grid and the rotating lidar, r max represents the maximum measurement distance of the lidar, r min represents the minimum measurement distance of the lidar.

[0132] S5: According to the approximate probability, screen a number of the first half-scan grids and a number of the second half-scan grids to obtain a number of first optimal half-scan grids and a number of second optimal half-scan grids;

[0133] Specifically, if the number of points in the grid is sufficient, then it can be used to extract the plane, and then the first optimal half-scan grid and a number of second optimal half-scan grids are obtained. After that, the GRSC method is used to extract the plane and match the plane, which specifically includes:

[0134] S6: Use the plane model fitting method to extract the plane point cloud in each of the first optimal half-scan grid and the second optimal half-scan grid;

[0135] Respectively, from all the grids constructed based on the half-scans S f and S l Extract the plane point cloud in each grid through the RANSAC plane model fitting method, and obtain the relevant parameters of the extracted plane according to the fitting parameters. It should be noted here that the RANSAC plane model fitting method is a relatively common method in the art, and thus is not the content protected by the present invention.

[0136] S7: Perform a plane matching operation on the plane point cloud in the first optimal half-scan grid and the second optimal half-scan grid to obtain a matching result;

[0137] Furthermore, as shown in reference Figure 4 The step S7 includes:

[0138] S71: Use one of the plane point clouds in the first optimal half-scanning grid and the plane point cloud in the second optimal half-scanning grid as the current plane point cloud, and use the other as the paired plane point cloud that matches the current plane point cloud for preliminary matching to obtain a candidate plane pair;

[0139] S72: Obtain the plane parameter pair of the current candidate plane pair;

[0140] Here, the plane parameter pair includes:

[0141] The centroid coordinates of the current plane point cloud and / or the paired plane point cloud, and the plane unit normal vector of the current plane point cloud and / or the paired plane point cloud.

[0142] S73: Obtain the associated plane judgment parameter according to the plane parameter pair;

[0143] Here, the associated plane judgment parameter includes:

[0144] The centroid distance between the centroids of the current plane point cloud and the paired plane point cloud, the projected centroid distance between the projection of the centroid coordinate of one of the current plane point cloud and / or the paired plane point cloud on the other and the centroid coordinate of the other, and the included angle between the unit normal vectors of the current plane point cloud and the paired plane point cloud.

[0145] S74: Judge whether the current candidate plane pair is an associated plane according to the associated plane judgment parameter. If so, go to step S75; otherwise, go to step S76;

[0146] Specifically, when the plane extraction is completed, two sets of plane parameters P f and P l will be formed, which come from the first half-scanning S f and the second half-scanning S l respectively. The purpose of the associated plane is to find all plane matches M ij (p f,i , p l,j ), where p f,i is the i-th plane parameter in P f , and p l,j is the j-th plane parameter in P l . The plane parameter p f,i is a tuple, denoted as p f,i (ct f,i , nv f,i , D f,i ), p f,i and p l,jIt consists of a central point of a plane, which is composed of the centroid of the plane, the unit normal vector of the plane, and the distance from the plane to the origin of the coordinate system. A similar plane parameter p l,j is denoted as p l,j (ct l,j , nv l,j , D l,j ). Instead of checking the distance between each point and the adjacent plane, we directly use the plane parameters and use a coarse-to-fine matching process to associate the planes. Optionally, referring to Figure 5 and Figure 6 shown, the sub-steps of step S54 are the coarse-to-fine matching process of the present invention, which includes:

[0147] S741: Determine whether the centroid distance is less than a first preset threshold. If so, enter step S742; otherwise, enter step S76;

[0148] Here, the centroid distance Δct ij is calculated in the following manner:

[0149] Δct ij = ct f,i - ct l,j

[0150] where ct f,i and ct l,j respectively represent the centroid of the current plane point cloud and the centroid of the paired plane point cloud.

[0151] S742: Determine whether the projected centroid distance is less than a second preset threshold. If so, enter step S743; otherwise, enter step S76;

[0152] The projected centroid distance is calculated in the following manner:

[0153]

[0154] where Δct ij represents the centroid distance, and nv l,j represents the unit normal vector of the current plane point cloud or the paired plane point cloud. is the product of Δct ij and the unit nv l,j . For Figure 6 some small planes from different half-scans shown in (b), they are coplanar but not close enough. A sufficiently small projected center point distance can indicate that these two planes are from the same physical plane but in different parts

[0155] S743: Determine whether the angle between the unit normal vectors is less than the third preset threshold. If so, proceed to step S75; otherwise, return to step S76.

[0156] The angle Δθ between the unit normal vectors ij is calculated by the following method:

[0157] Δθ ij = cos -1 (nv f,i ·nv l,j )

[0158] where nv f,i and nv l,j respectively represent the unit normal vector of the current plane point cloud and the unit normal vector of the paired plane point cloud.

[0159] S75: Determine whether the current candidate plane pair is the last group of candidate plane pairs. If so, output the associated plane as the matching result and proceed to step S8; otherwise, proceed to step S76;

[0160] S76: Re-obtain the candidate plane pair and return to step S71.

[0161] S8: Optimize the current extrinsic parameters of the rotating lidar using the matching result to obtain the new extrinsic parameters of the rotating lidar;

[0162] S9: Use the new extrinsic parameters as the current extrinsic parameters and return to step S2.

[0163] Optionally, before the step S1, the extrinsic parameter calibration method of the rotating lidar further includes:

[0164] Obtain the rotation angle of the rotating lidar;

[0165] Convert the coordinate system where the rotating lidar is located into a fixed coordinate system;

[0166] Specifically, as shown in Figure 7 , it shows the coordinate relationship of the three-dimensional rotating lidar system of the present Daming. Further, the coordinates l, m, and b respectively represent the three-dimensional lidar, the rotating motor shaft, and the fixed base. The corresponding coordinate systems are respectively designated as F l , F m and F b . The extrinsic parameters between the rotating lidar and the center of the rotating motor shaft are constant, denoted as T ml , which consists of the rotation matrix R ml and the translation vector t ml . The rotation matrix R ml is presented by intuitive Euler angles

[0167]

[0168] t ml is expressed as:

[0169] t ml = [t mlx t mly t mlz T

[0170] Due to non-observability, the rotation parameter ω along the rotation axis x x and the translation parameter t ml cannot be estimated. In addition, due to the ranging deviation of the lidar which can reach several centimeters, much larger than the general translational external parameters, t ml cannot be estimated in a non-specific environment either. Therefore, in the present invention, we will only estimate the two-dimensional rotation parameter and We represent the estimated rotation matrix R ml as

[0171]

[0172] where

[0173]

[0174]

[0175]

[0176] The transformation between the rotating motor shaft and the fixed base is represented by T bm which is also composed of the rotation matrix R bm and the translation vector t bm . Since the rotating lidar rotates around the x b axis of the fixed coordinate system F b by the rotation angle so T bm is variable. Since the origins and x-axes of the coordinate systems F m and F b coincide, R bm can be simplified to as

[0177]

[0178] and t bm can be regarded as a zero vector.

[0179] To obtain the fixed coordinate system F b ​The conversion point p in b , the origin point p l must first be converted from the lidar coordinate system to the rotating motor coordinate system F m , and then from F m to F b .

[0180]

[0181] Combination formula Then

[0182] Construct a first half-scan model and a second half-scan model according to the rotation angle and the origin of the fixed coordinate system;

[0183] Obtain the first half-scan point cloud according to the first half-scan model and / or obtain the second half-scan point cloud according to the second half-scan model.

[0184] Optionally, the extrinsic parameters of the rotating lidar include estimated extrinsic parameters, and the calculation method of the estimated extrinsic parameters is:

[0185]

[0186] where W* represents the set of estimated extrinsic parameters, F(·) represents the cost function of the similarity between the current plane point cloud and the paired plane point cloud and F f (·) and F l (·) respectively represent the objective function of the point cloud from the current plane to the midpoint cloud of the paired plane, and the objective function of the point cloud from the paired plane to the midpoint cloud of the current plane and (*) represents represents the point is the initial point in the rotating lidar coordinate system, f f (*) and f l (*) are the points and the plane p f,i and p l,j The function of the distance between them and w f and w l are the weights of the distance. In principle, the greater the distance, the greater the weight, and and represent the plane observation accuracy weights and ct f,i and ct l,jrespectively represent the centroid of the current planar point cloud and the centroid of the paired planar point cloud, s d 、s i respectively represent the distance between the rotating lidar and the target plane and the weight ratio of the incident angle, and s d +s i =1, p f,i and p l,j respectively represent the current planar point cloud and the paired planar point cloud. Since the point sets of S f 、S l sample the same physical surface when the rotating lidar is stationary, they belong to the plane matching M ij The plane p f,i and the plane p l,j are actually the same observation value of the same physical surface in space. Therefore, the present invention represents the estimated parameters as is the coordinate of the estimated parameter, represents the estimated parameter, r max represents the maximum measurement distance of the lidar; nv f,i and nv l,j respectively represent the unit normal vector of the current planar point cloud and the unit normal vector of the paired planar point cloud.

[0187] The present invention has the following beneficial effects:

[0188] 1) On the one hand, the curvature calculation is only carried out during the interval time of continuous scanning and does not occupy the calibration running time; on the other hand, the approximate probability provided by the FG strategy can help avoid some meaningless calculations in advance.

[0189] 2) The GRSC method first extracts the optimal plane in the grid where the probability provided by the FG strategy reaches the threshold, can make full use of more trivial planes in an unprepared environment, and then ensures reliable plane association between the first half scan and the second half scan according to the coarse-to-fine matching scheme. The FG strategy can correctly guide the GRSC method to extract reliable planes according to the approximate probability, achieving better external parameter calibration accuracy and robustness.

[0190] The above are only the preferred embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principle of the present invention shall be included within the protection scope of the present invention.

Claims

1. An external parameter calibration method for a rotating lidar, characterized in that, The method for calibrating the extrinsic parameters of the rotating lidar includes: S1: Obtain the first half-scan point cloud and the second half-scan point cloud scanned by the rotating lidar; S2: Determine whether the first half-scan point cloud and the second half-scan point cloud completely overlap. If so, output the current extrinsic parameters of the rotating lidar as the final extrinsic parameters. Otherwise, proceed to step S3; S3: Perform grid division on the first half-scan point cloud and the second half-scan point cloud respectively to obtain a number of divided first half-scan grids and a number of second half-scan grids; S4: Use the grid filtering method to obtain the approximate probability of the plane contained in each of the first half-scan grids and each of the second half-scan grids, which specifically includes the following sub-steps: Convert the independent scan frame point clouds in each of the first half-scan grids and each of the second half-scan grids into ordered depth maps respectively; Obtain two adjacent points in the first direction of the target point in the depth map; Perform curvature calculation on the target point and the two adjacent points in the first direction where it is located to obtain the first curvature calculation result; If the first curvature calculation result is the first preset result, mark the target point and the two adjacent points in the first direction where it is located, and perform curvature calculation on the two adjacent points in the second direction of the target point to obtain the second curvature calculation result; If the second curvature calculation result is the second preset result, mark the target point, the two adjacent points in the first direction where the target point is located, and the two adjacent points in the second direction where the target point is located as preset plane points; Perform coordinate transformation on all the preset plane points and convert them into the first half-scan grids and the second half-scan grids respectively to form new first half-scan grids and new second half-scan grids respectively; Perform downsampling operations on the first half-scan grids, second half-scan grids, new first half-scan grids, and new second half-scan grids; Perform grid division on the downsampled first half-scan grids, second half-scan grids, new first half-scan grids, and new second half-scan grids to obtain a number of first half-scan sub-grids, second half-scan sub-grids, new first half-scan sub-grids, and new second half-scan sub-grids; Obtain the approximate probability of the first half-scan grid according to the first half-scan sub-grid and the corresponding new first half-scan sub-grid; and / or obtain the approximate probability of the second half-scan grid according to the second half-scan sub-grid and the corresponding new second half-scan sub-grid; S5: Screen a number of the first half-scan grids and a number of the second half-scan grids according to the approximate probability to obtain a number of first optimal half-scan grids and a number of second optimal half-scan grids; S6: Use the plane model fitting method to extract the plane point clouds in each of the first optimal half-scan grids and second optimal half-scan grids; S7: Perform plane matching operations on the plane point clouds in the first optimal half-scan grids and second optimal half-scan grids to obtain the matching result; S8: Optimize the current extrinsic parameters of the rotating lidar using the matching result to obtain new extrinsic parameters of the rotating lidar; S9: Use the new extrinsic parameters as the current extrinsic parameters and return to step S2.

2. The external parameter calibration method of the rotating lidar according to claim 1, characterized in that Before the step S1, the extrinsic parameter calibration method of the rotating lidar further includes: Obtain the rotation angle of the rotating lidar; Convert the coordinate system where the rotating lidar is located into a fixed coordinate system; Construct a first half-scan model and a second half-scan model according to the rotation angle and the origin of the fixed coordinate system; Obtain the first half-scan point cloud according to the first half-scan model and / or obtain the second half-scan point cloud according to the second half-scan model.

3. The external parameter calibration method of the rotating lidar according to claim 2, characterized in that The first half-scan model is: The second half-scan model is: Among them, represents the first half-scanning model, represents the second half-scanning model, represents the coordinates of the nth point cloud in the fixed coordinate system, represents the rotation angle.

4. The external parameter calibration method of the rotating lidar according to claim 1, characterized in that The first curvature calculation result is obtained by the following method: Among them, represents the first direction, represents the angle between the th point and two adjacent points in the first direction where it is located, and respectively represent two adjacent points of the point in the first direction; The second curvature calculation result is obtained by the following method: in, Indicates the second direction, Indicates The curvature calculation result of a point in the second direction is: is a constant, represents the target point, Indicates the number of adjacent points of the target point. Represents the coordinates of the neighboring points of the target point; The approximate probability is obtained by the following method: Among them, represents the approximate probability of the th grid, represents the number of preset plane points within the th grid, represents the normalization parameter of the number of preset plane points and , represents the distance between the target grid and the rotating lidar, represents the maximum measurement distance of the lidar, represents the minimum measurement distance of the lidar.

5. The external parameter calibration method of the rotating lidar according to claim 1, characterized in that The step S7 includes: S71: Use one of the plane point cloud in the first optimal half-scan grid point cloud data and the plane point cloud in the second optimal half-scan grid point cloud data as the current plane point cloud, and use the other as the paired plane point cloud that matches the current plane point cloud for preliminary matching to obtain a candidate plane pair; S72: Obtain the plane parameter pair of the current candidate plane pair; S73: Obtain the associated plane judgment parameter according to the plane parameter pair; S74: Judge whether the current candidate plane pair is an associated plane according to the associated plane judgment parameter. If so, enter step S75; otherwise, enter step S76; S75: Judge whether the current candidate plane pair is the last group of candidate plane pairs. If so, output the associated plane as the matching result and enter step S8; otherwise, enter step S76; S76: Re-obtain the candidate plane pair and return to step S71.

6. The external parameter calibration method of the rotating lidar according to claim 5, characterized in that In the step S72, the plane parameter pair includes: The centroid coordinates of the current plane point cloud and / or the paired plane point cloud, and the plane unit normal vector of the current plane point cloud and / or the paired plane point cloud; In the step S73, the associated plane judgment parameter includes: The centroid distance between the centroids of the current plane point cloud and the paired plane point cloud, the projection centroid distance between the projection of the centroid coordinate of one of the current plane point cloud and / or the paired plane point cloud on the other and the centroid coordinate of the other, and the angle between the unit normal vectors of the current plane point cloud and the paired plane point cloud.

7. The external parameter calibration method of the rotating lidar according to claim 6, wherein The step S74 includes: S741: Judge whether the centroid distance is less than a first preset threshold. If so, enter step S742; otherwise, enter step S76; S742: Judge whether the projection centroid distance is less than a second preset threshold. If so, enter step S743; otherwise, enter step S76; S743: Judge whether the angle between the unit normal vectors is less than a third preset threshold. If so, enter step S75; otherwise, return to step S76.

8. The external parameter calibration method of the rotating lidar according to claim 6 or 7, characterized in that, The centroid distance is calculated as follows: Among them, and respectively represent the centroid of the current planar point cloud and the centroid of the paired planar point cloud; The centroid distance of the projection is calculated as follows: Among them, represents the centroid distance, represents the unit normal vector of the current plane point cloud or the paired plane point cloud; The included angle between the unit normal vectors is calculated by the following method: wherein, and respectively represent the unit normal vector of the current planar point cloud and the unit normal vector of the paired planar point cloud.

9. The external parameter calibration method of the rotating lidar according to claim 5, characterized in that, The extrinsic parameters of the rotating lidar include estimated extrinsic parameters; The calculation method of the estimated extrinsic parameters is: Among them, represents a set of estimated extrinsic parameters, is a cost function representing the similarity between the current planar point cloud and the paired planar point cloud, and , and respectively represent the objective function of the point cloud from the current plane to the paired plane, and the objective function of the point cloud from the paired plane to the current plane, and , , represents , represents the point is the initial point in the rotating lidar coordinate system, and are the points and the plane and is a function of the distance between them, and , , and are the weights of the distance, and , , and represent the plane observation accuracy weights, and , , and respectively represent the centroid of the current planar point cloud and the centroid of the paired planar point cloud, , respectively represent the weight ratios of the distance between the rotating lidar and the target plane and the incident angle, and , and respectively represent the current planar point cloud and the paired planar point cloud, is the coordinate of the estimated parameter, and , , represent the estimated parameters, represents the maximum measurement distance of the lidar; and respectively represent the unit normal vector of the current planar point cloud and the unit normal vector of the paired planar point cloud.

Images