Online Calibration Method, Device and System of Lidar for Driverless Mining Trucks

Abstract

The present invention relates to the technical field of calibration of driverless vehicles, and specifically discloses a method, device and system for online calibration of lidar for driverless mining trucks, including: obtaining lidar point cloud data of a driverless mining truck, and performing data preprocessing on the lidar point cloud data; constructing a local map based on the main lidar coordinate system, and associating and optimizing the preprocessed point cloud data of each auxiliary lidar with the local map; monitoring the real-time position parameters of the lidar; when it is determined according to the real-time position parameters of the lidar that the current lidar has a controllable position offset, re-executing the lidar calibration process to update the lidar calibration parameters; when it is determined according to the real-time position parameters of the lidar that the current lidar has an uncontrollable position offset, sending out an alarm message. The method for online calibration of lidar for driverless mining trucks provided by the present invention can realize automatic and accurate calibration of sensors of driverless vehicles.

Description

Technical Field

[0001] The present invention relates to the technical field of calibration of driverless vehicles, and particularly to an online calibration method for lidar of a driverless mining truck, an online calibration device for lidar of a driverless mining truck, and an online calibration system for lidar of a driverless mining truck. Background Art

[0002] In the field of driverless, the calibration of vehicle sensors refers to the process of calibrating various sensors mounted on the vehicle. These sensors include lidar, cameras, millimeter-wave radars, and inertial navigation, etc., which are responsible for sensing the surrounding environment of the vehicle and determining the data of the motion posture of the vehicle itself. The accuracy and consistency of the sensors are crucial for the safety and performance of the driverless system. The goal behind the calibration of sensors in a driverless vehicle is to ensure that these sensors can accurately sense and measure the objects, road conditions, and other traffic participants around the vehicle. The importance of sensor calibration lies in that it directly affects the perception and decision-making capabilities of the driverless system. If the sensor calibration is incorrect, it may lead to incorrect perception results, thereby affecting the system's recognition and understanding capabilities of obstacles, lane lines, traffic signals, etc. This may result in incorrect decisions and driving behaviors, and thus endanger driving safety. However, the current technology mainly obtains a relatively accurate relative position relationship between sensors through manual calibration, and most automatic calibration methods are affected by algorithm accuracy or messy data acquisition environments, etc., and their calibration accuracies mostly cannot meet the actual application requirements.

[0003] Therefore, how to achieve automatic and accurate calibration of vehicle sensors in a driverless vehicle has become a technical problem that needs to be urgently solved by those skilled in the art. Summary of the Invention

[0004] The present invention provides an online calibration method for lidar of a driverless mining truck, an online calibration device for lidar of a driverless mining truck, and an online calibration system for lidar of a driverless mining truck, which solves the problem in the related technology that automatic and accurate calibration of driverless vehicle sensors cannot be achieved.

[0005] As a first aspect of the present invention, there is provided an online calibration method for lidar of a driverless mining truck, which includes:

[0006] Obtain the lidar point cloud data of the driverless mining truck, and perform data preprocessing on the lidar point cloud data to obtain preprocessed lidar point cloud data, wherein at least one main lidar and at least two auxiliary lidars are provided on the driverless mining truck;

[0007] Construct a local map based on the main lidar coordinate system, and associate and optimize the preprocessed point cloud data of each auxiliary lidar with the local map to obtain lidar calibration parameters;

[0008] Determine the real-time position parameters of the lidar according to the preprocessed lidar point cloud data, and monitor the real-time position parameters of the lidar;

[0009] When it is determined that the current lidar has a controllable position offset according to the real-time lidar position parameters, re-execute the lidar calibration process to update the lidar calibration parameters;

[0010] When it is determined that the current lidar has an uncontrollable position offset according to the real-time lidar position parameters, send an alarm message.

[0011] Further, constructing a local map based on the main lidar coordinate system, and associating and optimizing the preprocessed point cloud data of each auxiliary lidar with the local map to obtain lidar calibration parameters, includes:

[0012] Construct a local map based on the preprocessed point cloud data of the main lidar coordinate system;

[0013] Match the preprocessed point cloud data of each auxiliary lidar with the point cloud in the local map to obtain the point cloud registration residuals of each auxiliary lidar;

[0014] Construct an objective cost function, and optimize the point cloud registration residuals of each auxiliary lidar according to the objective cost function to obtain the relative position relationship between each auxiliary lidar and the main lidar;

[0015] Obtain the lidar calibration parameters according to the relative position relationship between each auxiliary lidar and the main lidar.

[0016] Further, constructing a local map based on the preprocessed point cloud data of the main lidar coordinate system, includes:

[0017] Determine the first position relationship between the main lidar and the inertial navigation unit and the second position relationship of the inertial navigation unit relative to the world coordinate system;

[0018] Construct a local map based on the continuous preprocessed lidar point cloud data of the main lidar according to the first position relationship and the second position relationship.

[0019] Further, matching the preprocessed point cloud data of each auxiliary lidar with the point cloud in the local map to obtain the point cloud registration residuals of each auxiliary lidar, includes:

[0020] Search for local map feature points corresponding to the point cloud feature points of the current auxiliary lidar in the local map according to the point cloud preprocessing data of each auxiliary lidar;

[0021] Implement the registration of the point cloud feature points of each auxiliary lidar and the local map feature points according to the point-to-line registration method and the point-to-plane registration method, and obtain the point cloud registration residuals of each auxiliary lidar.

[0022] Further, construct an objective cost function, and optimize the point cloud registration residuals of each auxiliary lidar according to the objective cost function to obtain the relative position relationship between each auxiliary lidar and the main lidar, including:

[0023] Construct an objective cost function;

[0024] Under the objective cost function, jointly optimize the point cloud registration residuals of each auxiliary lidar according to the gradient descent method and the Mahalanobis norm;

[0025] Obtain the position relationship transformation matrix from the coordinate system of each auxiliary lidar to the coordinate system of the main lidar according to the joint optimization result;

[0026] Determine the relative position relationship between each auxiliary lidar and the main lidar according to the position relationship transformation matrix from the coordinate system of each auxiliary lidar to the coordinate system of the main lidar.

[0027] Further, determine the real-time position parameters of the lidar according to the lidar point cloud preprocessing data, and monitor the real-time position parameters of the lidar, including:

[0028] Extract the real-time position parameters of the lidar according to the lidar point cloud preprocessing data;

[0029] Determine the position offset of each lidar according to the real-time position parameters of the lidar;

[0030] If the position offset of the lidar is greater than a first preset threshold, compare the position offset of each lidar with a second preset threshold, where the first preset threshold is less than the second preset threshold;

[0031] If the position offset of the lidar is not greater than the second preset threshold, determine that the lidar has a controllable position offset;

[0032] If the position offset of the lidar is greater than the second preset threshold, determine that the lidar has an uncontrollable position offset.

[0033] Further, extract the real-time position parameters of the lidar according to the lidar point cloud preprocessing data, including:

[0034] Extract ground points from the preprocessed lidar point cloud data based on the minimum point residuals;

[0035] Determine the ground normal vector based on the ground points in the preprocessed lidar point cloud data;

[0036] Construct a horizontal plane in the lidar coordinate system and calculate the horizontal plane parallel vector;

[0037] Determine the real-time lidar position parameters based on the ground normal vector and the horizontal plane parallel vector, where the real-time lidar position parameters at least include: pitch angle, roll angle, and z-axis intercept.

[0038] Further, if the position offset of the lidar is not greater than the first preset threshold, record and save the current real-time lidar position parameters.

[0039] As another aspect of the present invention, there is provided a lidar online calibration device for an unmanned mining truck, which is used to implement the lidar online calibration method for an unmanned mining truck described above. The device includes:

[0040] A radar data preprocessing module, which is used to obtain the lidar point cloud data of the unmanned mining truck and perform data preprocessing on the lidar point cloud data to obtain preprocessed lidar point cloud data, where the lidar installed on the unmanned mining truck at least includes one main lidar and at least two auxiliary lidars;

[0041] A calibration module, which is used to construct a local map based on the main lidar coordinate system and associate and optimize the preprocessed point cloud data of each auxiliary lidar with the local map to obtain lidar calibration parameters;

[0042] A position monitoring module, which is used to determine the real-time lidar position parameters based on the preprocessed lidar point cloud data and monitor the real-time lidar position parameters;

[0043] A deviation correction module, which is used to re-execute the lidar calibration process to update the lidar calibration parameters when it is determined according to the real-time lidar position parameters that the lidar has a position deviation and the position deviation is within the preset threshold range;

[0044] An alarm module, which is used to send an alarm message when it is determined according to the real-time lidar position parameters that the lidar has a position deviation and the position deviation is not within the preset threshold range.

[0045] As another aspect of the present invention, there is provided a lidar online calibration system for driverless mining trucks, which includes: a lidar and the aforementioned lidar online calibration device for driverless mining trucks, and the lidar is communicatively connected to the lidar online calibration device for driverless mining trucks;

[0046] The lidar is disposed on the driverless mining truck and is used for collecting lidar point cloud data in real time;

[0047] The lidar online calibration device for driverless mining trucks is used for performing lidar calibration, lidar position monitoring and correction based on the lidar point cloud data.

[0048] The lidar online calibration method for driverless mining trucks provided by the present invention realizes the automatic calibration of the lidar of the driverless mining truck through the lidar point cloud data of the driverless mining truck. And due to the joint optimization of the local map based on the main lidar coordinate system and the point cloud data of the auxiliary lidar, the automatic calibration of the relative spatial positions of multiple lidars can be achieved. In addition, through the online monitoring of the lidar position, if a certain lidar has a position deviation, the position of the lidar can be corrected online again to meet the temporary use of the driverless mining truck. If the position deviation is too large, an alarm message is sent to make the driverless mining truck stop safely, ensuring driving safety. Therefore, the lidar online calibration method for driverless mining trucks provided by the present invention can achieve automatic and accurate calibration of the lidar of the driverless mining truck. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] The drawings are used to provide a further understanding of the present invention, and constitute a part of the specification, and are used to explain the present invention together with the following specific embodiments, but do not constitute a limitation to the present invention.

[0050] Figure 1 It is a flowchart of the lidar online calibration method for driverless mining trucks provided by the present invention.

[0051] Figure 2 It is a flowchart of the method for obtaining lidar calibration parameters provided by the present invention.

[0052] Figure 3 It is a flowchart of the method for constructing a local map provided by the present invention.

[0053] Figure 4 It is a flowchart of the point cloud matching between the auxiliary lidar point cloud and the local map provided by the present invention.

[0054] Figure 5a It is a schematic diagram of a two-dimensional KD tree provided by the present invention.

[0055] Figure 5bSchematic diagram of the three-dimensional KD tree provided by the present invention.

[0056] Figure 6 Flowchart of optimization according to the target cost function provided by the present invention.

[0057] Figure 7 Flowchart of monitoring the real-time position of the lidar provided by the present invention.

[0058] Figure 8 Flowchart of extracting the real-time position parameters of the lidar provided by the present invention.

[0059] Figure 9 Block diagram of the lidar online calibration device for driverless mining trucks provided by the present invention.

[0060] Figure 10 Block diagram of the lidar online calibration system for driverless mining trucks provided by the present invention. Detailed implementation manners

[0061] It should be noted that, without conflict, the embodiments in the present invention and the features in the embodiments may be combined with each other. The present invention will be described in detail below with reference to the drawings and in conjunction with the embodiments.

[0062] In order to enable those skilled in the art to better understand the solution of the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without making creative efforts shall fall within the protection scope of the present invention.

[0063] It should be noted that the terms "first", "second", etc. in the specification and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects, and do not have to be used to describe a specific order or sequence. It should be understood that such used data may be interchanged under appropriate circumstances so as to describe the embodiments of the present invention here. In addition, the terms "comprising" and "having" and any variations thereof are intended to cover non-exclusive inclusion. For example, a process, method, system, product or device that includes a series of steps or units does not necessarily have to be limited to those steps or units clearly listed, but may include other steps or units not clearly listed or inherent to these processes, methods, products or devices.

[0064] For the relative position calibration of multiple lidars on driverless transport mining trucks (hereinafter referred to as driverless mining trucks), since the number of lidars installed on driverless mining trucks is relatively large (usually 3 to 5), the task of fine manual calibration for each driverless mining truck is extremely complicated and there is a large amount of repetitive work. In addition, since the operation roads of driverless mining trucks are usually unstructured roads and the operation environment is usually relatively harsh, frequent vibrations may cause the lidars to shift from their original installation positions, resulting in the invalidation of the original calibration relationship, thereby affecting the obstacle perception accuracy and reducing the driving safety.

[0065] Based on this, in this embodiment, an online calibration method for lidars of driverless mining trucks is provided. Figure 1 It is a flowchart of the online calibration method for lidars of driverless mining trucks provided according to an embodiment of the present invention, as Figure 1 shown, including:

[0066] S100. Obtain the lidar point cloud data of the driverless mining truck, and perform data preprocessing on the lidar point cloud data to obtain lidar point cloud preprocessing data, where at least one main lidar and at least two auxiliary lidars are provided on the driverless mining truck;

[0067] In the embodiment of the present invention, a driverless mining truck equipped with 4 lidars is taken as an example for illustration. Specifically, the installation directions of the 4 lidars are 1 each in the front, left, right, and rear of the vehicle body, and the corresponding numbers are a, b, c, and d respectively.

[0068] Taking the lidar installed in the front of the vehicle body as the main lidar and the lidars installed in the left, right, and rear directions as auxiliary lidars, when the driverless mining truck is running, each lidar can obtain its own lidar point cloud data in real time. For the lidar point cloud data of the driverless mining truck, since the acquired original point cloud data may have factors such as noise that affect the calibration accuracy, it is necessary to perform preprocessing on the lidar point cloud data to remove noise and improve the calibration accuracy.

[0069] S200. Construct a local map based on the coordinate system of the main lidar, and associate and optimize the point cloud preprocessing data of each auxiliary lidar with the local map to obtain lidar calibration parameters;

[0070] In the embodiment of the present invention, a local map of its coordinate system is constructed based on the main lidar provided at the front end of the driverless mining truck, and the point cloud preprocessing data of each auxiliary lidar is associated and optimized with the local map, so as to realize the position calibration of the lidar and obtain lidar calibration parameters.

[0071] S300. Determine the real-time position parameters of the lidar based on the preprocessed lidar point cloud data, and monitor the real-time position parameters of the lidar;

[0072] Since the driverless mining truck is in a moving state, for the problem that the lidar on the driverless mining truck may affect the data accuracy due to position offset, etc., the real-time position parameters of the lidar are obtained through the preprocessed lidar point cloud data, and the real-time position parameters of the lidar are monitored, so that when it is found that the lidar has a position offset, correction can be performed to meet the use of the driverless mining truck.

[0073] S400. When it is determined that the current lidar has a controllable position offset according to the real-time position parameters of the lidar, re-execute the lidar calibration process to update the lidar calibration parameters;

[0074] In the embodiment of the present invention, when it is monitored that the real-time position parameters of the lidar have a position offset, it is necessary to determine whether the position offset is a controllable position offset or an uncontrollable position offset. If it is an uncontrollable position offset, repeat the foregoing lidar calibration process to update the lidar calibration parameters.

[0075] S500. When it is determined that the current lidar has an uncontrollable position offset according to the real-time position parameters of the lidar, send an alarm message.

[0076] When it is determined that the lidar has an uncontrollable position offset, an alarm message is sent to make the driverless mining truck stop safely and ensure the safety of driving.

[0077] Therefore, the lidar online calibration method for a driverless mining truck provided by the embodiment of the present invention realizes the automatic calibration of the lidar of the driverless mining truck through the lidar point cloud data of the driverless mining truck. And due to the joint optimization of the local map based on the main lidar coordinate system and the point cloud data of the auxiliary lidar, the automatic calibration of the relative spatial positions of multiple lidars can be realized. In addition, through the online monitoring of the lidar position, if a certain lidar has a position offset, the position of the lidar can be corrected online again to meet the temporary use of the driverless mining truck. If the position offset is too large, an alarm message is sent to make the driverless mining truck stop safely and ensure the safety of driving. Therefore, the lidar online calibration method for a driverless mining truck provided by the present invention can realize the automatic and accurate calibration of the lidar of the driverless mining truck.

[0078] It should be noted that before calibrating the lidar of the driverless mining truck, the lidar can be specifically time-synchronized first. Specifically, the GNSS time-synchronization method is used to perform time-synchronization on each lidar, and on this basis, linear interpolation is performed on each timestamp to eliminate the time offset between each lidar, so as to improve the lidar calibration accuracy.

[0079] When the lidar point cloud data of the lidar is acquired, the lidar point cloud data of each lidar is preprocessed, which may specifically include denoising of the point cloud data and dust removal processing, etc.

[0080] Specifically, the pcl library function is used to denoise the raw point cloud of the multi-lidar, reducing the adverse impact of noise points on the calibration accuracy. At the same time, since the operating environment of the unmanned mining vehicle is relatively harsh, there are usually dust, floating dust, gravel, etc. on the operating road, which has an adverse impact on the detection effect of the lidar and is prone to perception blind spots. Therefore, it is necessary to remove the dust in the point cloud during the multi-lidar calibration process. The present invention uses a segmentation clustering method to extract the dust point cloud clusters, and the DBSCAN algorithm is used to select the points closer to the sensor as the core points. If there are enough points in the neighborhood of this point, these points are all marked as core points. Subsequently, these core points are expanded to form point cloud clusters, and finally the obtained dust point cloud clusters are removed. Finally, the preprocessed point clouds of 4 lidars are obtained.

[0081] In an embodiment of the present invention, a local map based on the main lidar coordinate system is constructed, and the preprocessed point cloud data of each auxiliary lidar is associated and optimized with the local map to obtain lidar calibration parameters, such as Figure 2 shown, including:

[0082] S210. Construct a local map based on the preprocessed point cloud data of the main lidar coordinate system according to the preprocessed point cloud data of the main lidar coordinate system;

[0083] In an embodiment of the present invention, a local map of the main lidar coordinate system is constructed based on the preprocessed point cloud data of the main lidar coordinate system. Specifically, a local map based on the preprocessed point cloud data of the main lidar coordinate system is constructed according to the preprocessed point cloud data of the main lidar coordinate system, such as Figure 3 shown, including:

[0084] S211. Determine the first position relationship between the main lidar and the inertial navigation unit and the second position relationship of the inertial navigation unit relative to the world coordinate system;

[0085] In an embodiment of the present invention, the first position relationship (assumed to be known) T gl between the main lidar and the inertial navigation unit and the second position relationship T wg of the inertial navigation unit relative to the world coordinate system are determined.

[0086] S212. Construct a local map based on the continuous laser point cloud preprocessing data of the main lidar according to the first position relationship and the second position relationship.

[0087] Specifically, according to the first position relationship T gland the second positional relationship T wg Construct a sliding window-based local point cloud map based on consecutive laser point cloud frames, where the sliding window contains several consecutive frames of point clouds. The point cloud frames are described as follows:

[0088]

[0089] where represents the point cloud frame, a represents the main lidar number, and n represents the frame number. respectively represent the x, y, and z coordinates of the point cloud.

[0090] Based on the above, the local map can be described by the following formula:

[0091]

[0092] where represents the superposition operation of the point cloud frames.

[0093] S220. Match the preprocessed point cloud data of each auxiliary lidar with the point cloud in the local map to obtain the point cloud registration residuals of each auxiliary lidar;

[0094] In the embodiments of the present invention, for the preprocessed point cloud data of each auxiliary lidar, it is matched with each point cloud in the local map, and the result difference of the matching is optimized.

[0095] Specifically, match the preprocessed point cloud data of each auxiliary lidar with the point cloud in the local map to obtain the point cloud registration residuals of each auxiliary lidar, as Figure 4 shown, including:

[0096] S221. Search for the local map feature points corresponding to the point cloud feature points of the current auxiliary lidar in the local map according to the preprocessed point cloud data of each auxiliary lidar;

[0097] Construct the constraints between lidars b, c, and d and and perform pose transformation constraint construction and solution according to the selected feature points. Complete point cloud registration based on the scan-to-map point cloud data association method, so as to obtain the pose transformation relationship between lidars b, c, and d and the lidar.

[0098] S222. Implement the registration of the point cloud feature points of each auxiliary lidar and the local map feature points according to the point-to-line registration method and the point-to-plane registration method, and obtain the point cloud registration residuals of each auxiliary lidar.

[0099] In the embodiments of the present invention, to solve for the relationship between lidar b and An example of the pose transformation relationship between lidars is given. For lidars c and d, and The solution of the pose transformation relationship between lidars is the same.

[0100] (1) KD tree construction

[0101] When constructing the constraints between lidars, it is necessary to search for the corresponding points of the feature points in the current frame from the local map. The search methods mainly include linear scanning and constructing data indexes. Linear scanning, that is, exhaustive search, calculates the distance from each sample in the sample set to the target point in turn, and then extracts the point with the smallest distance to the target point as the nearest neighbor point. Since the point cloud data volume of 3D lidars is large, calculating the distance from each point to the target point in turn consumes a large amount of computing resources and has low efficiency. To improve the search efficiency of the corresponding points of the feature points, the embodiments of the present invention adopt a corresponding point search strategy based on constructing data indexes, and construct a KD tree containing the above-mentioned local map points, as Figure 5a and Figure 5b shown. The two-dimensional KD tree and three-dimensional KD tree are respectively shown. This data structure can realize the partitioning of k-dimensional space data, and is mainly used for the rapid search of multi-dimensional space data, with the characteristics of fast convergence speed and short nearest neighbor point query time. Each level of the KD tree allocates child nodes according to the root node. The left and right subtrees are respectively composed of points with coordinates smaller and larger than the root node, and each level of the tree is separated at the next root node. The KD tree can be quickly constructed using the splitting method. The root corresponds to the specified dimension, and the data in this dimension is divided according to the numerical size to realize the construction of the left and right subtrees, and the above steps are repeated in each subtree until there is only a single element in the tree.

[0102] (2) Corresponding point search and constraint construction

[0103] To construct the scan-to-map constraint, it is necessary to construct the constraint relationship between the feature points of the point cloud frame at the current moment and the local map. In order to improve the registration efficiency and accuracy, the present invention adopts a combined registration method of point-to-line registration method and point-to-plane registration method with faster convergence speed and higher solution accuracy to realize the optimal solution of the laser odometer.

[0104] Assume that the i-th moment is the current moment. For the point cloud at the current moment, for a certain edge point in it, use the KD tree to search for the edge point with the closest distance to this point in the point cloud (local map) at the previous moment, and search for the second closest point within the range of the upper and lower three scan lines of the scan line where is located. Using and two points, a feature line can be constructed:

[0105]

[0106] where m = [E F G] T represents the direction vector of this characteristic line, and represent the three-dimensional coordinates of point .

[0107] The edge point can be transformed to coordinate system through rotation and translation transformations. Let the rotation transformation be and the translation transformation be and:

[0108]

[0109] Combining the above content and constructing the residual based on the edge points in the lidar point cloud, that is the distance from the position in the coordinate system to the characteristic line:

[0110]

[0111] For a certain plane point in the point cloud at the current moment use the KD tree to search for the plane point in the previous moment's point cloud (local map) that is the closest to this point and search for the second-closest points and within the range of the upper and lower three scan lines of the scan line where and can be used to construct the characteristic plane:

[0112] Ax + By + Cz + D = 0,

[0113] where n = [A B C] T represents the normal vector of this characteristic plane, and D is the plane intercept.

[0114] Substitute into the plane equation, and it is easy to know:

[0115]

[0116] The characteristic point can be transformed to coordinate system through rotation and translation transformations, and we can get:

[0117]

[0118] However, there are errors in the rotation and translation transformations, which will cause the result of the above formula to be non-zero. Then the residual constructed based on the plane points in the lidar point cloud is:

[0119]

[0120] Based on the above, the point cloud registration residual between the b lidar and the lidar is:

[0121]

[0122] Similarly, it can be obtained that the point cloud registration residual between the lidar and the lidar

[0123] S230. Construct a target cost function, and optimize the point cloud registration residual of each auxiliary lidar according to the target cost function to obtain the relative position relationship between each auxiliary lidar and the master lidar;

[0124] In the embodiment of the present invention, for the above-mentioned point cloud registration residuals, the point cloud registration residuals are optimized by constructing a target cost function, so that the relative position relationship between each auxiliary lidar and the master lidar can be obtained.

[0125] Specifically, a target cost function is constructed, and the point cloud registration residual of each auxiliary lidar is optimized according to the target cost function to obtain the relative position relationship between each auxiliary lidar and the master lidar, as Figure 6 shown, including:

[0126] S231. Construct a target cost function;

[0127] S232. Under the target cost function, jointly optimize the point cloud registration residual of each auxiliary lidar according to the gradient descent method and the Mahalanobis norm;

[0128] S233. Obtain the position relationship transformation matrix from each auxiliary lidar coordinate system to the master lidar coordinate system according to the joint optimization result;

[0129] S234. Determine the relative position relationship between each auxiliary lidar and the master lidar according to the position relationship transformation matrix from each auxiliary lidar coordinate system to the master lidar coordinate system.

[0130] Specifically, for each point cloud registration residual, a target cost function is constructed, and the maximum a posteriori estimation is obtained by minimizing the sum of the prior and the Mahalanobis norm of all residuals using the gradient descent method to achieve joint non-linear optimization:

[0131]

[0132] where and respectively represent the measurement residuals of the left lidar, the right lidar and the rear lidar.

[0133] S240. Obtain the lidar calibration parameters according to the relative position relationship between each auxiliary lidar and the main lidar.

[0134] In the embodiment of the present invention, based on the determined relative position relationship between the auxiliary radar laser and the main radar laser, the lidar calibration parameters can be determined.

[0135] In the embodiment of the present invention, the working conditions of unmanned mining trucks are mostly long working hours, bumpy roads, ore collisions, etc. The lidar installed on the outer periphery of the vehicle is prone to position deviation. To detect and compensate for the position deviation in a timely manner, specifically, determine the real-time position parameters of the lidar according to the lidar point cloud preprocessing data, and monitor the real-time position parameters of the lidar, as Figure 7 shown, including:

[0136] S310. Extract the real-time position parameters of the lidar according to the lidar point cloud preprocessing data;

[0137] In the embodiment of the present invention, the position deviation of 3 degrees of freedom (roll, pitch, and z) is monitored through the ground points and the ground normal vector. When the position deviation is small and does not affect the safe and normal driving of the unmanned mining truck, compensate for the deviation online; when the position deviation is large and affects the safe driving of the unmanned mining truck, send an alarm message to the unmanned driving system and the safety officer.

[0138] Specifically, extract the real-time position parameters of the lidar according to the lidar point cloud preprocessing data, as Figure 8 shown, including:

[0139] S311. Extract the ground points in the lidar point cloud preprocessing data according to the minimum point residual;

[0140] S312. Determine the ground normal vector according to the ground points in the lidar point cloud preprocessing data;

[0141] S313. Construct a horizontal plane in the lidar coordinate system and calculate the horizontal plane parallel vector;

[0142] S314. Determine the real-time position parameters of the lidar according to the ground normal vector and the horizontal plane parallel vector, and the real-time position parameters of the lidar at least include: pitch angle, roll angle, and z-axis intercept.

[0143] Use the LPR (Least Point Residual) algorithm to extract 4 ground points detected by the lidar. This algorithm iteratively calculates the plane parameters by selecting the point set with the minimum residual, and can avoid the interference of noise points as much as possible.

[0144] For LiDAR point cloud Calculate the average value of the k points with the smallest z values in the point cloud Assume that these points are close to the ground and have the minimum height. Select the centroid (x c , y c , z c ) of these points as a point on the initial plane, and at the same time use the covariance matrix of these points for PCA (Principal Component Analysis) to obtain the ground normal vector Then the initial plane can be expressed as:

[0145] Ax + By + Cz + D = 0,

[0146] where, is the normal vector

[0147] For each point in the point cloud, calculate its distance to the fitted plane to construct the residual:

[0148]

[0149] Sort these residuals, and filter out the points that meet the conditions as the inlier points in the candidate plane according to a preset residual threshold. Then, use the filtered inlier set to recalculate the parameters of the plane model, and repeatedly perform the residual calculation and filtering of points to the plane until the fitted plane model converges (i.e., the change amount of the plane parameters is less than a certain threshold) or reaches the maximum number of iterations. When the algorithm converges, output the final plane parameters (A, B, C, D) as the fitting result

[0150] S320. Determine the position offset of each LiDAR according to the real-time position parameters of the LiDAR;

[0151] S330. If the position offset of the LiDAR is greater than the first preset threshold, then compare the position offset of each LiDAR with the second preset threshold, where the first preset threshold is less than the second preset threshold;

[0152] S340. If the position offset of the LiDAR is not greater than the second preset threshold, then determine that the LiDAR has a controllable position offset;

[0153] S350. If the position offset of the LiDAR is greater than the second preset threshold, then determine that the LiDAR has an uncontrollable position offset.

[0154] In the embodiment of the present invention, if the position offset of the LiDAR is not greater than the first preset threshold, then record and save the real-time position parameters of the current LiDAR.

[0155] When the embodiment of the present invention performs real-time position monitoring of the lidar, a monitoring sliding window is specifically constructed, and the ground normal vectors of 10 consecutive frames are included in the window. At the same time, a horizontal plane in the lidar coordinate system is constructed, and a vector parallel to the horizontal plane is calculated.

[0156]

[0157] Subsequently, the angle between the ground and the horizontal plane is calculated:

[0158]

[0159] According to and θ, a homogeneous transformation matrix is constructed, and roll, pitch, and z are extracted from the matrix. Subsequently, the variances of roll, pitch, and intercept z within the sliding window are calculated. If the variance of a certain parameter within the sliding window is greater than the set threshold φ1 and less than the set threshold φ2, it is considered that the parameter is abnormal, that is, the lidar has a position offset, and detailed diagnostic information is sent to the unmanned driving system, that is, which parameter of which radar has a problem; if the variance of a certain parameter within the sliding window is greater than the set threshold φ2, it is considered that the lidar has a large-scale position offset that cannot be corrected by online compensation. After the system subscribes to this message, it will immediately send a safe parking instruction to the decision-making module; if no lidar position offset is found within the sliding window (that is, the variance thresholds of all parameters are less than φ1), the roll, pitch, and z within the latest sliding window will be recorded in real time and saved in.txt format for subsequent offline monitoring of parameters.

[0160] When a lidar has a position offset, the unmanned driving system will send detailed diagnostic information and re-invoke the multi-lidar automatic calibration method. For the specific lidar, the algorithm is re-invoked, the corresponding residuals are constructed and optimized to generate new calibration parameters.

[0161] After the unmanned mining truck stops operating, it will be charged, maintained, and spare parts will be replaced, etc. During this process, the staff may cause a large offset of the lidar due to improper operation. If a certain unmanned mining truck does not have a lidar position offset during operation, after restarting the vehicle and turning on the unmanned driving system, compare the generated.txt files of all lidars with the 10-frame data in the first sliding window, and calculate the variance of these 20-frame data. If the variance of a certain parameter of a certain lidar is greater than the set threshold φ2, it is considered that the lidar has a serious position offset, and then the vehicle will stop safely and the on-site test personnel will be notified to repair it.

[0162] In summary, the online calibration method for lidar of driverless mining trucks provided by the present invention uses a non-linear optimization method to obtain accurate calibration parameters, saving labor costs while improving efficiency. In addition, relatively small position offsets can be detected in a timely manner and are not significant. For relatively large position offsets, this method can issue a warning message in a timely manner and cause the driverless mining truck to stop safely. Therefore, the online calibration method for lidar of driverless mining trucks provided by the present invention can achieve automatic calibration between lidars with high precision, self-monitoring and compensation of parameters to a great extent with less manual intervention, significantly improve the integration and debugging efficiency of the driverless system of driverless mining trucks, and ensure the safety of driverless mining trucks during operation.

[0163] As another embodiment of the present invention, there is provided a lidar online calibration device 100 for driverless mining trucks, which is used to implement the online calibration method for lidar of driverless mining trucks described above. Among them, as Figure 9 shown, it includes:

[0164] A radar data preprocessing module 110, which is used to obtain the lidar point cloud data of the driverless mining truck, and perform data preprocessing on the lidar point cloud data to obtain preprocessed lidar point cloud data. At least one main lidar and at least two auxiliary lidars are provided on the driverless mining truck;

[0165] A calibration module 120, which is used to construct a local map based on the main lidar coordinate system, and associate and optimize the preprocessed point cloud data of each auxiliary lidar with the local map to obtain lidar calibration parameters;

[0166] A position monitoring module 130, which is used to determine the real-time position parameters of the lidar according to the preprocessed lidar point cloud data, and monitor the real-time position parameters of the lidar;

[0167] A deviation correction module 140, which is used to re-execute the lidar calibration process to update the lidar calibration parameters when it is determined according to the real-time position parameters of the lidar that the lidar has a position offset and the position offset amount is within a preset threshold range;

[0168] An alarm module 150, which is used to send an alarm message when it is determined according to the real-time position parameters of the lidar that the lidar has a position offset and the position offset amount is not within a preset threshold range.

[0169] The on-line calibration device for lidar of driverless mining trucks provided by the present invention realizes the automatic calibration of the lidar of driverless mining trucks through the lidar point cloud data of driverless mining trucks. Moreover, due to the joint optimization of the local map based on the main lidar coordinate system and the point cloud data of the auxiliary lidar, the automatic calibration of the relative spatial positions of multiple lidars can be achieved. In addition, through the on-line monitoring of the lidar position, if a certain lidar has a position deviation, the position of this lidar can be corrected online again to meet the temporary use of the driverless mining truck. If the position deviation is too large, an alarm message will be sent to make the driverless mining truck stop safely, ensuring driving safety. Therefore, the on-line calibration device for lidar of driverless mining trucks provided by the present invention can realize the automatic and accurate calibration of the lidar of driverless mining trucks.

[0170] Regarding the specific working principle of the on-line calibration device for lidar of driverless mining trucks provided by the present invention, reference can be made to the description of the on-line calibration method for lidar of driverless mining trucks in the previous text, which will not be elaborated here.

[0171] As another embodiment of the present invention, an on-line calibration system 10 for lidar of driverless mining trucks is provided. Among them, as Figure 10 shown, it includes: a lidar 200 and the on-line calibration device 100 for lidar of driverless mining trucks described above. The lidar 200 is communicatively connected to the on-line calibration device 100 for lidar of driverless mining trucks;

[0172] The lidar 200 is arranged on the driverless mining truck and is used to collect lidar point cloud data in real time;

[0173] The on-line calibration device 100 for lidar of driverless mining trucks is used to perform lidar calibration and lidar position monitoring and correction according to the lidar point cloud data.

[0174] The on-line calibration system for lidar of driverless mining trucks provided by the present invention adopts the on-line calibration device for lidar of driverless mining trucks described above, realizes the automatic calibration of the lidar of driverless mining trucks through the lidar point cloud data of driverless mining trucks. Moreover, due to the joint optimization of the local map based on the main lidar coordinate system and the point cloud data of the auxiliary lidar, the automatic calibration of the relative spatial positions of multiple lidars can be achieved. In addition, through the on-line monitoring of the lidar position, if a certain lidar has a position deviation, the position of this lidar can be corrected online again to meet the temporary use of the driverless mining truck. If the position deviation is too large, an alarm message will be sent to make the driverless mining truck stop safely, ensuring driving safety. Therefore, the on-line calibration system for lidar of driverless mining trucks provided by the present invention can realize the automatic and accurate calibration of the lidar of driverless mining trucks.

[0175] It is understood that the above embodiments are merely exemplary embodiments adopted to illustrate the principles of the present invention. However, the present invention is not limited thereto. For those of ordinary skill in the art, various modifications and improvements can be made without departing from the spirit and essence of the present invention, and these modifications and improvements are also considered within the scope of protection of the present invention.

Claims

1. An online calibration method for lidar of driverless mining trucks, characterized in that, Including: Obtain the lidar point cloud data of the driverless mining truck, and perform data preprocessing on the lidar point cloud data to obtain preprocessed lidar point cloud data, wherein the lidar installed on the driverless mining truck includes at least one main lidar and at least two auxiliary lidars; Construct a local map based on the main lidar coordinate system, and associate and optimize the preprocessed point cloud data of each auxiliary lidar with the local map to obtain lidar calibration parameters; Determine the real-time position parameters of the lidar according to the preprocessed lidar point cloud data, and monitor the real-time position parameters of the lidar; When it is determined that the current lidar has a controllable position offset according to the real-time position parameters of the lidar, re-execute the lidar calibration process to update the lidar calibration parameters; When it is determined that the current lidar has an uncontrollable position offset according to the real-time position parameters of the lidar, send an alarm message; Wherein, constructing a local map based on the main lidar coordinate system, and associating and optimizing the preprocessed point cloud data of each auxiliary lidar with the local map to obtain lidar calibration parameters, includes: Construct a local map based on the preprocessed point cloud data of the main lidar coordinate system according to the preprocessed point cloud data of the main lidar coordinate system; Match the preprocessed point cloud data of each auxiliary lidar with the point cloud in the local map to obtain the point cloud registration residuals of each auxiliary lidar; Construct an objective cost function, and optimize the point cloud registration residuals of each auxiliary lidar according to the objective cost function to obtain the relative position relationship between each auxiliary lidar and the main lidar; Obtain lidar calibration parameters according to the relative position relationship between each auxiliary lidar and the main lidar; Wherein, constructing a local map based on the preprocessed point cloud data of the main lidar coordinate system includes: Determine the first position relationship between the main lidar and the inertial navigation unit and the second position relationship of the inertial navigation unit relative to the world coordinate system; Construct a local map based on the continuous lidar point cloud preprocessed data of the main lidar according to the first position relationship and the second position relationship.

2. The online calibration method for lidar of an unmanned mining truck according to claim 1, characterized in that Match the preprocessed point cloud data of each auxiliary lidar with the point cloud in the local map to obtain the point cloud registration residuals of each auxiliary lidar, including: Search for local map feature points corresponding to the point cloud feature points of the current auxiliary lidar in the local map according to the preprocessed point cloud data of each auxiliary lidar; Realize the registration of the point cloud feature points of each auxiliary lidar and the local map feature points according to the point-to-line registration method and the point-to-plane registration method, and obtain the point cloud registration residuals of each auxiliary lidar.

3. The online calibration method of the lidar for driverless mining trucks according to claim 1, characterized in that, Construct an objective cost function, and optimize the point cloud registration residuals of each auxiliary lidar according to the objective cost function to obtain the relative position relationship between each auxiliary lidar and the main lidar, including: Construct an objective cost function; Under the objective cost function, jointly optimize the point cloud registration residuals of each auxiliary lidar according to the gradient descent method and the Mahalanobis norm; Obtain the transformation matrix of the position relationship from each auxiliary lidar coordinate system to the main lidar coordinate system according to the joint optimization result; Determine the relative position relationship between each auxiliary lidar and the main lidar according to the transformation matrix of the position relationship from each auxiliary lidar coordinate system to the main lidar coordinate system.

4. The online calibration method for lidar of driverless mining trucks according to any one of claims 1 to 3, characterized in that Determine the real-time position parameters of the lidar according to the preprocessed lidar point cloud data, and monitor the real-time position parameters of the lidar, including: Extract the real-time position parameters of the lidar according to the preprocessed lidar point cloud data; Determine the position offset of each lidar according to the real-time position parameters of the lidar; If the position offset of this lidar is greater than the first preset threshold, then compare the position offset of each lidar with the second preset threshold, where the first preset threshold is less than the second preset threshold; If the position offset of this lidar is not greater than the second preset threshold, then determine that this lidar has a controllable position offset; If the position offset of this lidar is greater than the second preset threshold, then determine that this lidar has an uncontrollable position offset.

5. The online calibration method for lidar of driverless mining trucks according to claim 4, characterized in that, Extracting the real-time position parameters of the lidar according to the preprocessed lidar point cloud data includes: Extract the ground points in the preprocessed lidar point cloud data according to the minimum point residual; Determine the ground normal vector according to the ground points in the preprocessed lidar point cloud data; Construct a horizontal plane in the lidar coordinate system and calculate the horizontal plane parallel vector; Determine the real-time position parameters of the lidar according to the ground normal vector and the horizontal plane parallel vector, and the real-time position parameters of the lidar at least include: pitch angle, roll angle and z-axis intercept.

6. The online calibration method of lidar for driverless mining trucks according to claim 4, characterized in that If the position offset of this lidar is not greater than the first preset threshold, then record and save the current real-time position parameters of the lidar.

7. A lidar online calibration device for driverless mining trucks, which is used to implement the lidar online calibration method for driverless mining trucks described in any one of claims 1 to 6, and is characterized in that, Including: A radar data preprocessing module, configured to obtain the lidar point cloud data of an unmanned mining truck, and perform data preprocessing on the lidar point cloud data to obtain preprocessed lidar point cloud data, where the lidar arranged on the unmanned mining truck includes at least one main lidar and at least two auxiliary lidars; A calibration module, configured to construct a local map based on the main lidar coordinate system, and perform association optimization on the preprocessed point cloud data of each auxiliary lidar with the local map to obtain lidar calibration parameters; A position monitoring module, configured to determine the real-time position parameters of the lidar according to the preprocessed lidar point cloud data, and monitor the real-time position parameters of the lidar; A deviation correction module, configured to re-execute the lidar calibration process to update the lidar calibration parameters when it is determined according to the real-time position parameters of the lidar that the lidar has a position offset and the position offset is within the preset threshold range; An alarm module, configured to send an alarm message when it is determined according to the real-time position parameters of the lidar that the lidar has a position offset and the position offset is not within the preset threshold range.

8. An online calibration system for lidar of driverless mining trucks, characterized in that, Including: A lidar and the lidar online calibration device for an unmanned mining truck according to claim 7, where the lidar is communicatively connected to the lidar online calibration device for an unmanned mining truck; The lidar is installed on the driverless mining truck and is used to collect lidar point cloud data in real time; The lidar online calibration device for driverless mining trucks is used to perform lidar calibration, lidar position monitoring and correction based on the lidar point cloud data.

Images