Dual-LiDAR-IMU combined SLAM positioning method for semi-trailer

Abstract

The invention discloses a dual-LiDAR-IMU combined SLAM positioning method for a semi-trailer, and belongs to the technical field of unmanned driving and multi-sensor fusion. The method comprises the following steps: establishing a semi-trailer kinematic model, respectively constructing LiDAR-IMU SLAM subsystems for a tractor and a trailer, and outputting respective key frames; on the basis, the size of a hinge angle is calculated in real time according to a hinge kinematics relation; based on the key frame pose sequence and the hinge angle sequence of the tractor and the trailer, a factor graph is constructed, joint optimization is executed by calculating a joint optimization objective function, and the optimal estimation of the pose of the tractor and the trailer is solved by adopting a nonlinear least square iteration method. And outputting the optimized tractor position and posture sequence, the optimized trailer position and posture sequence and a positioning result that the hinge joint of the semi-trailer is consistent. Compared with an existing method for independently positioning a single vehicle, the method can explicitly describe the relative kinematics relation between the tractor and the trailer, and the positioning precision and stability of the whole vehicle are improved.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving technology, specifically to a dual LiDAR-IMU joint SLAM localization method for semi-trailer vehicles. Background Technology

[0002] With the development of intelligent driving and unmanned transportation, LiDAR-IMU SLAM (LiDAR-IMU SLAM) is widely used for autonomous vehicle localization. Existing methods typically obtain geometric constraints through scan-to-map and combine them with motion constraints such as IMU pre-integration for fusion optimization, thereby outputting the vehicle pose and map.

[0003] However, semi-trailer vehicles consist of a tractor and trailer connected by an articulated mechanism, making them articulated multi-body systems. If the traditional single-vehicle modeling approach of SLAM is still used, inconsistencies in the poses of the two vehicles and the amplification of drift accumulation can easily occur, making it difficult to obtain stable and reliable whole-vehicle positioning results. Therefore, a semi-trailer vehicle fusion positioning method with articulated structural constraints is needed to ensure positioning accuracy and pose consistency. Summary of the Invention

[0004] To address the aforementioned problems, this invention provides a dual LiDAR-IMU joint SLAM localization method for semi-trailer vehicles.

[0005] To achieve the above objectives, the technical solution adopted by this invention is as follows: a dual LiDAR-IMU joint SLAM positioning method for semi-trailer vehicles, which achieves joint consistency optimization of the tractor and trailer poses through articulated kinematic constraints, thereby improving the overall vehicle positioning accuracy and stability. The specific steps are as follows: S1. Establish a kinematic model of the semi-trailer vehicle in a two-dimensional Cartesian coordinate system. Construct LiDAR-IMU SLAM subsystems for the tractor and trailer respectively, and output the key frame pose sequences of the tractor and trailer respectively. S1.1 Based on three constraints, construct the velocity relationship between the tractor and the trailer and establish a kinematic model of the semi-trailer; The three constraints specifically include: (1) The movement of the vehicle is confined to a two-dimensional plane.

[0006] (2) The tractor and trailer are rigid bodies connected by an articulation point; The hinge point satisfies the positional consistency constraint in the world coordinate system.

[0007] (3) The hinge mechanism is a single-degree-of-freedom rotational joint.

[0008] Based on the above three constraints, the overall motion of the semi-trailer is a multi-body system consisting of two planar rigid bodies coupled by a hinge relationship.

[0009] Define parameters: The reference point for the tractor is the center of the lidar on the preceding vehicle. The reference point for the trailer is the center of the LiDAR on the rear vehicle. Hinged point; Hinged point The positional consistency constraint is satisfied in the world coordinate system, as expressed below: Meanwhile, the motion of the tractor and trailer in the plane can be expressed by the following velocity-related expressions: in, and These are the velocity components on the X and Y axes of the tractor reference point, respectively. and These are the velocity components on the X and Y axes of the trailer reference point, respectively. Let be the coordinates of the hinge point, where . Let x be the x-coordinate of the hinge point. The ordinate of the hinge point; Here are the coordinates of the tractor's reference point, where, Let x be the x-coordinate of the tractor reference point. The ordinate of the tractor reference point; Here are the coordinates of the trailer reference point, where, Let x be the x-coordinate of the trailer reference point. The ordinate of the trailer reference point. The longitudinal speed of the tractor. The longitudinal speed of the trailer. The yaw rate of the tractor unit. This refers to the trailer's yaw rate. The distance from the preset tractor reference point to the articulation point, The distance from the preset trailer reference point to the articulation point, and These are the heading angles of the tractor and trailer, respectively.

[0010] The above model is used to describe the kinematic relationship of a semi-trailer vehicle consisting of two planar rigid bodies coupled by a hinge joint.

[0011] S1.2 Based on the kinematic model of the semi-trailer vehicle, a LiDAR and an IMU are deployed on the tractor and trailer respectively, and a LiDAR-IMU subsystem for the tractor and a LiDAR-IMU subsystem for the trailer are constructed. The tractor subsystem and the trailer subsystem independently collect LiDAR point cloud data and IMU data and output the key frame pose sequences of the tractor and trailer respectively. S1.2.1 Based on the tractor-trailer LiDAR-IMU subsystem and trailer LiDAR-IMU subsystem, time alignment is performed on the LiDAR and IMU sensors, the IMU data is resampled to the LiDAR time axis and a unified time axis is established, and the synchronized LiDAR point cloud data and IMU data are output. S1.2.2 Based on the synchronized LiDAR point cloud data and IMU data, the IMU data is pre-integrated within the scanning time interval of two adjacent LiDAR point cloud frames to calculate the motion increment during the LiDAR point cloud data scanning time. Each point cloud in the LiDAR point cloud is compensated by timestamp interpolation to unify the entire frame point cloud to the same reference time, thus obtaining the distortion-free point cloud.

[0012] S1.2.3 Extract geometric features from the distortion-free point cloud of the current frame, establish a local map, match each feature point in the current frame with the features of the local map, construct laser geometric constraints by calculating the point-to-line residual and the point-to-surface residual, and solve the pose of the point cloud in the current frame.

[0013] The geometric features are edge feature points and planar feature points; S1.2.4. Perform nonlinear least squares optimization on the laser geometric constraints and IMU pre-integration constraints to construct the internal objective function and output the keyframe pose of the tractor. , Keyframe pose of trailer , It updates the local maps of the tractor and trailer, and outputs the keyframe pose sequences of the tractor and trailer.

[0014] The IMU pre-integration constraint is to pre-integrate the IMU data within the time interval of two adjacent LiDAR point cloud scanning frames. The expression for the internal objective function is: in, For keyframe pose, For laser geometric constraints, the values ​​are calculated using the formula (point to surface / point to line). For IMU pre-integration constraints, by The IMU pre-integration increment within the interval is consistent with the prediction of adjacent states.

[0015] The internal objective function is iteratively solved to obtain the optimal estimate for the frame, and keyframes are automatically determined based on factors such as pose change magnitude, time interval, or number of feature constraints. When a keyframe is determined, its pose is saved and used to update the local map. The final output is the keyframe pose of the tractor. Keyframe pose of trailer And update their respective local maps simultaneously.

[0016] S2. Based on IMU data, after aligning the timestamps of the tractor and trailer IMU data, the zero bias is estimated and corrected during the initialization phase. The angular velocity difference is then calculated, and the angular velocity difference is integrated within a preset length sliding window to update the articulation angle. For IMU data alignment and zero bias correction, the tractor IMU and trailer IMU respectively output yaw rate measurements. and Preferably, the two IMU data streams are aligned to a unified time axis; when the two timestamps are inconsistent, linear interpolation is performed on the angular velocity of one of the streams to obtain the same time. The angular velocity value.

[0017] During the system initialization phase, keep the vehicle stationary. The zero bias of the yaw rate is estimated at each sampling point, and the expression is as follows: , In the formula, For sampling point index, For the first Each sampling time, This is the zero-bias estimate of the yaw rate of the tractor's IMU. This is the zero-bias estimate of the yaw rate of the trailer IMU; Correct the angular velocity: , In the formula, The corrected yaw rate of the tractor IMU. The corrected yaw rate of the semi-trailer IMU; Calculate the difference in angular velocity : To suppress the cumulative drift caused by long-term integration, this invention uses a length of The integration is performed using a sliding window of sampling points, and trapezoidal integrals are preferably used to calculate the hinge angle increment within the window, as shown in the following expression: in, This is the discrete-time index for the current moment. Here, W is the sampling point index, and W is the preset sliding window length. For the first Each sampling time, , No. The estimated hinge angle at time 1. No. The difference in vehicle angular velocity at each sampling point This indicates that the angle is normalized to This is to avoid discontinuities caused by crossing angle boundaries.

[0018] S3. Based on the keyframe pose sequences and articulation angle sequences of the tractor and trailer, a factor graph is constructed. Joint optimization is performed by calculating the joint optimization objective function. The optimal estimate of the pose of the tractor and trailer is solved by the nonlinear least squares iterative method. The optimized tractor pose sequence, the optimized trailer pose sequence, and the articulated positioning result of the semi-trailer are output, thus completing the dual LiDAR-IMU joint SLAM positioning method for the semi-trailer.

[0019] S3.1 Based on the key frame pose sequence and articulation angle sequence of the tractor and trailer, by taking the key frame time of the tractor as the reference, aligning the key frame and articulation angle data of the trailer and setting the time tolerance interval as a gating, the aligned and effective tractor key frame pose factor, trailer key frame pose factor and articulation angle constraint factor are obtained. This invention sets a time tolerance interval Preferred selection Alignment is considered valid and factor graph fusion is performed if the following conditions are met: In the formula, The keyframe timestamp of the tractor unit at time k. To and Corresponding keyframe timestamps for the semi-trailer used for fusion. and The corresponding timestamps of the hinge angle observations used for fusion.

[0020] S3.2 Construct a factor map based on the tractor keyframe pose factors, trailer keyframe pose factors, and articulation angle constraint factors. Perform joint optimization by calculating the joint optimization objective function. Use the nonlinear least squares iterative method to solve for the optimal estimates of the tractor and trailer poses. Output the optimized tractor pose sequence, the optimized trailer pose sequence, and the articulated positioning results of the semi-trailer vehicle. Complete the dual LiDAR-IMU joint SLAM positioning method for semi-trailer vehicles. Under the condition of satisfying the tolerance range, at the key frame time The above three factors are introduced into the fusion optimization process, a factor graph is constructed and jointly solved, and the joint optimization objective function is as follows: in: , which is the prior residual of the pose factor of the key frame of the tractor vehicle; , which is the prior residual of the trailer keyframe pose factor; , representing the articulation angle residual, is used to characterize the kinematic consistency of the articulation between the two vehicles. The prior residual weighting coefficients for the pose factors of the tractor in keyframes. The prior residual weighting coefficients for the pose factors of the trailer keyframes. This is the weighting coefficient for the hinge angle residual.

[0021] Compared with the prior art, the present invention provides a method with the following beneficial effects: (1) Based on the separate construction of LiDAR–IMU SLAM subsystems for the tractor and trailer, this invention introduces articulated kinematic consistency constraints for the semi-trailer, thereby elevating the pose estimation of the two vehicles from "independent" to "structurally coupled" joint estimation. By explicitly adding articulated constraint factors to the factor graph, the relative heading relationship between the tractor and trailer is continuously controlled during the optimization process, which can effectively suppress the pose divergence and inconsistency of the two vehicles and ensure the consistency of the overall vehicle structure and the continuity of the trajectory.

[0022] (2) This invention integrates the keyframe factors of the tractor and trailer and the articulation constraint factors into a factor graph nonlinear least squares framework for joint optimization, and uses a sliding window and marginalization strategy to achieve online solution, thus fully integrating multi-source information while ensuring real-time performance. Compared with the traditional scheme that only positions a single vehicle independently, this invention can maintain higher positioning stability and consistency, thereby improving the positioning accuracy and robustness of the whole vehicle. Attached Figure Description

[0023] Figure 1 This is a flowchart of the steps of a dual LiDAR-IMU joint SLAM localization method for semi-trailer vehicles according to the present invention. Figure 2This is a flowchart of a dual LiDAR-IMU joint SLAM localization method for semi-trailer vehicles according to the present invention. Figure 3 This is a schematic diagram of the kinematic model of the semi-trailer vehicle of the present invention; Figure 4 This is a real-world image of the experimental vehicle used in this invention. Figure 5 This is a superimposed image of the motion trajectory of the method of the present invention and single-vehicle SLAM compared with the actual GPS trajectory; Figure 6 This is a superimposed graph of absolute trajectory error curves comparing the method of this invention and single-vehicle SLAM with the actual GPS trajectory. Figure 7 This is a graph of the cumulative error distribution function of the present invention. Detailed Implementation

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

[0025] like Figure 1 and Figure 2 As shown, a dual LiDAR-IMU joint SLAM localization method for semi-trailer vehicles includes the following steps: S1. Establish a kinematic model of the semi-trailer vehicle in a two-dimensional Cartesian coordinate system. Construct LiDAR-IMU SLAM subsystems for the tractor and trailer respectively, and output the key frame pose sequences of the tractor and trailer respectively. S1.1 Based on three constraints, construct the velocity relationship expression between the tractor and the trailer and establish a kinematic model of the semi-trailer; The three constraints specifically include: (1) The vehicle motion is confined to a two-dimensional plane, and the effects of pitch and roll on the kinematic relationship are ignored; (2) The tractor and trailer are rigid bodies connected by an articulation point, and their internal structures do not deform; (3) The hinge mechanism is a single-degree-of-freedom rotary joint, which only allows relative rotation about the vertical axis.

[0026] Based on the above three constraints, the overall motion of the semi-trailer is a multi-body system consisting of two planar rigid bodies coupled by a hinge relationship.

[0027] Define parameters: The reference point for the tractor is the center of the lidar on the preceding vehicle. The reference point for the trailer is the center of the LiDAR on the rear vehicle. Hinged point; Hinged point The positional consistency constraint is satisfied in the world coordinate system, as expressed below: Meanwhile, the motion of the tractor and trailer in the plane can be expressed by the following velocity-related expressions: in, and These are the velocity components on the X and Y axes of the tractor reference point, respectively. and These are the velocity components on the X and Y axes of the trailer reference point, respectively. Let be the coordinates of the hinge point, where Let x be the x-coordinate of the hinge point. The ordinate of the hinge point; Here are the coordinates of the tractor's reference point, where Let x be the x-coordinate of the tractor reference point. The ordinate of the tractor reference point; Here are the coordinates of the trailer reference point, where Let x be the x-coordinate of the trailer reference point. The ordinate of the trailer reference point. The longitudinal speed of the tractor. The longitudinal speed of the trailer. The yaw rate of the tractor unit. This refers to the trailer's yaw rate. The distance from the preset tractor reference point to the articulation point, In this embodiment, the distance from the preset trailer reference point to the articulation point is used. =0.73m, the center point of the front lidar (IMU) is the reference point for the tractor. =4.825m, the center point of the rear lidar (IMU) is the reference point for the trailer; and These are the heading angles of the tractor and trailer, respectively.

[0028] The above model is used to describe the kinematic relationship of a semi-trailer vehicle consisting of two planar rigid bodies coupled by a hinge joint.

[0029] S1.2 Based on the kinematic model of the semi-trailer vehicle, a LiDAR and an IMU are deployed on the tractor and trailer respectively, forming a tractor LiDAR-IMU subsystem and a trailer LiDAR-IMU subsystem. The two subsystems independently collect LiDAR point cloud data and IMU data and output key frame pose sequences of the tractor and trailer.

[0030] S1.2.1. Time alignment is performed on the LiDAR and IMU sensors, and the IMU data is resampled to the LiDAR time axis and a unified time axis is established. The tractor unit and trailer unit respectively acquire LiDAR point cloud sequences and IMU (angular velocity, acceleration) sequences. Due to the different frequencies and timestamp discrepancies between the two types of sensors, interpolation is used to resample the IMU data onto the LiDAR point cloud timeline, and a unified timeline is established. Based on this, the keyframe timestamps of the tractor and trailer subsystems are aligned to ensure that they are at the same keyframe time. The keyframes of the tractor and trailer are then obtained and output.

[0031] S1.2.2 Based on the synchronized LiDAR point cloud data and IMU data, within the scanning time interval of two adjacent LiDAR point cloud frames, the IMU outputs angular velocity and acceleration at a high frequency. This invention pre-integrates the IMU data within the scanning time interval to obtain the relative pose increment used to describe the motion within the scanning period. Furthermore, considering the point-by-point sampling characteristics of LiDAR scanning, motion compensation is performed on the point cloud based on the timestamps of each point, uniformly transforming the entire frame of point cloud to the same reference time, thereby obtaining a distortion-free point cloud.

[0032] S1.2.3 Extract geometric features from the distortion-free point cloud of the current frame, establish a local map, match each feature point in the current frame with the features of the local map, construct laser geometric constraints by calculating the point-to-line residual and the point-to-surface residual, solve the pose of the point cloud of the current frame, align the current frame with the local map, and output the pose estimate of the current frame relative to the local map. Geometric features are extracted from the distortion-free point cloud, with edge feature point sets and planar feature point sets being preferred. A local map is then built. In the scan-to-map matching stage between the current frame and the local map features, each feature point in the current frame is associated with the local map features. The expressions for the geometric feature extraction edge feature point set and the planar feature point set are as follows: Point-to-line residual (Edge feature points): in, For the two endpoints of the line segment that matches the edge point in the local map, For the feature points of the current frame, For the current pose transformation; Point-to-surface residual (planar feature points): in, and These are the local plane normal and a point on the plane, respectively. By minimizing the point-to-line residual and the point-to-area residual, the pose estimate of the current frame relative to the local map is obtained, thus aligning the current frame with the local map.

[0033] S1.2.4. Nonlinear optimization, keyframe determination, and local map update incorporating IMU pre-integration constraints. Laser geometric constraints and IMU pre-integration constraints are unified into a nonlinear least squares framework, and the expression for the system's internal objective function is constructed as follows: in, For keyframe pose, The laser geometry constraint is given by the equation (point to surface / point to line). For IMU pre-integration constraints, by The IMU pre-integration increment within the interval is consistent with the predictions of adjacent states. The objective function is iteratively solved to obtain the optimal estimate for the frame, and keyframes are determined based on factors such as pose change magnitude, time interval, or number of feature constraints. When a keyframe is determined, its pose is saved and used to update the local map. The final output is the tractor keyframe pose. Keyframe pose of trailer And update their respective local maps simultaneously.

[0034] S2. Based on IMU data, after aligning the timestamps of the tractor and trailer IMU data, the zero bias is estimated and corrected during the initialization phase. Then, the angular velocity difference is calculated, and the angular velocity difference is integrated within a preset length sliding window to update the hinge angle and obtain the hinge angle sequence. For IMU data alignment and zero bias correction, the tractor IMU and trailer IMU respectively output yaw rate measurements. and Preferably, the two IMU data streams are aligned to a unified time axis; when the two timestamps are inconsistent, linear interpolation is performed on the angular velocity of one of the streams to obtain the same time. angular velocity value, This is the discrete-time index for the current moment.

[0035] During the system initialization phase, keep the vehicle stationary. The zero bias of the yaw rate is estimated at each sampling point, and the expression is as follows: , In the formula, For sampling point index, For the first Each sampling time, This is the zero-bias estimate of the yaw rate of the tractor's IMU. This is the zero-bias estimate of the yaw rate of the trailer IMU; Correct the angular velocity: , In the formula, The corrected yaw rate of the tractor IMU. The corrected yaw rate of the semi-trailer IMU; Calculate the difference in angular velocity : To suppress the cumulative drift caused by long-term integration, this invention uses a length of The integration is performed using a sliding window of sampling points, and trapezoidal integrals are preferably used to calculate the hinge angle increment within the window, as shown in the following expression: in, This is the discrete-time index for the current moment. Here, W is the sampling point index, and W is the preset sliding window length. For the first Each sampling time, , No. The estimated hinge angle at time 1. No. The difference in vehicle angular velocity at each sampling point This indicates that the angle is normalized to This is to avoid discontinuities caused by crossing angle boundaries.

[0036] S3. Based on the keyframe pose sequences and articulation angle sequences of the tractor and trailer, a factor graph is constructed. Joint optimization is performed by calculating the joint optimization objective function. The optimal estimate of the pose of the tractor and trailer is solved by the nonlinear least squares iterative method. The optimized tractor pose sequence, the optimized trailer pose sequence, and the articulated positioning result of the semi-trailer are output, thus completing the dual LiDAR-IMU joint SLAM positioning method for the semi-trailer.

[0037] S3.1 Based on the key frame pose sequence and articulation angle sequence of the tractor and trailer, by taking the key frame time of the tractor as the reference, aligning the key frame and articulation angle data of the trailer and setting the time tolerance interval as a gating, the aligned and effective tractor key frame pose factor, trailer key frame pose factor and articulation angle constraint factor are obtained. After completing the keyframe pose output and articulation angle of the two LiDAR-IMU SLAM subsystems for the tractor and trailer, After continuous estimation, the present invention performs time alignment and gating on the three types of information in the final fusion module, and constructs hinge angle constraint factors under the factor graph framework to achieve joint consistency optimization of the tractor and trailer poses.

[0038] The tractor and trailer subsystems output keyframe pose sequences respectively. , The keyframe pose output frequency is preferably 5 Hz; the hinge angle is obtained by integrating the angular velocity difference between the two IMUs, and the output frequency is preferably 200 Hz, forming a hinge angle sequence. Because the timestamps and frequencies of the three types of data are inconsistent, alignment is required to ensure consistency of constraints at the same time.

[0039] Preferred key frame moments of the tractor This serves as the fusion reference time. For the tractor's keyframe pose at this time, the timestamp is retrieved from both the trailer's keyframe pose sequence and the hinge angle sequence. The corresponding trailer keyframe pose and hinge angle are obtained by comparing with the closest data.

[0040] To avoid erroneous constraints caused by time inconsistencies, this invention sets a time tolerance interval. Preferred selection Alignment is considered valid and factor graph fusion is performed if the following conditions are met: In the formula, The keyframe timestamp of the tractor unit at time k. To and Corresponding keyframe timestamps for the semi-trailer used for fusion. and The corresponding timestamps of the hinge angle observations used for fusion.

[0041] If any condition is not met, the time span of the three is considered too large. In this keyframe, no articulation constraint factor is introduced, and cross-vehicle fusion is not performed. Only the tractor and trailer subsystems output their respective positioning results independently. Conversely, when the alignment is effective, the tractor keyframe pose, trailer keyframe pose, and articulation angle measurement are used as data at the same time to construct the articulation constraint factor and perform joint optimization.

[0042] Tractor Key Pose Frame Factor: Used to characterize the observation consistency and motion continuity of the tractor in this keyframe, it originates from the scan-to-map constraints and IMU constraints within the tractor's LiDAR–IMU SLAM subsystem. This factor ensures that the optimized tractor pose remains consistent with the output of the tractor subsystem.

[0043] Trailer keyframe pose factor: This factor characterizes the observation consistency and motion continuity of the trailer within the current keyframe and is derived from the scan-to-map constraints and IMU constraints within the trailer's LiDAR–IMU SLAM subsystem. This factor ensures that the optimized trailer pose remains consistent with the output of the trailer subsystem.

[0044] Articulation angle constraint factor: Used to reflect the kinematic consistency of the articulation between the tractor and trailer in a semi-trailer vehicle. This factor is estimated from the articulation angle. It provides measurement inputs to constrain the relative heading relationship between the tractor and trailer. Its core function is to supplement the constraint by utilizing the structural relationship of the vehicles themselves when the external environment does not provide sufficient heading constraints, thereby suppressing heading drift and inconsistency in the positions of the two vehicles.

[0045] S3.2 Construct a factor map based on the tractor keyframe pose factors, trailer keyframe pose factors, and articulation angle constraint factors. Perform joint optimization by calculating the joint optimization objective function. Use the nonlinear least squares iterative method to solve for the optimal estimates of the tractor and trailer poses. Output the optimized tractor pose sequence, the optimized trailer pose sequence, and the articulated positioning results of the semi-trailer vehicle. Complete the dual LiDAR-IMU joint SLAM positioning method for semi-trailer vehicles. Under the condition of satisfying the tolerance range, at the key frame time The above three factors are introduced into the fusion optimization process, a factor graph is constructed and jointly solved, and the joint optimization objective function is as follows: in: , which is the prior residual of the pose factor of the key frame of the tractor vehicle; , which is the prior residual of the trailer keyframe pose factor; , representing the articulation angle residual, is used to characterize the kinematic consistency of the articulation between the two vehicles. The prior residual weighting coefficients for the pose factors of the tractor in keyframes. The prior residual weighting coefficients for the pose factors of the trailer keyframes. The weighting coefficients for the hinge angle residuals are used to reflect the reliability of the three types of data. In this embodiment, the initial weighting coefficients are set to... , , The weighting coefficients are updated based on the joint optimization objective function.

[0046] After ensuring alignment and constructing a joint objective function, this invention employs a nonlinear least squares iterative method to solve for the optimal estimation of the tractor and trailer poses, specifically including the following steps: S3.2.1 When the tractor and trailer subsystem generates a new keyframe At that time, initial keyframe values ​​were obtained from the LiDAR-IMU SLAM subsystems of the tractor and trailer respectively: , The hinge angle at that moment is calculated based on S2. ; S3.2.2 After ensuring alignment and constructing the objective function, this invention employs a nonlinear least squares iterative method to solve for the optimal estimate of the tractor and trailer poses. Let the variable X to be optimized in this case be: S3.2.3, The residuals of the tractor keyframe factor, trailer keyframe factor, and articulation angle constraint factor are uniformly denoted as: The objective function can then be written in the form of the sum of squared residuals: Use the keyframe poses output by the tractor and trailer subsystems as initial values ​​for optimization: Hinge As the observation input for the hinge factor; in the current iteration value Perform a first-order Taylor expansion on the residuals in the vicinity: in, For the incremental demand, For the overall Jacobian matrix.

[0047] The linearized least squares problem is transformed into normal equations: in, , S3.2.3. Solve using a nonlinear least squares iterative method, and update the variables after obtaining the increment: in" "" indicates an incremental update of the pose variable.

[0048] The iteration stops and the result is output when any of the following conditions are met: (1) Less than the preset threshold; (2) The decrease in the objective function is less than the preset threshold; (3) Reach the maximum number of iterations (preferably 1 to 5).

[0049] The final output is the optimized tractor and trailer positions. , .

[0050] To verify the effectiveness of the proposed dual LiDAR-IMU joint SLAM localization method for semi-trailer vehicles, a real-vehicle experiment was conducted and compared with the traditional single-vehicle localization method. The experimental vehicle used a semi-trailer equipped with a LiDAR and an inertial measurement unit (IMU), with GPS installed only on the tractor. The localization errors of the single-vehicle localization algorithm, the fusion localization algorithm, and the actual GPS trajectory were compared. This embodiment uses seven indicators—root mean square error, average error, median error, standard deviation of error, minimum / maximum error, and squared error—to comprehensively evaluate the localization results. The experiment compared the proposed fusion localization algorithm with the single-vehicle localization algorithm; the relevant performance indicators are shown in Table 1. Table 1 Experimental performance indicators Table 1 shows the performance comparison results of the two algorithms in the same test scenario. The fusion algorithm of the present invention is significantly better than the single vehicle positioning algorithm in most key indicators.

[0051] Figure 4 The image shows the experimental vehicle for the algorithm of this invention. Figure 3 The kinematic modeling shown is consistent with the modeling. Figure 5 The image shown is a superimposed diagram of the actual motion trajectories in this experiment; Figure 6The APE curves of different localization algorithms are superimposed on the entire experimental trajectory. Each curve represents the localization error of different algorithms at the same time point. The superimposed graph shows that the error curve of the single-vehicle algorithm fluctuates significantly throughout the trajectory, especially in complex turns or rapidly changing environments, where the localization error is more pronounced and large error peaks appear. In contrast, the error curve of the fusion localization algorithm is significantly more stable, with the localization error remaining at a lower level and the error peaks relatively smaller, demonstrating its higher stability and robustness.

[0052] like Figure 7 As shown, the fusion positioning algorithm of this invention has a higher proportion of positioning results within a lower error range. Compared with the single-vehicle positioning algorithm, the fusion positioning algorithm significantly improves positioning accuracy within a smaller error range. Especially within the error range of less than 0.5 meters, the positioning results of the fusion algorithm are significantly better than those of the single-vehicle positioning algorithm, indicating that the fusion algorithm can more stably approximate the true location. Figure 7 In the analysis, the curve of the fusion algorithm increases rapidly within the lower error range, indicating that most positioning errors are concentrated in a small area, while the single-vehicle algorithm shows a more dispersed performance across a larger error range. This further demonstrates the advantage of the fusion algorithm in improving accuracy. By comparing the trajectory errors of the single-vehicle positioning algorithm, the fusion positioning algorithm, and the GPS true values, the following key conclusions are drawn: Based on experimental results, the dual LiDAR-IMU joint SLAM positioning method of this invention significantly improves the positioning accuracy of the tractor in different environments by combining GPS data and LiDAR-IMU data. Compared with the traditional single-vehicle positioning algorithm, the fusion algorithm reduces the root mean square error by about 10% and exhibits higher accuracy and stability in terms of error distribution and trajectory error fluctuation.

[0053] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A dual LiDAR-IMU joint SLAM localization method for semi-trailer vehicles, characterized in that, Includes the following steps: S1. Establish a kinematic model of the semi-trailer vehicle in a two-dimensional Cartesian coordinate system. By deploying LiDAR and IMU on the tractor and trailer respectively, collect LiDAR point cloud data and IMU data, and construct LiDAR-IMU SLAM subsystems for the tractor and trailer respectively, and output the key frame pose sequences of the tractor and trailer. S2. Based on IMU data, after aligning the timestamps of the tractor and trailer IMU data, the zero bias is estimated and corrected during the initialization phase. The angular velocity difference is then calculated, and the angular velocity difference is integrated within a preset length sliding window to update the articulation angle. S3. Based on the keyframe pose sequences and articulation angle sequences of the tractor and trailer, a factor graph is constructed. Joint optimization is performed by calculating the joint optimization objective function. The optimal estimation of the pose of the tractor and trailer is solved by the nonlinear least squares iterative method. The optimized tractor pose sequence, the optimized trailer pose sequence, and the articulated positioning results of the semi-trailer are output, thus completing the dual LiDAR-IMU joint SLAM positioning method for the semi-trailer. The factor graph includes three factors: tractor keyframe pose factor, trailer keyframe pose factor, and hinge angle constraint factor.

2. The dual LiDAR-IMU joint SLAM localization method for semi-trailer vehicles according to claim 1, characterized in that, S1 specifically includes the following steps: S1.1 Based on three constraints, construct the velocity relationship between the tractor and the trailer and establish a kinematic model of the semi-trailer; The expression for the velocity relationship between the tractor and the trailer is: ； ； In the formula, and These are the velocity components on the X and Y axes of the tractor reference point, respectively. and These are the velocity components on the X and Y axes of the trailer reference point, respectively. The yaw rate of the tractor unit. Yaw rate of trailer The longitudinal speed of the tractor. The longitudinal speed of the trailer; S1.2 Based on the kinematic model of the semi-trailer vehicle, a LiDAR and an IMU are deployed on the tractor and trailer respectively, and a LiDAR-IMU subsystem for the tractor and a LiDAR-IMU subsystem for the trailer are constructed. The tractor subsystem and the trailer subsystem independently collect LiDAR point cloud data and IMU data and output key frame pose sequences of the tractor and trailer respectively.

3. The dual LiDAR-IMU joint SLAM localization method for semi-trailer vehicles according to claim 2, characterized in that, S1.2 specifically includes the following steps: S1.2.1 Based on the tractor-mounted LiDAR-IMU subsystem and the trailer-mounted LiDAR-IMU subsystem, time alignment is performed on the LiDAR and IMU sensors, the IMU data is resampled to the LiDAR time axis and a unified time axis is established, and the synchronized LiDAR point cloud data and IMU data are output. S1.2.2 Based on the synchronized LiDAR point cloud data and IMU data, the IMU data is pre-integrated within the scanning time interval of two adjacent LiDAR point cloud frames to calculate the motion increment during the LiDAR point cloud data scanning time. Each point cloud in the LiDAR point cloud is interpolated and compensated according to the timestamp, and the entire frame of point cloud is unified to the same reference time to obtain the distortion-free point cloud. S1.2.3 Extract geometric features from the distortion-free point cloud of the current frame and build a local map. Match each feature point in the current frame with the features of the local map. Construct laser geometric constraints by calculating the point-to-line residual and the point-to-surface residual and solve the pose of the point cloud in the current frame. The geometric features are edge feature points and planar feature points; Point-to-line residual The calculation expression is: ； in, For the two endpoints of the line segment that matches the edge point in the local map, For the feature points of the current frame, For the current pose transformation; Point-to-surface residual The calculation expression is: ； in, and These are the local plane normal and a point on the plane, respectively. S1.2.

4. Perform nonlinear least squares optimization on the laser geometric constraints and IMU pre-integration constraints to construct the internal objective function and output the keyframe pose of the tractor. , Keyframe pose of trailer , It updates the local map of the tractor and trailer, and outputs the keyframe pose sequence of the tractor and trailer. The IMU pre-integration constraint is to pre-integrate the IMU data within the time interval of two adjacent LiDAR point cloud scanning frames. The expression for the internal objective function is: ； in, For keyframe pose, For laser geometric constraints, Pre-integration constraints for the IMU.

4. The dual LiDAR-IMU joint SLAM localization method for semi-trailer vehicles according to claim 2, characterized in that, In S1.1, the three constraints specifically include: (1) The vehicle's motion is confined to a two-dimensional plane; (2) The tractor and trailer are rigid bodies connected by an articulation point; The hinge point satisfies a positional consistency constraint in the world coordinate system, as expressed below: ； ； In the formula, Let x be the x-coordinate of the hinge point. The ordinate of the hinge point is... Let x be the x-coordinate of the trailer reference point. The ordinate of the trailer reference point. The distance from the preset tractor reference point to the articulation point, The distance from the preset trailer reference point to the articulation point, and These are the heading angles of the tractor and trailer, respectively. (3) The hinge mechanism is a single-degree-of-freedom rotational joint.

5. The dual LiDAR-IMU joint SLAM localization method for semi-trailer vehicles according to claim 1, characterized in that, In step S2, the expression for updating the hinge angle by integrating the angular velocity difference within a preset length sliding window is: ； ； in, This is the discrete-time index for the current moment. Here, W is the sampling point index, and W is the preset sliding window length. For the first Each sampling time, , No. The estimated hinge angle at time 1. No. The difference in vehicle angular velocity at each sampling point This indicates that the angle is normalized to .

6. The dual LiDAR-IMU joint SLAM localization method for semi-trailer vehicles according to claim 1, characterized in that, S3 specifically includes the following steps: S3.1 Based on the key frame pose sequence and articulation angle sequence of the tractor and trailer, by taking the key frame time of the tractor as the reference, aligning the key frame and articulation angle data of the trailer and setting the time tolerance interval as a gating, the aligned and effective tractor key frame pose factor, trailer key frame pose factor and articulation angle constraint factor are obtained. S3.

2. Construct a factor map based on the tractor keyframe pose factors, trailer keyframe pose factors, and articulation angle constraint factors. Perform joint optimization by calculating the joint optimization objective function. Use a nonlinear least squares iterative method to solve for the optimal estimates of the tractor and trailer poses. Output the optimized tractor pose sequence, the optimized trailer pose sequence, and the articulated positioning results of the semi-trailer vehicle. Complete the dual LiDAR-IMU joint SLAM positioning method for the semi-trailer vehicle.

7. A dual LiDAR-IMU joint SLAM localization method for semi-trailer vehicles according to claim 6, characterized in that, In S3, the specific criteria for determining the time tolerance interval are as follows: ； ； ； In the formula, The keyframe timestamp of the tractor unit at time k. To and Corresponding keyframe timestamps for the semi-trailer used for fusion. and Corresponding timestamps of the hinge angle observations used for fusion This is the default value.

8. A dual LiDAR-IMU joint SLAM localization method for semi-trailer vehicles according to claim 6, characterized in that, In S3, the expression for the joint optimization objective function is: ； in, The prior residuals are the pose factors of the keyframes of the tractor vehicle. For the prior residuals of the trailer keyframe pose factors, For the hinge angle residual, The prior residual weighting coefficients for the pose factors of the tractor in keyframes. The prior residual weighting coefficients for the pose factors of the trailer keyframes. This is the weighting coefficient for the hinge angle residual.

Images