Laser radar online calibration method, device and system for unmanned mine card

Abstract

The invention relates to the technical field of unmanned vehicle calibration, and particularly discloses a laser radar online calibration method, device and system for an unmanned mine card, and the method comprises the steps: obtaining the laser radar point cloud data of the unmanned mine card, and carrying out the data preprocessing of the laser radar point cloud data; a local map based on a main laser radar coordinate system is constructed, and correlation optimization is performed on the point cloud preprocessing data of each auxiliary laser radar and the local map; monitoring real-time position parameters of the laser radar; when it is determined that the current laser radar has controllable position offset according to the real-time position parameters of the laser radar, the laser radar calibration process is executed again to update the calibration parameters of the laser radar; when it is determined that the current laser radar has uncontrollable position offset according to the real-time position parameters of the laser radar, alarm information is sent out. The laser radar on-line calibration method for the unmanned mine card provided by the invention can realize automatic and accurate calibration of an unmanned vehicle sensor.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned vehicle calibration, and in particular to an online laser radar calibration method for an unmanned mining truck, an online laser radar calibration device for an unmanned mining truck, and an online laser radar calibration system for an unmanned mining truck. Background Art

[0002] In the field of autonomous driving, vehicle sensor calibration refers to the process of calibrating various sensors mounted on the vehicle. These sensors include lidar, cameras, millimeter-wave radar, and inertial navigation, which are responsible for sensing the vehicle's surroundings and determining the data of the vehicle's motion posture. The accuracy and consistency of sensors are critical to the safety and performance of autonomous driving systems. The goal behind the calibration of sensors for autonomous vehicles is to ensure that these sensors can accurately sense and measure objects, road conditions, and other traffic participants around the vehicle. The importance of sensor calibration lies in its direct impact on the perception and decision-making capabilities of autonomous driving systems. If the sensor calibration is incorrect, it may lead to erroneous perception results, thereby affecting the system's ability to identify and understand obstacles, lane lines, and traffic signals. This may lead to incorrect decisions and driving behaviors, which in turn endangers driving safety. However, current technologies mainly use manual calibration to obtain a more accurate relative position relationship between sensors, and most automatic calibration methods are affected by algorithm accuracy or the clutter of the data acquisition environment, and their calibration accuracy is mostly unable to meet the actual application requirements.

[0003] Therefore, how to achieve automatic and accurate calibration of vehicle sensors in unmanned vehicles 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 a laser radar online calibration method for an unmanned mining truck, a laser radar online calibration device for an unmanned mining truck, and a laser radar online calibration system for an unmanned mining truck, which solve the problem in the related art that the automatic and accurate calibration of the unmanned vehicle sensor cannot be achieved.

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

[0006] Acquire laser radar point cloud data of an unmanned mining truck, and perform data preprocessing on the laser radar point cloud data to obtain laser radar point cloud preprocessing data, wherein the laser radar set on the unmanned mining truck includes at least one main laser radar and at least two auxiliary laser radars;

[0007] Constructing a local map based on the main laser radar coordinate system, and optimizing the point cloud preprocessing data of each auxiliary laser radar with the local map to obtain laser radar calibration parameters;

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

[0009] When it is determined according to the real-time position parameters of the laser radar that the current laser radar has a controllable position deviation, the laser radar calibration process is re-executed to update the laser radar calibration parameters;

[0010] When it is determined based on the real-time position parameters of the laser radar that an uncontrollable position deviation occurs, an alarm message is issued.

[0011] Furthermore, a local map based on the main laser radar coordinate system is constructed, and the point cloud preprocessing data of each auxiliary laser radar is associated and optimized with the local map to obtain the laser radar calibration parameters, including:

[0012] Construct a local map based on the main laser radar coordinate system according to the point cloud preprocessing data of the main laser radar coordinate system;

[0013] Matching the point cloud preprocessed data of each auxiliary laser radar with the point cloud in the local map to obtain the point cloud registration residual of each auxiliary laser radar;

[0014] Constructing a target cost function, and optimizing the point cloud registration residual of each auxiliary laser radar according to the target cost function, so as to obtain the relative position relationship between each auxiliary laser radar and the main laser radar;

[0015] The laser radar calibration parameters are obtained according to the relative position relationship between each auxiliary laser radar and the main laser radar.

[0016] Furthermore, a local map based on the main laser radar coordinate system is constructed according to the point cloud preprocessing data of the main laser radar coordinate system, including:

[0017] Determine a first positional relationship between the main laser radar and the inertial navigation unit and a second positional relationship of the inertial navigation unit relative to the world coordinate system;

[0018] A local map based on the continuous laser point cloud preprocessing data of the main laser radar is constructed according to the first positional relationship and the second positional relationship.

[0019] Furthermore, the point cloud preprocessed data of each auxiliary laser radar is matched with the point cloud in the local map to obtain the point cloud registration residual of each auxiliary laser radar, including:

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

[0021] The point cloud feature points of each auxiliary laser radar are registered with the local map feature points according to the point-to-line registration method and the point-to-plane registration method, and the point cloud registration residual of each auxiliary laser radar is obtained.

[0022] Furthermore, a target cost function is constructed, and the point cloud registration residual of each auxiliary laser radar is optimized according to the target cost function to obtain the relative position relationship between each auxiliary laser radar and the main laser radar, including:

[0023] Construct the target cost function;

[0024] Under the target cost function, the point cloud registration residual of each auxiliary lidar is jointly optimized according to the gradient descent method and the Mahalanobis norm;

[0025] According to the joint optimization result, the positional relationship transformation matrix from each auxiliary laser radar coordinate system to the main laser radar coordinate system is obtained;

[0026] The relative position relationship between each auxiliary laser radar and the main laser radar is determined according to the position relationship transformation matrix from each auxiliary laser radar coordinate system to the main laser radar coordinate system.

[0027] Furthermore, determining the real-time position parameters of the laser radar according to the laser radar point cloud preprocessing data and monitoring the real-time position parameters of the laser radar includes:

[0028] Extract the real-time position parameters of the LiDAR based on the pre-processed data of the LiDAR point cloud;

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

[0030] If the position offset of the laser radar is greater than a first preset threshold, comparing the position offset of each laser radar with a second preset threshold, wherein the first preset threshold is less than the second preset threshold;

[0031] If the position offset of the laser radar is not greater than the second preset threshold, it is determined that the laser radar has a controllable position offset;

[0032] If the position offset of the laser radar is greater than the second preset threshold, it is determined that the laser radar has an uncontrollable position offset.

[0033] Furthermore, the real-time position parameters of the laser radar are extracted according to the laser radar point cloud preprocessing data, including:

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

[0035] Determine a ground normal vector based on the ground points in the laser radar point cloud preprocessing data;

[0036] Construct the horizontal plane in the laser radar coordinate system and calculate the vector parallel to the horizontal plane;

[0037] The real-time position parameters of the laser radar are determined according to the ground normal vector and the horizontal plane parallel vector, and the real-time position parameters of the laser radar include at least: a pitch angle, a roll angle and a z-axis intercept.

[0038] Furthermore, if the position offset of the laser radar is not greater than a first preset threshold, the current real-time position parameters of the laser radar are recorded and saved.

[0039] As another aspect of the present invention, a laser radar online calibration device for an unmanned mining truck is provided, which is used to implement the laser radar online calibration method for an unmanned mining truck as described above, and includes:

[0040] A radar data preprocessing module is used to obtain the laser radar point cloud data of the unmanned mining truck and perform data preprocessing on the laser radar point cloud data to obtain the laser radar point cloud preprocessing data, wherein the laser radar set on the unmanned mining truck includes at least one main laser radar and at least two auxiliary laser radars;

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

[0042] A position monitoring module, used to determine the real-time position parameters of the laser radar according to the laser radar point cloud preprocessing data, and monitor the real-time position parameters of the laser radar;

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

[0044] The alarm module is used to issue an alarm message when it is determined based on the real-time position parameters of the laser radar that the laser radar has a position offset and the position offset amount is not within a preset threshold range.

[0045] As another aspect of the present invention, a laser radar online calibration system for an unmanned mining truck is provided, which includes: a laser radar and the laser radar online calibration device for an unmanned mining truck as described above, wherein the laser radar is communicatively connected to the laser radar online calibration device for an unmanned mining truck;

[0046] The laser radar is arranged on the unmanned mining truck and is used to collect the laser radar point cloud data in real time;

[0047] The laser radar online calibration device for unmanned mining trucks is used to perform laser radar calibration and laser radar position monitoring and deviation correction based on laser radar point cloud data.

[0048] The laser radar online calibration method for unmanned mining trucks provided by the present invention realizes the automatic calibration of the laser radar of the unmanned mining truck through the laser radar point cloud data of the unmanned mining truck, and due to the joint optimization of the local map based on the main laser radar coordinate system and the point cloud data of the auxiliary laser radar, the automatic calibration of the relative spatial position of multiple laser radars can be realized. In addition, through the online monitoring of the laser radar position, if a laser radar has a position offset, the position of the laser radar can be corrected online to meet the temporary use of the unmanned mining truck. If the position offset is too large, an alarm message is sent to make the unmanned mining truck stop safely to ensure driving safety. Therefore, the laser radar online calibration method for unmanned mining trucks provided by the present invention can realize the automatic and accurate calibration of the laser radar of the unmanned mining truck. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0050] Figure 1 The present invention provides a flow chart of the online calibration method of the laser radar for unmanned mining trucks.

[0051] Figure 2 A flow chart of the method for obtaining laser radar calibration parameters provided by the present invention.

[0052] Figure 3 A flow chart of the method for constructing a local map provided by the present invention.

[0053] Figure 4 A flow chart of point cloud matching between the auxiliary laser radar point cloud and the local map provided by the present invention.

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

[0055] Figure 5bThis is a schematic diagram of the three-dimensional KD tree provided by the present invention.

[0056] Figure 6 A flowchart for optimization according to a target cost function provided by the present invention.

[0057] Figure 7 A flow chart for monitoring the real-time position of a laser radar provided by the present invention.

[0058] Figure 8 A flowchart for extracting real-time position parameters of a laser radar provided by the present invention.

[0059] Fig. 9 This is a structural block diagram of the laser radar online calibration device for unmanned mining trucks provided by the present invention.

[0060] Fig.10 This is a structural block diagram of the laser radar online calibration system for unmanned mining trucks provided by the present invention. DETAILED DESCRIPTION

[0061] It should be noted that, in the absence of conflict, the embodiments of 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 accompanying drawings and in combination with the embodiments.

[0062] In order to enable those skilled in the art to better understand the scheme of the present invention, the technical scheme in the embodiment of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiment of the present invention. Obviously, the described embodiment is only a part of the embodiment of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work should fall within the scope of protection 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 are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchanged where appropriate, so as to describe the embodiments of the present invention described herein. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0064] For the relative position calibration of multiple laser radars of unmanned transport mining trucks (hereinafter referred to as unmanned mining trucks), since there are a large number of laser radars installed on unmanned mining trucks (usually 3 to 5), it is a cumbersome task to perform detailed manual calibration for each unmanned mining truck, and there is a lot of repetitive work. In addition, since the operating roads of unmanned mining trucks are usually unstructured roads and the operating environment is usually harsh, frequent vibrations may cause the laser radar to deviate from the original installation position, resulting in the failure of the original calibration relationship, thereby affecting the obstacle perception accuracy and reducing driving safety.

[0065] Based on this, in this embodiment, a laser radar online calibration method for an unmanned mining truck is provided. Figure 1 : is a flow chart of an online laser radar calibration method for an unmanned mining truck according to an embodiment of the present invention. Figure 1 As shown, including:

[0066] S100, obtaining laser radar point cloud data of an unmanned mining truck, and performing data preprocessing on the laser radar point cloud data to obtain laser radar point cloud preprocessing data, wherein the laser radar set on the unmanned mining truck includes at least one main laser radar and at least two auxiliary laser radars;

[0067] In the embodiment of the present invention, an unmanned mining vehicle equipped with four laser radars is used as an example for description. Specifically, the four laser radars are installed one each in the front, left, right and rear of the vehicle body, and the corresponding numbers are a, b, c and d respectively.

[0068] The laser radar installed in front of the vehicle body is used as the main laser radar, and the laser radars installed on the left, right and rear directions are used as auxiliary laser radars. When the unmanned mining truck is running, each laser radar can obtain its own laser radar point cloud data in real time. For the laser radar point cloud data of the unmanned mining truck, since the acquired raw point cloud data may contain noise and other factors that affect the calibration accuracy, it is necessary to pre-process the laser radar point cloud data to remove noise and improve the calibration accuracy.

[0069] S200, constructing a local map based on the main laser radar coordinate system, and associating and optimizing the point cloud preprocessing data of each auxiliary laser radar with the local map to obtain laser radar calibration parameters;

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

[0071] S300, determining the real-time position parameters of the laser radar according to the laser radar point cloud preprocessing data, and monitoring the real-time position parameters of the laser radar;

[0072] Since the unmanned mining truck is in motion, in order to address the problem of position offset and other issues that may affect the data accuracy of the LiDAR on the unmanned mining truck, the real-time position parameters of the LiDAR are obtained by preprocessing the LiDAR point cloud data, and the real-time position parameters of the LiDAR are monitored. This allows correction to be made when position offset is found to meet the use of the unmanned mining truck.

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

[0074] In an embodiment of the present invention, when a position offset is detected in the real-time position parameters of the laser radar, 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, the aforementioned radar calibration process is repeated to update the laser radar calibration parameters.

[0075] S500: When it is determined according to the real-time position parameters of the laser radar that an uncontrollable position deviation occurs in the current laser radar, an alarm message is issued.

[0076] When it is determined that the laser radar has an uncontrollable position deviation, an alarm message is issued to enable the unmanned mining truck to stop safely and ensure driving safety.

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

[0078] It should be noted that before calibrating the laser radar of the unmanned mining truck, the laser radar can be timed first. Specifically, the GNSS timing method is used to time each laser radar, and on this basis, linear interpolation is performed on each time stamp to eliminate the time offset between each laser radar, thereby improving the calibration accuracy of the laser radar.

[0079] When the laser radar point cloud data of the laser radar is obtained, the laser radar point cloud data of each laser radar is preprocessed, which may specifically include point cloud data denoising and dust removal.

[0080] Specifically, the pcl library function is used to denoise the original point cloud of multiple laser radars to reduce the adverse effects of noise points on calibration accuracy. At the same time, due to the harsh working environment of unmanned mining vehicles, there are usually dust, floating dust, gravel, etc. on the working road, which have an adverse effect on the detection effect of the laser radar and are prone to perception blind spots. Therefore, it is necessary to remove the dust in the point cloud in the multi-laser radar calibration process. The present invention uses a segmentation clustering method to extract dust point cloud clusters, and uses the DBSCAN algorithm to select points closer to the sensor as core points. If the neighborhood of the point contains enough points, these points are marked as core points. These core points are then expanded to form point cloud clusters, and finally the dust point cloud clusters are removed. Finally, the preprocessed point clouds of 4 laser radars are obtained.

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

[0082] S210, constructing a local map based on the main laser radar coordinate system according to the point cloud preprocessing data of the main laser radar coordinate system;

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

[0084] S211, determining a first position relationship between the main laser radar and the inertial navigation unit and a second position relationship of the inertial navigation unit relative to the world coordinate system;

[0085] In the embodiment of the present invention, a first position relationship (assuming it is known) T between the main laser radar and the inertial navigation unit is determined. gl and the second position relationship T of the inertial navigation unit relative to the world coordinate system wg .

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

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

[0088]

[0089] in, represents the point cloud frame, a represents the main lidar number, n represents the frame number, They represent the x-coordinate, y-coordinate, and z-coordinate of the point cloud respectively.

[0090] Based on the above, the local map can be described as follows:

[0091]

[0092] in, Represents the overlay operation of point cloud frames.

[0093] S220, matching the point cloud preprocessing data of each auxiliary laser radar with the point cloud in the local map to obtain the point cloud registration residual of each auxiliary laser radar;

[0094] In an embodiment of the present invention, the point cloud preprocessed data of each auxiliary laser radar is matched with each point cloud of the local map, and the difference in matching results is optimized.

[0095] Specifically, the point cloud preprocessed data of each auxiliary laser radar is matched with the point cloud in the local map to obtain the point cloud registration residual of each auxiliary laser radar, such as Figure 4 As shown, including:

[0096] S221, searching the local map for local map feature points corresponding to the point cloud feature points of the current auxiliary laser radar according to the point cloud preprocessing data of each auxiliary laser radar;

[0097] Construct b, c, d laser radar and The constraints between b, c, d and lidar are obtained by constructing and solving the pose transformation constraints based on the selected feature points, and completing the point cloud registration based on the scan-to-map point cloud data association method. The pose transformation relationship between lidars.

[0098] S222. According to the point-to-line registration method and the point-to-plane registration method, the point cloud feature points of each auxiliary laser radar are registered with the local map feature points, and the point cloud registration residual of each auxiliary laser radar is obtained.

[0099] In the embodiment of the present invention, the b laser radar and The relationship between the position transformation of the laser radar is given as an example. The laser radar c and d are The solution to the posture transformation relationship between laser radars is similar.

[0100] (1) KD tree construction

[0101] When constructing constraints between laser radars, it is necessary to search for corresponding points of the feature points of the current frame from the local map. There are two main search methods: linear scanning and building a data index. 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 minimum distance to the target point as the nearest neighbor. Since the amount of three-dimensional laser radar point cloud data is large, calculating the distance between each point and the target point in turn consumes computing resources and is inefficient. In order to improve the efficiency of the corresponding point search of feature points, an embodiment of the present invention adopts a corresponding point search strategy based on building a data index to construct a KD tree containing the above-mentioned local map points, such as Figure 5a and Figure 5b The two-dimensional KD tree and the three-dimensional KD tree are shown respectively. This data structure can realize k-dimensional spatial data partitioning and is mainly used for fast search of multi-dimensional spatial data. It has the characteristics of fast convergence speed and short neighbor point query time. Each level of the KD tree implements child node allocation according to the root node. The left and right subtrees are composed of points with coordinates less than and greater than the root node respectively. Each level of the tree is separated at the next node. The KD tree can be quickly constructed using the partitioning method. The root corresponds to the specified dimension. The data of this dimension is divided according to the numerical value to realize the construction of the left and right subtrees. The above steps are repeated in each subtree until the tree contains only a single element.

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

[0103] To construct scan-to-map constraints, 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 combination of point-to-line registration method and point-to-surface registration method with faster convergence speed and higher solution accuracy to achieve the optimization solution of laser odometer.

[0104] Assume that time i is the current time, for the point cloud at the current time An edge point in Use the KD tree to search for the edge point that is closest to the point in the point cloud (local map) at the previous moment and in Search for the next closest point within the three scan lines above and below the scan line use and Two points can construct feature lines:

[0105]

[0106] Where m = [EFG] T represents the direction vector of the feature line, and Indicate point The three-dimensional coordinates of .

[0107] The edge points can be transformed by rotation and translation Convert to In the coordinate system, the rotation transformation is The translation transformation is and:

[0108]

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

[0110]

[0111] For the current point cloud A point in the plane Use the KD tree to search for the plane point closest to the point in the point cloud (local map) at the previous moment and in Search for the next closest point within the three scan lines above and below the scan line and use and Constructible feature surfaces:

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

[0113] Where n = [ABC] T represents the normal vector of the characteristic plane, and D is the plane intercept.

[0114] Will Substituting into the plane equation, it is easy to know:

[0115]

[0116] The feature points can be transformed by rotation and translation Convert to In the coordinate system, 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, b laser radar and The residual error of point cloud registration between lidars is:

[0121]

[0122] Similarly, we can get LiDAR and Point cloud registration residuals between lidars

[0123] S230, constructing a target cost function, and optimizing the point cloud registration residual of each auxiliary laser radar according to the target cost function, to obtain a relative position relationship between each auxiliary laser radar and the main laser radar;

[0124] In an embodiment of the present invention, for each of 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 laser radar and the main laser radar can be obtained.

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

[0126] S231, constructing a target cost function;

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

[0128] S233, obtaining a positional relationship transformation matrix from each auxiliary laser radar coordinate system to the main laser radar coordinate system according to the joint optimization result;

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

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

[0131]

[0132] in, and Represent the measurement residuals of the left lidar, right lidar and rear lidar respectively.

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

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

[0135] In the embodiment of the present invention, the working conditions of the unmanned mining vehicle are mostly long working hours, bumpy roads, ore bumps, etc. The laser radar installed on the periphery of the vehicle body is prone to position deviation. In order to timely detect the position deviation and compensate for it, specifically, the real-time position parameters of the laser radar are determined according to the laser radar point cloud preprocessing data, and the real-time position parameters of the laser radar are monitored, such as Figure 7 As shown, including:

[0136] S310, extracting laser radar real-time position parameters according to laser radar point cloud preprocessing data;

[0137] In an embodiment of the present invention, the position offset of three degrees of freedom (roll, pitch and z) is monitored by ground points and ground normal vectors. When the position offset is small and does not affect the safe and normal driving of the unmanned mine car, the offset is compensated online; when the position offset is large and affects the safe driving of the unmanned mine car, an alarm message needs to be sent to the unmanned driving system and the safety officer.

[0138] Specifically, the real-time position parameters of the LiDAR are extracted based on the preprocessed data of the LiDAR point cloud, such as Figure 8 As shown, including:

[0139] S311, extracting ground points in the laser radar point cloud preprocessing data according to the minimum point residual;

[0140] S312, determining a ground normal vector according to the ground points in the laser radar point cloud preprocessing data;

[0141] S313, constructing a horizontal plane in the laser radar coordinate system, and calculating a vector parallel to the horizontal plane;

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

[0143] The LPR (Least Point Residual) algorithm is used to extract the four ground points detected by the lidar. The algorithm can avoid the interference of noise points as much as possible by selecting the point set with the minimum residual and iteratively calculating the plane parameters.

[0144] for LiDAR point cloud Calculate the average value of the k points with the smallest z value in the point cloud Assume that these points are close to the ground and have the smallest height. Select the centroid of these points (x c ,y c , z c ) as a point on the initial plane, and use the covariance matrix of these points to perform 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] in, 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] These residuals are sorted, and points that meet the conditions are screened out as candidate plane inliers according to the preset residual threshold. Then, the parameters of the plane model are recalculated using the screened inliers, and the residual calculation and screening from point to plane are repeated until the fitted plane model converges (that is, the change in plane parameters is less than a certain threshold) or the maximum number of iterations is reached. When the algorithm converges, the final plane parameters (A, B, C, D) are output as the fitting results.

[0150] S320, determining the position offset of each laser radar according to the real-time position parameter of the laser radar;

[0151] S330: If the position offset of the laser radar is greater than a first preset threshold, compare the position offset of each laser radar with a second preset threshold, wherein the first preset threshold is less than the second preset threshold;

[0152] S340: If the position offset of the laser radar is not greater than the second preset threshold, determining that a controllable position offset occurs to the laser radar;

[0153] S350: If the position offset of the laser radar is greater than the second preset threshold, it is determined that the laser radar has an uncontrollable position offset.

[0154] In an embodiment of the present invention, if the position offset of the laser radar is not greater than a first preset threshold, the current real-time position parameters of the laser radar are recorded and saved.

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

[0156]

[0157] Then calculate the angle between the ground and the horizontal plane:

[0158]

[0159] according to The homogeneous transformation matrix is ​​constructed by θ and roll, pitch and z are extracted from the matrix, and then the variance of roll, pitch and intercept z in the sliding window is calculated. If the variance of a parameter in the sliding window is greater than the set threshold φ1 and less than the set threshold φ2, the parameter is considered abnormal, that is, the laser radar 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 parameter in the sliding window is greater than the set threshold φ2, it is considered that the laser radar has a large position offset that cannot be corrected by online compensation. After the system subscribes to the message, it will immediately send a safe parking instruction to the decision module; if the laser radar position offset is not found in the sliding window (that is, the variance thresholds of each parameter are less than φ1), the roll, pitch and z in the latest sliding window will be recorded in real time and saved in .txt format for subsequent offline parameter monitoring.

[0160] When a lidar has a position deviation, the unmanned driving system will issue detailed diagnostic information and re-call the multi-lidar automatic calibration method. For the specific radar, it will re-call the algorithm, construct the corresponding residual and optimize it to generate new calibration parameters.

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

[0162] In summary, the laser radar online calibration method for unmanned mining trucks provided by the present invention uses a nonlinear optimization method to obtain accurate calibration parameters, saving labor costs while improving efficiency. In addition, it can promptly discover that a small relative position offset is not long. For a large relative position offset, the method can promptly issue a warning message and allow the unmanned mining truck to stop safely. Therefore, the laser radar online calibration method for unmanned mining trucks provided by the present invention can achieve high-precision automatic calibration and parameter self-monitoring and compensation between laser radars while greatly reducing manual intervention, significantly improving the efficiency of preparation and debugging of the unmanned mining truck's unmanned driving system, while ensuring the safety of the unmanned mining truck during operation.

[0163] As another embodiment of the present invention, a laser radar online calibration device 100 for an unmanned mining truck is provided, which is used to implement the laser radar online calibration method for an unmanned mining truck as described above, wherein: Fig. 9 As shown, including:

[0164] The radar data preprocessing module 110 is used to obtain the laser radar point cloud data of the unmanned mining truck and perform data preprocessing on the laser radar point cloud data to obtain the laser radar point cloud preprocessing data, wherein the laser radar set on the unmanned mining truck includes at least one main laser radar and at least two auxiliary laser radars;

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

[0166] A position monitoring module 130, used to determine the real-time position parameters of the laser radar according to the laser radar point cloud preprocessing data, and monitor the real-time position parameters of the laser radar;

[0167] The correction module 140 is used to re-execute the laser radar calibration process to update the laser radar calibration parameters when it is determined that the laser radar has a position deviation according to the real-time position parameters of the laser radar and the position deviation is within a preset threshold range;

[0168] The alarm module 150 is used to issue an alarm message when it is determined based on the real-time position parameters of the laser radar that the laser radar has a position offset and the position offset amount is not within a preset threshold range.

[0169] The laser radar online calibration device for unmanned mining trucks provided by the present invention realizes the automatic calibration of the laser radar of the unmanned mining truck through the laser radar point cloud data of the unmanned mining truck, and due to the joint optimization of the local map based on the main laser radar coordinate system and the point cloud data of the auxiliary laser radar, it can realize the automatic calibration of the relative spatial position of multiple laser radars. In addition, through the online monitoring of the laser radar position, if a laser radar has a position offset, the position of the laser radar can be corrected online to meet the temporary use of the unmanned mining truck. If the position offset is too large, an alarm message is sent to make the unmanned mining truck stop safely to ensure driving safety. Therefore, the laser radar online calibration device for unmanned mining trucks provided by the present invention can realize the automatic and accurate calibration of the laser radar of the unmanned mining truck.

[0170] Regarding the specific working principle of the laser radar online calibration device for unmanned mining trucks provided by the present invention, reference can be made to the description of the laser radar online calibration method for unmanned mining trucks in the previous text, which will not be repeated here.

[0171] As another embodiment of the present invention, a laser radar online calibration system 10 for an unmanned mining truck is provided, wherein: Fig.10 As shown, it includes: a laser radar 200 and the laser radar online calibration device 100 for unmanned mining trucks as described above, and the laser radar 200 is communicatively connected with the laser radar online calibration device 100 for unmanned mining trucks;

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

[0173] The laser radar online calibration device 100 for an unmanned mining truck is used to perform laser radar calibration and laser radar position monitoring and deviation correction based on laser radar point cloud data.

[0174] The laser radar online calibration system for unmanned mining trucks provided by the present invention adopts the laser radar online calibration device for unmanned mining trucks described above, and realizes the automatic calibration of the laser radar of the unmanned mining truck through the laser radar point cloud data of the unmanned mining truck. Moreover, due to the joint optimization of the local map based on the main laser radar coordinate system and the point cloud data of the auxiliary laser radar, the automatic calibration of the relative spatial positions of multiple laser radars can be realized. In addition, through the online monitoring of the laser radar position, if a laser radar has a position offset, the position of the laser radar can be corrected online to meet the temporary use of the unmanned mining truck. If the position offset is too large, an alarm message is sent to make the unmanned mining truck stop safely to ensure driving safety. Therefore, the laser radar online calibration system for unmanned mining trucks provided by the present invention can realize the automatic and accurate calibration of the laser radar of the unmanned mining truck.

[0175] It is to be understood that the above embodiments are merely exemplary embodiments used to illustrate the principles of the present invention, but 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 to be within the scope of protection of the present invention.

Claims

1. A laser radar online calibration method for unmanned mining trucks, characterized in that: include: Acquire laser radar point cloud data of an unmanned mining truck, and perform data preprocessing on the laser radar point cloud data to obtain laser radar point cloud preprocessing data, wherein the laser radar set on the unmanned mining truck includes at least one main laser radar and at least two auxiliary laser radars; Constructing a local map based on the main laser radar coordinate system, and optimizing the point cloud preprocessing data of each auxiliary laser radar with the local map to obtain laser radar calibration parameters; Determine the real-time position parameters of the laser radar according to the laser radar point cloud preprocessing data, and monitor the real-time position parameters of the laser radar; When it is determined according to the real-time position parameters of the laser radar that the current laser radar has a controllable position deviation, the laser radar calibration process is re-executed to update the laser radar calibration parameters; When it is determined based on the real-time position parameters of the laser radar that an uncontrollable position deviation occurs, an alarm message is issued.

2. The online laser radar calibration method for unmanned mining trucks according to claim 1 is characterized in that: Construct a local map based on the main laser radar coordinate system, and associate and optimize the point cloud preprocessing data of each auxiliary laser radar with the local map to obtain the laser radar calibration parameters, including: Construct a local map based on the main laser radar coordinate system according to the point cloud preprocessing data of the main laser radar coordinate system; Matching the point cloud preprocessed data of each auxiliary laser radar with the point cloud in the local map to obtain the point cloud registration residual of each auxiliary laser radar; Constructing a target cost function, and optimizing the point cloud registration residual of each auxiliary laser radar according to the target cost function, so as to obtain the relative position relationship between each auxiliary laser radar and the main laser radar; The laser radar calibration parameters are obtained according to the relative position relationship between each auxiliary laser radar and the main laser radar.

3. The online laser radar calibration method for unmanned mining trucks according to claim 2 is characterized in that: Construct a local map based on the main LiDAR coordinate system based on the point cloud preprocessing data of the main LiDAR coordinate system, including: Determine a first positional relationship between the main laser radar and the inertial navigation unit and a second positional relationship of the inertial navigation unit relative to the world coordinate system; A local map based on the continuous laser point cloud preprocessing data of the main laser radar is constructed according to the first positional relationship and the second positional relationship.

4. The online laser radar calibration method for unmanned mining trucks according to claim 2 is characterized in that: The point cloud preprocessed data of each auxiliary laser radar is matched with the point cloud in the local map to obtain the point cloud registration residual of each auxiliary laser radar, including: Searching for local map feature points corresponding to the point cloud feature points of the current auxiliary laser radar in the local map according to the point cloud preprocessing data of each auxiliary laser radar; The point cloud feature points of each auxiliary laser radar are registered with the local map feature points according to the point-to-line registration method and the point-to-plane registration method, and the point cloud registration residual of each auxiliary laser radar is obtained.

5. The online laser radar calibration method for unmanned mining trucks according to claim 2 is characterized in that: Constructing a target cost function, and optimizing the point cloud registration residual of each auxiliary laser radar according to the target cost function, to obtain the relative position relationship between each auxiliary laser radar and the main laser radar, including: Construct the target cost function; Under the target cost function, the point cloud registration residual of each auxiliary lidar is jointly optimized according to the gradient descent method and the Mahalanobis norm; According to the joint optimization result, the positional relationship transformation matrix from each auxiliary laser radar coordinate system to the main laser radar coordinate system is obtained; The relative position relationship between each auxiliary laser radar and the main laser radar is determined according to the position relationship transformation matrix from each auxiliary laser radar coordinate system to the main laser radar coordinate system.

6. The online laser radar calibration method for unmanned mining trucks according to any one of claims 1 to 5, characterized in that: Determining the real-time position parameters of the laser radar according to the laser radar point cloud preprocessing data, and monitoring the real-time position parameters of the laser radar, including: Extract the real-time position parameters of the LiDAR based on the pre-processed data of the LiDAR point cloud; Determine the position offset of each laser radar according to the real-time position parameters of the laser radar; If the position offset of the laser radar is greater than a first preset threshold, comparing the position offset of each laser radar with a second preset threshold, wherein the first preset threshold is less than the second preset threshold; If the position offset of the laser radar is not greater than the second preset threshold, it is determined that the laser radar has a controllable position offset; If the position offset of the laser radar is greater than the second preset threshold, it is determined that the laser radar has an uncontrollable position offset.

7. The online laser radar calibration method for unmanned mining trucks according to claim 6 is characterized in that: Extract the real-time position parameters of the LiDAR based on the pre-processed LiDAR point cloud data, including: Extract ground points from the lidar point cloud preprocessing data based on the minimum point residual; Determine a ground normal vector based on the ground points in the laser radar point cloud preprocessing data; Construct the horizontal plane in the laser radar coordinate system and calculate the vector parallel to the horizontal plane; The real-time position parameters of the laser radar are determined according to the ground normal vector and the horizontal plane parallel vector, and the real-time position parameters of the laser radar include at least: a pitch angle, a roll angle and a z-axis intercept.

8. The online laser radar calibration method for unmanned mining trucks according to claim 6 is characterized in that: If the position offset of the laser radar is not greater than the first preset threshold, the current real-time position parameters of the laser radar are recorded and saved.

9. A laser radar online calibration device for an unmanned mining truck, used to implement the laser radar online calibration method for an unmanned mining truck as claimed in any one of claims 1 to 8, characterized in that: include: A radar data preprocessing module is used to obtain the laser radar point cloud data of the unmanned mining truck and perform data preprocessing on the laser radar point cloud data to obtain the laser radar point cloud preprocessing data, wherein the laser radar set on the unmanned mining truck includes at least one main laser radar and at least two auxiliary laser radars; A calibration module is used to construct a local map based on the main laser radar coordinate system, and associate and optimize the point cloud preprocessing data of each auxiliary laser radar with the local map to obtain laser radar calibration parameters; A position monitoring module, used to determine the real-time position parameters of the laser radar according to the laser radar point cloud preprocessing data, and monitor the real-time position parameters of the laser radar; A correction module is used to re-execute the laser radar calibration process to update the laser radar calibration parameters when it is determined that the laser radar has a position deviation according to the real-time position parameters of the laser radar and the position deviation is within a preset threshold range; The alarm module is used to issue an alarm message when it is determined based on the real-time position parameters of the laser radar that the laser radar has a position offset and the position offset amount is not within a preset threshold range.

10. A laser radar online calibration system for unmanned mining trucks, characterized in that: include: A laser radar and the laser radar online calibration device for an unmanned mining truck as claimed in claim 9, wherein the laser radar is communicatively connected to the laser radar online calibration device for an unmanned mining truck; The laser radar is arranged on the unmanned mining truck and is used to collect the laser radar point cloud data in real time; The laser radar online calibration device for unmanned mining trucks is used to perform laser radar calibration and laser radar position monitoring and deviation correction based on laser radar point cloud data.

Images