A multi-laser radar automatic calibration method and system for heavy load AGV

Abstract

The application discloses a kind of multi-laser radar automatic calibration method and system for heavy load AGV, for the non-ideal motion error such as wheel train different step, ground skidding, rotation center offset of heavy load AGV, abandon traditional ideal motion model.The method first builds reference point cloud map and initializes coordinate system, collects and pre-processes multiple radar point clouds;Again, valid frames are screened in combination with point cloud matching score, the extrinsic parameter between radars is solved by weighted fusion, and the validity of the extrinsic parameter is verified using independent check frames;Finally, combined with AGV straight line, rotation motion, the heading angle deviation and translation offset are respectively solved, and the pose of each radar relative to the kinematic center of the vehicle is obtained.The scheme has strong motion interference resistance, excellent calibration accuracy and repeatability, can adapt to full load working condition, realizes full-automatic calibration, and is suitable for industrial heavy load AGV batch deployment.

Description

Technical Field

[0001] This invention relates to the field of Automated Guided Vehicle (AGV) technology, specifically to a multi-laser radar automatic calibration method and system for heavy-duty AGVs. Background Technology

[0002] In the navigation and environmental perception system of heavy-duty automated guided vehicles (AGVs), multiple lidar sensors are typically deployed on the vehicle body to acquire multi-view environmental information in order to achieve omnidirectional environmental perception and high-reliability operation. The data collected by multiple lidar sensors need to be fused and processed in a unified coordinate system. Therefore, accurately obtaining the spatial extrinsic parameters between each lidar sensor and each lidar sensor relative to the vehicle's kinematic center becomes a key fundamental issue in system deployment.

[0003] Existing multi-LiDAR calibration schemes typically establish calibration constraints based on ideal motion models, assuming, for example, that the vehicle can maintain stable linear motion, standard circular motion, or a high-precision odometer trajectory. However, for heavy-duty AGVs, due to the large vehicle load, complex drive structure, and uncertainties in the industrial ground environment, non-ideal motion phenomena commonly occur during actual operation. These include issues such as asynchronous drive wheels, changes in ground adhesion, low-speed vibration, and steering drift. Therefore, non-ideal motion errors will occur, including but not limited to: inertial lag under heavy load conditions; trajectory drift caused by asynchronous multiple drive wheels; local slippage caused by changes in ground adhesion conditions; rotation center offset during stationary rotation; attitude disturbances caused by the long vehicle body structure; and inconsistencies between traditional ideal motion models and actual motion trajectories.

[0004] The aforementioned non-ideal motion errors lead to: unstable pose estimation results between different time frames; single-frame point cloud matching results being easily affected by local noise; large random fluctuations in radar extrinsic parameter estimation; kinematic models established based on the ideal trajectory assumption being difficult to accurately hold; and poor consistency in repeated calibration results. Especially during the stationary rotation of a heavy-duty AGV, due to the accumulation of wheel-to-wheel errors and the influence of inertia, the actual rotation center of the vehicle is prone to deviate from the theoretical kinematic center, further making it difficult for traditional calibration methods based on ideal rotation models to obtain stable results.

[0005] Therefore, how to achieve stable automatic calibration of multiple lidars under non-ideal motion conditions has become a technical challenge in the current deployment of heavy-duty AGV sensing systems. Summary of the Invention

[0006] To address the technical problem of non-ideal motion errors in heavy-duty AGVs during actual industrial operation, the present invention aims to propose an automatic calibration method and system for multiple lidar sensors used in heavy-duty AGVs.

[0007] To achieve the objectives of this invention, the technical solution provided by this invention is as follows: First aspect This invention provides an automatic calibration method for multiple lidar sensors used in heavy-duty AGVs, wherein the heavy-duty AGVs have the ability to rotate in place, and includes the following steps: Step S1: Preprocess the calibration environment and construct a reference point cloud map for subsequent point cloud matching. Simultaneously, coordinate system initialization is completed; Step S2: Based on the constructed reference point cloud map, the calibration environment is perceived and collected in real time by each lidar on the heavy-duty AGV, and the collected data is processed in a unified format to obtain a multi-radar point cloud data sequence. Step S3: Based on the multi-radar point cloud data sequence, estimate the pose of each lidar in each time frame to obtain the pose transformation matrix of each lidar in the map coordinate system. Match score ; Step S4: Based on the pose transformation matrix of each LiDAR in the map coordinate system Match score The external parameter relationships between each lidar are solved to obtain the results from the lidar... Stable extrinsic parameters relative to the main lidar A ; Step S5: Determine the source from the lidar Stable extrinsic parameters relative to the main lidar A Is it valid? If valid, proceed to step S6; if invalid, obtain the multi-radar point cloud data of the next time frame and return to step S3. Step S6: Calculate the heading angle deviation between the main lidar A and the heavy-duty AGV body coordinate system; Step S7: Calculate the translational offset of the main lidar A relative to the kinematic center of the heavy-duty AGV; Step S8: Calculate the pose transformation relationship of each lidar relative to the kinematic center of the heavy-duty AGV based on the heading angle deviation and translation offset.

[0008] Further, step S1 includes: Step S1.1: Lay a linear magnetic strip in the calibration environment to constrain the linear motion of the heavy-duty AGV, and use the calibration lidar to scan the calibration environment in advance to obtain the three-dimensional point cloud data of the calibration environment; Step S1.2: Generate a reference point cloud map based on the 3D point cloud data of the calibration environment. ; Step S1.3: Complete the reference point cloud map After construction, the heavy-duty AGV to be calibrated is placed at the preset starting position in the calibration site and kept stationary; the initial relative relationship between each lidar coordinate system and the map coordinate system is given through the system configuration file.

[0009] Further, step S2 includes: Step S2.1: During the calibration process, each lidar synchronously collects environmental data at a fixed sampling frequency and records it in discrete time frames; The time frame corresponding to the sampling moment is the th sampling moment. frame, , , Indicates the total number of frames captured; In the In the frame, lidar The collected point cloud data are as follows: ; in, Represents the first point in the point cloud data. One point; Indicates the first Frame-based lidar The number of points collected; point In its corresponding radar coordinate system, it is expressed in homogeneous coordinate form as follows: ; in, These represent the points on the lidar. Spatial coordinates along the lower coordinate axes of the coordinate system; Step S2.2: Perform preprocessing operations on the point cloud data, including outlier removal, downsampling, and time synchronization processing, to obtain a multi-radar point cloud data sequence.

[0010] Further, step S3 includes: Step S3.1: In the first... In the frame, for lidar ,Will As the source point cloud, and will be used as a reference point cloud map As the target point cloud, a point cloud matching algorithm is used to perform registration calculations between the two, estimate the pose of the lidar in the map coordinate system, spatially align the source point cloud and the target point cloud, and solve the rigid body transformation relationship that minimizes the matching error between the two. Step S3.2: After matching calculation, in the... In the frame, for lidar The pose transformation matrix relative to the map coordinate system is obtained. Simultaneously, the point cloud matching algorithm also outputs a matching score to characterize the matching quality. , ; Step S3.3: Repeat the above process for all time frames. and all lidar The corresponding pose estimation results can be obtained from both. and matching score .

[0011] Further, step S4 includes: Step S4.1: Select lidar A as the master lidar A, and the other lidars as slave lidars. Calculate the first Frame extrinsic transformation matrix ; For spatial points The representation in the main lidar A coordinate system is as follows The representation in the map coordinate system is as follows Then we have: Formula 1: ;in, For the first The pose of the main lidar A in the map coordinate system within the frame; At the same time, this spatial point From lidar Representation in coordinate system as Then we have: Formula 2: ;in, For the first From LiDAR in frames Pose in map coordinate system; From the master lidar A coordinate system to the slave lidar The transformation relationship of the coordinate system satisfies: Formula 3: ; Based on Formula 1, Formula 2, and Formula 3, we obtain the following Formula 4: Formula 4: ; Based on Formula 1 and Formula 4, we obtain the following Formula 5: Formula 5: ; Based on Formula 5, we obtain Formula 6 as follows: Formula Six: ; Based on Formula 6, we obtain Formula 7 as follows: Formula 7: ;in:( · ) -1This represents the matrix inversion operation; Through the above calculations, the corresponding single-frame extrinsic parameter estimation results can be obtained for each time frame; Step S4.2: Based on the matching score Perform a matching quality threshold judgment to obtain Frame validity time frame; Step S4.3: According to Time frames in the effective time frame Match score and Frames of valid time are weighted and fused to obtain the result from the lidar. Stable extrinsic parameters relative to the main lidar A Specifically, it includes: (1) Targeting The first frame in the effective time frame Frame matching score After normalization, we obtain the first... The weight coefficients corresponding to the frames, where, : ; Where: summation index variable, ; (2) Decompose the extrinsic parameters of a single frame to obtain the first frame. Frame translation vector The formula is as follows: ; in: For the first The frame rotation matrix; (3) Perform a weighted average on the translation vectors to obtain the fused translation vector. : ; (4) For the first Frame rotation matrix Perform weighted fusion to obtain the fused rotation matrix. : First, convert the rotation matrix into a unit quaternion. : ; Secondly, for unit quaternions We perform a weighted average and normalize to obtain the merged unit quaternion. : ; Finally, the merged unit quaternion Convert back to rotation matrix to obtain the merged rotation matrix. : ; (5) Combine the fused rotation matrix and the fused translation vector to obtain the result from the lidar. Stable extrinsic parameters relative to the main lidar A : .

[0012] Further, step S5 includes: Step S5.1: Select independent data frames that did not participate in the weighted fusion calculation as verification frames; Let the selected check frame index be: , The total number of check frames; For each verification frame, obtain the point cloud data corresponding to each LiDAR. ,in: For the first Point cloud data of main lidar A in the frame. For the first From LiDAR in frames Point cloud data; Step S5.2: Based on the data from the lidar Stable extrinsic parameters relative to the main lidar A The point cloud data from the lidar will be uniformly converted to the coordinate system of the main lidar (A coordinate system), as shown in the following formula: ; in: The point cloud data after transformation to the main lidar A coordinate system; For LiDAR Coordinate transformation to main lidar A; Step S5.3: In the coordinate system of the main lidar A, fuse the point cloud data of each lidar to obtain the first... Point cloud data set after frame fusion : ; in: This indicates that the point cloud data of the main lidar A will be merged with the point cloud data of all the transformed lidars. This represents the union of all points in the lidar point cloud data; Step S5.4: [The text appears to be incomplete and contains several grammatical errors. A more accurate translation would require Transform to map coordinate system to obtain the first... Frame-by-frame point cloud in map coordinate system : ; in: For the first The pose of the main lidar A in the frame; Step S5.5: [The text appears to be incomplete and contains several grammatical errors. A more accurate translation would require the full context.] and Perform point cloud matching to obtain closed-loop matching scores. ; Step S5.6: Statistically analyze the closed-loop matching scores of all verification frames and calculate the average closed-loop matching score. : ; Step S5.7: When the condition is met If the verification is successful, the stable extrinsic parameters are considered valid; otherwise, the verification fails, the multi-radar point cloud data of the next time frame is obtained, and the process returns to step S3 to re-perform point cloud matching and extrinsic parameter estimation. This is the closed-loop verification threshold.

[0013] Further, step S6 includes: Step S6.1: During the straight-line travel of the heavy-duty AGV along the linear magnetic strip, continuously acquire the pose of the main LiDAR A in the map coordinate system at a fixed frequency: ,in, For the second time frame index; Step S6.2: Extract the translation component: ,in, for Axial translation components; for Axial translation components; for Axial translation components; Step S6.3: Process the acquired location sequence Performing line fitting in the plane only... and The fitted line is obtained as follows: ; in, The slope of the line. The intercept is... This represents the coordinate values ​​of the independent variable used in the fitting process. The coordinates of the dependent variable used in the fitting process represent the direction angle of the line. Let be the angle between the actual direction of AGV movement and the X-axis of the map coordinate system, as shown in the formula below: ; Step S6.4: Based on the pose of the main lidar A Extract the heading angle from its rotation component, and take the average attitude over multiple frames. The average rotation matrix is ​​then: ; Calculate the orientation angle of the main lidar A for: ; In the formula: The average rotation matrix is ​​the first Line 1 Column elements, It is the arctangent function in the four quadrants; Based on the difference between the actual movement direction of the heavy-duty AGV and the orientation of the main lidar A, the heading angle deviation of the main lidar A coordinate system relative to the heavy-duty AGV body coordinate system is calculated. The formula is as follows: .

[0014] Further, step S7 includes: Step S7.1: During the in-situ uniform rotation motion of the heavy-duty AGV, continuously record the pose of the main LiDAR A in the map coordinate system at a fixed frequency: ,in, For the third time frame index; Step S7.2: From pose Extracting the translation component: ; in, Indicates the time frame index. They represent the first The three-dimensional coordinates of the main lidar A in the map coordinate system at frame time; Step S7.3: Since the movement of the heavy-duty AGV is restricted to a plane, its two-dimensional projection point is taken: ; By the point set By performing circle fitting, the following circle model is established: ; in, Represents the coordinates of the center of the circle. Indicates the radius of the circle; Solving for the optimal center parameters using the least squares method: ; in, Indicates the number of valid sampled frames used in the fitting; and It is the independent variable in least squares optimization. This represents the assumed x-coordinate of the circle's center during the least squares optimization process. This represents the assumed ordinate of the circle's center during the least squares optimization process; The obtained coordinates of the center of the circle are the position of the kinematic center of the heavy-duty AGV in the map coordinate system, denoted as: ; Step S7.4: Take the average pose of the main lidar A during the rotation process, and record the rotation matrix from the main lidar A coordinate system to the map coordinate system for the rotation part. The translation part is the position vector of the main lidar A in the map coordinate system. Then the coordinates of the kinematic center in the main lidar A coordinate system are expressed as: ; in: ; ; ; in, It represents the set of all orthogonal rotation matrices in three-dimensional space; Represents the three-dimensional real number space; Step S7.5: [The text appears to be incomplete and contains several grammatical errors. A more accurate translation would require the full context.] Represented as a planar offset, as follows: ; in, and These represent the forward and lateral offsets in the main lidar A coordinate system, respectively. Setting the Z component to zero indicates that the kinematic center is offset from the lidar in the same horizontal plane.

[0015] Further, step S8 includes: Step S8.1: Calculate the pose transformation relationship between the main lidar A coordinate system and the kinematic center of the heavy-duty AGV, using the following formula: ; in: The homogeneous transformation matrix from the main lidar A coordinate system to the kinematic center of the heavy-duty AGV; Rotation angle around the z-axis The rotation matrix; the matrix below Represents the zero vector, the one above. Represents homogeneous coordinate components; Step S8.2: Calculate from lidar The pose transformation relationship relative to the kinematic center of the heavy-duty AGV: .

[0016] Second aspect This invention provides an automatic calibration system for multiple lidar sensors used in heavy-duty AGVs, for executing the aforementioned automatic calibration method for multiple lidar sensors used in heavy-duty AGVs.

[0017] Compared with the prior art, the beneficial effects of the present invention are as follows: This invention introduces matching scores as a confidence index to perform weighted fusion of extrinsic parameters from multiple frames, giving higher weight to high-quality matching results during the fusion process. This achieves the following: effectively suppressing the interference of low-quality frames on calibration results and improving the stability of extrinsic parameter estimation; and still obtaining robust calibration results in the presence of dynamic obstacles or local environmental degradation. In addition, this invention combines the actual motion characteristics of the vehicle, constrains the vehicle to travel along an approximately ideal straight line through magnetic navigation, calculates the vehicle's actual direction of travel through trajectory fitting, and uses the circular trajectory generated by in-place rotation to determine the position of the vehicle's kinematic center through circle fitting, thus effectively realizing the calibration of vehicle body parameters.

[0018] In addition, this invention constructs a closed-loop verification mechanism to reapply the calibration results to the original point cloud data and match them with the benchmark map again. The matching score is used as an evaluation index to achieve a quantitative assessment of the calibration results, rather than relying on subjective human judgment. Attached Figure Description

[0019] Figure 1 This is a schematic diagram of the automatic calibration method for multiple lidar sensors used in heavy-duty AGVs provided in an embodiment of the present invention. Detailed Implementation

[0020] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of the present invention.

[0021] It should be noted that the acquisition of data and collection of information in this application are legal, compliant, or obtained with the consent of the subject of the data collection.

[0022] It should be noted that the proposed solution does not rely on the heavy-duty AGV to form an ideal straight line trajectory or an ideal circular trajectory. Instead, it achieves stable automatic calibration between multiple lidars and the vehicle's kinematic center by using a magnetic control scheme, combining constraint analysis of multi-frame pose stability, and decoupling motion characteristics under different motion modes.

[0023] like Figure 1As shown, this embodiment of the invention provides an automatic calibration method for multiple lidar sensors used in heavy-duty AGVs, wherein the heavy-duty AGVs have the ability to rotate in place, and includes the following steps: Step S1: Preprocess the calibration environment and construct a reference point cloud map for subsequent point cloud matching, while completing the coordinate system initialization; Specifically, it includes the following: Step S1.1: Lay a linear magnetic strip in the calibration site to constrain the linear motion of the heavy-duty AGV to be calibrated, and use the calibration lidar to scan the calibration environment in advance to obtain the three-dimensional point cloud data of the calibration environment. Step S1.2: Generate a reference point cloud map based on the 3D point cloud data of the calibration environment. , represented as: ; in, Indicates the number of points in the reference point cloud map; Indicates the first The spatial coordinates of a point in the map coordinate system Indicates the first The coordinate system to which each point belongs is the map coordinate system, and each spatial coordinate... Represented in homogeneous coordinate form as follows: ; in, They represent the first The coordinates of a point along the X, Y, and Z axes in the map coordinate system; Represents the transpose of a vector; constant term Used to construct homogeneous coordinate forms; Step S1.3: After completing the construction of the reference point cloud map, place the heavy-duty AGV to be calibrated at the preset starting position in the calibration site and keep it stationary; the initial relative relationship between the coordinate systems of each lidar of the heavy-duty AGV and the map coordinate system is given through the system configuration file; Define LiDAR The rigid body transformation matrix from the coordinate system to the map coordinate system is: ; in: This indicates the lidar number, used to distinguish different lidars; The rigid body transformation matrix is ​​expressed in homogeneous transformation form as follows: ; in: Represents the rotation matrix; It represents the set of all orthogonal rotation matrices in three-dimensional space; Represents the translation vector; Representing the three-dimensional zero vector ; Any in lidar Points in coordinate system All coordinates are converted to the map coordinate system through the following relationship: ; in, This represents the homogeneous coordinates of the point in the map coordinate system; This represents the multiplication operation between a matrix and a vector.

[0024] Step S2: Based on the constructed reference point cloud map By using the lidars on the heavy-duty AGV, the calibration environment is perceived and collected in real time, and the collected data is processed in a unified format to obtain a multi-radar point cloud data sequence. Specifically, it includes the following: Step 2.1: During the calibration process, each lidar synchronously collects environmental data at a fixed sampling frequency and records it in discrete time frames; The time frame corresponding to the sampling moment is the th sampling moment. frame, , , Indicates the total number of frames captured; In the In the frame, lidar The collected point cloud data are as follows: ; in, Represents the first point in the point cloud data. One point; Indicates the first Frame-based lidar The number of points collected; point In its corresponding radar coordinate system, it is expressed in homogeneous coordinate form as follows: ; in, These represent the points on the lidar. Spatial coordinates along the lower coordinate axes of the coordinate system; It should be noted that on heavy-duty AGVs, when the lidar is a single-line lidar, its raw output data is typically two-dimensional laser scan data (LaserScan), represented in polar coordinates, including distance values ​​and corresponding angle information. To standardize subsequent processing, the LaserScan data needs to be converted into three-dimensional point cloud data. Specifically, using the polar coordinate to Cartesian coordinate conversion relationship, the distance and angle information corresponding to each laser beam are converted into spatial point coordinates, thereby constructing point cloud data. When the lidar is a multi-line lidar, its output data is already in three-dimensional point cloud format, and can be directly used as the aforementioned point cloud data. No additional conversion is required for use.

[0025] Step 2.2: To improve the stability and computational efficiency of subsequent point cloud matching, preprocessing operations can be performed on the point cloud data to obtain a multi-radar point cloud data sequence. The preprocessing operations include the following steps: (1) Filter the point cloud data by distance range and remove abnormal points that exceed the preset minimum and maximum distance range; (2) Voxel filtering is used to downsample the point cloud to reduce the point cloud data density and reduce the amount of computation; (3) Time synchronization processing of point cloud data to reduce the error caused by the difference in sampling time between multiple radars.

[0026] Step S3: Based on the multi-radar point cloud data sequence, estimate the pose of each lidar in each time frame to obtain the pose transformation matrix of each lidar in the map coordinate system. Match score ; In the In the frame, for lidar ,Will As the source point cloud, and will be used as a reference point cloud map As the target point cloud, a point cloud matching algorithm is used to perform registration calculations between the two to estimate the pose of the lidar in the map coordinate system.

[0027] The point cloud matching algorithm uses the normal distribution transformation algorithm to spatially align the source point cloud and the target point cloud and solve the rigid body transformation relationship that minimizes the matching error between the two. After matching calculation, for the th Frame-based lidar The pose transformation matrix relative to the map coordinate system is obtained. ; Meanwhile, the point cloud matching algorithm also outputs a matching score to characterize the matching quality. ; in: ; It should be noted that the closer the score is to 1, the higher the degree of matching between the current frame point cloud and the reference map, and the more reliable the pose estimation result.

[0028] In actual calculations, to improve matching stability, the odometer information of the AGV or the initial configuration of the system can be combined to provide an initial pose estimate as the initial input of the point cloud matching algorithm, thereby accelerating the convergence speed and reducing the risk of local optima.

[0029] Through the above process, for all time frames and all lidar The corresponding pose estimation results can be obtained from both. and matching score This provides basic data support for subsequent calibration of multiple radar extrinsic parameters.

[0030] Step S4: Based on the pose transformation matrix of each LiDAR in the map coordinate system Match score The extrinsic parameter relationships between each lidar are solved to obtain the stable extrinsic parameter parameters of each slave lidar relative to the master lidar A. ; Specifically, lidar A is designated as the master lidar, and the other lidars are designated as slave lidars. Using the coordinate system of the main lidar A as a unified reference coordinate system, the coordinates of the secondary lidar are calculated. The extrinsic parameter transformation relationship relative to the main lidar A.

[0031] It should be noted that, from the perspective of lidar This refers to all other lidars on the heavy-duty AGV besides the main lidar A. These can be lidar B, lidar C, or others.

[0032] Specifically, the steps include the following: Step S4.1: Calculate the first... Frame extrinsic transformation matrix ; For spatial points The representation in the main lidar A coordinate system is as follows The representation in the map coordinate system is as follows Then we have: Formula 1: ;in, For the first The pose of the main lidar A in the map coordinate system within the frame; At the same time, this spatial point From lidar Representation in coordinate system as Then we have: Formula 2: ;in, For the first From LiDAR in frames Pose in map coordinate system; Due to the change from the master lidar A coordinate system to the slave lidar The transformation relationship of the coordinate system satisfies: Formula 3: ; Based on Formula 1, Formula 2, and Formula 3, we obtain the following Formula 4: Formula 4: ; Based on Formula 1 and Formula 4, we obtain the following Formula 5: Formula 5: ; Based on Formula 5, we obtain Formula 6 as follows: Formula Six: ; Based on Formula 6, we obtain Formula 7 as follows: Formula 7: ; in:( · ) -1 This represents the matrix inversion operation.

[0033] Through the above calculations, the corresponding single-frame extrinsic parameter estimation results can be obtained for each frame.

[0034] Step S4.2: Based on the matching score Perform a matching quality threshold judgment to obtain Frame validity time frame; Specifically: When the following conditions are met: At that time, it was believed that the first The extrinsic transformation matrix of the frame is reliable, and the corresponding extrinsic transformation matrix is ​​retained; otherwise, it is considered that the first frame is not reliable. The extrinsic transformation matrix of a frame is unreliable and is therefore discarded, not participating in subsequent fusion calculations; among which, This is a preset matching quality threshold.

[0035] After screening, the following were obtained The extrinsic transformation matrix of the effective time frame is used in subsequent extrinsic fusion calculations.

[0036] Step S4.3: According to Time frames in the effective time frame Match score and The effective time frames are weighted and fused to obtain the stable extrinsic parameters of each slave lidar relative to the master lidar A. ; Specific implementation methods include: (1) Targeting Any time frame in the frame validity time frame Match score After normalization, we obtain the first... The weight coefficients corresponding to the frames, where, : ; Where: summation index variable, ; (2) Decompose the extrinsic parameters of a single frame to obtain the first frame. Frame translation vector The formula is as follows: ; in: For the first The frame rotation matrix; (3) Perform a weighted average on the translation vectors to obtain the fused translation vector. : ; (4) For the first Frame rotation matrix Perform weighted fusion to obtain the fused rotation matrix. : First, convert the rotation matrix into a unit quaternion. : ; Secondly, for unit quaternions We perform a weighted average and normalize to obtain the merged unit quaternion. : ; Finally, the merged unit quaternion Convert back to rotation matrix to obtain the merged rotation matrix. : ; (5) Combine the fused rotation matrix and the fused translation vector to obtain the stable extrinsic parameters of each slave lidar relative to the master lidar A. : .

[0037] Step S5: Determine whether the stable extrinsic parameters of each slave lidar relative to the master lidar A are valid; if valid, proceed to step S6; if invalid, acquire the multi-radar point cloud data of the next time frame and return to step S3. Step S5.1: Select independent data frames that did not participate in the weighted fusion calculation as verification frames; It should be noted that the independent data frames here can be newly acquired or previously acquired data frames that did not participate in the weighted fusion calculation in step S4; Let the selected check frame index be: , The total number of check frames; For each frame Obtain point cloud data corresponding to each lidar. ,in: For the first Point cloud data of main lidar A in the frame. For the first From LiDAR in frames Point cloud data.

[0038] Step S5.2: Based on the stable extrinsic parameters of each slave lidar relative to the master lidar A The point cloud data from the lidar will be uniformly converted to the coordinate system of the main lidar (A coordinate system), as shown in the following formula: ; in: The point cloud data after transformation to the main lidar A coordinate system; For LiDAR Coordinate transformation to main lidar A; Step S5.3: In the coordinate system of the main lidar A, fuse the point cloud data of each lidar to obtain the... Point cloud data set after frame fusion : ; in: This indicates that the point cloud data of the main lidar A will be merged with the point cloud data of all the transformed lidars. This represents the union of all points in the lidar point cloud data; Step S5.4: [The text appears to be incomplete and contains several grammatical errors. A more accurate translation would require Transform to map coordinate system to obtain the first... Frame-by-frame point cloud in map coordinate system : ; in: The pose of the main lidar is at position A. Step S5.5: [The text appears to be incomplete and contains several grammatical errors. A more accurate translation would require and Perform point cloud matching to obtain closed-loop matching scores. ; in: The larger the value, the more likely it is that the point cloud and the reference ground are fused. Figure 1 The higher the consistency; Step S5.6: Statistically analyze the closed-loop matching scores of all verification frames and calculate the average closed-loop matching score. : ; Step S5.7: When the condition is met If the verification is successful, the stable extrinsic parameters are considered valid; otherwise, the verification fails, the multi-radar point cloud data of the next time frame is obtained, and the process returns to step S3 to re-perform point cloud matching and extrinsic parameter estimation. This is the closed-loop verification threshold.

[0039] If the number of consecutive invalid determinations exceeds a set threshold (e.g., 30 times), the calibration process will be terminated and an error will be reported, and manual confirmation will be required to check whether there are any abnormalities in the status of the vehicle, sensors, etc. Step S6: Calculate the heading angle deviation between the main lidar A coordinate system and the heavy-duty AGV body coordinate system; the specific implementation method is as follows: Step S6.1: During the straight-line travel of the heavy-duty AGV along the linear magnetic strip, continuously acquire the pose of the main LiDAR A in the map coordinate system at a fixed frequency: ,in, For the second time frame index; Step S6.2: Extract the translation component: ,in, for Axial translation components; for Axial translation components; for Axial translation components; Step S6.3: Process the acquired location sequence Performing line fitting in the plane only... and The fitted line is obtained as follows: ; in, The slope of the line. The intercept is... This represents the coordinate values ​​of the independent variable used in the fitting process. This represents the coordinate values ​​of the dependent variable used in the fitting process; Then the direction angle of the line Let be the angle between the actual direction of AGV movement and the X-axis of the map coordinate system, as shown in the formula below: .

[0040] Step S6.4: Based on the pose of the main lidar A Extract the heading angle from its rotation component, and take the average attitude over multiple frames. The average rotation matrix is ​​then: ; Calculate the orientation angle of the main lidar A for: ; In the formula: The average rotation matrix is ​​the first Line number Column elements, Using the four-quadrant arctangent function, the heading angle deviation of the main lidar A coordinate system relative to the heavy-duty AGV body coordinate system is calculated based on the difference between the actual movement direction of the heavy-duty AGV and the orientation of the main lidar A. The formula is as follows: .

[0041] Step S7: Calculate the translational offset of the main lidar A relative to the kinematic center of the heavy-duty AGV, specifically including the following: Step S7.1: During the in-situ uniform rotation motion of the heavy-duty AGV, continuously record the pose data of the main LiDAR A in the map coordinate system at a fixed frequency. ,in, For the third time frame index; Step S7.2: Extract the translation component from the pose: ; in, Indicates the time frame index. They represent the first The three-dimensional coordinates of the main lidar A in the map coordinate system at frame time; Step S7.3: Since the movement of the heavy-duty AGV is restricted to a plane, its two-dimensional projection point is taken: ; By the point set By performing circle fitting, the following circle model is established: ; in, Represents the coordinates of the center of the circle. Indicates the radius of the circle.

[0042] Solving for the optimal center parameters using the least squares method: ; in, Indicates the number of valid sampled frames used in the fitting; The obtained coordinates of the center of the circle are the position of the kinematic center of the heavy-duty AGV in the map coordinate system, denoted as: ; Step S7.4: Take the average pose of the main lidar A during the rotation process, and record the rotation matrix from the main lidar A coordinate system to the map coordinate system for the rotation part. The translation part is the position vector of the main lidar A in the map coordinate system. Then the coordinates of the kinematic center in the main lidar A coordinate system are expressed as:

[0043] ; in: ; ; .

[0044] Step S7.5: Calculate the result obtained in S7.4 Represented as a planar offset: ; in, and The first two components of the calculation result from S7.4 are taken respectively, representing the forward and lateral offsets in the main lidar A coordinate system. The Z component is set to zero to indicate that the kinematic center is offset from the lidar in the same horizontal plane. Through the above process, the kinematic center is automatically calibrated based on the vehicle's rotation trajectory, providing a translation parameter basis for constructing a unified coordinate transformation relationship in the subsequent step S8.

[0045] Step S8: Calculate the pose transformation relationship of each lidar relative to the kinematic center of the heavy-duty AGV. Step S8.1: Calculate the pose transformation relationship between the main lidar A coordinate system and the kinematic center of the heavy-duty AGV: ; in, Take the plane offset calculated in step S7.5: ; The homogeneous transformation between the main lidar A and the kinematic center of the heavy-duty AGV is: ; in, The homogeneous transformation matrix from the main lidar A coordinate system to the kinematic center of the heavy-duty AGV; Rotation angle around the z-axis The rotation matrix; the matrix below Represents the zero vector, the one above. This represents the homogeneous coordinate components.

[0046] Step S8.2 Calculate the pose transformation relationship from the lidar relative to the kinematic center of the heavy-duty AGV: .

[0047] To verify the effectiveness of the automatic calibration method of this invention under non-ideal motion conditions of heavy-duty AGVs, 10 heavy-duty AGVs were selected for comparative testing. All heavy-duty AGVs adopted a multi-drive differential structure, with a rated load of 2-5 tons, and each AGV was equipped with three 16-line lidar sensors.

[0048] The test environment was uniformly set to a real factory environment with dynamic obstacles and on-site operational interference to simulate real industrial operation scenarios.

[0049] The comparison methods include: (1) manual calibration method; (2) conventional automatic calibration method; and (3) the automatic calibration method of the present invention.

[0050] Among them: the manual calibration method uses manual measurement and adjustment to complete the calibration of radar extrinsic parameters; the conventional automatic calibration method uses an automatic calibration method based on single point cloud matching; the method of this invention uses an automatic calibration method based on multi-frame stability constraints, motion mode decoupling, and closed-loop consistency verification.

[0051] Evaluation criteria description: High-precision reference measurement equipment is used to obtain real external parameter results as benchmark values, and the calibration results obtained by each method are evaluated for error.

[0052] The calibration compliance conditions were set as follows: position error ≤ 2 cm; attitude angle error ≤ 1°. The calibration time, compliance rate, and consistency of repeated calibrations for each method were also statistically analyzed.

[0053] The overall statistical results are shown in Table 1: Table 1

[0054] Furthermore, to verify the adaptability of the present invention under heavy load and complex working conditions, repeated calibration tests were conducted under no-load, half-load, and full-load conditions. Specifically: no-load condition: the AGV was not loaded with goods; half-load condition: loaded with 50% of the rated load; full-load condition: loaded with the maximum rated load.

[0055] The load adaptability test results are shown in Table 2: Table 2

[0056] Test results show that as the load increases, the position and attitude errors of conventional automatic calibration methods fluctuate significantly. In some tests, the point cloud matching results are unstable and calibration fails due to low-speed vehicle vibration, asynchronous drive wheels, and local slippage. However, the method of this invention can maintain stable calibration results under different load conditions. Its position error fluctuation range is controlled within 0.3 cm and its angle error fluctuation range is controlled within 0.2°, and no calibration failures occur.

[0057] Furthermore, for the same heavy-duty AGV, 10 repeated calibration experiments were conducted continuously under the same environmental conditions, and the dispersion between the calibration results was statistically analyzed.

[0058] The results of the repeated calibration consistency test are shown in Table 3: Table 3

[0059] Experimental results show that the maximum deviation of the extrinsic parameter translation results of the conventional automatic calibration method reaches 1.4 cm and the maximum deviation of the attitude angle reaches 1.4°; while the maximum deviation of the extrinsic parameter translation results of the method of the present invention is controlled within 1.2 cm and the maximum deviation of the attitude angle is controlled within 1.1°, and the dispersion of multiple calibration results is reduced.

[0060] Through the above tests, using the technical solution of this invention, all equipment met the calibration accuracy requirements; the calibration time was reduced to less than 15 minutes; the calibration results of multiple devices were concentrated; high stability was maintained even under full load operation and dynamic industrial environment; and the consistency of repeated calibration results was good.

[0061] In addition, the present invention provides an automatic calibration system for multiple lidar sensors for heavy-duty AGVs, for executing the aforementioned automatic calibration method for multiple lidar sensors for heavy-duty AGVs.

[0062] Finally, it should be noted that the above embodiments are merely illustrative and explanatory of the present invention, and are not intended to limit the present invention to the scope of the described embodiments. Furthermore, those skilled in the art will understand that the present invention is not limited to the above embodiments, and many more variations and modifications can be made based on the teachings of the present invention, all of which fall within the scope of protection claimed by the present invention.

Claims

1. An automatic calibration method for multiple lidar sensors used in heavy-duty AGVs, wherein the heavy-duty AGVs have the ability to rotate in place, characterized in that, Includes the following steps: Step S1: Preprocess the calibration environment and construct a reference point cloud map for subsequent point cloud matching. Simultaneously, coordinate system initialization is completed; Step S2: Based on the constructed reference point cloud map, the calibration environment is perceived and collected in real time by each lidar on the heavy-duty AGV, and the collected data is processed to unify the format to obtain a multi-radar point cloud data sequence. Step S3: Based on the multi-radar point cloud data sequence, estimate the pose of each lidar in each time frame to obtain the pose transformation matrix of each lidar in the map coordinate system. Match score ; Step S4: Based on the pose transformation matrix of each LiDAR in the map coordinate system Match score The external parameter relationships between each lidar are solved to obtain the results from the lidar... Stable extrinsic parameters relative to the main lidar A ; Step S5: Determine the source from the lidar Stable extrinsic parameters relative to the main lidar A Is it valid? If valid, proceed to step S6; if invalid, obtain the multi-radar point cloud data of the next time frame and return to step S3. Step S6: Calculate the heading angle deviation between the main lidar A and the heavy-duty AGV body coordinate system; Step S7: Calculate the translational offset of the main lidar A relative to the kinematic center of the heavy-duty AGV; Step S8: Calculate the pose transformation relationship of each lidar relative to the kinematic center of the heavy-duty AGV based on the heading angle deviation and translation offset.

2. The automatic calibration method for multiple lidar sensors used in heavy-duty AGVs according to claim 1, characterized in that, Step S1 includes: Step S1.1: Lay a linear magnetic strip in the calibration environment to constrain the linear motion of the heavy-duty AGV, and use the calibration lidar to scan the calibration environment in advance to obtain the three-dimensional point cloud data of the calibration environment; Step S1.2: Generate a reference point cloud map based on the 3D point cloud data of the calibration environment. ; Step S1.3: Complete the reference point cloud map After construction, the heavy-duty AGV is placed at the preset starting position in the calibration site to keep it stationary; the initial relative relationship between each lidar coordinate system and the map coordinate system is given through the system configuration file.

3. The automatic calibration method for multiple lidar sensors used in heavy-duty AGVs according to claim 2, characterized in that, Step S2 includes: Step S2.1: During the calibration process, each lidar of the heavy-duty AGV synchronously collects environmental data at a fixed sampling frequency and records it in discrete time frames; The time frame corresponding to the sampling moment is the th sampling moment. frame, , , Indicates the total number of frames captured; In the In the frame, lidar The collected point cloud data are as follows: ； in, Represents the first point in the point cloud data. One point; Indicates the first Frame-based lidar The number of points collected; point In its corresponding radar coordinate system, it is expressed in homogeneous coordinate form as follows: ； in, These represent the points on the lidar. Spatial coordinates along the lower coordinate axes of the coordinate system; Step S2.2: Perform preprocessing operations on the point cloud data, including outlier removal, downsampling, and time synchronization processing, to obtain a multi-radar point cloud data sequence.

4. The automatic calibration method for multiple lidar sensors used in heavy-duty AGVs according to claim 3, characterized in that, Step S3 includes: Step S3.1: In the first... In the frame, for lidar ,Will As the source point cloud, and will reference the point cloud map As the target point cloud, a point cloud matching algorithm is used to perform registration calculations between the two, estimate the pose of the lidar in the map coordinate system, spatially align the source point cloud and the target point cloud, and solve the rigid body transformation relationship that minimizes the matching error between the two. Step S3.2: After matching calculation, in the... In the frame, for lidar The pose transformation matrix relative to the map coordinate system is obtained. Simultaneously, the point cloud matching algorithm also outputs a matching score to characterize the matching quality. , ; Step S3.3: Repeat the above process for all time frames. and all lidar The corresponding pose estimation results can be obtained from both. and matching score .

5. The automatic calibration method for multiple lidar sensors used in heavy-duty AGVs according to claim 4, characterized in that, Step S4 includes: Step S4.1: Select lidar A as the master lidar, and the other lidars as slave lidars. Calculate the first Frame extrinsic transformation matrix ; For spatial points The representation in the main lidar A coordinate system is as follows The representation in the map coordinate system is as follows Then we have: Formula 1: ;in, For the first The pose of the main lidar A in the map coordinate system within the frame; At the same time, this spatial point From lidar The representation in coordinate system is Then we have: Formula 2: ;in, For the first From LiDAR in frames Pose in map coordinate system; From the coordinate system of the master lidar (A) to that of the slave lidar The transformation relationship of the coordinate system satisfies: Formula 3: ; Based on Formula 1, Formula 2, and Formula 3, we obtain the following Formula 4: Formula 4: ; Based on Formula 1 and Formula 4, we obtain the following Formula 5: Formula 5: ; Based on Formula 5, we obtain Formula 6 as follows: Formula Six: ; Based on Formula 6, we obtain Formula 7 as follows: Formula 7: ;in:( · ) -1 This represents the matrix inversion operation; Through the above calculations, the corresponding single-frame extrinsic parameter estimation results can be obtained for each time frame; Step S4.2: Based on the matching score Perform a matching quality threshold judgment to obtain Frame validity time frame; Step S4.3: According to Time frames within a frame's effective time frame Match score and Frames of valid time are weighted and fused to obtain the result from the lidar. Stable extrinsic parameters relative to the main lidar A Specifically, it includes: (1) Targeting The first frame in the effective time frame Frame matching score After normalization, we obtain the first... The weight coefficients corresponding to the frames, where, : ； Where: summation index variable, ; (2) Decompose the extrinsic parameters of a single frame to obtain the first frame. Frame translation vector The formula is as follows: ； in: For the first The frame rotation matrix; (3) Perform a weighted average on the translation vectors to obtain the fused translation vector. : ； (4) For the first Frame rotation matrix Perform weighted fusion to obtain the fused rotation matrix. : First, convert the rotation matrix into a unit quaternion. : ； Secondly, for unit quaternions We perform a weighted average and normalize to obtain the merged unit quaternion. : ； Finally, the merged unit quaternion Convert back to rotation matrix to obtain the merged rotation matrix. : ； (5) Combine the fused rotation matrix and the fused translation vector to obtain the result from the lidar. Stable extrinsic parameters relative to the main lidar A : 。 6. The automatic calibration method for multiple lidar sensors used in heavy-duty AGVs according to claim 5, characterized in that, Step S5 includes: Step S5.1: Select independent data frames that did not participate in the weighted fusion calculation as verification frames; Let the selected check frame index be: , The total number of check frames; For each verification frame, obtain the point cloud data corresponding to each LiDAR. ,in: For the first Point cloud data of main lidar A in the frame. For the first From LiDAR in frames Point cloud data; Step S5.2: Based on the data from the lidar Stable extrinsic parameters relative to the main lidar A It will be from lidar Point cloud data is uniformly transformed to the coordinate system of the main LiDAR (A coordinate system), as follows: ； in: The point cloud data after transformation to the main lidar A coordinate system; For LiDAR Coordinate transformation to main lidar A; Step S5.3: In the coordinate system of the main lidar A, fuse the point cloud data of each lidar to obtain the first... Point cloud data set after frame fusion : ； in: This indicates that the point cloud data of the main lidar A will be compared with the transformed data of all the slave lidars. Point cloud data merging; This indicates that for all lidar... Union of point cloud data; Step S5.4: [The text appears to be incomplete and contains several grammatical errors. A more accurate translation would require Transform to map coordinate system to obtain the first Frame-by-frame point cloud in map coordinate system : ； in: For the first The pose of the main lidar A in the frame; Step S5.5: [The text appears to be incomplete and contains several grammatical errors. A more accurate translation would require and Perform point cloud matching to obtain closed-loop matching scores. ; Step S5.6: Statistically analyze the closed-loop matching scores of all verification frames and calculate the average closed-loop matching score. : ； Step S5.7: When the condition is met If the verification is successful, the stable extrinsic parameters are considered valid; otherwise, the verification fails, the multi-radar point cloud data of the next time frame is obtained, and the process returns to step S3 to re-perform point cloud matching and extrinsic parameter estimation. This is the closed-loop verification threshold.

7. The automatic calibration method for multiple lidar sensors used in heavy-duty AGVs according to claim 6, characterized in that, Step S6 includes: Step S6.1: During the straight-line travel of the heavy-duty AGV along the linear magnetic strip, continuously acquire the pose of the main LiDAR A in the map coordinate system at a fixed frequency: ,in, For the second time frame index; Step S6.2: Extract the translation component: ,in, for Axial translation components; for Axial translation components; for Axial translation components; Step S6.3: Process the acquired location sequence Performing line fitting in the plane only... and The fitted line is obtained as follows: ； in, The slope of the line. The intercept is... This represents the coordinate values ​​of the independent variable used in the fitting process. The coordinates of the dependent variable used in the fitting process represent the direction angle of the line. Let be the angle between the actual direction of AGV movement and the X-axis of the map coordinate system, as shown in the formula below: ； Step S6.4: Based on the pose of the main lidar A Extract the heading angle from its rotation component, and take the average attitude over multiple frames. The average rotation matrix is ​​then: ; Calculate the orientation angle of the main lidar A for: ； In the formula: The average rotation matrix is ​​the first Line 1 Column elements, It is the arctangent function in the four quadrants; Based on the difference between the actual movement direction of the heavy-duty AGV and the orientation of the main lidar A, the heading angle deviation of the main lidar A coordinate system relative to the heavy-duty AGV body coordinate system is calculated. The formula is as follows: 。 8. The automatic calibration method for multiple lidar sensors used in heavy-duty AGVs according to claim 7, characterized in that, Step S7 includes: Step S7.1: During the in-situ uniform rotation motion of the heavy-duty AGV, continuously record the pose of the main LiDAR A in the map coordinate system at a fixed frequency: ,in, For the third time frame index; Step S7.2: From pose Extracting the translation component: ； in, Indicates the time frame index. They represent the first The three-dimensional coordinates of the main lidar A in the map coordinate system at frame time; Step S7.3: Since the movement of the heavy-duty AGV is restricted to a plane, its two-dimensional projection point is taken: ； By the point set By performing circle fitting, the following circle model is established: ； in, Represents the coordinates of the center of the circle. Indicates the radius of the circle; Solving for the optimal center parameters using the least squares method: ； in, Indicates the number of valid sampled frames used in the fitting; and It is the independent variable in least squares optimization. This represents the assumed x-coordinate of the circle's center during the least squares optimization process. This represents the assumed ordinate of the circle's center during the least squares optimization process; The obtained coordinates of the center of the circle are the position of the kinematic center of the heavy-duty AGV in the map coordinate system, denoted as: ； Step S7.4: Take the average pose of the main lidar A during the rotation process, and record the rotation matrix from the main lidar A coordinate system to the map coordinate system for the rotation part. The translation part is the position vector of the main lidar A in the map coordinate system. Then the coordinates of the kinematic center in the main lidar A coordinate system are expressed as: ； in: ; ; ; in, It represents the set of all orthogonal rotation matrices in three-dimensional space; Represents the three-dimensional real number space; Step S7.5: [The text appears to be incomplete and contains several grammatical errors. A more accurate translation would require the full context.] Represented as a planar offset, as follows: ； in, and These represent the forward and lateral offsets in the main lidar A coordinate system, respectively. Setting the Z component to zero indicates that the kinematic center is offset from the lidar in the same horizontal plane.

9. The automatic calibration method for multiple lidar sensors used in heavy-duty AGVs according to claim 8, characterized in that, Step S8 includes: Step S8.1: Calculate the pose transformation relationship between the main lidar A coordinate system and the kinematic center of the heavy-duty AGV, using the following formula: ； in, Take the plane offset calculated in step S7.5: ； The homogeneous transformation between the main lidar A and the kinematic center of the heavy-duty AGV is: ； in: The homogeneous transformation matrix from the main lidar A coordinate system to the kinematic center of the heavy-duty AGV; Rotation angle around the z-axis The rotation matrix; the matrix below Represents the zero vector, the one above. Represents homogeneous coordinate components; Step S8.2: Calculate from lidar The pose transformation relationship relative to the kinematic center of the heavy-duty AGV: 。 10. A multi-laser radar automatic calibration system for heavy-duty AGVs, characterized in that, This method is used to perform an automatic calibration method for multiple lidar sensors used in heavy-duty AGVs as described in any one of claims 1-9.

Images