Port unmanned container truck heading angle calibration method, system, device and storage medium

Abstract

This invention discloses a method, system, device, and storage medium for lidar heading angle calibration of unmanned container trucks in ports. The method includes lidar inertial navigation heading angle calibration, mechanical parameter fitting heading angle calibration, vehicle body block heading angle calibration, and dynamic trajectory heading angle calibration. Multiple heading angle results are fused to obtain more accurate and robust heading angle calibration results. Furthermore, this method does not rely on inertial navigation or other sensors and is adaptable to all working conditions. Through the fusion and optimization of multiple calibrations, the safety level of vehicle operation is improved while effectively avoiding problems such as large algorithm errors caused by sensor failure, thereby preventing a series of issues such as vehicle deviation from the driving route, false detection, and forced stopping.

Description

Technical Field

[0001] This invention belongs to the field of intelligent driving technology and relates to sensor calibration, specifically to a method, system, device and storage medium for calibrating the heading angle of lidar for unmanned container trucks in ports. Background Technology

[0002] In recent years, with the rapid development of autonomous driving technology, port logistics has become an important scenario for the commercialization of autonomous driving in commercial vehicles. AIV (Autonomous Intelligent Vehicle) trucks, as a new type of unmanned, pure electric intelligent transfer vehicle for ports, undertake key operational tasks such as loading, unloading, and transshipment of containers. AIV vehicles are typically equipped with multiple sensors to obtain various information about the vehicle and its operating environment. Among them, LiDAR, as a core perception device, is usually deployed in multiple units around the vehicle to achieve 360° environmental perception without blind spots.

[0003] The mounting position and attitude parameters of a LiDAR sensor on a vehicle are collectively referred to as extrinsic parameters, comprising six degrees of freedom: three position parameters (x, y, z) and three attitude parameters (roll, pitch, and yaw). The process of calculating these extrinsic parameters is called extrinsic parameter calibration. The accuracy of extrinsic parameter calibration directly affects the perception performance of the autonomous driving system, with the accuracy requirement for the yaw angle being particularly stringent. Because AIVs operate at high speeds during operation, even a small deviation in yaw angle calibration can cause the vehicle to deviate from its intended route, leading to a series of safety issues such as false detections and emergency braking.

[0004] Current mainstream lidar calibration methods have the following shortcomings:

[0005] 1. Limitations of point cloud registration algorithms: Most existing calibration methods are based on point cloud registration algorithms. Although these methods can calibrate six degrees of freedom parameters of the sensor, they lack specific optimization or constraints for the heading angle, resulting in insufficient calibration accuracy for the heading angle. Since the heading angle has the most significant impact on autonomous driving systems, this calibration method is difficult to meet the high-precision requirements of AIVs for the heading angle.

[0006] 2. Inertial Navigation Dependence: Some improved algorithms optimize point cloud stacking by combining inertial navigation system (INS) data. While this method can improve calibration accuracy, its effectiveness is heavily dependent on the accuracy of the INS. In complex operating environments such as ports, INS signals are easily interfered with or temporarily fail, leading to significant deviations in calibration results and insufficient robustness of the algorithm. Summary of the Invention

[0007] To address the aforementioned problems, the main objective of this invention is to design a lidar heading angle calibration method, system, device, and storage medium for unmanned container trucks in ports, thereby solving the problem of low heading angle calibration accuracy, as well as the limitations of point cloud registration algorithms and inertial navigation dependence issues in the heading angle calibration process.

[0008] To achieve the above objectives, the present invention adopts the following technical solution:

[0009] A method for calibrating the heading angle of a lidar system for unmanned container trucks in ports is proposed. The method includes laser inertial navigation heading angle calibration, mechanical parameter fitting heading angle calibration, vehicle body block heading angle calibration, and dynamic trajectory heading angle calibration. The method also fuses the results of multiple heading angles and outputs the optimized heading angle.

[0010] Specifically, the steps include the following:

[0011] The input data includes LiDAR point cloud data, inertial navigation data obtained from inertial navigation calculations, and vehicle mechanical parameters. The inertial navigation data includes the vehicle's real-time position, attitude, and velocity, while the vehicle mechanical parameters include the installation position and shape of the vehicle body blocks, the radar installation position, and the dimensions of the vehicle body steel frame structure.

[0012] Data preprocessing: preliminary external parameters of the lidar are obtained through point cloud registration; inertial navigation data is interpolated to obtain the vehicle position and attitude under the lidar point cloud timestamp.

[0013] Laser inertial navigation heading angle calibration: Based on S-shaped trajectory point cloud stitching and KNN error optimization, the extrinsic parameters after laser inertial navigation calibration are obtained;

[0014] Mechanical parameter fitting and heading angle calibration: Based on the preprocessing results and the design parameters of the radar installation position, the external parameters after fitting and calibrating the mechanical parameters are obtained;

[0015] Vehicle body block heading angle calibration: Based on the external parameters calibrated by fitting mechanical parameters and the design parameters of the vehicle body block installation position and shape, external parameters with the vehicle body block as a reference are obtained;

[0016] Dynamic trajectory heading angle calibration: Based on straight trajectory constraints and point cloud map quality assessment, the extrinsic parameters of dynamic trajectory constraints are obtained;

[0017] Multi-heading result fusion: The external parameters after laser inertial navigation calibration, the external parameters after fitting mechanical parameter calibration, the external parameters with vehicle body blocks as reference, and the external parameters of dynamic trajectory constraints are fused to output the optimized external parameter heading angle.

[0018] As a further description of the present invention, laser inertial navigation azimuth calibration includes the following steps:

[0019] The vehicle to be calibrated performs an S-shaped trajectory driving, and LiDAR point cloud data and INS odometer data are collected;

[0020] Read LiDAR point cloud data and INS odometry data, and preprocess them through downsampling and filtering; based on the constant velocity assumption, perform linear interpolation on the INS odometry data to obtain the INS odometry data information corresponding to each point cloud.

[0021] Based on the initial extrinsic parameter estimation Text, the point cloud of the same object scanned by the LiDAR at each time step is transformed into the initial coordinate system {ins0} of the inertial navigation system to generate a global point cloud map with multiple frames superimposed.

[0022] Use the global point cloud map as the source and make a copy as the target. Create a KDTree spatial index for the target and calculate the Euclidean distance between any point P in the source map and the K nearest points in the target other than P.

[0023] Define the KNN error loss function:

[0024] max map quality = f(T) ext |PointCloud, odometry);

[0025] Here, map quality refers to the object of optimization, namely the quality of the global point cloud map; Text is the state variable of optimization, namely the initial estimated extrinsic parameters; and pointcloud and odometry are prior information for the optimization process, namely the global point cloud map and INS odometry data, respectively.

[0026] For cases where the loss function is less than a set threshold, the BOBYQA local optimization algorithm is used to optimize the extrinsic parameters of the global point cloud map, thereby obtaining the optimized extrinsic heading angle of the lidar.

[0027] As a further description of the present invention, the mechanical parameter fitting heading angle calibration includes the following steps:

[0028] Based on the theoretical installation positions of the lidar at the four corners of the vehicle body, the estimated installation positions are determined. The expressions for the theoretical position point set P and the estimated position point set Q at the four corners of the vehicle body are:

[0029] P = {pi} = {(xi,yi,zi)}, i = 1, 2, 3, 4;

[0030] Q = {qi} = {(xi′,yi′,zi′)}, i = 1, 2, 3, 4;

[0031] The centroids of the theoretical and estimated point sets are calculated using the following expression:

[0032]

[0033] in, Let the centroid of the theoretical location point set be... To estimate the centroid of the set of location points;

[0034] Translate the coordinates of the theoretical and estimated location point sets to the centroid:

[0035]

[0036] Where, p′ i With q′ i Decentralized coordinates;

[0037] Construct the covariance matrix H:

[0038]

[0039] The singular value decomposition of the covariance matrix H is expressed as follows:

[0040] H=U∑V T ;

[0041] Where U and V are orthogonal matrices, and S is a diagonal matrix;

[0042] The rotation matrix R is extracted from the singular value decomposition result, expressed as:

[0043] R = VU T ;

[0044] If det(R) < 0, it is corrected to a rotation matrix, expressed as:

[0045] R = V·diag(1, 1, -1)·U T ;

[0046] Calculate the translation vector t:

[0047]

[0048] Combine the rotation matrix R and the translation vector t into a homogeneous transformation matrix T:

[0049]

[0050] Output the mechanical parameter fitting extrinsic parameters, i.e. the homogeneous transformation matrix, to achieve mechanical parameter fitting and heading angle calibration for rigid body transformation;

[0051] The heading angle is the rotation angle about the vehicle's Z-axis, which is extracted from the rotation matrix R, and its expression is:

[0052]

[0053] θ = arctan 2(R) 21 R11 );

[0054] Where the matrix elements R11 = cosθ, R21 = sinθ;

[0055] The heading angle deviation of the radar installation attitude is quantified by the heading angle θ.

[0056] As a further description of the present invention, the yaw angle calibration of the vehicle body block includes the following steps:

[0057] The design parameters for the installation position and shape of the vehicle body blocks, as well as the LiDAR point cloud, are obtained. The blocks are arranged in a straight line along the edge of the vehicle body and are parallel to the vehicle's heading.

[0058] Point cloud clusters of vehicle body blocks are extracted from lidar point cloud data, and the mounting axis direction of the blocks is fitted using the RANSAC algorithm;

[0059] Based on the parallel constraint between the mounting axis of the stop and the heading of the vehicle body, a heading angle calibration loss function is constructed to optimize the rotation component of the lidar extrinsic parameters.

[0060] As a further description of the present invention, dynamic trajectory heading angle calibration includes the following steps:

[0061] Collect lidar point cloud data and inertial navigation data of the vehicle traveling in a straight line, construct dynamic trajectory constraints, and constrain the normal velocity component of the vehicle in the global coordinate system to be zero.

[0062] A radar odometer is generated based on the vehicle's driving trajectory. After setting the normal velocity component to zero, multiple frames of point cloud are superimposed to generate a local point cloud map.

[0063] Use the global point cloud map as the source and make a copy as the target. Create a KDTree spatial index for the target and calculate the Euclidean distance between any point P in the source map and the K nearest points in the target other than P.

[0064] Define the KNN error loss function:

[0065] max map quality = f(T) ext |PointCloud, odometry);

[0066] Among them, map quality is the object of optimization, namely the quality of the global point cloud map; Text is the state variable of optimization, namely the initial estimated extrinsic parameters; pointcloud and odometry are the prior information of the optimization process, namely the global point cloud map and INS odometry data information, respectively.

[0067] For cases where the loss function is less than a set threshold, the BOBYQA local optimization algorithm is used to optimize the extrinsic parameters of the global point cloud map, thereby obtaining the optimized extrinsic heading angle of the lidar.

[0068] As a further description of the present invention, the fusion of multi-heading results, and the output of optimized extrinsic heading angles based on variance filtering and mean calculation, includes the following steps:

[0069] The external parameter heading angle results for laser inertial navigation heading angle calibration, mechanical parameter fitting heading angle calibration, vehicle body block heading angle calibration, and dynamic trajectory heading angle calibration were obtained respectively.

[0070] After calibration, when the data timestamps are aligned and the coordinates are consistent, calculate the mean and variance of the heading angle;

[0071] If the variance is less than the set threshold, the optimized extrinsic heading angle result is the mean of the calibrated heading angle;

[0072] If the variance is greater than the set threshold, the calibration result is discarded, and the optimized extrinsic heading angle result is the mean of the remaining calibration heading angles.

[0073] A lidar heading angle calibration system for unmanned container trucks in ports includes:

[0074] The data acquisition module is used to acquire lidar point cloud data, inertial navigation data, and vehicle mechanical parameters;

[0075] The preprocessing module registers the point cloud data and interpolates and synchronizes the inertial navigation data.

[0076] The calibration execution module performs laser inertial navigation heading angle calibration, mechanical parameter fitting heading angle calibration, vehicle block heading angle calibration, and dynamic trajectory heading angle calibration.

[0077] The fusion decision module outputs the optimized extrinsic heading angle based on variance screening and mean calculation.

[0078] An electronic device includes a processor, a communication interface, a memory, and a communication bus, wherein the processor, the communication interface, and the memory communicate with each other via the communication bus, and the memory is used to store computer programs;

[0079] The processor is configured to perform the above-described method by running the computer program stored in the memory.

[0080] A computer-readable storage medium storing a computer program, wherein the computer program, when executed by a processor, implements the above-described method.

[0081] Compared with the prior art, the technical advantages of the present invention are as follows:

[0082] This invention provides a method, system, device, and storage medium for lidar heading angle calibration of unmanned container trucks in ports. The method includes lidar inertial navigation heading angle calibration, mechanical parameter fitting heading angle calibration, vehicle body block heading angle calibration, and dynamic trajectory heading angle calibration. It also fuses multiple heading angle results to obtain more accurate and robust heading angle calibration results. Furthermore, this method does not rely on inertial navigation or other sensors and is adaptable to all working conditions. Through the fusion and optimization of multiple calibrations, the safety level of vehicle operation is improved while effectively avoiding problems such as large algorithm errors caused by sensor failure, thereby preventing a series of issues such as vehicle deviation from the driving route, false detection, and forced stopping. Attached Figure Description

[0083] Figure 1 This is a schematic diagram of the heading angle calibration method of the present invention;

[0084] Figure 2 Side view of the vehicle body block layout when calibrating the heading angle of the vehicle body block according to the present invention;

[0085] Figure 3 This is a top view of the vehicle body block layout for calibrating the heading angle of the vehicle body block according to the present invention.

[0086] In the diagram, 1 is the container, 2 is the vehicle body block, 3 is the lidar, and 4 is the AIV vehicle body. Detailed Implementation

[0087] The present invention will now be described in detail with reference to the accompanying drawings:

[0088] In one embodiment of the present invention, a method for calibrating the heading angle of a lidar system for unmanned container trucks in ports is disclosed, with reference to... Figure 1-3 As shown, the method includes laser inertial navigation heading angle calibration, mechanical parameter fitting heading angle calibration, vehicle block heading angle calibration, dynamic trajectory heading angle calibration, and multi-heading result fusion to obtain heading angle calibration results with higher accuracy and better robustness than existing technologies.

[0089] The overall process of this method includes the following steps:

[0090] The input data includes LiDAR point cloud data, inertial navigation data obtained from inertial navigation calculations, and vehicle mechanical parameters. The inertial navigation data includes the vehicle's real-time position, attitude, and velocity, while the vehicle mechanical parameters include the installation position and shape of the vehicle body blocks, the radar installation position, and the dimensions of the vehicle body steel frame structure.

[0091] The input data is preprocessed, including obtaining the preliminary extrinsic parameters of the lidar through point cloud registration, and interpolating the inertial navigation data to obtain the vehicle position and attitude under the lidar point cloud timestamp.

[0092] Laser inertial navigation heading angle calibration: Based on S-shaped trajectory point cloud stitching and KNN error optimization, the extrinsic parameters after laser inertial navigation calibration are obtained;

[0093] Mechanical parameter fitting and heading angle calibration: Based on the preprocessing results and the design parameters of the radar installation position, the external parameters after fitting and calibrating the mechanical parameters are obtained;

[0094] Vehicle body block heading angle calibration: Based on the external parameters calibrated by fitting mechanical parameters and the design parameters of the vehicle body block installation position and shape, external parameters with the vehicle body block as a reference are obtained;

[0095] Dynamic trajectory heading angle calibration: Based on straight trajectory constraints and point cloud map quality assessment, the extrinsic parameters of dynamic trajectory constraints are obtained;

[0096] Multi-heading result fusion: The external parameters after laser inertial navigation calibration, the external parameters after fitting mechanical parameter calibration, the external parameters with vehicle body blocks as reference, and the external parameters of dynamic trajectory constraints are fused to output the optimized external parameter heading angle.

[0097] In this embodiment, the inertial navigation system calculates the vehicle's real-time position, attitude, and velocity, and the process is as follows:

[0098] 1. Attitude calculation:

[0099] The rotation from the carrier coordinate system to the navigation coordinate system is calculated by integrating the angular velocity of the gyroscope, and the equivalent rotation vector method is used to suppress the error.

[0100] (1) Calculation of angle increment:

[0101]

[0102] Where, ω b Let θ be the angular velocity of the carrier coordinate system. k This represents the angular increment within the time interval.

[0103] (2) Equivalent Rotation Vector (Two-sample Algorithm):

[0104]

[0105] Where φ is the equivalent rotation vector.

[0106] (3) Quaternion update:

[0107]

[0108] in, The process involves quaternion multiplication, ultimately transforming the result into an attitude matrix.

[0109] 2. Position and velocity calculation:

[0110] The position is determined by two integrations using accelerometer data and the attitude matrix.

[0111] (1) Calculation of acceleration of navigation system:

[0112]

[0113] Among them, a b Let g be the acceleration of the carrier coordinate system, and g be the acceleration due to gravity.

[0114] (2) Velocity integral:

[0115]

[0116] (3) Positional integral:

[0117]

[0118] Wherein, the subscript b represents the vehicle coordinate system, and the superscript n represents the navigation coordinate system. Let be the coordinate rotation matrix from the carrier system to the navigation system.

[0119] In this embodiment, point cloud registration is used to obtain the preliminary extrinsic parameters of the lidar. The process is as follows:

[0120] The ICP (Iterative ClosestPoint) algorithm is an iterative method for point cloud registration that aims to solve for the optimal rigid body transformation (rotation and translation) by minimizing the distance between corresponding points in two point clouds.

[0121] 1. Initialization

[0122] Input source point cloud P = {p i} and the target point cloud Q = {q j} and set the initial transformation matrix (usually the identity matrix).

[0123] 2. Iteration steps

[0124] (1) Matching corresponding points:

[0125] For each point p in the source point cloud P i Find the nearest neighbor point q in the target point cloud Q. j This forms corresponding point pairs, and KD trees are commonly used to accelerate the search.

[0126] (2) Remove outliers (optional)

[0127] Remove corresponding point pairs that are too far apart (using threshold filtering) to improve registration robustness.

[0128] (3) Calculate the optimal transformation

[0129] Solve for the rotation matrix (R and translation vector t) such that the mean square error of the corresponding points after the transformation is minimized. Specifically:

[0130] Calculate the centroid of the two point clouds:

[0131] Centroid coordinates: p′ i =p i -μ p ,q′ j =q j -μ q ;

[0132] Construct the covariance matrix H = ∑(q′) j )(p′ i ) T And perform SVD decomposition on H: H=U∑V T ;

[0133] Rotation matrix R = VU T Translation vector t = μ q -Rμ p .

[0134] (4) Application transformation

[0135] Applying R and t to the source point cloud P, we obtain the updated point cloud P′=R·P+t.

[0136] (5) Determining convergence

[0137] Calculate the error (such as the average distance between corresponding points). If the error is less than the threshold or the maximum number of iterations is reached, stop; otherwise, continue iterating.

[0138] 3. Output

[0139] The final transformation matrices R and t are used to align the transformed source point cloud with the target point cloud.

[0140] In this embodiment, the vehicle position and attitude under the lidar point cloud timestamp are obtained through interpolation processing, and the process is as follows:

[0141] Given the lidar point cloud timestamps and inertial navigation pose data, the process of calculating the vehicle pose at the corresponding timestamp through interpolation can be summarized into the following steps, involving time synchronization, interpolation methods, and coordinate transformation.

[0142] 1. Time alignment and data synchronization

[0143] Timestamp matching: Align the timestamps of the LiDAR point cloud with the pose timestamps of the inertial navigation system (INS) output. Since INS data is usually output at high frequencies (e.g., 100Hz), it is necessary to find the two adjacent INS pose data points that are adjacent to each point cloud timestamp.

[0144] Linear interpolation: Based on the time difference ratio, linear interpolation is performed on the poses of adjacent inertial navigation systems. Assuming the point cloud timestamp is t, and the adjacent inertial navigation timestamps are t1 and t2, the interpolation weights are... Calculate the intermediate pose.

[0145] 2. Pose interpolation method

[0146] Translation interpolation: Directly perform linear interpolation on the translation vector t, that is:

[0147] t interp = (1-α)t1+αt2;

[0148] Rotation interpolation: If Euler angles or rotation matrices are used, they need to be converted to quaternions for spherical linear interpolation (SLERP) to avoid interpolation errors.

[0149] 3. Coordinate transformation and point cloud correction

[0150] Pose matrix generation: The interpolated translation and rotation are combined into a 4×4 transformation matrix Tinterp, which represents the pose of the vehicle body under the point cloud timestamp.

[0151] Point cloud distortion correction: Using interpolation pose, each laser point is further compensated for motion distortion according to its time offset (such as the relative time within the radar rotation cycle) and unified to the same coordinate system.

[0152] It should be noted that the inertial navigation calculation, point cloud registration, and interpolation processing described above are all mature algorithms, and this embodiment includes, but is not limited to, the above calculation process.

[0153] This embodiment provides a detailed analysis of the four calibration methods described above, as follows:

[0154] I. Laser Inertial Navigation Angle Calibration

[0155] The vehicle to be calibrated performs an S-shaped trajectory driving at a slow speed for 100-200 meters in a specific scenario, collecting LiDAR point cloud data and INS odometry data; the LiDAR point cloud data and INS odometry data are read and preprocessed through downsampling, filtering and other methods; based on the assumption of constant speed, the INS odometry data is linearly interpolated to obtain the INS odometry data information corresponding to each point cloud.

[0156] Suppose that at time m, the radar scans object O, forming a point cloud, and obtains the odometer information T_ins_m at the timestamp corresponding to that point by interpolating ins. Then, at time n, the radar scans object O again and calculates the odometer information T_ins_n of ins (vehicle body) at time n. Based on the lidar point cloud data, inertial navigation data, and preprocessing results, and given the initial extrinsic parameter estimate Text, the point clouds of the same object scanned by the lidar at each time are transformed to the initial inertial navigation coordinate system {ins0} to generate a multi-frame superimposed global point cloud map.

[0157] It is now known that the odometry information from the ins coordinate system is very accurate. If the extrinsic parameter estimation is also very accurate, then the positions of all point clouds projected back to the ins coordinate system corresponding to the object O should be infinitely close, resulting in a very high-resolution point cloud map. If the vehicle position and attitude accuracy obtained after interpolation is high, then when the LiDAR point clouds at different times are transformed to the corresponding vehicle pose, the accuracy of the point cloud stitching will depend solely on the extrinsic parameters. By optimizing the point cloud stitching, to evaluate the quality of the constructed map, the global point cloud map is used as the source, and a copy is used as the target. A KDTree spatial index is created for the target, and the Euclidean distance between any point P in the source map and the K nearest points in the target (excluding P) is calculated to evaluate the map quality.

[0158] Define the KNN error loss function:

[0159] maxmapquality = f(T) ext |Point(Cloud, odometry);

[0160] In this formula, `map quality` refers to the quality of the global point cloud map, `Text` is the state variable being optimized (i.e., the initial estimated extrinsic parameters), and `pointcloud` and `odometry` are prior information for the optimization process, representing the global point cloud map and INS odometry data, respectively. This formula indicates that, given the prior knowledge of the point cloud map and odometry data, maximizing the point cloud map quality allows for optimization of the extrinsic parameters, leading to the extrinsic parameter results for laser inertial navigation system calibration.

[0161] If the map construction quality is very high, the features and structure are obvious, and all point clouds corresponding to the same object O should be very similar. Theoretically, this loss function should be small. Conversely, if the map structure is unclear, the features are ambiguous, or even lack any features or structure, this loss function will be very large. Assuming we have some initial estimates and the constructed map already has a certain structure and relatively clear features, i.e., for cases where the loss function is less than a set threshold, we use the BOBYQA local optimization algorithm to optimize the extrinsic parameters of the global point cloud map, solving for the high-precision extrinsic parameters of the LiDAR, including the optimized heading angle of the LiDAR.

[0162] II. Mechanical Parameter Fitting and Heading Angle Calibration

[0163] The calibration step takes the results of data preprocessing and the design parameters for the radar installation location from the relevant mechanical parameters as inputs, and outputs the extrinsic parameters after fitting the mechanical parameters.

[0164] Specifically, assume the theoretical installation positions of the lidar at the four corners of the vehicle are (x1, y1, z1), (x2, y2, z2), (x3, y3, z3), and (x4, y4, z4); now, give our estimated installation positions (x1', y1', z1'), (x2', y2', z2'), (x3', y3', z3'), and (x4', y4', z4'); based on the theoretical installation positions of the lidar at the four corners of the vehicle, determine the estimated installation positions. Then, the expression for the theoretical position point set P and the estimated position point set Q at the four corners of the vehicle is:

[0165] P = {pi} = {(xi,yi,zi)}, i = 1, 2, 3, 4;

[0166] Q = {qi} = {(xi′,yi′,zi′)}, i = 1, 2, 3, 4;

[0167] 1. Decentralization:

[0168] The centroids of the theoretical and estimated point sets are calculated using the following expression:

[0169]

[0170] in, Let the centroid of the theoretical location point set be... To estimate the centroid of the set of location points;

[0171] Translate the coordinates of the theoretical and estimated location point sets to the centroid:

[0172]

[0173] Where, p′ i With q′i Decentralized coordinates;

[0174] 2. Construct the covariance matrix H:

[0175]

[0176] 3. Singular Value Decomposition (SVD)

[0177] The singular value decomposition of the covariance matrix H is expressed as follows:

[0178] H=U∑V T ;

[0179] Where U and V are orthogonal matrices, and S is a diagonal matrix;

[0180] The rotation matrix R is extracted from the singular value decomposition result, expressed as:

[0181] R = V∪ T ;

[0182] If det(R) < 0, it is corrected to a rotation matrix, expressed as:

[0183] R = V·diag(1, 1, -1)·U T ;

[0184] 4. Translation vector

[0185] Calculate the translation vector t:

[0186]

[0187] 5. Transformation Matrix

[0188] Combine the rotation matrix R and the translation vector t into a homogeneous transformation matrix T:

[0189]

[0190] Output the mechanical parameter fitting extrinsic parameters, i.e. the homogeneous transformation matrix, to achieve mechanical parameter fitting and heading angle calibration for rigid body transformation;

[0191] 6. Calculate the heading angle

[0192] The heading angle is the rotation angle about the vehicle's Z-axis, which is extracted from the rotation matrix R, and its expression is:

[0193]

[0194] θ=arctan2(R 21 R 11 );

[0195] Where the matrix elements R11 = cosθ, R21 = sinθ;

[0196] The heading angle deviation of the radar installation attitude is quantified by the heading angle θ.

[0197] III. Vehicle Body Block Heading Angle Calibration

[0198] Since AIVs are used to transport containers, metal blocks are needed on both sides of the vehicle body to restrict the position of the containers. For example... Figure 2-3 As shown, there are 4 vehicle body blocks 2 on each of the left and right sides of the AIV vehicle body 4. The container 1 is placed in the middle of the vehicle body blocks 2. The lidar 3 is installed at the front and rear ends of the vehicle body. Since the installation position of the vehicle body blocks 2 is a straight line along the edge of the vehicle body, the vehicle body heading must be parallel to the blocks on the left and right sides. Based on this constraint, the subsequent radar heading angle is calibrated.

[0199] Specifically, the calibration of the vehicle body stop yaw angle includes the following steps:

[0200] The design parameters for the installation position and shape of the vehicle body blocks, as well as the LiDAR point cloud, are obtained. The blocks are arranged in a straight line along the edge of the vehicle body and are parallel to the vehicle's heading.

[0201] Point cloud clusters of vehicle body blocks are extracted from lidar point cloud data, and the mounting axis direction of the blocks is fitted using the RANSAC algorithm;

[0202] Based on the parallel constraint between the mounting axis of the stop and the heading of the vehicle body, a heading angle calibration loss function is constructed to optimize the rotation component of the lidar extrinsic parameters.

[0203] IV. Dynamic trajectory heading angle calibration

[0204] The calibration process in this step requires the vehicle to travel in a straight line for approximately 100 meters and record a dynamic trajectory. Specifically, it includes the following steps:

[0205] Collect lidar point cloud data and inertial navigation data of the vehicle traveling in a straight line, construct dynamic trajectory constraints, and constrain the normal velocity component in the vehicle's global coordinate system to be zero (the velocity in the global coordinate system is along the tangential direction of the trajectory and has no normal velocity component); under this constraint of zero normal velocity component, optimize the local point cloud map, then the lateral layering of the point cloud is caused by the heading angle from the vehicle system to the global system.

[0206] Radar odometers are generated based on vehicle trajectories. After setting the normal velocity component to zero, multiple frames of point clouds are superimposed to generate a local point cloud map. When the extrinsic parameter estimation of the odometer heading angle is very accurate, all point clouds corresponding to the object should not be layered. The accuracy of point cloud stitching will depend only on the extrinsic parameters, especially the assumed heading angle. That is, by optimizing point cloud stitching, the heading angle can be calculated.

[0207] To evaluate the quality of the constructed map, a global point cloud map is used as the source, and a copy is used as the target. A KDTree spatial index is constructed for the target, and the Euclidean distance between any point P in the source map and the K nearest points in the target (excluding P) is calculated.

[0208] Define the KNN error loss function:

[0209] max map quality = f(T) ext |PointCloud, odometry);

[0210] In this formula, `map quality` refers to the quality of the global point cloud map, `Text` is the state variable being optimized (i.e., the initial estimated extrinsic parameters), and `pointcloud` and `odometry` are prior information for the optimization process, representing the global point cloud map and INS odometry data, respectively. This formula indicates that, given the prior knowledge of the point cloud map and odometry data, maximizing the point cloud map quality allows for optimization of the extrinsic parameters, resulting in the extrinsic parameter results for dynamic trajectory calibration.

[0211] If the map construction quality is very high, the features and structure are obvious, and all point clouds corresponding to the same object O should be very similar. Theoretically, this loss function should be small. Conversely, if the map structure is unclear, the features are ambiguous, or even lack any features or structure, this loss function will be very large. Assuming we have some initial estimates and the constructed map already has a certain structure and relatively clear features, i.e., for cases where the loss function is less than a set threshold, we use the BOBYQA local optimization algorithm to optimize the extrinsic parameters of the global point cloud map, obtaining the optimized heading angle for the LiDAR.

[0212] In this embodiment, a fusion strategy is used to select the corresponding heading angle for fusion. Specifically, the fusion of multiple heading results is achieved through policy and probability constraints. When more than two calibrations are used, the optimized extrinsic heading angle is output based on variance filtering and mean calculation, including the following steps:

[0213] The external parameter heading angle results for laser inertial navigation heading angle calibration, mechanical parameter fitting heading angle calibration, vehicle body block heading angle calibration, and dynamic trajectory heading angle calibration were obtained respectively.

[0214] After calibration, when the data timestamps are aligned and the coordinates are consistent, calculate the mean and variance of the heading angle;

[0215] If the variance is less than the set threshold, the optimized extrinsic heading angle result is the mean of the calibrated heading angle;

[0216] If the variance is greater than the set threshold, the calibration result will be removed and considered as an abnormal calibration operation. The result will not be included in the fusion. The optimized extrinsic heading angle result is the mean of the remaining calibration heading angles.

[0217] Output the final optimized extrinsic parameters, where the heading angle of the extrinsic parameters is the heading angle after fusion optimization.

[0218] In another embodiment of the invention, a lidar heading angle calibration system for unmanned container trucks in ports is also included, the system comprising:

[0219] The data acquisition module is used to acquire lidar point cloud data, inertial navigation data, and vehicle mechanical parameters;

[0220] The preprocessing module registers the point cloud data and interpolates and synchronizes the inertial navigation data.

[0221] The calibration execution module performs laser inertial navigation heading angle calibration, mechanical parameter fitting heading angle calibration, vehicle block heading angle calibration, and dynamic trajectory heading angle calibration.

[0222] The fusion decision module outputs the optimized extrinsic heading angle based on variance screening and mean calculation.

[0223] The calibration execution module described above can be configured with the following sub-modules according to actual needs:

[0224] A laser-inertial navigation heading calibration submodule is used to obtain the heading angle using a laser-inertial navigation optimization strategy;

[0225] A heading calibration submodule is used to fit the mechanical parameters of the heading angle by utilizing the machining location of the radar installation.

[0226] The vehicle body’s special structure is used as a point cloud optimization constraint to calculate the heading angle of the vehicle body block heading calibration submodule.

[0227] By utilizing the characteristics of the driving trajectory and optimizing the point cloud based on kinematic features, a dynamic trajectory heading calibration submodule is used to calculate the heading angle.

[0228] The above content discloses the method and system of the present invention, which have the following advantages over the prior art:

[0229] 1. In the calibration of the vehicle heading angle, the present invention obtains the heading angle by one or more algorithms respectively, and then fuses them to obtain a vehicle heading angle with higher accuracy and greater robustness, thereby improving the calibration accuracy of the heading angle;

[0230] 2. The various calibration methods of this invention do not rely on inertial navigation or other sensors, have a wide range of applications, and have small calibration accuracy errors when other sensors fail.

[0231] 3. This invention combines multiple calibration methods to obtain a high-precision heading angle, thereby improving the safety level of vehicle driving and avoiding a series of problems such as vehicle deviation from the driving route, false detection, and forced stopping.

[0232] In another embodiment of the invention, an electronic device is also included, which may include a processor and a memory storing instructions from a computer program.

[0233] Specifically, in this embodiment, the processor may include a central processing unit (CPU), a specific integrated circuit, or one or more integrated circuits that can be configured in this embodiment; the memory may include a mass storage device for data or instructions, including but not limited to a hard disk drive (HDD), floppy disk drive, flash memory, optical disk, magneto-optical disk, magnetic tape, or Universal Serial Bus (USB) drive, or a combination of two or more of these; where appropriate, the memory may include removable or non-removable (or fixed) media; in a particular embodiment, the memory is a non-volatile solid-state memory. In a particular embodiment, the memory includes a read-only memory (ROM). Where appropriate, the ROM may be a mask-programmed ROM, a programmable ROM (PROM), an erasable PROM (EPROM), an electrically erasable PROM (EEPROM), an electrically rewritable ROM (EAROM), or flash memory, or a combination of two or more of these.

[0234] The processor described above implements the heading angle calibration method disclosed in this invention by reading and executing computer program instructions stored in the memory.

[0235] It should also be noted that the electronic device in this embodiment may further include a communication interface and a communication bus. The processor, memory, and communication interface are connected via the communication bus to complete communication between them. The communication interface is mainly used to realize communication between the various units, modules, devices, or equipment in this embodiment of the invention.

[0236] The aforementioned communication bus comprises hardware, software, or a combination of both, coupling components of online data traffic devices together. Where appropriate, the communication bus may include one or more buses.

[0237] In addition, in conjunction with the heading angle calibration method in the above embodiments, the embodiments of the present invention can be implemented by providing a computer storage medium, on which computer program instructions are stored; the computer program instructions are executed by a processor using the above-described heading angle calibration method.

[0238] It should be clarified that the present invention is not limited to the methods, systems, and devices disclosed above, but also includes various changes, modifications, and additions made by those skilled in the art based on the ideas of the present invention, or changes in the order of steps.

[0239] When implemented in hardware, this invention can be electronic circuits, application-specific integrated circuits (ASICs), appropriate firmware, plug-ins, function cards, etc. When implemented in software, the elements of this invention are programs or code segments used to perform the required tasks. These programs or code segments can be stored on a machine-readable medium or transmitted via a data signal carried on a carrier wave through a transmission medium or communication link. "Machine-readable medium" can include any medium capable of storing or transmitting information, such as electronic circuits, semiconductor memory devices, ROM, flash memory, erasable ROM (EROM), floppy disks, optical disks, hard disks, fiber optic media, radio frequency links, etc. The code segments can be downloaded via computer networks such as the Internet or intranets.

[0240] The above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Any other modifications or equivalent substitutions made by those skilled in the art to the technical solutions of the present invention, as long as they do not depart from the spirit and scope of the technical solutions of the present invention, should be covered within the scope of the claims of the present invention.

Claims

1. A method for calibrating the heading angle of a lidar system for unmanned container trucks in ports, characterized in that, This method includes laser inertial navigation heading angle calibration, mechanical parameter fitting heading angle calibration, vehicle block heading angle calibration, dynamic trajectory heading angle calibration, and multi-heading result fusion to output the optimized heading angle; Specifically, the steps include the following: The input data includes LiDAR point cloud data, inertial navigation data obtained from inertial navigation calculations, and vehicle mechanical parameters. The inertial navigation data includes the vehicle's real-time position, attitude, and velocity, while the vehicle mechanical parameters include the installation position and shape of the vehicle body blocks, the radar installation position, and the dimensions of the vehicle body steel frame structure. Data preprocessing: preliminary external parameters of the lidar are obtained through point cloud registration; inertial navigation data is interpolated to obtain the vehicle position and attitude under the lidar point cloud timestamp. Laser inertial navigation heading angle calibration: Based on S-shaped trajectory point cloud stitching and KNN error optimization, the extrinsic parameters after laser inertial navigation calibration are obtained; Mechanical parameter fitting and heading angle calibration: Based on the preprocessing results and the design parameters of the radar installation position, the external parameters after fitting and calibrating the mechanical parameters are obtained; Vehicle body block heading angle calibration: Based on the external parameters calibrated by fitting mechanical parameters and the design parameters of the vehicle body block installation position and shape, external parameters with the vehicle body block as a reference are obtained; Dynamic trajectory heading angle calibration: Based on straight trajectory constraints and point cloud map quality assessment, the extrinsic parameters of dynamic trajectory constraints are obtained; Multi-heading result fusion: The external parameters after laser inertial navigation calibration, the external parameters after fitting mechanical parameter calibration, the external parameters with vehicle body blocks as reference, and the external parameters constrained by dynamic trajectory are fused to output the optimized external parameter heading angle; The laser inertial navigation azimuth calibration includes the following steps: The vehicle to be calibrated performs an S-shaped trajectory driving, and LiDAR point cloud data and INS odometer data are collected; Read LiDAR point cloud data and INS odometry data, and preprocess them through downsampling and filtering; based on the constant velocity assumption, perform linear interpolation on the INS odometry data to obtain the INS odometry data information corresponding to each point cloud. Based on the initial external parameter estimation T ext The point cloud of the same object scanned by the lidar at each time moment is transformed to the initial coordinate system ins0 of the inertial navigation system to generate a global point cloud map with multiple frames superimposed. Use the global point cloud map as the source and make a copy as the target. Create a KDTree spatial index for the target and calculate the Euclidean distance between any point P in the source map and the K nearest points in the target other than P. Define the KNN error loss function: ； Where map quality is the object of optimization, namely the quality of the global point cloud map, T ext It is the state variable of the optimization, that is, the initial estimated extrinsic parameter. Pointcloud and odometry are the prior information of the optimization process, which are the global point cloud map and INS odometry data information, respectively. For cases where the loss function is less than a set threshold, the BOBYQA local optimization algorithm is used to optimize the extrinsic parameters of the global point cloud map, thereby obtaining the optimized extrinsic heading angle of the lidar.

2. The method for calibrating the heading angle of a lidar system for unmanned container trucks in ports according to claim 1, characterized in that: Mechanical parameter fitting and heading angle calibration include the following steps: Based on the theoretical installation positions of the lidar at the four corners of the vehicle body, the estimated installation positions are determined. The expressions for the theoretical position point set P and the estimated position point set Q at the four corners of the vehicle body are: P ={ pi}={( xi , yi , zi )}, i =1,2,3,4； Q ={ qi}={( xi ′, yi ′, zi ′)}, i =1,2,3,4； The centroids of the theoretical and estimated point sets are calculated using the following expression: ， ； in, Let the centroid of the theoretical location point set be... To estimate the centroid of the set of location points; Translate the coordinates of the theoretical and estimated location point sets to the centroid: ， ； in, and Decentralized coordinates; Construct the covariance matrix H: ； The singular value decomposition of the covariance matrix H is expressed as follows: ； Where U and V are orthogonal matrices, and Σ is a diagonal matrix; The rotation matrix R is extracted from the singular value decomposition result, expressed as: ； If det( R If ) < 0, it is corrected to a rotation matrix, expressed as: ； Calculate the translation vector t: ； Combine the rotation matrix R and the translation vector t into a homogeneous transformation matrix T: ； Output the mechanical parameter fitting extrinsic parameters, i.e. the homogeneous transformation matrix, to achieve mechanical parameter fitting and heading angle calibration for rigid body transformation; The heading angle is the rotation angle about the vehicle's Z-axis, which is extracted from the rotation matrix R, and its expression is: ； ； Among them, matrix elements R 11 = cos θ , R 21=sin θ ; By heading angle θ Quantify the heading angle deviation of the radar installation attitude.

3. The method for calibrating the heading angle of a lidar system for unmanned container trucks in ports according to claim 1, characterized in that: The calibration of the vehicle body stop angle includes the following steps: The design parameters for the installation position and shape of the vehicle body blocks, as well as the LiDAR point cloud, are obtained. The blocks are arranged in a straight line along the edge of the vehicle body and are parallel to the vehicle's heading. Extract point cloud clusters of vehicle body blocks from lidar point cloud data, and fit the mounting axis direction of the blocks using the RANSAC algorithm; Based on the parallel constraint between the mounting axis of the stop and the heading of the vehicle body, a heading angle calibration loss function is constructed to optimize the rotation component of the lidar extrinsic parameters.

4. The method for calibrating the heading angle of a lidar for unmanned container trucks in ports according to claim 1, characterized in that: Dynamic trajectory heading angle calibration includes the following steps: Collect lidar point cloud data and inertial navigation data of the vehicle traveling in a straight line, construct dynamic trajectory constraints, and constrain the normal velocity component of the vehicle in the global coordinate system to be zero. A radar odometer is generated based on the vehicle's driving trajectory. After setting the normal velocity component to zero, multiple frames of point cloud are superimposed to generate a local point cloud map. Use the global point cloud map as the source and make a copy as the target. Create a KDTree spatial index for the target and calculate the Euclidean distance between any point P in the source map and the K nearest points in the target other than P. Define the KNN error loss function: ； Where map quality is the object of optimization, namely the quality of the global point cloud map, T ext It is the state variable of the optimization, that is, the initial estimated extrinsic parameter. Pointcloud and odometry are the prior information of the optimization process, which are the global point cloud map and INS odometry data information, respectively. For cases where the loss function is less than a set threshold, the BOBYQA local optimization algorithm is used to optimize the extrinsic parameters of the global point cloud map, thereby obtaining the optimized extrinsic heading angle of the lidar.

5. The method for calibrating the heading angle of a lidar system for unmanned container trucks in ports according to claim 1, characterized in that: The multi-heading results are fused into an optimized external parameter heading angle based on variance filtering and mean calculation, including the following steps: The external parameter heading angle results for laser inertial navigation heading angle calibration, mechanical parameter fitting heading angle calibration, vehicle body block heading angle calibration, and dynamic trajectory heading angle calibration were obtained respectively. After calibration, when the data timestamps are aligned and the coordinates are consistent, calculate the mean and variance of the heading angle; If the variance is less than the set threshold, the optimized extrinsic heading angle result is the mean of the calibrated heading angle; If the variance is greater than the set threshold, the values ​​that produce this large variance in the external parameter heading angle results of laser inertial navigation heading angle calibration, mechanical parameter fitting heading angle calibration, vehicle block heading angle calibration, and dynamic trajectory heading angle calibration will be removed. The optimized external parameter heading angle result is the mean of the remaining calibrated heading angles.

6. A lidar heading angle calibration system for unmanned container trucks in ports, as described in any one of claims 1-5, characterized in that, include: The data acquisition module is used to acquire lidar point cloud data, inertial navigation data, and vehicle mechanical parameters; The preprocessing module registers the point cloud data and interpolates and synchronizes the inertial navigation data. The calibration execution module performs laser inertial navigation heading angle calibration, mechanical parameter fitting heading angle calibration, vehicle block heading angle calibration, and dynamic trajectory heading angle calibration. The fusion decision module outputs the optimized extrinsic heading angle based on variance screening and mean calculation.

7. An electronic device comprising a processor, a communication interface, a memory, and a communication bus, wherein, The processor, the communication interface, and the memory communicate with each other via the communication bus, wherein the memory is used to store computer programs; The processor is configured to perform the method of any one of claims 1-5 by running the computer program stored in the memory.

8. A computer-readable storage medium, characterized in that, The storage medium stores a computer program, wherein the computer program, when executed by a processor, implements the method of any one of claims 1-5.

Images