Course angle calibration method, system and device for port unmanned container truck and storage medium

Abstract

The invention discloses a laser radar course angle calibration method, system and device for a port unmanned container truck and a storage medium, and the method comprises the steps: calibrating a laser inertial navigation course angle, calibrating a mechanical parameter fitting course angle, calibrating a vehicle body stop dog course angle, calibrating a dynamic track course angle, and carrying out the multi-course result fusion. The course angle calibration result with higher precision and better robustness is obtained, and the method does not need to depend on inertial navigation and other sensors and has all-working-condition scene adaptability; through fusion optimization of multiple kinds of calibration, the safety level of vehicle driving is improved, meanwhile, the problems of large algorithm errors and the like caused by sensor failure are effectively avoided, and therefore a series of problems of vehicle driving route deviation, false detection, forced parking and the like are prevented.

Description

Technical Field

[0001] The present invention belongs to the field of intelligent driving technology and relates to sensor calibration, and specifically to a laser radar heading angle calibration method, system, equipment and storage medium for unmanned container trucks in ports. Background Art

[0002] In recent years, with the rapid development of autonomous driving technology, port logistics has become a key application for the commercialization of autonomous commercial vehicles. AIVs (Autonomous Intelligent Vehicles), a new type of unmanned, all-electric intelligent port transport vehicle, undertake critical operations such as loading, unloading, and transshipping containers at ports. AIVs are typically equipped with a variety of sensors to obtain various information about the vehicle and its operating environment. LiDARs, the core sensing device, are typically deployed in multiple locations around the vehicle body to achieve 360-degree environmental awareness.

[0003] The installation position and attitude parameters of the lidar sensor on the vehicle are collectively referred to as extrinsic parameters, which include six degrees of freedom: three position parameters (x, y, z) and three attitude parameters (roll, pitch, and yaw). The process of calculating the sensor's 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 requirements for heading angle being particularly stringent. Because AIVs operate at high speeds, even slight deviations in heading 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 stops.

[0004] The current mainstream lidar calibration has the following shortcomings:

[0005] 1. Limitations of point cloud registration algorithms: Existing calibration methods are mostly based on point cloud registration algorithms. While these methods can calibrate the six degrees of freedom (DOF) of the sensor, they lack specific optimization or constraints for heading angle, resulting in insufficient heading angle calibration accuracy. Since heading angle has the most significant impact on autonomous driving systems, this calibration method cannot meet the high heading angle precision requirements of AIVs.

[0006] 2. Inertial navigation dependency: Some improved algorithms combine inertial navigation system (INS) data for point cloud stacking optimization. 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 susceptible to interference or temporary failure, leading to significant deviations in calibration results and insufficient algorithm robustness. Summary of the Invention

[0007] In response to the above problems, the main purpose of the present invention is to design a lidar heading angle calibration method, system, equipment and storage medium for unmanned container trucks in ports, so as to solve the problem of low heading angle calibration accuracy, as well as the limitations of the point cloud registration algorithm and the inertial navigation dependence problem during the heading angle calibration process.

[0008] In order to achieve the above purpose, the present invention adopts the following technical solutions:

[0009] A laser radar heading angle calibration method for unmanned container trucks in ports. 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 output the optimized heading angle.

[0010] The specific steps include:

[0011] Obtain input data, including lidar point cloud data, inertial navigation data obtained from inertial navigation solution, and vehicle body mechanical parameters. The inertial navigation data includes the vehicle's real-time position, attitude, and speed. The vehicle body mechanical parameters include the installation position and shape of the vehicle body block, the radar installation position, and the dimensions of the vehicle body steel frame structure.

[0012] Data preprocessing: obtaining preliminary external parameters of the lidar through point cloud registration, interpolating the inertial navigation data, and obtaining the vehicle position and posture at the timestamp of the lidar point cloud;

[0013] Laser inertial navigation heading calibration: Based on S-shaped trajectory point cloud stitching and KNN error optimization, the external 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 the mechanical parameter calibration are obtained;

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

[0016] Dynamic trajectory heading angle calibration: Based on straight line trajectory constraints and point cloud map quality assessment, the external 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 referenced by the vehicle body block, and the dynamic trajectory constraint external parameters are fused to output the optimized external parameter heading angle.

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

[0019] The vehicle to be calibrated executes an S-shaped trajectory to collect lidar point cloud data and INS odometry data;

[0020] Read the LiDAR point cloud data and INS odometry data information, and pre-process them through downsampling and filtering. Based on the constant speed assumption, linearly interpolate 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 moment is transformed into the inertial navigation initial coordinate system {ins0} to generate a global point cloud map with multiple frames superimposed;

[0022] Take the global point cloud map as the source and copy it as the target, build 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 except P;

[0023] Define the KNN error loss function:

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

[0025] Among them, map quality is the object of optimization, that is, the quality of the global point cloud map, Text is the optimized state quantity, that is, the initial estimated external parameter, pointcloud and odometry are the prior information of the optimization process, which are the global point cloud map and INS odometer data information respectively.

[0026] When the loss function is less than the set threshold, the BOBYQA local optimization algorithm is used to optimize the external parameters of the global point cloud map to obtain the optimized external parameter 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] The estimated installation position is determined based on the theoretical installation position of the lidar at the four corners of the vehicle body. The theoretical position point set P and the estimated position point set Q of the four corners of the vehicle body are expressed as:

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

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

[0031] Calculate the centroid of the theoretical position point set and the estimated position point set. The expression is:

[0032]

[0033] in, is the centroid of the theoretical position point set, is the estimated centroid of the location point set;

[0034] Translate the coordinates of the theoretical position point set and the estimated position point set to the centroid:

[0035]

[0036] Among them, p′ i and q′ i Decentralized coordinates;

[0037] Construct the covariance matrix H:

[0038]

[0039] Perform singular value decomposition on the covariance matrix H, and the expression is:

[0040] H=U∑V T ;

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

[0042] Extract the rotation matrix R from the singular value decomposition result, the expression is:

[0043] R=VU T ;

[0044] If det(R)<0, it is corrected to a rotation matrix, and the expression is:

[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 mechanical parameter fitting external parameters, i.e. homogeneous transformation matrix, to achieve mechanical parameter fitting heading angle calibration of rigid body transformation;

[0051] The heading angle is the rotation angle around the vehicle's Z axis, which is extracted from the rotation matrix R and expressed as:

[0052]

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

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

[0055] The heading angle θ is used to quantify the heading angle deviation of the radar installation attitude.

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

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

[0058] Extract the point cloud cluster of the vehicle body stopper from the lidar point cloud data, and fit the stopper installation axis direction using the RANSAC algorithm;

[0059] Based on the parallel constraint between the axis direction of the block installation and the vehicle heading, a heading angle calibration loss function is constructed to optimize the rotation component of the lidar extrinsic parameter.

[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] Generate a radar odometer based on the vehicle's trajectory, set the normal velocity component to zero, and then superimpose multiple frames of point clouds to generate a local point cloud map;

[0063] Take the global point cloud map as the source and copy it as the target, build 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 except 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, that is, the quality of the global point cloud map, Text is the optimized state quantity, that is, the initial estimated external parameter, pointcloud and odometry are the prior information of the optimization process, namely the global point cloud map and INS odometry data information;

[0067] When the loss function is less than the set threshold, the BOBYQA local optimization algorithm is used to optimize the external parameters of the global point cloud map to obtain the optimized external parameter heading angle of the lidar.

[0068] As a further description of the present invention, the fusion of multiple heading results is to output the optimized extrinsic heading angle according to variance screening and mean calculation, including the following steps:

[0069] Obtain 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 respectively;

[0070] When the timestamps of the data obtained after calibration are aligned and the coordinates are unified, the mean and variance of the heading angle are calculated;

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

[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 laser radar heading angle calibration system for unmanned container trucks in ports, comprising:

[0074] Data acquisition module, used to obtain lidar point cloud data, inertial navigation data and vehicle mechanical parameters;

[0075] The pre-processing module registers the point cloud data and interpolates and synchronizes the inertial navigation data;

[0076] Calibration execution module, which 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 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, and the memory is used to store a computer program;

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

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

[0081] Compared with the prior art, the technical effects of the present invention are:

[0082] The present invention provides a laser radar heading angle calibration method, system, equipment and storage medium for unmanned container trucks in ports. 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 a heading angle calibration result with higher accuracy and better robustness. The method does not rely on other sensors such as inertial navigation and has adaptability to all working conditions. Through the fusion optimization of multiple calibrations, while improving the safety level of vehicle driving, it effectively avoids problems such as large algorithm errors caused by sensor failure, thereby preventing the occurrence of a series of problems such as vehicle deviation from the driving route, false detection, and forced stop. BRIEF DESCRIPTION OF THE DRAWINGS

[0083] Figure 1 Schematic diagram of the flow of the heading angle calibration method of the present invention;

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

[0085] Figure 3 This is a top view of the layout of the vehicle body stoppers during calibration of the vehicle body stopper heading angle of the present invention.

[0086] In the figure, 1. container, 2. vehicle body block, 3. lidar, 4. AIV vehicle body. DETAILED DESCRIPTION

[0087] The present invention is described in detail below with reference to the accompanying drawings:

[0088] In one embodiment of the present invention, a laser radar heading angle calibration method for unmanned container trucks in ports is disclosed. Figure 1-3 As shown in the figure, 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 a heading angle calibration result with higher accuracy and better robustness than the existing technology.

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

[0090] Obtain input data, including lidar point cloud data, inertial navigation data obtained from inertial navigation solution, and vehicle body mechanical parameters. The inertial navigation data includes the vehicle's real-time position, attitude, and speed. The vehicle body mechanical parameters include the installation position and shape of the vehicle body block, the radar installation position, and the dimensions of the vehicle body steel frame structure.

[0091] Preprocess the input data, including obtaining preliminary external parameters of the lidar through point cloud registration, and interpolating the inertial navigation data to obtain the vehicle position and attitude at the lidar point cloud timestamp;

[0092] Laser inertial navigation heading calibration: Based on S-shaped trajectory point cloud stitching and KNN error optimization, the external 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 the mechanical parameter calibration are obtained;

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

[0095] Dynamic trajectory heading angle calibration: Based on straight line trajectory constraints and point cloud map quality assessment, the external 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 referenced by the vehicle body block, and the dynamic trajectory constraint external parameters are fused to output the optimized external parameter heading angle.

[0097] In this embodiment, the inertial navigation solution obtains the real-time position, attitude, and speed of the vehicle. The process is as follows:

[0098] 1. Posture solution:

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

[0100] (1) Angular increment calculation:

[0101]

[0102] Among them, ω b is the angular velocity of the carrier coordinate system, θ k is 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, It is quaternion multiplication, which is finally converted into attitude matrix

[0109] 2. Position and speed calculation:

[0110] The accelerometer data is used in conjunction with the attitude matrix to perform two integrations to solve the position.

[0111] (1) Navigation system acceleration calculation:

[0112]

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

[0114] (2) Speed integral:

[0115]

[0116] (3) Position integral:

[0117]

[0118] Among them, the subscript b is the carrier coordinate system, and the superscript n is the navigation coordinate system. is the coordinate rotation matrix from the carrier system to the navigation system.

[0119] In this embodiment, point cloud registration is used to obtain the initial external parameters of the laser radar. The process is as follows:

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

[0121] 1. Initialization

[0122] Input source point cloud P = {p i} and 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 , forming corresponding point pairs, and KD tree is often used to accelerate the search.

[0126] (2) Eliminate outliers (optional)

[0127] Remove corresponding point pairs with too large a distance (using threshold filtering) to improve the robustness of the registration.

[0128] (3) Calculate the optimal transformation

[0129] Solve the rotation matrix (R and translation vector t$, so that the mean square error of the corresponding points after transformation is minimized), specifically:

[0130] Calculate the centroid of two point clouds:

[0131] Decentering 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) Apply transformation

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

[0136] (5) Judgment of 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 matrix R and t aligns the transformed source point cloud with the target point cloud.

[0140] In this embodiment, the vehicle position and posture at the timestamp of the lidar point cloud are obtained through interpolation processing. The process is as follows:

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

[0142] 1. Time alignment and data synchronization

[0143] Timestamp matching: Align the timestamp of the lidar point cloud with the timestamp of the inertial navigation output. Since inertial navigation data is usually output at a high frequency (such as 100Hz), it is necessary to find the two inertial navigation pose data adjacent to each point cloud timestamp.

[0144] Linear interpolation method: linear interpolation is performed on adjacent inertial navigation poses based on the time difference ratio. Assuming that the point cloud timestamp is t, and the adjacent inertial navigation timestamps are t1 and t2, the interpolation weight is Compute intermediate poses.

[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 using Euler angles or rotation matrices, convert them 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 at the point cloud timestamp.

[0151] Point cloud dedistortion: Using interpolated poses, each laser point is further compensated for motion distortion based on its time offset (such as the relative time within the radar rotation cycle) and unified into the same coordinate system.

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

[0153] This embodiment specifically analyzes the four calibration methods mentioned above, as follows:

[0154] 1. Laser Inertial Navigation Angle Calibration

[0155] The vehicle to be calibrated executes an S-shaped trajectory, driving slowly in an S-shape for 100-200 meters in a specific scenario to collect lidar point cloud data and INS odometry data. The lidar point cloud data and INS odometry data are read and pre-processed through downsampling and filtering. Based on the constant speed assumption, the INS odometry data is linearly interpolated to obtain the INS odometry data information corresponding to each point cloud.

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

[0157] It is now known that the odometry information of ins is very accurate. If the external parameter estimation is also very accurate, then the positions of all point clouds corresponding to this object O projected back to the ins coordinate system should be infinitely close, and accordingly a very high-definition point cloud map should be obtained. If the vehicle position and posture obtained after interpolation are highly accurate, then when the lidar point cloud at different times is transformed to the corresponding vehicle posture, the accuracy of the point cloud stitching will only depend on the external parameters. By optimizing the point cloud stitching, in order to evaluate the quality of the established map, the global point cloud map is used as the source, and a copy is used as the target. A KDTree spatial index is established for the target, and the Euclidean distance between any point P in the source map and the K points closest to P in the target is calculated to evaluate the quality of the map.

[0158] Define the KNN error loss function:

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

[0160] Here, map quality is the optimization target, namely the quality of the global point cloud map; Text is the optimized state quantity, namely the initial estimated extrinsic parameter; pointcloud and odometry are the prior information for the optimization process, namely the global point cloud map and INS odometry data, respectively. This formula indicates that, given the prior conditions of known point cloud map and odometry data, by maximizing point cloud map quality, the extrinsic parameters can be optimized to obtain the extrinsic parameter results for laser inertial navigation calibration.

[0161] If the map is constructed with excellent quality, the features and structure are clear, and all point clouds corresponding to the same object O should be very close. Theoretically, the loss function should be small. Conversely, if the map structure is unclear, the features are unclear, or even featureless or structureless, the loss function will be very large. Assuming a certain initial estimate, the constructed map already has a certain structure and relatively clear features. In other words, if 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, obtaining high-precision extrinsic parameters for the LiDAR, including the optimized heading angle.

[0162] 2. Mechanical parameter fitting and heading angle calibration

[0163] The calibration of this step takes as input the results of data preprocessing and the design parameters of the radar installation position among the relevant mechanical parameters, and outputs the external parameters after fitting the mechanical parameters.

[0164] Specifically, assuming that the theoretical installation positions of the radars at the four corners of the vehicle body are (x1, y1, z1), (x2, y2, z2), (x3, y3, z3), (x4, y4, z4); now give your estimated installation positions (x1', y1', z1'), (x2', y2', z2'), (x3', y3', z3'), (x4', y4', z4'); based on the theoretical installation positions of the lidars at the four corners of the vehicle body, determine the estimated installation positions. Then, the theoretical position point set P and the estimated position point set Q of the four corners of the vehicle body are expressed as follows:

[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] Calculate the centroid of the theoretical position point set and the estimated position point set. The expression is:

[0169]

[0170] in, is the centroid of the theoretical position point set, is the estimated centroid of the location point set;

[0171] Translate the coordinates of the theoretical position point set and the estimated position point set to the centroid:

[0172]

[0173] Among them, p′ i and q′i Decentralized coordinates;

[0174] 2. Construct the covariance matrix H:

[0175]

[0176] 3. Singular Value Decomposition (SVD)

[0177] Perform singular value decomposition on the covariance matrix H, and the expression is:

[0178] H=U∑V T ;

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

[0180] Extract the rotation matrix R from the singular value decomposition result, the expression is:

[0181] R=V∪ T ;

[0182] If det(R)<0, it is corrected to a rotation matrix, and the expression is:

[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 mechanical parameter fitting external parameters, i.e. homogeneous transformation matrix, to achieve mechanical parameter fitting heading angle calibration of rigid body transformation;

[0191] 6. Calculate the heading angle

[0192] The heading angle is the rotation angle around the vehicle's Z axis, which is extracted from the rotation matrix R and expressed as:

[0193]

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

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

[0196] The heading angle θ is used to quantify the heading angle deviation of the radar installation attitude.

[0197] 3. Calibration of vehicle body block heading angle

[0198] Since AIV is used to transport containers, metal blocks are required on the left and right sides of the vehicle body to limit the position of the containers. Figure 2-3 As shown, there are four body blocks 2 on each side of the AIV body 4, the container 1 is placed in the middle of the body blocks 2, and the lidar 3 is installed at the front and rear ends of the body. Since the body block 2 is installed in a straight line along the edge of the body, the 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 vehicle body block heading angle calibration includes the following steps:

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

[0201] Extract the point cloud cluster of the vehicle body stopper from the lidar point cloud data, and fit the stopper installation axis direction using the RANSAC algorithm;

[0202] Based on the parallel constraint between the axis direction of the block installation and the vehicle heading, a heading angle calibration loss function is constructed to optimize the rotation component of the lidar extrinsic parameter.

[0203] 4. Dynamic trajectory heading angle calibration

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

[0205] The LiDAR point cloud data and inertial navigation data of the vehicle traveling in a straight line are collected to construct dynamic trajectory constraints, constraining the normal velocity component of the vehicle in the global coordinate system to be zero (the velocity in the global coordinate system is along the tangential direction of the trajectory and has no lateral velocity component). When optimizing the local point cloud map with this constraint, the lateral stratification of the point cloud is caused by the heading angle from the vehicle system to the global system.

[0206] A radar odometer is generated based on the vehicle's trajectory. The normal velocity component is set to zero and 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 corresponding objects should have no layering, and the accuracy of point cloud stitching will only depend on the extrinsic parameters, especially the assumed heading angle. That is, by optimizing the point cloud stitching, the heading angle can be calculated.

[0207] To evaluate the quality of the established map, the global point cloud map is used as the source, and a copy is made 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 other than P is calculated.

[0208] Define the KNN error loss function:

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

[0210] Here, map quality is the optimization target, namely the quality of the global point cloud map; Text is the optimized state quantity, namely the initial estimated extrinsic parameter; pointcloud and odometry are the prior information for the optimization process, namely the global point cloud map and INS odometry data, respectively. This formula indicates that, given the prior conditions of known point cloud map and odometry data, by maximizing point cloud map quality, the extrinsic parameters can be optimized to obtain the extrinsic parameter results for dynamic trajectory calibration.

[0211] If the map is constructed with excellent quality, the features and structure are clear, and all point clouds corresponding to the same object O should be very close. Theoretically, the loss function should be small. Conversely, if the map structure is unclear, the features are unclear, or even featureless or structureless, the loss function will be very large. Assuming a certain initial estimate, the constructed map already has a certain structure and relatively clear features. In other words, if 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 to obtain the optimized heading angle of the lidar.

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

[0213] Obtain 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 respectively;

[0214] When the timestamps of the data obtained after calibration are aligned and the coordinates are unified, the mean and variance of the heading angle are calculated;

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

[0216] If the variance is greater than the set threshold, the calibration result is discarded and considered to be abnormal. The result is not 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 parameter is the heading angle after fusion optimization.

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

[0219] Data acquisition module, used to obtain lidar point cloud data, inertial navigation data and vehicle mechanical parameters;

[0220] The pre-processing module registers the point cloud data and interpolates and synchronizes the inertial navigation data;

[0221] Calibration execution module, which 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 above calibration execution module can be configured with the following submodules according to actual needs:

[0224] A laser inertial navigation calibrator module that uses a laser-inertial navigation optimization strategy to obtain the heading angle;

[0225] A heading calibration submodule is used to fit the mechanical parameters of the heading angle using the machining position of the radar installation;

[0226] The vehicle block heading calibration submodule uses the vehicle body's unique structure as a point cloud optimization constraint to calculate the heading angle.

[0227] The dynamic trajectory heading calibration submodule utilizes the characteristics of the driving trajectory, optimizes the point cloud according to the kinematic characteristics, and calculates the heading angle.

[0228] The method and system of the present invention are disclosed above. Compared with the prior art, the present invention has the following advantages:

[0229] 1. When calibrating the vehicle heading angle, the present invention obtains one or more algorithms separately and fuses them to obtain a vehicle heading angle with higher progress and robustness, thereby improving the calibration accuracy of the heading angle;

[0230] 2. The multiple calibration methods of the present invention do not rely on other sensors such as inertial navigation, are applicable to a wide range of scenarios, and the calibration accuracy error is small when other sensors fail;

[0231] 3. The present invention combines multiple calibration methods to obtain high-precision heading angles, 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 stop.

[0232] Another embodiment of the present invention further includes an electronic device, which may include a processor and a memory storing computer program instructions.

[0233] Specifically, in this embodiment, the processor may include a central processing unit (CPU), or a specific integrated circuit, or may be configured as one or more integrated circuits of this embodiment; the memory may include a large-capacity memory for data or instructions, including but not limited to a hard disk drive (HDD), a floppy disk drive, a flash memory, an optical disk, a magneto-optical disk, a magnetic tape, or a 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 specific embodiment, the memory is a non-volatile solid-state memory. In a specific 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 a flash memory, or a combination of two or more of these.

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

[0235] It should also be noted that the electronic device of this embodiment may also include a communication interface and a communication bus. The processor, memory, and communication interface are connected via the communication bus and communicate with each other. The communication interface is primarily used to enable communication between the various units, modules, devices, or equipment in the embodiments of the present invention.

[0236] The communication bus mentioned above includes hardware, software or a combination of both, coupling the components of the online data flow device to each other. Where appropriate, the communication bus may include one or more buses.

[0237] In addition, in combination with the heading angle calibration method in the above embodiment, an embodiment of the present invention can be implemented by providing a computer storage medium, which stores computer program instructions; the computer program instructions are executed by a processor to perform the above heading angle calibration method.

[0238] It should be clear 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 the present invention is implemented in hardware, it may be an electronic circuit, an application-specific integrated circuit, appropriate firmware, a plug-in, a function card, etc. When implemented in software, the elements of the present invention are programs or code segments used to perform the required tasks. The programs or code segments may be stored in a machine-readable medium or uploaded via a data signal carried in a carrier wave over a transmission medium or communication link. "Machine-readable medium" may include any medium capable of storing or transmitting information, such as an electronic circuit, a semiconductor memory device, ROM, flash memory, erasable ROM (EROM), a floppy disk, an optical disk, a hard disk, an optical fiber, a radio frequency link, etc. The code segments may be downloaded via a computer network such as the Internet or an intranet.

[0240] The above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit the scope of the present invention. Other modifications or equivalent substitutions made to the technical solutions of the present invention by ordinary technicians in this field should be included in the scope of the claims of the present invention as long as they do not depart from the spirit and scope of the technical solutions of the present invention.

Claims

1. A laser radar heading angle calibration method for unmanned container trucks in ports, characterized by: 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 output the optimized heading angle. The specific steps include: Obtain input data, including lidar point cloud data, inertial navigation data obtained from inertial navigation solution, and vehicle body mechanical parameters. The inertial navigation data includes the vehicle's real-time position, attitude, and speed. The vehicle body mechanical parameters include the installation position and shape of the vehicle body block, the radar installation position, and the dimensions of the vehicle body steel frame structure. Data preprocessing: obtaining preliminary external parameters of the lidar through point cloud registration, interpolating the inertial navigation data, and obtaining the vehicle position and posture at the timestamp of the lidar point cloud; Laser inertial navigation heading calibration: Based on S-shaped trajectory point cloud stitching and KNN error optimization, the external 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 the mechanical parameter calibration are obtained; Body stopper heading angle calibration: Based on the external parameters calibrated by fitting mechanical parameters and the design parameters of the body stopper installation position and shape, the external parameters with the body stopper as a reference are obtained; Dynamic trajectory heading angle calibration: Based on straight line trajectory constraints and point cloud map quality assessment, the external 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 referenced by the vehicle body block, and the external parameters of dynamic trajectory constraints are fused to output the optimized external parameter heading angle.

2. The laser radar heading angle calibration method for unmanned container trucks in ports according to claim 1 is characterized by: Laser inertial navigation heading calibration includes the following steps: The vehicle to be calibrated executes an S-shaped trajectory to collect lidar point cloud data and INS odometry data; Read lidar point cloud data and INS odometry data, and pre-process them through downsampling and filtering; Based on the constant speed assumption, the INS odometry data is linearly interpolated to obtain the INS odometry data information corresponding to each point cloud; Based on the initial extrinsic parameter estimation Text, the point cloud of the same object scanned by the lidar at each moment is transformed into the inertial navigation initial coordinate system {ins0} to generate a global point cloud map with multiple frames superimposed; Take the global point cloud map as the source and copy it 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 except P. Define the KNN error loss function: max map quality=f(T ext |PointCloud,odometry); Among them, map quality is the object of optimization, that is, the quality of the global point cloud map, Text is the optimized state quantity, that is, the initial estimated external parameter, pointcloud and odometry are the prior information of the optimization process, namely the global point cloud map and INS odometry data information; When the loss function is less than the set threshold, the BOBYQA local optimization algorithm is used to optimize the external parameters of the global point cloud map to obtain the optimized external parameter heading angle of the lidar.

3. The laser radar heading angle calibration method for unmanned container trucks in ports according to claim 1 is characterized by: Mechanical parameter fitting and heading angle calibration include the following steps: The estimated installation position is determined based on the theoretical installation position of the lidar at the four corners of the vehicle body. The theoretical position point set P and the estimated position point set Q of the four corners of the vehicle body are expressed as: P = {pi} = {(xi,yi,zi)}, i = 1, 2, 3, 4; Q={qi}={(xi′,yi′,zi′)},i=1,2,3,4; Calculate the centroid of the theoretical position point set and the estimated position point set. The expression is: in, is the centroid of the theoretical position point set, is the estimated centroid of the location point set; Translate the coordinates of the theoretical position point set and the estimated position point set to the centroid: Among them, p′ i and q′ i Decentralized coordinates; Construct the covariance matrix H: Perform singular value decomposition on the covariance matrix H, and the expression is: H=U∑V T ; Among them, U and V are orthogonal matrices, and S is a diagonal matrix; Extract the rotation matrix R from the singular value decomposition result, the expression is: R=VU T ; If det(R)<0, it is corrected to a rotation matrix, and the expression is: R=V·diag(1,1,-1)·U T ; Calculate the translation vector t: Combine the rotation matrix R and the translation vector t into a homogeneous transformation matrix T: Output mechanical parameter fitting external parameters, that is, homogeneous transformation matrix, to achieve mechanical parameter fitting heading angle calibration of rigid body transformation; the heading angle is the rotation angle around the vehicle Z axis, which is extracted from the rotation matrix R, and the expression is: θ=arctan2(R 21 ,R 11 ); Among them, the matrix elements R11 = cosθ, R21 = sinθ; The heading angle θ is used to quantify the heading angle deviation of the radar installation attitude.

4. The laser radar heading angle calibration method for unmanned container trucks in ports according to claim 1 is characterized by: The vehicle body block heading angle calibration includes the following steps: Obtain the design parameters of the installation position and shape of the vehicle body stoppers, as well as the LiDAR point cloud. The stoppers are arranged in a straight line along the edge of the vehicle body and parallel to the vehicle heading. Extract the point cloud cluster of the vehicle body stopper from the lidar point cloud data, and fit the stopper installation axis direction using the RANSAC algorithm; Based on the parallel constraint between the axis direction of the block installation and the vehicle heading, a heading angle calibration loss function is constructed to optimize the rotation component of the lidar extrinsic parameter.

5. The laser radar heading angle calibration method for unmanned container trucks in ports according to claim 1 is characterized by: 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; Generate a radar odometer based on the vehicle's trajectory, set the normal velocity component to zero, and then superimpose multiple frames of point clouds to generate a local point cloud map; Take the global point cloud map as the source and copy it as the target, build 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 except P; Define the KNN error loss function: max map quality=f(T ext |PointCloud,odometry); Among them, map quality is the object of optimization, that is, the quality of the global point cloud map, Text is the optimized state quantity, that is, the initial estimated external parameter, pointcloud and odometry are the prior information of the optimization process, namely the global point cloud map and INS odometry data information; When the loss function is less than the set threshold, the BOBYQA local optimization algorithm is used to optimize the external parameters of the global point cloud map to obtain the optimized external parameter heading angle of the lidar.

6. The laser radar heading angle calibration method for unmanned container trucks in ports according to claim 1 is characterized by: The fusion of multiple heading results is to output the optimized extrinsic heading angle based on variance screening and mean calculation, including the following steps: Obtain 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 respectively; When the timestamps of the data obtained after calibration are aligned and the coordinates are unified, the mean and variance of the heading angle are calculated; If the variance is less than the set threshold, the optimized extrinsic heading angle result is the mean of the calibrated heading angles; 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.

7. A laser radar heading angle calibration system for unmanned container trucks in ports according to the method according to any one of claims 1 to 6, characterized in that: include: Data acquisition module, used to obtain lidar point cloud data, inertial navigation data and vehicle mechanical parameters; The pre-processing module registers the point cloud data and interpolates and synchronizes the inertial navigation data; Calibration execution module, which 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.

8. 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 execute the method according to any one of claims 1 to 6 by running the computer program stored in the memory.

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

Images