Extrinsic calibration method and device for lidar and machine readable storage medium

Abstract

The application relates to the field of artificial intelligence, and discloses a method and device for calibrating external parameters of a laser radar and a machine readable storage medium. The method comprises the following steps: acquiring initial point clouds collected by the laser radar during rotation of a holder; dividing the initial point clouds into a first point cloud set and a second point cloud set; determining at least three first feature points from the first point cloud set, and determining, for each first feature point, a neighborhood point cloud, an in-neighborhood plane normal vector and an in-neighborhood plane quality degree of the neighborhood point cloud; determining at least three second feature points corresponding to the at least three first feature points respectively from the second point cloud set; constructing a distance function between the at least three first feature points and the at least three second feature points in combination with the in-neighborhood plane normal vector and the in-neighborhood plane quality degree; and obtaining a calibration result of external parameters of a three-dimensional reconstruction system of the laser radar by solving a minimum value of the distance function. In this way, the calibration of the external parameters is realized, and the complexity of the calibration operation is reduced.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of artificial intelligence, in particular to a method and device for calibrating external parameters of a laser radar and a machine readable storage medium. BACKGROUND

[0002] With the rapid development of artificial intelligence and semiconductor technology, smart construction has gradually entered people's field of vision. Compared with the construction management of traditional construction sites, smart construction integrates more sensors, artificial intelligence and virtual reality and other high-tech technologies. Smart construction site uses information technology to accurately control the progress of the project and simulate the construction scene, and through the management of construction process data, it conducts deep data mining analysis, provides project construction trend prediction and project expert plan, and establishes an interconnected, intelligent production and scientific management smart construction information ecosystem. The main demand information source of the smart construction site is the three-dimensional reconstruction point cloud model. However, it is impossible to obtain a large scene (radius ≥ 100 meters) three-dimensional reconstruction point cloud model directly by a single laser radar. At present, the optimal method is to control the pan-tilt to drive the laser radar to collect multiple frames of point cloud data at different angles for splicing, thereby realizing the reconstruction of a large scene range. However, there is a rotation pose offset between the rotation center of the pan-tilt and the rotation center of the laser radar, so the external parameter calibration of the laser radar three-dimensional reconstruction system is needed to realize the point cloud splicing of multiple frames of data and the three-dimensional point cloud reconstruction model of a large scene.

[0003] A systematic calibration method for external parameters of a three-dimensional reconstruction system based on a laser radar is provided in the prior art. In this method, the calibration of the laser radar reconstruction system is completed by using object space trajectories, only one specially designed target is used, and there is no need to consider the time synchronization problem in the calibration process, which can effectively reduce the calibration cost and complexity and improve the automation degree of the calibration. However, the calibration in this method can only be applied to calibration scenes containing specific calibration objects such as calibration targets and calibration balls. By filtering out background noise in advance and comparing the specific calibration objects in the repeated area relative to the current pose state, the circular hole of the specific object (target) is extracted, the circular hole center coordinates are fitted, multiple sets of data are repeatedly collected, and the external parameters of the laser radar are calculated by using an optimization algorithm. Because this method requires specific calibration objects and the calibration objects need to be located in the repeated area of adjacent frames, and the rotating laser radar scanning method has a large number of rotations, if calibration is performed, multiple calibration is adopted, and there is an error accumulation in the calibration. The error of the previous frame affects the calibration of the next frame, and when the error accumulates to a certain amount, the calibration fails. In addition, the specific calibration objects need to be placed in the repeated area of the previous and next frames during the calibration process, which is a complicated operation and increases the complexity of the calibration operation. SUMMARY

[0004] The present application relates to the field of artificial intelligence, in particular to a method and device for calibrating external parameters of a laser radar and a machine readable storage medium.

[0005] To achieve the above object, one aspect of the present application provides an external parameter calibration method for a laser radar, wherein the laser radar is arranged on a gimbal, the external parameter calibration method comprising: acquiring an initial point cloud collected by the laser radar during rotation of the gimbal; dividing the initial point cloud into a first point cloud set and a second point cloud set, wherein the first point cloud set and the second point cloud set at least partially overlap; determining at least three first feature points from the first point cloud set, and determining, for each of the first feature points, a neighborhood point cloud, an in-neighborhood plane normal vector and an in-neighborhood plane quality degree of the neighborhood point cloud; determining at least three second feature points corresponding to the at least three first feature points respectively from the second point cloud set; constructing a distance function between the at least three first feature points and the at least three second feature points in combination with the in-neighborhood plane normal vector and the in-neighborhood plane quality degree; and obtaining an external parameter calibration result of a three-dimensional reconstruction system of the laser radar by solving a minimum value of the distance function.

[0006] Optionally, dividing the initial point cloud into the first point cloud set and the second point cloud set comprises: dividing the initial point cloud into a first initial point cloud set and a second initial point cloud set according to a distribution region of the initial point cloud in a spherical coordinate system of the laser radar; and converting the first initial point cloud set and the second initial point cloud set into a coordinate system corresponding to the gimbal respectively based on a set rotation and translation matrix to obtain the first point cloud set and the second point cloud set.

[0007] Optionally, obtaining the external parameter calibration result of the three-dimensional reconstruction system of the laser radar by solving the minimum value of the distance function comprises: obtaining an updated rotation and translation matrix by solving the minimum value of the distance function; returning to execute the step of converting the first initial point cloud set and the second initial point cloud set into the coordinate system corresponding to the gimbal respectively based on the set rotation and translation matrix to obtain the first point cloud set and the second point cloud set in a case where an iteration end condition is not met; and taking the updated rotation and translation matrix as the external parameter calibration result of the three-dimensional reconstruction system of the laser radar in a case where the iteration end condition is met.

[0008] Optionally, in the distance function, for any first feature point of the at least three first feature points, a difference between a first distance and a second distance is taken as a loss, and a corresponding in-neighborhood plane quality degree of the first feature point is taken as a weighting weight, the first distance is a distance from an in-neighborhood plane normal vector corresponding to the first feature point to the first feature point, and the second distance is a distance from the in-neighborhood plane normal vector corresponding to the first feature point to a second feature point corresponding to the first feature point.

[0009] Optionally, the distance function is: wherein i is an identity of a first feature point in the at least three first feature points; is a coordinate of the first feature point i; x i is a coordinate of a second feature point corresponding to the first feature point i, and H1 is a set rotation and translation matrix; n i T is a plane normal vector in the neighborhood corresponding to the first feature point i; w i is a plane quality degree in the neighborhood corresponding to the first feature point i.

[0010] Optionally, the updated rotation and translation matrix is obtained by solving a minimum value of the distance function, comprising: in a case where the distance function is a nonlinear function, constructing a Lagrange function corresponding to the distance function; performing minimum value solving on the Lagrange function to determine an intermediate rotation and translation matrix; determining an approximation degree value between the distance function and the Lagrange function based on the intermediate rotation and translation matrix; in a case where the approximation degree value is greater than a preset approximation degree value threshold, determining the intermediate rotation and translation matrix as the updated rotation and translation matrix; in a case where the approximation degree value is not greater than the preset approximation degree value threshold, updating the Lagrange function, and returning to perform the step of determining the intermediate rotation and translation matrix by performing minimum value solving on the Lagrange function.

[0011] Optionally, the Lagrange function corresponding to the distance function is constructed, comprising: constructing the Lagrange function by performing first-order Taylor expansion on the distance function and introducing a trust region optimization radius and a Lagrange multiplier; wherein the updating the Lagrange function in a case where the approximation degree value is not greater than the preset approximation degree value threshold comprises: updating the trust region optimization radius in a case where the approximation degree value is not greater than the preset approximation degree value threshold.

[0012] Optionally, the approximation degree value is determined based on the following formula: wherein ρ is the approximation degree value; J(H2) is a first-order derivative of f(H2); ΔH2 is an increment; J(H2) T is a transpose of J(H2); i is an identity of a first feature point in the at least three first feature points; is a coordinate of the first feature point i; x i is a coordinate of a second feature point corresponding to the first feature point i, and H2 is an intermediate rotation and translation matrix; n i Ta normal vector of a plane in the neighborhood corresponding to the first feature point i.

[0013] Optionally, the normal vector of the plane in the neighborhood is a unit normal vector, and for any first feature point in the at least three first feature points, determining the normal vector of the plane in the neighborhood and the normal quality of the plane in the neighborhood of the neighborhood point cloud comprises: determining eigenvalues and eigenvectors of the neighborhood point cloud based on a preset covariance matrix, wherein the eigenvector corresponding to the smallest eigenvalue is the normal vector of the plane in the neighborhood; and determining the normal quality of the plane in the neighborhood based on a preset normal quality of the plane in the neighborhood formula and the determined eigenvalues.

[0014] Optionally, determining the normal quality of the plane in the neighborhood based on the preset normal quality of the plane in the neighborhood formula and the determined eigenvalues comprises determining the normal quality of the plane in the neighborhood based on the following formula: wherein w i is the normal quality of the plane in the neighborhood corresponding to the first feature point i; trace(∑(x i , r)) represents a trace of the preset covariance matrix; x i represents the coordinates of the first feature point i; λ3, λ2, λ1 are eigenvalues of the preset covariance matrix, and λ3≥λ2≥λ1.

[0015] Correspondingly, another aspect of the embodiment of the present application provides a device for calibrating an external parameter of a laser radar, which comprises: a processing module, configured to: acquire an initial point cloud collected by the laser radar during rotation of the cloud platform; divide the initial point cloud into a first point cloud set and a second point cloud set, the first point cloud set and the second point cloud set at least partially overlapping; determine at least three first feature points from the first point cloud set, and for each first feature point, determine a neighborhood point cloud, a normal vector of a plane in the neighborhood of the neighborhood point cloud, and a normal quality of the plane in the neighborhood; determine at least three second feature points corresponding to the at least three first feature points from the second point cloud set; construct a distance function between the at least three first feature points and the at least three second feature points in combination with the normal vector of the plane in the neighborhood and the normal quality of the plane in the neighborhood; and obtain an external parameter calibration result of a three-dimensional reconstruction system of the laser radar by solving a minimum value of the distance function.

[0016] In addition, another aspect of the embodiment of the present application also provides a machine readable storage medium, which stores instructions, and the instructions, when executed by a processor, cause the processor to be configured to execute the above-mentioned external parameter calibration method.

[0017] By the technical solution, the initial point cloud is divided into a first point cloud set and a second point cloud set, the in-neighbor plane normal vector and the in-neighbor plane quality degree of the neighborhood point cloud are determined for each first feature point in the at least three feature points in the first point cloud set and the second feature point, the distance function between the at least three first feature points and the at least three second feature points is constructed in combination with the in-neighbor plane normal vector and the in-neighbor plane quality degree, the minimum value of the distance function is solved to obtain the external parameter calibration result of the laser radar three-dimensional reconstruction system, so that the external parameter calibration is realized. In addition, the specific calibration object is not required in the implementation of the external parameter calibration, and the specific calibration object is more required to be located in the adjacent frame repeating area, the complexity of the calibration operation is reduced, and the probability of causing the calibration failure can be reduced.

[0018] Other features and advantages of the present application will be described in detail in the following detailed description. BRIEF DESCRIPTION OF DRAWINGS

[0019] The accompanying drawings are included to provide a further understanding of the present application, and constitute a part of the specification, and are used together with the following detailed description to explain the present application, but do not constitute a limitation of the present application. In the drawings:

[0020] Figure 1 is a flow chart of the external parameter calibration method for the laser radar provided by an embodiment of the present application;

[0021] Figure 2 is a logic diagram of the external parameter calibration method for the laser radar provided by another embodiment of the present application; and

[0022] Figure 3 is a schematic diagram of the gimbal driving the laser radar to rotate.

[0023] LEGEND OF DRAWINGS

[0024] 1 laser radar 2 gimbal DETAILED DESCRIPTION

[0025] The specific embodiments of the present application will be described in detail below with reference to the accompanying drawings. It should be understood that the specific embodiments described herein are only used to illustrate and explain the present application, and are not used to limit the present application.

[0026] An aspect of an embodiment of the present application provides an external parameter calibration method for a laser radar.

[0027] Figure 1 is a flow chart of the external parameter calibration method for the laser radar provided by an embodiment of the present application.

[0028] The laser radar is arranged on the gimbal. As shown in Figure 1As shown, the method includes steps S10-S15. For ease of illustration, the execution subject of the method can be a processor.

[0029] In step S10, an initial point cloud collected by a laser radar in a process of rotating a holder is acquired. For example, the laser radar can be a multi-line laser radar. For example, the holder can be horizontally rotated or rotated in other directions.

[0030] In step S11, the processor divides the initial point cloud into a first point cloud set and a second point cloud set, and the first point cloud set and the second point cloud set at least partially overlap. For example, the division can be based on an angle of rotation of the laser radar and / or a rotation translation matrix of a rotation center of the laser radar to a rotation center of the holder. Where the division is based on the angle, the angle can be evenly divided or unevenly divided, as long as the initial point cloud can be divided into two parts according to the angle and the two parts at least partially overlap. Alternatively, the first point cloud set and the second point cloud set are determined according to the following formula, χ j represents the first point cloud set, χ k represents the second point cloud set, δ2 is an angle of rotation of the laser radar, R z is a rotation matrix according to a rotation axis of the laser radar, H1 represents a rotation translation matrix set initially (which can be obtained by measurement calculation or conventional calibration), R and respectively represent a radius and an azimuth angle in a spherical coordinate system of the laser radar, and l represents a coordinate of a feature point.

[0031] In step S12, at least three first feature points are determined from the first point cloud set, and a neighborhood point cloud is determined for each first feature point, as well as a neighborhood-in-plane normal vector and a neighborhood-in-plane quality degree of the neighborhood point cloud. In addition, for any first feature point, the neighborhood point cloud is a set of feature points adjacent to the first feature point and having a distance reaching a preset distance. The preset distance can be determined according to specific conditions. Alternatively, in the embodiment of the present application, the neighborhood point cloud can be determined by a K-D tree. Specifically, the first feature points in the first point cloud set are indexed by the K-D tree; for any first feature point, the feature points adjacent thereto are determined based on the K-D tree; and among the adjacent feature points, the feature points having a distance reaching a preset distance from the first feature point are determined, thereby determining the neighborhood point cloud. Alternatively, in the embodiment of the present application, for any first feature point, the preset distance can be the distance between the farthest feature point adjacent to the first feature point and the first feature point. In this way, the preset distance for any first feature point to determine the neighborhood point cloud is variable, realizing dynamic selection of the neighborhood, solving the problem that the feature point noise submerges the real data due to too small fixed neighborhood, and avoiding the loss of data point cloud features due to too large fixed neighborhood. The neighborhood-in-plane normal vector and the neighborhood-in-plane quality degree of the neighborhood point cloud are determined. Specifically, the neighborhood-in-plane normal vector and the neighborhood-in-plane quality degree can be determined by a covariance matrix.

[0032] In step S13, at least three second feature points corresponding to the at least three first feature points are determined from the second point cloud set. For any first feature point, the corresponding second feature point is a feature point in the second point cloud set belonging to the same category as the first feature point. Specifically, the second feature points can be determined based on the K-D tree. Based on the K-D tree established for the first point cloud set, the feature points in the second point cloud set are classified, thereby finding the second feature points corresponding to the first feature points in the first point cloud set.

[0033] In step S14, the distance function between the at least three first feature points and the at least three second feature points is constructed in combination with the neighborhood-in-plane normal vector and the neighborhood-in-plane quality degree.

[0034] Alternatively, in the embodiment of the present application, in the distance function, for any first feature point in the at least three first feature points, the difference between a first distance and a second distance is taken as a loss, and the neighborhood-in-plane quality degree corresponding to the first feature point is taken as a weighting weight, the first distance is the distance from the neighborhood-in-plane normal vector corresponding to the first feature point to the first feature point, and the second distance is the distance from the neighborhood-in-plane normal vector corresponding to the first feature point to the second feature point corresponding to the first feature point.

[0035] In step S15, the minimum value of the distance function is solved, thereby obtaining the extrinsic calibration result of the laser radar three-dimensional reconstruction system.

[0036] By the technical solution, the initial point cloud is divided into the first point cloud set and the second point cloud set, the in-neighbor plane normal vector and the in-neighbor plane quality degree of the neighborhood point cloud are determined for each first feature point in the at least three feature points in the first point cloud set and the second feature point, the distance function between the at least three first feature points and the at least three second feature points is constructed in combination with the in-neighbor plane normal vector and the in-neighbor plane quality degree, the minimum value of the distance function is solved to obtain the external parameter calibration result of the laser radar three-dimensional reconstruction system, so that the external parameter calibration is realized. In addition, the specific calibration object is not required in the realization of the external parameter calibration, and the specific calibration object is more required to be located in the adjacent frame repeating area, the complexity of the calibration operation is reduced. In addition, the calibration is not required for each frame, the calibration error accumulation can be avoided, and the probability of causing the calibration failure is reduced.

[0037] Optionally, in the embodiment of the application, the initial point cloud is divided into the first point cloud set and the second point cloud set, which can include the following content. According to the distribution area of the initial point cloud in the spherical coordinate system of the laser radar, the initial point cloud is divided into the first initial point cloud set and the second initial point cloud set. Optionally, in the embodiment of the application, the distribution area corresponding to the first initial point cloud set and the distribution area corresponding to the second initial point cloud set can be mutually symmetrical. In addition, the angle of the distribution area can be 90°-180°. Based on the set rotation translation matrix, the first initial point cloud set and the second initial point cloud set are respectively converted into the coordinate system corresponding to the cloud head to obtain the first point cloud set and the second point cloud set. Optionally, the first point cloud set and the second point cloud set are determined according to the following formula, χ j represents the first point cloud set, χ k represents the second point cloud set, δ2 is the angle of the rotation of the laser radar, R z is the rotation matrix according to the rotation axis of the laser radar, H1 represents the set rotation translation matrix, R and respectively represent the radius and the azimuth angle under the spherical coordinate system of the laser radar, and l represents the coordinates of the feature point.

[0038] Optionally, in the embodiment of the present application, the external parameter calibration result of the laser radar three-dimensional reconstruction system obtained by solving the minimum value of the distance function can include the following contents. The updated rotation and translation matrix is obtained by solving the minimum value of the distance function. In the case where the iteration end condition is not met, the updated rotation and translation matrix is set as the rotation and translation matrix, and the step of converting the first initial point cloud set and the second initial point cloud set into the first point cloud set and the second point cloud set in the coordinate system corresponding to the cloud head based on the set rotation and translation matrix is executed. After returning to this step, the corresponding contents are continued to be executed, specifically, the above-mentioned steps S12-S14 and the updated rotation and translation matrix obtained by solving the minimum value of the distance function can be referred to. In the case where the iteration end condition is met, the updated rotation and translation matrix is taken as the external parameter calibration result of the laser radar three-dimensional reconstruction system. Optionally, in the embodiment of the present application, the iteration end condition can be determined according to specific conditions, for example, it can be the number of iterations set in advance. By continuously repeating the determination of the rotation and translation matrix, the accuracy of the calibrated external parameter can be improved.

[0039] Optionally, in the embodiment of the present application, the distance function is: wherein i is the identifier of the first feature point in the at least three first feature points; is the coordinate of the first feature point i; x i is the coordinate of the second feature point corresponding to the first feature point i, and H1 is the set rotation and translation matrix; i T is the plane normal vector in the neighborhood corresponding to the first feature point i; i is the plane quality degree in the neighborhood corresponding to the first feature point i. In addition, the plane normal vector in the neighborhood corresponding to the first feature point i is the plane normal vector in the neighborhood of the neighborhood point cloud corresponding to the first feature point i, and the plane quality degree in the neighborhood corresponding to the first feature point i is the plane quality degree in the neighborhood of the neighborhood point cloud corresponding to the first feature point i. In addition, in the embodiment of the present application, in the case where the at least three first feature points include effective feature points, i is the identifier of the effective feature point.

[0040] Optionally, in the embodiment of the present application, the updated rotation and translation matrix is obtained by solving the minimum value of the distance function, which can include the following. In the case of a nonlinear distance function, a Lagrangian function corresponding to the distance function is constructed. The minimum value of the Lagrangian function is solved to determine the intermediate rotation and translation matrix. For example, the Lagrangian function includes a set rotation and translation matrix, the Lagrangian function is differentiated with respect to H1 (H1 represents the set rotation and translation matrix, and ΔH1 is the increment of H1), the expression of the Lagrangian function is optimized, and thus the intermediate rotation and translation matrix is obtained. Based on the intermediate rotation and translation matrix, an approximation degree value between the distance function and the Lagrangian function is determined. In the case where the approximation degree value is greater than a preset approximation degree threshold value, the intermediate rotation and translation matrix is determined as the updated rotation and translation matrix. In the case where the approximation degree value is not greater than the preset approximation degree threshold value, the Lagrangian function is updated, and the step of solving the minimum value of the Lagrangian function to determine the intermediate rotation and translation matrix is returned to be executed. After returning to solve the minimum value of the Lagrangian function to determine the intermediate rotation and translation matrix, the step of calculating the approximation degree value and judging the size relationship between the approximation degree value and the preset approximation degree threshold value is continuously executed until the approximation degree value is greater than the preset approximation degree threshold value.

[0041] Optionally, in the embodiment of the present application, the Lagrangian function corresponding to the distance function can include the following. The Lagrangian function is constructed by first-order Taylor expansion of the distance function and introduction of a trust region optimization radius and a Lagrangian multiplier. In the case where the approximation degree value is not greater than the preset approximation degree threshold value, the Lagrangian function is updated, including: in the case where the approximation degree value is not greater than the preset approximation degree threshold value, the trust region optimization radius is updated. For example, the trust region optimization radius can be updated according to the following formula: μ' = kμ, k is a constant, μ' is the updated trust region optimization radius, and μ is the trust region optimization radius before updating.

[0042] Optionally, in the embodiment of the present application, the approximation degree value is determined based on the following formula: wherein ρ is the approximation degree value; J(H2) is the first-order derivative of f(H2); ΔH2 is the increment; J(H2) T is the transpose of J(H2); i is the identification of the first feature point in the at least three first feature points; is the coordinate of the first feature point i; x i is the coordinate of the second feature point corresponding to the first feature point i, H2 is the intermediate rotation and translation matrix; n i T is the plane normal vector in the neighborhood corresponding to the first feature point i.

[0043] It is worth mentioning that H2 and H1 are both rotation translation matrices, and in the process of iterative calculation of the rotation translation matrix, H2 can be updated as H1, or H1 can be updated as H2, and the different subscripts are used to better distinguish the application of the rotation translation matrix in different calculation stages.

[0044] Optionally, in the embodiment of the present application, the in-neighborhood plane normal vector is a unit normal vector, and the in-neighborhood plane unit normal vector and the in-neighborhood plane quality degree of the neighborhood point cloud can include the following contents for any first feature point in the at least three first feature points. Based on the preset covariance matrix, the eigenvalue and eigenvector corresponding to the neighborhood point cloud are determined, wherein the eigenvector corresponding to the smallest eigenvalue is the in-neighborhood plane unit normal vector. Based on the preset in-neighborhood plane quality degree formula and the determined eigenvalue, the in-neighborhood plane quality degree is determined.

[0045] Optionally, in the embodiment of the present application, the in-neighborhood plane quality degree is determined based on the preset in-neighborhood plane quality degree formula and the determined eigenvalue, which includes determining the in-neighborhood plane quality degree based on the following formula: wherein w i is the in-neighborhood plane quality degree corresponding to the first feature point i; trace(∑(x i , r)) represents the trace of the preset covariance matrix; x i represents the coordinates of the first feature point i; λ3, λ2, λ1 are eigenvalues of the preset covariance matrix, and λ3≥λ2≥λ1.

[0046] Optionally, in the embodiment of the present application, the at least three first feature points include effective feature points, and the effective feature points are feature points whose number of adjacent feature points in the first point cloud set is greater than a preset number of adjacent feature points threshold. For any first feature point in the first point cloud set, if the number of adjacent feature points adjacent to it in the first point cloud set (the number of adjacent feature points) is greater than the preset number of adjacent feature points threshold, the feature point is an effective feature point. Optionally, in the embodiment of the present application, the number of adjacent feature points of a certain first feature point can be determined by the K-D tree established for the first point cloud set. In the embodiment of the present application, the at least three first feature points include effective feature points, and the external parameter calibration is realized based on the effective feature points.

[0047] The following will be described in combination with Figure 2 and Figure 3The method for calibrating the external parameters of a laser radar is exemplarily introduced. In the embodiment, for the problem of calibrating the external parameters of a large-scene three-dimensional reconstruction system, a method for calibrating the external parameters of a laser radar three-dimensional reconstruction system based on 360-degree overlapping area matching and iterative optimization is provided. First, the pan-tilt head drives the multi-line laser radar to rotate in a 360-degree range with a resolution of 1°, and point cloud data is collected and saved. The point cloud is divided into two parts, i.e., an upper half circle and a lower half circle, by taking 180 degrees as a division point. The upper half circle point cloud is a first point cloud set, and the lower half circle point cloud is a second point cloud set. The calibration of the external parameters of the laser radar three-dimensional reconstruction system is converted into a point cloud registration algorithm by using the rotation property (the rotation center of the pan-tilt head coincides with the rotation center of the laser radar, and the front and back half circle models are consistent without offset). Second, a K-D tree index is established for the upper half circle point cloud. The neighborhood range of each feature point is clustered to realize dynamic neighborhood selection, and the covariance in the neighborhood range of each feature point is calculated. The neighborhood-in-plane normal vector of the point cloud clustering plane is estimated by using the covariance, and the planeness of the neighborhood-in-plane normal vector of the point cloud is evaluated by using the eigenvalue of the covariance matrix. Finally, the corresponding points of the front and back half circles are found by using the K-D tree index, and the distance difference between two points and the corresponding neighborhood-in-plane unit normal vector is taken as an error. A loss function of a quasi-registration nonlinear least squares is constructed by using the neighborhood-in-plane quality of the corresponding neighborhood. The rotation and translation matrix, i.e., the external parameter matrix of the rotating laser radar reconstruction system, is obtained by optimization. The method can solve the external parameter calibration of the laser radar three-dimensional reconstruction system without a calibration object, and realize the reconstruction of a large scene. The multi-line laser radar is a radar system for detecting the position, velocity and other characteristic quantities of a target by emitting a laser beam. Its working principle is to emit a measuring signal, compare the received signal emitted from the target with the emitted signal, and obtain the relevant information of the target, such as the distance, direction, height, velocity and attitude of the target, after appropriate processing. The three-dimensional reconstruction is a mathematical model suitable for computer representation and processing of a three-dimensional object, which is the basis for processing, operating and analyzing the properties of the object in a computer environment, and is a key technology for establishing a virtual reality in a computer to express the objective world.

[0048] The method for calibrating the external parameters of a laser radar three-dimensional reconstruction system is introduced in detail below. Figure 2 and Figure 3 The method for calibrating the external parameters of a laser radar three-dimensional reconstruction system is introduced in detail below. Figure 3 As shown in FIG. 1, the pan-tilt head 2 drives the laser radar 1 to rotate with a high-precision angular resolution (for example, 1°), and the three-dimensional point cloud of a large scene is acquired by controlling the pan-tilt head 2 to rotate horizontally by 360° while controlling the laser radar 1.

[0049] S1, pre-processing of the lidar 1 point cloud data. First, convert the lidar spherical coordinate system to the Cartesian coordinate system, as shown in the following formula: wherein R, θ, respectively represent the radius, polar angle, and azimuth angle in the spherical coordinate system, and the feature point is a three-dimensional feature point. Secondly, the feature points included in the region scanned by the lidar 1 are divided into upper half circle point cloud and lower half circle point cloud according to the angle, and the mathematical expression is as shown in the formula: wherein χ j represents the upper half circle point cloud; χ k represents the lower half circle point cloud; R z represents the rotation matrix according to the z-axis rotation, and the angle of rotation can be obtained by reading the gimbal serial port data; H1 represents the set rotation translation matrix from the rotation center of the lidar 1 to the rotation center of the gimbal 2; and l represents the three-dimensional coordinates of the feature point. Finally, according to the laser reflectivity threshold and the laser distance threshold (laser measurement distance), the noise is removed.

[0050] S2, index establishment for the upper half circle point cloud to realize dynamic neighborhood selection. Based on the K-D tree (K-dimensional Euclidean space organization point data structure), the data of the upper half circle point cloud is indexed, so as to find out the feature points of each cluster point cloud in the scattered and disordered point cloud, that is, to classify and index the feature points in the upper half circle point cloud. For any first feature point in the upper half circle point cloud, the neighboring feature points are found through the K-D tree, and it is judged whether the number of neighboring feature points of the neighboring feature points satisfies whether it is greater than or equal to a preset neighboring feature point number threshold (for example: 50); if it satisfies, the feature point is an effective feature point, and then the farthest distance r is used as the radius to search, and all the feature points in the range form a neighborhood point cloud, wherein the farthest distance is the distance between the farthest neighboring feature point and the feature point. Dynamic neighborhood selection is realized, which solves the problem that the feature point noise is submerged in the real data due to the too small fixed neighborhood, and avoids the loss of data point cloud features due to the too large fixed neighborhood.

[0051] S3, according to S1 and S2, the upper half circle point cloud and the neighborhood point cloud based on the K-D tree index are obtained. First, for the neighborhood point cloud of any effective feature point obtained in S2, the neighborhood in-plane unit normal vector of the neighborhood point cloud is solved based on the preset covariance matrix. The mathematical expression of the preset covariance matrix is as follows: wherein x i represents the coordinates of the effective feature point i; m represents the neighborhood point cloud set centroid point, and the mathematical expression is x j represents the three-dimensional coordinates of the feature point j in the neighborhood point cloud, and N(x i , r) represents the feature point set in the neighborhood point cloud with a radius of r; p(x i , x jr) represents a weight value, and p(x i , x j ; r) = 1.0 / ||x i -x j || 2 , the inverse ratio of the distance between the feature point j in the neighborhood point cloud and the corresponding effective feature point i is taken as the weight value. Secondly, by calculating the eigenvalues and eigenvectors of the preset covariance matrix in the neighborhood point cloud of the current effective feature point, the eigenvector corresponding to the minimum eigenvalue is taken as the neighborhood-in-plane unit normal vector of the neighborhood point cloud. The neighborhood-in-plane quality degree is solved by using the preset neighborhood-in-plane quality degree formula. The preset neighborhood-in-plane quality degree formula is as follows: wherein λ3, λ2, λ1 are eigenvalues of the preset covariance matrix, and λ3≥ λ2≥ λ1, w i represents the neighborhood-in-plane quality degree of the effective feature point i, and trace(∑(x i , r)) represents the trace of the preset covariance matrix.

[0052] S4, according to the information obtained in S1, S2 and S3, the feature point corresponding to the feature point in the upper half circle point cloud is found in the lower half circle point cloud through the K-D index tree that has been established, that is, for each effective feature point in the upper half circle point cloud, the second feature point is found in the lower half circle point cloud based on the K-D index tree. For any effective feature point in the upper half circle point cloud, the difference between the distance from the neighborhood-in-plane normal vector of the neighborhood point cloud corresponding to the effective feature point to the effective feature point and the distance from the neighborhood-in-plane normal vector to the corresponding second feature point is taken as the loss, and the neighborhood-in-plane quality degree of the neighborhood point cloud corresponding to the effective feature point is taken as the weight of the weighting, to construct a target loss function (distance function) of the nonlinear least squares, which is specifically as follows: wherein, is the coordinate of the effective feature point i in the upper half circle point cloud; x i ' is the coordinate of the second feature point corresponding to the effective feature point i in the lower half circle point cloud; H1 is a set rotation and translation matrix; n i T is the neighborhood-in-plane normal vector of the neighborhood point cloud corresponding to the effective feature point i; w i is the neighborhood-in-plane quality degree of the neighborhood point cloud corresponding to the effective feature point i. In the embodiment of the application, the relationship between the point and the plane is used to construct the loss function, which can make the obtained H more accurate and easier to converge, and has better adaptability to the scene.

[0053] S5, according to the target loss function constructed in S4 and the parameters solved in S1-S3, firstly, the formula Performing a first-order Taylor expansion at f(H), and adding the trust region optimization radius μ, the loss function is rewritten as follows: argmin{∑ i w i ||f(H1)+J(H1) T *ΔH1|| 2}st||D*ΔH1|| 2 ≤μ, where D is the coefficient matrix; J(H1) T Let J(H1) be the transpose of J(H1), where J(H1) is the first derivative; ΔH1 is the increment; and st represents the constraint. Next, by using Lagrange multipliers to incorporate the constraint into the objective loss function, a Lagrange function is constructed, as shown in the following equation: Where λ is a Lagrange multiplier. Based on the Lagrange function, differentiating H1 yields the optimized result (J(H1)J(H1)). T +λD T D) ΔH1=-J(H1)f(H1), based on this optimized formula, the intermediate rotation and translation matrix H2 is obtained. Finally, after the solution is completed, according to... Calculate the approximation value ρ, which represents the degree of approximation between the target loss function and the Lagrange function. If the calculated approximation value ρ is greater than a preset approximation value threshold, convergence is determined and the iteration ends; otherwise, update... Reconstruct the Lagrange function. Repeat the process of calculating the intermediate rotation and translation matrix to obtain the intermediate rotation and translation matrix and the approximation value, until the approximation value ρ is greater than the preset approximation value threshold. The iteration ends, and the optimized parameter H2 is output. H2 is the updated rotation and translation matrix. Optionally, in this embodiment of the invention, in addition to using Lagrange multipliers, preset gradient descent (Gauss-Newton, LM, etc.), singular value solving, or neural networks and machine learning algorithms are used to solve the preset target loss function.

[0054] S6. If the preset number of iterations has not been reached, replace the set rotation and translation matrices in the upper and lower semicircular point clouds in step S1 with the updated rotation and translation matrices obtained from the solution, redivide the upper and lower semicircular point clouds, repeat steps S2-S5, and iterate continuously until the preset number of iterations is completed (iteration termination condition), and output the optimal rotation and translation matrix.

[0055] In summary, the embodiment of the present application provides an external parameter calibration method suitable for a rotating multi-line laser radar for large scene reconstruction, and specifically includes the following aspects. 1) An external parameter calibration method for a laser radar-based three-dimensional reconstruction system without a specific calibration object (calibration target or calibration ball) in the calibration scene is provided. By combining the prior information of the high-precision angle of the holder, 360-degree point clouds are collected, and the point clouds are divided into upper half circle point clouds and lower half circle point clouds according to the angle. The external parameter calibration problem of the laser radar three-dimensional reconstruction system is converted into an upper and lower half circle feature registration algorithm by using the rotation characteristics, and then the rotation and translation matrix of the multi-line laser radar external parameter is solved, which simplifies the calibration process, improves the efficiency of the external parameter calibration of the laser radar three-dimensional reconstruction system, improves the practicability of the algorithm, and has a relatively broad application prospect. 2) Based on the holder rotation driving the laser radar to reconstruct the system for a large scene, the laser radar used does not need a high-line laser radar (96 or 128 line bundle), and the cost is relatively low, which improves the practicability of the method. 3) K-D trees are used to index the point clouds, and the K-D tree point cloud range search is used to calculate the neighborhood point clouds of each three-dimensional point, so as to realize dynamic neighborhood range selection. By using dynamic neighborhood instead of fixed neighborhood, the noise of feature points caused by too small fixed neighborhood is reduced, and the loss of data point cloud features caused by too large fixed neighborhood is avoided. 4) The distance difference (eigenvalue) between the point to plane and the corresponding point to plane is used as the input of the loss function, and the robustness is stronger than that of the distance difference between the point and the corresponding point, which avoids the loss function from falling into an overfitting state. 5) The neighborhood plane quality degree is used to evaluate the effectiveness of the clustered point cloud flatness, and the loss function of point cloud registration is introduced. By weighting the distance error of the point and the corresponding point to the plane, the contribution of the point cloud flatness with low clustering to the loss function is reduced, and the convergence of the algorithm is improved.

[0056] Correspondingly, another aspect of the embodiment of the present application provides an external parameter calibration device for a laser radar, which comprises: a processing module, configured to: acquire initial point clouds collected by the laser radar during rotation of the holder; divide the initial point clouds into a first point cloud set and a second point cloud set, the first point cloud set and the second point cloud set at least partially overlap; determine at least three first feature points from the first point cloud set, and determine, for each first feature point, neighborhood point clouds, a neighborhood-in-plane normal vector and a neighborhood-in-plane quality degree of the neighborhood point clouds; determine at least three second feature points corresponding to the at least three first feature points from the second point cloud set; construct a distance function between the at least three first feature points and the at least three second feature points by combining the neighborhood-in-plane normal vector and the neighborhood-in-plane quality degree; and obtain an external parameter calibration result of a laser radar three-dimensional reconstruction system by solving the minimum value of the distance function.

[0057] Optionally, in the embodiment of the present application, the initial point cloud is divided into the first point cloud set and the second point cloud set, comprising: dividing the initial point cloud into a first initial point cloud set and a second initial point cloud set according to a distribution region of the initial point cloud in a spherical coordinate system of the laser radar; and converting the first initial point cloud set and the second initial point cloud set into the coordinate system corresponding to the holder respectively based on the set rotation translation matrix to obtain the first point cloud set and the second point cloud set.

[0058] Optionally, in the embodiment of the present application, the external parameter calibration result of the laser radar three-dimensional reconstruction system is obtained by solving the minimum value of the distance function, comprising: obtaining an updated rotation translation matrix by solving the minimum value of the distance function; in the case of not satisfying the iteration end condition, taking the updated rotation translation matrix as the set rotation translation matrix, returning to execute the step of converting the first initial point cloud set and the second initial point cloud set into the coordinate system corresponding to the holder respectively based on the set rotation translation matrix to obtain the first point cloud set and the second point cloud set; in the case of satisfying the iteration end condition, taking the updated rotation translation matrix as the external parameter calibration result of the laser radar three-dimensional reconstruction system.

[0059] Optionally, in the embodiment of the present application, in the distance function, for any first feature point in the at least three first feature points, the difference between the first distance and the second distance is taken as the loss, and the planar quality degree in the neighborhood corresponding to the first feature point is taken as the weighted weight, the first distance is the distance from the plane normal vector in the neighborhood corresponding to the first feature point to the first feature point, and the second distance is the distance from the plane normal vector in the neighborhood corresponding to the first feature point to the second feature point corresponding to the first feature point.

[0060] Optionally, in the embodiment of the present application, the distance function is: wherein i is an identifier of the first feature point in the at least three first feature points; is the coordinate of the first feature point i; x i is the coordinate of the second feature point corresponding to the first feature point i, and H1 is the set rotation translation matrix; i T is the plane normal vector in the neighborhood corresponding to the first feature point i; w i is the planar quality degree in the neighborhood corresponding to the first feature point i.

[0061] Optionally, in the embodiment of the present application, the updated rotation and translation matrix is obtained by solving the minimum value of the distance function, comprising: in the case that the distance function is a nonlinear function, constructing a Lagrange function corresponding to the distance function; performing minimum value solving on the Lagrange function to determine an intermediate rotation and translation matrix; determining an approximation degree value between the distance function and the Lagrange function based on the intermediate rotation and translation matrix; in the case that the approximation degree value is greater than a preset approximation degree value threshold, determining the intermediate rotation and translation matrix as the updated rotation and translation matrix; in the case that the approximation degree value is not greater than the preset approximation degree value threshold, updating the Lagrange function, and returning to perform the step of determining the intermediate rotation and translation matrix by performing minimum value solving on the Lagrange function.

[0062] Optionally, in the embodiment of the present application, the Lagrange function corresponding to the distance function is constructed, comprising: constructing the Lagrange function by performing first-order Taylor expansion on the distance function and introducing a trust region optimization radius and a Lagrange multiplier; wherein, in the case that the approximation degree value is not greater than the preset approximation degree value threshold, updating the Lagrange function comprises: in the case that the approximation degree value is not greater than the preset approximation degree value threshold, updating the trust region optimization radius.

[0063] Optionally, in the embodiment of the present application, the approximation degree value is determined based on the following formula: Wherein, ρ is the approximation degree value; J(H2) is the first derivative of f(H2); ΔH2 is the increment; J(H2) T is the transpose of J(H2); i is the identification of the first feature point in the at least three first feature points; is the coordinate of the first feature point i; x i is the coordinate of the second feature point corresponding to the first feature point i, H2 is the intermediate rotation and translation matrix; n i T is the plane normal vector in the neighborhood corresponding to the first feature point i.

[0064] Optionally, in the embodiment of the present application, the plane normal vector in the neighborhood is a unit normal vector, and for any first feature point in the at least three first feature points, determining the plane unit normal vector in the neighborhood of the neighborhood point cloud and the plane quality degree in the neighborhood comprises: determining the eigenvalue and the eigenvector corresponding to the neighborhood point cloud based on a preset covariance matrix, wherein the eigenvector corresponding to the smallest eigenvalue is the plane unit normal vector in the neighborhood; and determining the plane quality degree in the neighborhood based on a preset plane quality degree in the neighborhood formula and the determined eigenvalue.

[0065] Optionally, in the embodiment of the present application, determining the plane quality degree in the neighborhood based on the preset plane quality degree in the neighborhood formula and the determined eigenvalue comprises determining the plane quality degree in the neighborhood based on the following formula: wherein, w i is the planar quality degree in the neighborhood corresponding to the first feature point i; trace(∑(x i , r)) represents the trace of the preset covariance matrix; x i represents the coordinates of the first feature point i; λ3, λ2, λ1 are eigenvalues of the preset covariance matrix, and λ3≥λ2≥λ1.

[0066] The specific working principle and benefits of the external parameter calibration device for the laser radar provided by the embodiments of the present application are similar to those of the external parameter calibration method for the laser radar provided by the embodiments of the present application, and will not be repeated here.

[0067] In addition, another aspect of the embodiments of the present application further provides a machine readable storage medium, and the machine readable storage medium has instructions stored thereon, and the instructions are configured to cause the processor to be configured to perform the external parameter calibration method described in the above embodiments when the instructions are executed by the processor.

[0068] The preferred embodiments of the present application are described in detail above in combination with the drawings, but the present application is not limited to the specific details in the above embodiments, and various simple modifications can be made to the technical solutions of the present application within the technical concept of the present application, and these simple modifications all belong to the protection scope of the present application.

[0069] In addition, it should be noted that each specific technical feature described in the above specific embodiments can be combined in any appropriate manner without contradiction. In order to avoid unnecessary repetition, the present application will not further describe various possible combinations.

[0070] In addition, various different embodiments of the present application can also be combined in any manner, as long as it does not deviate from the technical concept of the present application, and it should also be considered as disclosed by the present application.

Claims

1. A method for calibrating extrinsic parameters of a lidar, wherein the lidar is mounted on a pan-tilt unit, characterized in that, The external parameter calibration method comprises: acquiring an initial point cloud collected by the laser radar during rotation of the holder; dividing the initial point cloud into a first point cloud set and a second point cloud set, the first point cloud set and the second point cloud set at least partially overlapping; determining at least three first feature points from the first point cloud set, and determining, for each of the first feature points, a neighborhood point cloud, an in-neighborhood plane normal vector and an in-neighborhood plane quality degree of the neighborhood point cloud; determining, from the second point cloud set, at least three second feature points respectively corresponding to the at least three first feature points; constructing a distance function between the at least three first feature points and the at least three second feature points in combination with the in-neighborhood plane normal vector and the in-neighborhood plane quality degree; solving a minimum value of the distance function to obtain an external parameter calibration result of the laser radar three-dimensional reconstruction system.

2. The extrinsic calibration method of claim 1, wherein, The dividing of the initial point cloud into the first point cloud set and the second point cloud set comprises: dividing the initial point cloud into a first initial point cloud set and a second initial point cloud set according to a distribution region of the initial point cloud in a spherical coordinate system of the laser radar; based on a set rotation translation matrix, converting the first initial point cloud set and the second initial point cloud set into the coordinate system corresponding to the holder respectively to obtain the first point cloud set and the second point cloud set.

3. The extrinsic calibration method of claim 2, wherein, The solving of the minimum value of the distance function to obtain the external parameter calibration result of the laser radar three-dimensional reconstruction system comprises: solving the minimum value of the distance function to obtain an updated rotation translation matrix; in a case where an iteration end condition is not met, taking the updated rotation translation matrix as the set rotation translation matrix, and returning to execute the step of converting the first initial point cloud set and the second initial point cloud set into the coordinate system corresponding to the holder respectively based on the set rotation translation matrix to obtain the first point cloud set and the second point cloud set; in a case where the iteration end condition is met, taking the updated rotation translation matrix as the external parameter calibration result of the laser radar three-dimensional reconstruction system.

4. The extrinsic calibration method of claim 1, wherein, In the distance function, for any first feature point in the at least three first feature points, a difference between a first distance and a second distance is taken as a loss, and an in-neighborhood plane quality degree corresponding to the first feature point is taken as a weighting weight, the first distance is a distance from an in-neighborhood plane normal vector corresponding to the first feature point to the first feature point, and the second distance is a distance from the in-neighborhood plane normal vector corresponding to the first feature point to a second feature point corresponding to the first feature point.

5. The method of calibrating extrinsic parameters according to claim 4, wherein, The distance function is: wherein, is an identity for a first feature point of the at least three first feature points; is a coordinate of the first feature point ; , is a coordinate of a second feature point corresponding to the first feature point , is a set rotation and translation matrix; is the in-neighborhood plane normal vector corresponding to the first feature point ; is the in-neighborhood plane quality degree corresponding to the first feature point .

6. The method of claim 3, wherein, The solving of the minimum value of the distance function to obtain the updated rotation translation matrix comprises: in a case where the distance function is a nonlinear function, constructing a Lagrange function corresponding to the distance function; performing minimum value solving on the Lagrange function to determine an intermediate rotation translation matrix; based on the intermediate rotation translation matrix, determining an approximation degree value between the distance function and the Lagrange function; In a case where the approximation degree value is greater than a preset approximation degree value threshold, the intermediate rotation and translation matrix is determined as the updated rotation and translation matrix; In a case where the approximation degree value is not greater than the preset approximation degree value threshold, the Lagrange function is updated, and the step of determining the intermediate rotation and translation matrix by solving the minimum value of the Lagrange function is performed again.

7. The method of calibrating extrinsic parameters according to claim 6, wherein, The Lagrange function corresponding to the distance function is constructed, including: The Lagrange function is constructed by performing first-order Taylor expansion on the distance function and introducing a trust region optimization radius and a Lagrange multiplier; The approximation degree value is determined based on the following formula:

8. The method of calibrating extrinsic parameters according to claim 7, wherein, The in-neighborhood plane unit normal vector and the in-neighborhood plane quality degree of the neighborhood point cloud are determined for any first feature point in the at least three first feature points, including: wherein is the degree of approximation value; is a first derivative; is an increment; is a transpose; is an identification of a first feature point of the at least three first feature points; is a coordinate of a first feature point ; , is a coordinate of a second feature point corresponding to a first feature point , is an intermediate rotation and translation matrix; is the normal vector of the plane within the neighborhood corresponding to a first feature point .

9. The method of claim 1, wherein, The eigenvalue and eigenvector of the neighborhood point cloud are determined based on a preset covariance matrix, wherein the eigenvector corresponding to the smallest eigenvalue is the in-neighborhood plane unit normal vector; and The in-neighborhood plane quality degree is determined based on a preset in-neighborhood plane quality degree formula and the determined eigenvalue. The in-neighborhood plane quality degree is determined based on the preset in-neighborhood plane quality degree formula and the determined eigenvalue, including determining the in-neighborhood plane quality degree based on the following formula:

10. The method of calibrating extrinsic parameters according to claim 9, wherein, The external parameter calibration device comprises: wherein is a first feature point a corresponding in-neighborhood planarity quality measure; denotes a trace of a preset covariance matrix; denotes a coordinate of the first feature point ; is an eigenvalue of the preset covariance matrix, and . 11.A device for calibrating extrinsic parameters of a laser radar, the laser radar being arranged on a holder, characterized in that, The processing module is configured to: obtain an initial point cloud collected by the laser radar during rotation of the holder; divide the initial point cloud into a first point cloud set and a second point cloud set, the first point cloud set and the second point cloud set at least partially overlapping; determine at least three first feature points from the first point cloud set, and determine a neighborhood point cloud, an in-neighborhood plane normal vector and an in-neighborhood plane quality degree of the neighborhood point cloud for each first feature point; determine at least three second feature points corresponding to the at least three first feature points from the second point cloud set; construct a distance function between the at least three first feature points and the at least three second feature points in combination with the in-neighborhood plane normal vector and the in-neighborhood plane quality degree; obtain an external parameter calibration result of the laser radar three-dimensional reconstruction system by solving the minimum value of the distance function. The instruction, when executed by a processor, causes the processor to be configured to perform the external parameter calibration method according to any one of claims 1 to 10.

12. A machine-readable storage medium having stored thereon instructions, the instructions being executable by a machine to cause the machine to perform operations comprising: ​

Images