A dual-laser SLAM localization method and system for articulated vehicles

Abstract

This invention discloses a dual-laser SLAM localization method and system for articulated vehicles, belonging to the field of autonomous driving and multi-sensor fusion localization technology. The method includes: independently running laser SLAM algorithms at the tractor and trailer ends respectively, constructing a point cloud matching model based on feature points; fusing IMU measurement information and point cloud clustering detection results to construct a multi-sensor collaborative articulation angle detection algorithm; and designing a two-stage extrinsic parameter optimization strategy: the first stage utilizes radar odometer information and articulation angle constraints to construct an ICP-based extrinsic parameter optimization model to obtain initial estimates of the extrinsic parameters; the second stage performs joint optimization by using the matching pose constraints between the trailer radar and the tractor map, combined with the trailer's own localization results. By constructing a factor graph backend optimization model for the multi-laser radar system and introducing four types of factor constraints, global consistency optimization is achieved. The proposed method achieves significant improvements in both localization accuracy and system robustness.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving and multi-sensor fusion positioning technology, specifically to a dual-laser SLAM positioning method and system for articulated vehicles. Background Technology

[0002] With the rapid development of autonomous driving and intelligent transportation systems, the demand for unmanned vehicles in scenarios such as logistics transportation, port operations, and mining engineering is constantly increasing. Simultaneous Localization and Mapping (SLAM) technology, with its high-precision perception of environmental geometry, has become one of the core technologies in the field of unmanned vehicle localization and mapping. This type of method usually assumes that the vehicle body is a single rigid body and that the sensor extrinsic parameters are fixed.

[0003] However, articulated vehicles are connected to the trailer via an articulation mechanism, and there is significant relative rotation between the two during operation, making the rigid body assumption of single-vehicle SLAM difficult to apply. Existing multi-radar SLAM schemes for articulated vehicles mostly follow the single-vehicle framework, which has the following shortcomings: First, the extrinsic parameters between the tractor and trailer radars are difficult to estimate accurately online, and small changes during dynamic operation can easily lead to map stitching misalignment; Second, the back-end fusion lacks effective articulation motion constraints and cross-radar observation constraints, which can easily cause trajectory drift in feature-sparse scenarios. Furthermore, it relies solely on the articulation angle as the only constraint between the two vehicles, lacking cross-radar observation constraints between the trailer radar and the tractor map. In long-distance operation or feature-sparse scenarios, the cumulative drift of the trailer trajectory is difficult to suppress effectively.

[0004] In summary, for the unique structure of articulated vehicles, there is an urgent need for a laser SLAM localization method capable of explicitly modeling the relative motion between the tractor and trailer, fusing information from multiple sensors, and achieving multi-radar collaborative optimization. This method should ensure positioning accuracy while possessing good system robustness and engineering practicality to meet the application requirements of unmanned articulated vehicles in real-world, complex scenarios. Summary of the Invention

[0005] To address the aforementioned problems, this invention provides a dual-laser SLAM positioning method and system for articulated vehicles.

[0006] To achieve the above objectives, the present invention provides the following technical solution: a dual-laser SLAM positioning method and system for articulated vehicles, specifically including the following steps: S1. Construct a dual-laser SLAM positioning system based on lidar point cloud data and IMU data; The dual-laser SLAM positioning system includes: a lidar odometry module, a hinge angle detection module, a two-stage extrinsic parameter optimization module, and a factor graph backend optimization module; The lidar point cloud data includes: lidar point cloud data of the vehicle front and lidar point cloud data of the trailer; The IMU data includes: IMU data from the truck head and IMU data from the trailer.

[0007] S2. Based on LiDAR point cloud and IMU data, a LiDAR odometry module is constructed to output the vehicle front radar pose sequence and trailer radar pose sequence. The lidar odometer module runs the lidar SLAM algorithm independently at the tractor end and trailer end, respectively. The specific steps of running the laser SLAM algorithm are as follows: using IMU data to perform motion distortion correction on the laser radar point cloud data, and extracting features from the distorted point cloud to obtain a set of edge feature points and a set of planar feature points; constructing a Scan-to-Map point cloud matching model based on the edge feature points and planar feature points, and obtaining the vehicle front radar pose sequence and trailer radar pose sequence by establishing point-to-line residual constraints and point-to-plane residual constraints and performing nonlinear optimization.

[0008] The lidar odometer module includes: a vehicle front radar odometer and a trailer radar odometer; S3. Based on the lidar odometer module, an articulation angle detection module is constructed to obtain the estimated value of the articulation angle between the tractor and the trailer; The articulation angle detection module is constructed as follows: The angular velocity data of the tractor-mounted IMU and trailer-mounted IMU are integrated to obtain the change in heading angle between the tractor-mounted and trailer-mounted IMUs. The relative angular velocity difference between the heading angles of the tractor-mounted and trailer-mounted IMUs is calculated to obtain an IMU-based articulation angle estimate. After clustering and detecting the trailer target using the tractor-mounted lidar point cloud, a linear fitting based on principal component analysis is performed, and the mean and covariance matrix of the center points are calculated. The covariance matrix is ​​then subjected to eigenvalue decomposition to output the articulation angle point cloud observations. Finally, the Kalman filter method is used to fuse the IMU articulation angle estimate with the articulation angle point cloud observations and perform a state update to obtain the articulation angle estimate.

[0009] S4. Based on the lidar odometer module and articulation angle detection module, a two-stage extrinsic parameter optimization module is constructed to estimate the extrinsic parameters between the vehicle front radar and the trailer radar. The two-stage extrinsic parameter optimization module includes a first-stage optimization and a second-stage optimization.

[0010] S5. Based on the lidar odometer module, articulation angle detection module, and two-stage extrinsic parameter optimization module, the vehicle front pose and trailer pose are jointly optimized by constructing a factor graph backend optimization module to obtain a globally consistent localization result for the vehicle front and trailer, thus completing the dual-laser SLAM localization method for articulated vehicles. The factor graph backend optimization module includes: vehicle front radar odometer constraint factor, articulation angle kinematic constraint factor, trailer radar odometer constraint factor, cross-radar matching constraint factor, and joint optimization objective function; The joint optimization objective function includes: the residual function of the vehicle front radar odometer, the kinematic error function of the articulation angle, the residual function of the trailer radar odometer, and the residual function of the cross-radar matching constraint.

[0011] Specifically, step S2 includes the following steps: S2.1 Based on the point cloud of the vehicle-mounted LiDAR and the point cloud of the trailer LiDAR, after using the angular velocity and linear velocity information of the vehicle-mounted inertial measurement unit (IMU) and the trailer IMU during the scanning period, the relative pose change of the LiDAR during a single frame scan is estimated by integration. After motion compensation is performed on the point cloud of the vehicle-mounted LiDAR and the trailer LiDAR, motion distortion correction processing is performed, and the distortion-corrected point cloud is output.

[0012] S2.2 Based on the distortion-reduced point cloud, the point cloud is classified after calculating the local curvature. A threshold is set to distinguish between edge corner points and planar points, and the set of edge feature points and planar feature points are output. The formula for calculating the curvature is as follows: In the formula, Indicates the first The curvature value at each point, For the first A 3D vector of a point cloud point. The index number of the current point in the point cloud. This is the neighborhood offset.

[0013] By setting a threshold, the sets of edge feature points can be obtained separately. and the set of planar feature points ; S2.3. Based on the set of edge feature points and the set of planar feature points in the current frame, construct a pose estimation model based on feature points to obtain the pose of the vehicle front radar and the pose of the trailer radar in the current frame, thus completing the lidar odometer estimation process. The feature-point-based pose estimation model specifically involves: constructing a point cloud matching model between the current frame's edge feature point set, the current frame's planar feature point set, and local map keyframes, and defining the overall residual objective function of the lidar odometer; constructing a nonlinear least squares optimization problem by minimizing the overall residual objective function, and using the Levenberg-Marquardt method for iterative solution to obtain the current frame's vehicle front radar pose and the current frame's trailer radar pose; specifically, in S2.3, the expression for the overall residual objective function of the lidar odometer is as follows: In the formula, The overall residual of the lidar odometry, For the feature points of the current frame point cloud, The pose of the current frame; This represents the residual from the point to the line. Represents the residual from a point to a surface; , These represent the set of edge feature points and the set of planar feature points, respectively. The index number of the edge feature point. The index number of the planar feature point; , These represent the rotation matrix and translation vector of the current frame's lidar coordinate system relative to the map point cloud coordinate system, respectively. For line feature points in the current frame point cloud, For the map and The first matching point, For the map and The second matching point, , Add points to the cloud on the map The two closest points in the consistency line feature satisfy the condition that they are not on the same laser horizontal scanning line; These are the surface feature points in the point cloud of the current frame. For the map and The first matching point, For the map and The second matching point, For the map and The third matching point, , , Add points to the cloud on the map The three nearest points in the consistency surface feature satisfy the condition that the three points are not on the same laser horizontal scanning line.

[0014] S2.4. Execute the lidar odometer estimation process S2.1-S2.3 independently at the tractor end and trailer end respectively to construct the tractor-head radar odometer and trailer radar odometer, obtain the tractor-head radar pose sequence and trailer radar pose sequence, and complete the construction of the lidar odometer module.

[0015] Specifically, step S3 includes the following steps: S3.1 Execute the initialization of the system hinge angle and output the initial hinge angle; S3.2 After preprocessing the point cloud of the LiDAR at the front of the vehicle, cluster detection is performed on the point cloud of the LiDAR on the trailer to output a set of local point cloud clusters. The preprocessing operations include: removing ground points and noise points, and performing voxel downsampling; The specific method for clustering and detecting the point cloud of the trailer lidar is to use an adaptive Euclidean clustering method to cluster the point cloud; Because the data from the 16-line LiDAR is relatively sparse, the trailer cannot form a complete overall cluster in a single frame scan. Instead, it appears as local point cloud clusters hit by varying numbers of line beams. Therefore, in a single frame point cloud, only the local point cloud clusters formed by each line beam scanning the trailer can be obtained, and multiple line beams result in a set of local point cloud clusters. ,in, Indicates the total number of local point cloud clusters; S3.3. Based on the local point cloud cluster set, calculate the center point of each point cloud cluster and construct the center point set of the local point cloud cluster set; The definition of a local point cloud cluster is as follows: In the formula, For the th point in the cloud cluster One point, The index number of the points within the point cloud cluster ， This represents the total number of points in the point cloud cluster. , and These represent the first and second points in the point cloud cluster, respectively. The horizontal, vertical, and height coordinates of each point; Through calculation The minimum and maximum values ​​of a local point cloud cluster along each coordinate axis are constructed. Align the bounding box with the axis of a local point cloud cluster to obtain the center point of the point cloud cluster. The calculation expression is as follows: in, and Indicates in The maximum and minimum values ​​of the horizontal coordinates of a local point cloud cluster. and Indicates in The maximum and minimum values ​​of the vertical coordinates of a local point cloud cluster. and Indicates in The maximum and minimum values ​​of the height direction coordinates of a local point cloud cluster are used to construct a set of center points based on the calculation expression for the center point of the point cloud cluster. .

[0016] S3.4. Based on the set of center points of the local point cloud cluster, perform linear fitting based on principal component analysis and calculate the mean and covariance matrix of the center points. Then, perform eigenvalue decomposition on the covariance matrix and output the observation values ​​of the hinged corner point cloud. The mean of the center point The calculation expression is as follows: The covariance matrix The calculation expression is as follows: For covariance matrix Perform eigenvalue decomposition and extract the eigenvector corresponding to the largest eigenvalue. As the principal direction of the trailer in the tractor's coordinate system, where... , and These are the eigenvectors. x , y and z Quantity; The principal direction is converted into an observation of the hinge angle, and the hinge angle point cloud observation value is output. The expression is as follows: ; In the formula, It is the arctangent function; S3.5 Based on the IMU data of the truck head and the trailer, the relative angular velocity between the truck head IMU and the trailer IMU is calculated to construct a discrete kinematic model of the articulation angle and output the articulation angle estimate based on the IMU. Let the coordinate system of the vehicle's front lidar be... The coordinate system of the suspended lidar is Read the rotation from the gyroscope angular velocity of the axle's front end and trailer angular velocity Define the difference in relative angular velocity between the tractor and the trailer. The expression is: ; S3.6. Based on the discrete kinematic model of the hinge angle, a point cloud hinge angle detection observation model is constructed by building the prediction equation of a one-dimensional Kalman filter, calculating the Kalman gain and updating the state to complete the construction of the hinge angle detection module.

[0017] Specifically, in S3.4, the expression for the discrete kinematic model of the hinge angle is: ; In the formula, For time intervals, For process covariance, express The estimated hinge angle at any given moment is defined as the angle from the trailer's orientation to the tractor's orientation as positive. The hinge angle at the previous moment. It follows a Gaussian distribution, and k is the time index.

[0018] Specifically, step S4 includes the following steps: S4.1, Global Coordinate System Based on Front Radar Odometer Global coordinate system of trailer radar odometer Establish an external parameter model between the vehicle front radar coordinate system and the trailer radar coordinate system; The external parameter model between the vehicle head radar coordinate system and the trailer radar coordinate system is specifically the transformation relationship between the global coordinate system of the trailer radar odometer and the global coordinate system of the vehicle head radar odometer. S4.2 Based on the vehicle front radar pose sequence, trailer radar pose sequence, predicted hinge angle and initial hinge angle, after calculating the predicted pose of the vehicle front radar, construct the residual function for the first stage of extrinsic parameter optimization, perform the first stage optimization and use Gauss-Newton iterative solution to output the initial value of the optimized extrinsic parameters. The predicted value of the tractor unit is calculated using a trailer radar odometer and motion constraints of the articulated vehicle. Then, an optimization function based on ICP matching is established using the predicted value and the odometer reading from the tractor unit radar to obtain the world system exoparameters. Trailer radar odometer provides information on trailer radar speed. The pose at time is ,pass Position at any given moment, trailer kinematics model and world exoparameters Calculate the predicted pose of the front radar The calculation formula is as follows: ; In the formula, express The estimated hinge angle at time [time]. The distance from the rotation center to the front radar. The distance from the rotation center to the trailer radar, for Trailer radar in key frames of time in coordinate system The vehicle's position and orientation. On the other hand, the front radar odometer operates independently to obtain the vehicle's position at any given time. observation pose Select The group-matched pose points are used to construct the residual function for the first-stage extrinsic parameter optimization between the predicted pose and the vehicle's odometer observations. The expression is as follows: ; In the formula, N is the total number of keyframes, and k is the time index. As a logarithmic mapping from Lie groups to Lie algebras, minimizing the aforementioned residual function transforms the nonlinear optimization problem into a least-squares problem in the Lie algebra space, which is then solved iteratively using the Gauss-Newton method; the initial values ​​of the optimized extrinsic parameters are... The expression is as follows: ; in, The initial hinge angle; S4.3 Based on the optimized initial values ​​of extrinsic parameters, Scan-to-Map cross-radar point cloud matching is performed on the map established by the trailer radar point cloud and the truck front radar point cloud to obtain pose observation values. Then, the residual function of the second stage extrinsic parameter optimization is constructed, and the second stage optimization is performed. Gauss-Newton iteration is used to solve the problem to obtain the extrinsic parameter estimation results between the truck front radar and the trailer radar, thus completing the construction of the two-stage extrinsic parameter optimization module. Constructing the residual function for the second stage of extrinsic parameter optimization The expression is as follows: ; In the formula, for The pose observation value at time.

[0019] Specifically, step S5 includes the following steps: S5.1 Based on the vehicle front radar pose sequence and trailer radar pose sequence, construct system state optimization variables; The system state optimization variables include: the pose set variables of the front radar and the pose set variables of the trailer radar; Pose set variables of the front radar The expression is as follows: ; Pose set variables of trailer radar The expression is as follows: ; in, and These represent keyframes. The 3D poses of the vehicle-mounted radar and the trailer radar in their respective world coordinate systems at any given time, where N is the total number of keyframes. k For time index, For rigid body transformation in three-dimensional space; S5.2 Based on the front radar odometer, construct the front radar odometer constraint factor and the front radar odometer residual function; Front odometer factor The relative pose measurement of adjacent keyframes is provided by the front radar odometer, and the residual function of the front radar odometer is... The definition is as follows: ; The front radar odometer constraint factor constrains the continuity of the vehicle's front pose over time. S5.3 Based on the hinge angle detection module, construct the hinge angle kinematic constraint factor and the hinge angle kinematic error function; Hinged angle kinematic constraint factor The predicted pose of the front radar calculated by the articulation angle detection module is constructed with the predicted pose of adjacent key frames; Constructing the kinematic error function of the hinge angle The expression is: ; S5.4 Based on the trailer radar odometer, construct the trailer radar odometer constraint factor and the trailer radar odometer residual function; Trailer radar odometer constraint factor The trailer radar odometer provides relative pose measurements between adjacent keyframes, and the trailer radar odometer residual function... Defined as: ; The constraint factor of the trailer radar odometer evolves continuously over time and is used to constrain the trailer's pose. S5.5 Based on the lidar odometry module, construct the cross-radar matching constraint factor and the residual function of the cross-radar matching constraint; Cross-radar matching constraint factor Specifically, when the trailer radar enters the area mapped by the tractor radar, a scan-to-map cross-radar point cloud matching is performed between the trailer radar point cloud and the map created by the tractor radar to obtain... Observation constraints of trailer under the cab map at all times The cross-radar matching constraint factor is constructed using the relative pose transformation between two adjacent frames, and the residual function of the cross-radar matching constraint is... Defined as: ; S5.6. Based on the vehicle-mounted radar odometer constraint factor, articulation angle kinematic constraint factor, trailer radar odometer constraint factor, and cross-radar matching constraint factor, a joint optimization objective function is constructed for joint optimization to complete the dual-laser SLAM localization method for articulated vehicles.

[0020] The expression for the joint optimization objective function is as follows: ; In the formula, , and The number of feature points matched from the point cloud is directly proportional to the number of feature points matched. Determined by the covariance matrix of the Kalman filter, For the pose set variables of the front radar, The pose set variables of the trailer radar.

[0021] A dual-laser SLAM positioning system for articulated vehicles, used to perform S1-S5, includes a lidar odometry module, an articulation angle detection module, a two-stage extrinsic parameter optimization module, and a factor graph back-end optimization module. The lidar odometer module is connected to the front lidar, the front IMU, the trailer lidar, and the trailer IMU, respectively. The articulation angle detection module is connected to the front lidar, the front IMU, and the trailer IMU, respectively; the two-stage extrinsic parameter optimization module is connected to the lidar odometer module and the articulation angle detection module, respectively. The factor graph backend optimization module is connected to the lidar odometer module, the hinge angle detection module, and the two-stage extrinsic parameter optimization module, respectively. Compared with the prior art, the present invention provides a dual-laser SLAM positioning method and system for articulated vehicles, which has the following advantages: 1) This invention employs a two-stage extrinsic parameter optimization framework. The first stage utilizes the relative motion constraints of the radar odometer and the consistency constraints of the articulated angle motion to construct a coarse extrinsic parameter estimation model based on ICP, obtaining initial extrinsic parameter values. The second stage introduces Scan-to-Map matching pose constraints between the trailer radar and the vehicle front map, jointly optimizing them with the trailer's own SLAM trajectory to achieve refined extrinsic parameter estimation. This strategy effectively addresses the shortcomings of traditional methods, such as reliance on offline calibration and difficulty in adapting to dynamic changes, enabling the dual-radar system to adaptively correct extrinsic parameter deviations during vehicle operation, ensuring accurate stitching of the point cloud maps at both ends.

[0022] 2) This invention introduces a cross-radar matching factor to directly match the trailer radar point cloud with the high-quality map already constructed by the tractor radar, providing independent global observation constraints for the trailer's pose. This factor can effectively block the cumulative propagation of errors at the trailer end, especially in long-distance straight-line driving or feature-sparse scenarios, significantly suppressing the drift trend of the trailer trajectory.

[0023] 3) This invention provides motion consistency constraints for extrinsic parameter optimization through hinge angle detection, enabling rapid convergence of coarse extrinsic parameter estimation. The refined extrinsic parameter results then provide accurate initial values ​​for cross-radar matching, forming a closed-loop enhancement mechanism of "angle estimation—extrinsic parameter calibration—cross-radar observation." The four types of constraint factors complement each other in the back-end optimization. When the odometer constraint weakens due to environmental degradation, the hinge angle constraint and cross-radar matching constraint provide redundant information, maintaining system observability and thus ensuring global consistency of positioning under complex conditions.

[0024] 4) The four modules of this invention form a close collaborative relationship: the lidar odometer module provides stable point cloud input for the articulation angle detection module, and provides pose sequence for extrinsic parameter optimization; the articulation angle detection module provides kinematic constraints for extrinsic parameter optimization and articulation angle observation for factor graph optimization; the two-stage extrinsic parameter optimization module provides accurate extrinsic parameters for factor graph optimization, so that the pose of the vehicle head and trailer can be unified to the same reference frame; the factor graph back-end optimization module comprehensively utilizes the output of the first three modules to achieve global consistency optimization; this invention can provide high-precision and robust positioning services for articulated vehicles in autonomous driving tasks in turning, low-speed maneuvering, long-distance operation and complex environments. Attached Figure Description

[0025] Figure 1 This is a schematic diagram of the overall structure of the present invention; Figure 2 This is the articulated vehicle kinematic model used in this invention; Figure 3 This is the factor graph model used in this invention; Figure 4 This is the experimental platform used in this invention; Figure 5 This is a satellite cloud image of the experimental scenario in this embodiment; Figure 6 This is a trajectory error distribution diagram of LIOSAM in this embodiment and the algorithm of the present invention, wherein, Figure 6 (a) shows the trajectory error distribution of the LIOSAM algorithm. Figure 6 (b) is a trajectory error distribution diagram of the algorithm of the present invention. Detailed Implementation

[0026] 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.

[0027] like Figure 1 The diagram shows the overall structure of the invention. The lidar odometer module runs the lidar SLAM algorithm independently at the vehicle front and trailer ends to obtain stable initial positioning results. To address the point cloud distortion problem in a single lidar frame during vehicle movement, motion distortion correction is first performed on the original point cloud using inertial measurement unit (IMU) data synchronized with the lidar. By interpolating and estimating the vehicle's attitude changes within the scanning cycle, each lidar point is uniformly compensated to the same reference time, thereby reducing the impact of vehicle steering and acceleration / deceleration on the point cloud geometry. Based on this, feature extraction is performed on the distorted point cloud, and it is classified according to its local geometric attributes. Representative edge and planar feature points are selected to enhance the representation of the environmental structure and reduce the computational complexity of matching. Subsequently, a Scan-to-Map point cloud matching model is constructed based on the extracted feature points, matching the current frame feature points with a local map constructed from historical keyframes. By establishing geometric constraints between points and lines, and between points and surfaces, a nonlinear optimization problem is constructed and iteratively solved to obtain continuous radar odometry estimation results by solving the pose transformation of the current radar relative to the local map. As the system runs, frames that meet the conditions are selected as keyframes and used to update the local map to ensure the stability and real-time performance of the matching process. The above process is executed independently at the tractor end and the trailer end, achieving decoupled operation of the two laser SLAM systems before the extrinsic parameters are accurately estimated, providing reliable initial pose input for subsequent articulation angle estimation and multi-radar collaborative optimization.

[0028] To estimate the articulation angle between the tractor and trailer, this invention employs a detection method that integrates inertial measurement information and point cloud geometric information. First, the angular velocity data from the IMUs on both sides of the tractor and trailer are integrated to obtain their respective heading angle changes. The difference between these two values ​​is then used to calculate a relative rotation angle estimate based on inertial information. Next, the trailer target is clustered using the point cloud data from the tractor's lidar, and the articulation angle is estimated based on the principal direction of the clustered point cloud, serving as an external observation of the articulation angle. Finally, the inertial estimation result and the point cloud observation result are fused using a Kalman filter method to achieve a stable estimate of the articulation angle.

[0029] The two-stage extrinsic parameter optimization module constructs a two-stage extrinsic parameter estimation framework. The first stage utilizes the relative motion constraints provided by the radar odometer and the vehicle motion consistency constraints characterized by the articulation angle to establish a coarse extrinsic parameter estimation model based on ICP. The second stage introduces Scan-to-Map matching pose constraints between the trailer radar and the front map, and performs co-optimization of these constraints with the trailer's own SLAM trajectory to achieve a refined estimation of extrinsic parameters under geometric consistency constraints.

[0030] The factor graph backend optimization module models the multi-LiDAR system consisting of the tractor and trailer as a unified factor graph optimization problem. Using time-aligned keyframes as graph nodes, it introduces trailer radar odometer factors, tractor radar odometer factors, trailer radar positioning constraint factors (cross-radar matching factors) in the tractor coordinate system, and articulation angle motion constraint factors. By unifying these multi-source constraints into the same factor graph framework, a joint optimization approach is used in the backend to collaboratively estimate the poses of the tractor and trailer, ensuring consistency of various constraints at the global scale, thereby obtaining stable and consistent overall positioning results.

[0031] Specifically, the following steps are included: S1. Construct a dual-laser SLAM positioning system based on lidar point cloud data and IMU data; The dual-laser SLAM positioning system includes: a lidar odometry module, a hinge angle detection module, a two-stage extrinsic parameter optimization module, and a factor graph backend optimization module; The lidar point cloud data includes: lidar point cloud data of the vehicle front and lidar point cloud data of the trailer; The IMU data includes: IMU data from the truck head and IMU data from the trailer; S2. Based on LiDAR point cloud and IMU data, a LiDAR odometry module is constructed to output the vehicle front radar pose sequence and trailer radar pose sequence. The lidar odometer module runs the lidar SLAM algorithm independently at the tractor end and trailer end, respectively. The specific steps of running the laser SLAM algorithm are as follows: using IMU data to perform motion distortion correction on the laser radar point cloud data, and extracting features from the distorted point cloud to obtain a set of edge feature points and a set of planar feature points; constructing a Scan-to-Map point cloud matching model based on the edge feature points and planar feature points, and obtaining the vehicle front radar pose sequence and trailer radar pose sequence by establishing point-to-line residual constraints and point-to-plane residual constraints and performing nonlinear optimization. Specifically, the following steps are included: S2.1 Based on the point cloud of the vehicle front lidar and the point cloud of the trailer lidar, motion distortion correction processing is performed through the vehicle front IMU and the trailer IMU to output the distortion-corrected point cloud; The angular velocity and linear velocity information during the scanning cycle are obtained by using an inertial measurement unit (IMU) synchronized with the lidar. The relative pose change of the lidar during a single frame scan is estimated by integration, and motion compensation is performed on the original point cloud to obtain a distortion-free point cloud. S2.2 Based on the distortion-reduced point cloud, the point cloud is classified after calculating the local curvature. A threshold is set to distinguish between edge corner points and planar points, and the set of edge feature points and planar feature points are output. By calculating the local curvature of the point cloud, the point cloud is classified according to the magnitude of curvature, thereby extracting planar feature points and planar corner points. The formula for calculating curvature is as follows: In the formula, Indicates the first The curvature value at each point, For the first A 3D vector of a point cloud point. The index number of the current point in the point cloud. This is the neighborhood offset; By setting a threshold, the sets of edge feature points can be obtained separately. and the set of planar feature points .

[0032] S2.3. Based on the set of edge feature points and the set of planar feature points in the current frame, construct a pose estimation model based on feature points to obtain the pose of the vehicle front radar and the pose of the trailer radar in the current frame, thus completing the lidar odometer estimation process. The feature-point-based pose estimation model is specifically as follows: a point cloud matching model is constructed between the current frame edge feature point set and the current frame planar feature point set and the local map keyframe, and the overall residual objective function of the lidar odometer is defined; by minimizing the overall residual objective function, a nonlinear least squares optimization problem is constructed, and the Levenberg-Marquardt method is used to perform iterative solution to obtain the current frame vehicle front radar pose and the current frame trailer radar pose; This invention assumes the current pose to be estimated is... Based on the extracted feature points, a point cloud matching model is constructed between the current frame point cloud and local map keyframes. The overall residual objective function of the lidar odometry can be expressed as: In the formula, The overall residual of the lidar odometry, For the feature points of the current frame point cloud, The pose of the current frame; This represents the residual from the point to the line. Represents the residual from a point to a surface; , These represent the set of edge feature points and the set of planar feature points, respectively. The index number of the edge feature point. The index number of the planar feature point; , These represent the rotation matrix and translation vector of the current frame's lidar coordinate system relative to the map point cloud coordinate system, respectively. For line feature points in the current frame point cloud, For the map and The first matching point, For the map and The second matching point, , Add points to the cloud on the map The two closest points in the consistency line feature satisfy the condition that they are not on the same laser horizontal scanning line; These are the surface feature points in the point cloud of the current frame. For the map and The first matching point, For the map and The second matching point, For the map and The third matching point, , , Add points to the cloud on the map The three closest points in the consistency surface feature satisfy the condition that these three points are not on the same laser horizontal scanning line. By minimizing the residual objective function, a nonlinear least squares optimization problem is constructed, and the Levenberg-Marquardt (LM) method is used for iterative solution to obtain the optimal pose estimate of the radar in the current frame. The radar odometer estimation process at the trailer end is the same as that at the truck head end, and the pose of the truck head radar can be obtained separately. With trailer radar position .

[0033] S2.4. Execute the lidar odometer estimation process S2.1-S2.3 independently at the tractor end and trailer end respectively to construct the tractor-head radar odometer and trailer radar odometer, obtain the tractor-head radar pose sequence and trailer radar pose sequence, and complete the construction of the lidar odometer module.

[0034] The vehicle front radar pose sequence and trailer radar pose sequence are the lidar odometer results of the vehicle front radar and trailer radar, respectively.

[0035] S3. Based on the lidar odometer module, an articulation angle detection module is constructed to obtain the estimated value of the articulation angle between the tractor and the trailer; The articulation angle detection module is constructed as follows: The angular velocity data of the tractor-mounted IMU and trailer-mounted IMU are integrated to obtain the change in heading angle between the tractor-mounted and trailer-mounted IMUs. The relative angular velocity difference between the heading angles of the tractor-mounted and trailer-mounted IMUs is calculated to obtain an IMU-based articulation angle estimate. After clustering and detecting the trailer target using the tractor-mounted lidar point cloud, a linear fitting based on principal component analysis is performed, and the mean and covariance matrix of the center points are calculated. The covariance matrix is ​​then subjected to eigenvalue decomposition to output the articulation angle point cloud observations. Finally, the Kalman filter method is used to fuse the IMU articulation angle estimate and the articulation angle point cloud observations, and a state update is performed to obtain the articulation angle estimate. Specifically, the following steps are included: S3.1 Execute the initialization of the system hinge angle and output the initial hinge angle; At the initial moment of the system, there may be a non-zero articulation angle between the tractor and the trailer, therefore it is necessary to adjust the initial articulation angle. Initialization is performed. At system startup, the vehicle is kept stationary, and the articulation angle is continuously estimated using a point cloud clustering detection method. The average of multiple measurements is then used as the initial value. .

[0036] S3.2 After preprocessing the point cloud of the LiDAR at the front of the vehicle, cluster detection is performed on the point cloud of the LiDAR on the trailer to output a set of local point cloud clusters. The preprocessing operations include: removing ground points and noise points, and performing voxel downsampling; The specific method for clustering and detecting the point cloud of the trailer lidar is to use an adaptive Euclidean clustering method to cluster the point cloud; Because the data from the 16-line LiDAR is relatively sparse, the trailer cannot form a complete overall cluster in a single frame scan. Instead, it appears as local point cloud clusters hit by varying numbers of line beams. Therefore, in a single frame point cloud, only the local point cloud clusters formed by each line beam scanning the trailer can be obtained, and multiple line beams result in a set of local point cloud clusters. ,in, This represents the total number of local point cloud clusters.

[0037] S3.3. Based on the local point cloud cluster set, calculate the center point of each point cloud cluster and construct the center point set of the local point cloud cluster set; With the first Taking a point cloud cluster containing a set of points as an example, the calculation method is the same for other point cloud clusters. The definition of a local point cloud cluster is as follows: in, For the th point in the cloud cluster One point, The index number of the points within the point cloud cluster ， This represents the total number of points in the point cloud cluster. , and These represent the first and second points in the point cloud cluster, respectively. The horizontal, vertical, and height coordinates of each point; By calculating the minimum and maximum values ​​of the point cloud cluster along each coordinate axis, its axis-aligned bounding box (AABB) is constructed, and the center point of the point cloud cluster is obtained from this. in, and Indicates in The maximum and minimum values ​​of the horizontal coordinates of a local point cloud cluster. and Indicates in The maximum and minimum values ​​of the vertical coordinates of a local point cloud cluster. and Indicates in By analyzing the maximum and minimum values ​​of the elevation coordinates of local point cloud clusters, and following the steps outlined above, the centers of other local point cloud clusters can be obtained. These centers then form a set of center points. .

[0038] S3.4. Based on the set of center points of the local point cloud cluster, perform linear fitting based on principal component analysis and calculate the mean and covariance matrix of the center points. Then, perform eigenvalue decomposition on the covariance matrix and output the observation values ​​of the hinged corner point cloud. Perform a linear fit based on principal component analysis on the center point and calculate the mean of the center point. : Then, the mean matrix is ​​constructed, and the covariance matrix is ​​calculated: Then, the covariance matrix... Perform eigenvalue decomposition and extract the eigenvector corresponding to the largest eigenvalue. As the principal direction of the trailer in the tractor's coordinate system, where... , and These are the eigenvectors. x , y and z Quantity; The principal direction is converted into an observation of the hinge angle, and the hinge angle point cloud observation value is output. The expression is as follows: In the formula, It is the arctangent function.

[0039] S3.5 Based on the IMU data of the truck head and the trailer, the relative angular velocity between the truck head IMU and the trailer IMU is calculated to construct a discrete kinematic model of the articulation angle and output the articulation angle estimate based on the IMU. Let the coordinate system of the vehicle's front lidar be... The coordinate system of the suspended lidar is Read the rotation from the gyroscope angular velocity of the axle's front end and trailer angular velocity Define the relative angular velocity difference between the truck head IMU and the trailer IMU. The expression is: The expression for the discrete kinematic model of the hinge angle is: in, For time intervals, For process covariance, express The estimated hinge angle at any given moment is defined as the angle from the trailer's orientation to the tractor's orientation as positive. The hinge angle at the previous moment. It follows a Gaussian distribution.

[0040] S3.6 Based on the discrete kinematic model of hinge angle, after constructing the prediction equation of one-dimensional Kalman filter, a point cloud hinge angle detection observation model is constructed, the Kalman gain is calculated and the state is updated to complete the construction of the hinge angle detection module. Based on the discrete kinematic model of the hinge angle, the prediction equation of the one-dimensional Kalman filter is constructed: in, To utilize the prior estimates obtained from the IMU, To utilize the posterior estimate obtained from the IMU, For the prior uncertainty, For posterior uncertainty, The process covariance is used. The hinge angle, obtained through point cloud clustering and geometric analysis, is used as a system observation. The point cloud hinge angle detection observation model is as follows: in, These are observations of the articulated corner point cloud. To observe noise, satisfy .

[0041] The expression for the Kalman gain calculation formula is: The expression for state update is: The expression for covariance update is: S4. Based on the lidar odometer module and articulation angle detection module, a two-stage extrinsic parameter optimization module is constructed to obtain the extrinsic parameter estimation results between the vehicle front radar and the trailer radar. The two-stage extrinsic parameter optimization module includes a first-stage optimization and a second-stage optimization. The two-stage extrinsic optimization module specifically includes: the first stage uses the relative motion constraints of the radar odometer and the motion consistency constraints provided by the hinge angle to construct an extrinsic model based on ICP matching; the second stage uses the Scan-to-Map matching pose constraints between the trailer radar and the front map, and combines them with the trailer radar's own SLAM trajectory for joint optimization, and the extrinsic estimation results between the front radar and the trailer radar. like Figure 2 As shown, Figure 2 middle This is the installation location for the front radar. This is the installation location for the trailer radar. It serves as the rotation center for both the trailer and the tractor. It is the hinge angle. The distance from the rotation center to the front radar. This refers to the distance from the rotation center to the trailer radar.

[0042] S4.1, Global Coordinate System Based on Front Radar Odometer Global coordinate system of trailer radar odometer Establish an external parameter model between the vehicle front radar coordinate system and the trailer radar coordinate system; Define the transformation relationship between the global coordinate system of the trailer radar odometer and the global coordinate system of the vehicle front radar odometer. , As a reference for the world system, Transformation of quantities in coordinate system to This unifies the trajectories at both ends to the same reference frame.

[0043] S4.2 Based on the vehicle front radar pose sequence, trailer radar pose sequence, predicted hinge angle and initial hinge angle, after calculating the predicted pose of the vehicle front radar, construct the residual function for the first stage of extrinsic parameter optimization, perform the first stage optimization and use Gauss-Newton iterative solution to output the initial value of the optimized extrinsic parameters. Specifically, the predicted value of the vehicle's front end is calculated using the trailer radar odometer and the motion constraints of the articulated vehicle. Then, an optimization function based on ICP matching is established using the predicted value and the odometer reading from the front radar to obtain the world system exoparameters. Trailer radar odometer provides information on trailer radar speed. The pose at time is ,pass Position at any given moment, trailer kinematics model and world exoparameters Calculate the predicted pose of the front radar The calculation formula is as follows: in, express The estimated hinge angle at time t is obtained through the hinge angle measurement module. The distance from the rotation center to the front radar. The distance from the rotation center to the trailer radar, for Trailer radar in key frames of time in coordinate system The vehicle's position and orientation. On the other hand, the front radar odometer operates independently to obtain the vehicle's position at any given time. observation pose Select For the group-matched pose points, construct the residual function for the first-stage extrinsic parameter optimization between the predicted pose and the odometer observations: in, Given the logarithmic mapping from Lie groups to Lie algebras, by minimizing the aforementioned residual function, the nonlinear optimization problem can be transformed into a least-squares problem in the Lie algebra space, and solved iteratively using the Gauss-Newton method. The initial values ​​of the extrinsic parameters for optimization are set as follows: in, This is the initial hinge angle.

[0044] S4.3 Based on the optimized initial values ​​of extrinsic parameters, Scan-to-Map cross-radar point cloud matching is performed on the map established by the trailer radar point cloud and the truck front radar point cloud to obtain pose observation values. Then, the residual function of the second stage extrinsic parameter optimization is constructed, and the second stage optimization is performed. Gauss-Newton iteration is used to solve the problem to obtain the extrinsic parameter estimation results between the truck front radar and the trailer radar, thus completing the construction of the two-stage extrinsic parameter optimization module. After the first phase of optimization is completed, the trailer radar has entered the map area already mapped by the tractor radar. Scan-to-Map cross-radar point cloud matching can then be performed using the map created from the trailer radar point cloud and the tractor radar point cloud to obtain... pose observations at time 1 This point cloud matching method is consistent with the Scan-to-map matching method in the aforementioned radar odometry module, both being based on feature point cloud matching, and therefore will not be elaborated further. When acquiring... After matching the poses, the residual function for the second-stage extrinsic parameter optimization can be constructed, as shown in the following expression: . The initial value for optimization in this stage is the result of the first stage. Iterative solutions using Gauss-Newton are also employed to obtain the extrinsic parameter estimation results between the vehicle-mounted radar and the trailer radar.

[0045] S5. Based on the lidar odometer module, articulation angle detection module, and two-stage extrinsic parameter optimization module, a factor graph backend optimization module is constructed to jointly optimize the position and posture of the tractor and trailer. Specifically, using time-aligned keyframes as graph nodes, the following factors are introduced: trailer radar odometer factor, tractor radar odometer factor, trailer radar positioning constraint factor (cross-radar matching factor) in the tractor radar coordinate system, and articulation angle motion constraint factor. By constructing a multi-source constraint factor graph optimization model and solving it jointly, a globally consistent positioning result for the tractor and trailer is obtained. The constructed factor graph model is as follows: Figure 3 As shown.

[0046] S5.1 Based on the vehicle front radar pose sequence and trailer radar pose sequence, construct system state optimization variables; The system state optimization variables include: the pose set variables of the front radar and the pose set variables of the trailer radar; Pose set variables of the front radar The expression is as follows: Pose set variables of trailer radar The expression is as follows: in, and These represent keyframes. The 3D poses of the vehicle-mounted radar and the trailer radar in their respective world coordinate systems at any given time, where N is the total number of keyframes. k For time indexing.

[0047] S5.2 Based on the front radar odometer, construct the front radar odometer constraint factor and the front radar odometer residual function; Front odometer factor The relative pose measurement of adjacent keyframes is provided by the front radar odometer. The residual function of the front radar odometer is defined as follows: , This factor constrains the continuity of the vehicle's frontal pose over time.

[0048] S5.3 Based on the hinge angle detection module, construct the hinge angle kinematic constraint factor and the hinge angle kinematic error function; Articulation detection module output time Hinge angle estimate Then utilize the trailer's position. With hinge angle Calculate the predicted pose of the front radar The calculation method is as follows: in, This indicates the distance from the articulation point to the trailer radar. This represents the distance from the hinge point to the front radar, and further utilizes the predicted poses of adjacent keyframes to construct the hinge angle kinematic constraint factor. The expression for the kinematic error function of the hinge angle is: .

[0049] S5.4 Based on the trailer radar odometer, construct the trailer radar odometer constraint factor and the trailer radar odometer residual function; This factor is determined by the relative pose measurement between adjacent keyframes provided by the trailer radar odometer. The trailer radar odometer residual function is defined as follows: , This factor constraint evolves continuously over time and is mainly used to constrain the trailer's position and orientation.

[0050] S5.5 Based on the lidar odometry module, construct the cross-radar matching constraint factor and the residual function of the cross-radar matching constraint; Once the trailer radar enters the area mapped by the tractor radar, a scan-to-map cross-radar point cloud matching can be performed between the trailer radar point cloud and the map created by the tractor radar to obtain... Observation constraints of trailer under the cab map at all times The binary constraint factor, i.e., the cross-radar matching constraint factor, is constructed using the relative pose transformation between two adjacent frames. The residual function of the cross-radar matching constraint is defined as: In the formula, As a reference for the world system, The pose to be optimized for the trailer radar in the coordinate system at time k. for The observation constraints of the trailer being positioned under the map at all times.

[0051] S5.6. Based on the vehicle front radar odometer constraint factor, articulation angle kinematic constraint factor, trailer radar odometer constraint factor, and cross-radar matching constraint factor, a joint optimization objective function is constructed for joint optimization to obtain the globally consistent localization result of the vehicle front and trailer, thus completing the dual-laser SLAM localization method for articulated vehicles. Combining the constraints of the vehicle-mounted radar odometer, the articulation angle kinematics constraint, the trailer radar odometer constraint, and the cross-radar matching constraint, a joint optimization objective function is constructed, expressed as follows: In the formula, , and The information matrices represent three types of residuals: vehicular odometer, trailer odometer, and cross-radar matching. The matrix values ​​are proportional to the number of feature points matched from the point cloud. The information matrix corresponding to the kinematic constraints of the hinge angle is determined by the covariance matrix of the Kalman filter. For the pose set variables of the front radar, The pose set variables of the trailer radar.

[0052] To demonstrate the innovativeness of this invention, real-world experiments were conducted for verification, and it was compared with the classic algorithm LIO-SAM in the field. LIO-SAM uses a LiDAR and IMU at the front of the vehicle as data sources. The experimental platform used is as follows: Figure 4 As shown, the platform consists of a tractor unit and a trailer. The tractor unit is 1.7 meters long, and the trailer is 5.5 meters long. Both the tractor unit and the trailer are equipped with the same type of LiDAR and IMU, and all sensors are connected to the industrial control computer via ROS communication. The tractor unit also features a high-precision integrated navigation system to provide positioning reference values. The satellite cloud image of the experimental scenario is shown below. Figure 5 As shown, the pentagram indicates the starting point, and the red arrow indicates the direction of movement. During the experiment, the experimental platform was manually controlled to move along a prescribed route to collect data. The data was then processed offline, and the positioning trajectories were obtained by running LIOSAM and the algorithm of this invention. The positioning result of the combined navigation was used as a reference positioning. The trajectory error distributions of the positioning by LIOSAM and the algorithm of this invention are shown below. Figure 6 As shown, it can be seen that the trajectories of LIOSAM and the algorithm of this invention are close to the reference trajectory, and there is no obvious drift, making it difficult to qualitatively identify significant differences.

[0053] Table 1: Comparison of APE metrics between LIOSAM and the algorithm of this invention Table 1 compares the Absolute Pose Error (APE) metrics of LIOSAM and the algorithm of this invention. The metrics are the root mean square, mean, median, minimum, and maximum error. Quantitative analysis of these five metrics shows that the algorithm of this invention is significantly superior to the LIOSAM algorithm. All five metrics of this invention are lower than those of LIOSAM, especially the maximum error metric, where this invention's is 0.85, while LIOSAM's is 1.06, representing a 20% reduction. In summary, the experimental analysis demonstrates that this invention can significantly improve positioning accuracy, providing better positioning services for articulated vehicles, and illustrating the advanced nature of this invention.

[0054] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A dual-laser SLAM localization method for articulated vehicles, characterized in that, Includes the following steps: S1. Construct a dual-laser SLAM positioning system based on lidar point cloud data and IMU data; The dual-laser SLAM positioning system includes: a lidar odometry module, a hinge angle detection module, a two-stage extrinsic parameter optimization module, and a factor graph backend optimization module; S2. Based on LiDAR point cloud and IMU data, a LiDAR odometry module is constructed to output the vehicle front radar pose sequence and trailer radar pose sequence. The specific steps of running the laser SLAM algorithm are as follows: using IMU data to perform motion distortion correction on the laser radar point cloud data, and extracting features from the distorted point cloud to obtain a set of edge feature points and a set of planar feature points; constructing a Scan-to-Map point cloud matching model based on the edge feature points and planar feature points, and obtaining the vehicle front radar pose sequence and trailer radar pose sequence by establishing point-to-line residual constraints and point-to-plane residual constraints and performing nonlinear optimization. S3. Based on the lidar odometer module, an articulation angle detection module is constructed to obtain the estimated value of the articulation angle between the tractor and the trailer; The articulation angle detection module is constructed as follows: The angular velocity data of the tractor-mounted IMU and trailer-mounted IMU are integrated to obtain the change in heading angle between the tractor-mounted and trailer-mounted IMUs. The relative angular velocity difference between the heading angles of the tractor-mounted and trailer-mounted IMUs is calculated to obtain an IMU-based articulation angle estimate. After clustering and detecting the trailer target using the tractor-mounted lidar point cloud, a linear fitting based on principal component analysis is performed, and the mean and covariance matrix of the center points are calculated. The covariance matrix is ​​then subjected to eigenvalue decomposition to output the articulation angle point cloud observations. Finally, the Kalman filter method is used to fuse the IMU articulation angle estimate and the articulation angle point cloud observations, and a state update is performed to obtain the articulation angle estimate. S4. Based on the lidar odometer module and articulation angle detection module, a two-stage extrinsic parameter optimization module is constructed to estimate the extrinsic parameters between the vehicle front radar and the trailer radar. S5. Based on the lidar odometer module, articulation angle detection module, and two-stage extrinsic parameter optimization module, the vehicle front pose and trailer pose are jointly optimized by constructing a factor graph backend optimization module to obtain a globally consistent localization result for the vehicle front and trailer, thus completing the dual-laser SLAM localization method for articulated vehicles.

2. The dual-laser SLAM positioning method for articulated vehicles according to claim 1, characterized in that, S2 specifically includes the following steps: S2.1 Based on the point cloud of the vehicle front lidar and the lidar of the trailer, after using the angular velocity and linear velocity information of the vehicle front inertial measurement unit (IMU) and the trailer inertial measurement unit (IMU) during the scanning period, the relative pose change of the lidar during a single frame scan is estimated by integration. After motion compensation is performed on the point cloud of the vehicle front lidar and the lidar of the trailer, motion distortion correction processing is performed, and the distortion-corrected point cloud is output. S2.2 Based on the distortion-reduced point cloud, the point cloud is classified after calculating the local curvature. A threshold is set to distinguish between edge corner points and planar points, and the set of edge feature points and planar feature points are output. The formula for calculating the curvature is as follows: In the formula, Indicates the first The curvature value at each point, For the first A 3D vector of a point cloud point. The index number of the current point in the point cloud. This is the neighborhood offset; By setting a threshold, the sets of edge feature points can be obtained separately. and the set of planar feature points ; S2.

3. Based on the set of edge feature points and the set of planar feature points in the current frame, construct a pose estimation model based on feature points to obtain the pose of the vehicle front radar and the pose of the trailer radar in the current frame, thus completing the lidar odometer estimation process. The feature-point-based pose estimation model is specifically as follows: a point cloud matching model is constructed between the current frame edge feature point set and the current frame planar feature point set and the local map key frame, and the overall residual objective function of the lidar odometer is defined; by minimizing the overall residual objective function, a nonlinear least squares optimization problem is constructed, and the Levenberg-Marquardt method is used to perform iterative solution to obtain the current frame vehicle front radar pose and the current frame trailer radar pose; S2.

4. Execute the lidar odometer estimation process S2.1-S2.3 independently at the tractor end and trailer end respectively to construct the tractor-head radar odometer and trailer radar odometer, obtain the tractor-head radar pose sequence and trailer radar pose sequence, and complete the construction of the lidar odometer module.

3. The dual-laser SLAM positioning method for articulated vehicles according to claim 2, characterized in that, In S2.3, the overall residual objective function expression of the lidar odometry is as follows: In the formula, The overall residual of the lidar odometry, For the feature points of the current frame point cloud, The pose of the current frame; This represents the residual from the point to the line. Represents the residual from a point to a surface; , These represent the set of edge feature points and the set of planar feature points, respectively. The index number of the edge feature point. The index number of the planar feature point; , These represent the rotation matrix and translation vector of the current frame's lidar coordinate system relative to the map point cloud coordinate system, respectively. For line feature points in the current frame point cloud, For the map and The first matching point, For the map and The second matching point, , Add points to the cloud on the map The two closest points in the consistency line feature satisfy the condition that they are not on the same laser horizontal scanning line; These are the surface feature points in the point cloud of the current frame. For the map and The first matching point, For the map and The second matching point, For the map and The third matching point, , , Add points to the cloud on the map The three nearest points in the consistency surface feature satisfy the condition that the three points are not on the same laser horizontal scanning line.

4. The dual-laser SLAM positioning method for articulated vehicles according to claim 1, characterized in that, S3 specifically includes the following steps: S3.1 Execute the initialization of the system hinge angle and output the initial hinge angle; S3.2 After preprocessing the point cloud of the LiDAR at the front of the vehicle, cluster detection is performed on the point cloud of the LiDAR on the trailer to output a set of local point cloud clusters. The preprocessing operations include: removing ground points and noise points, and performing voxel downsampling; The specific method for clustering and detecting the point cloud of the trailer lidar is to use an adaptive Euclidean clustering method to cluster the point cloud; Because the data from the 16-line LiDAR is relatively sparse, the trailer cannot form a complete overall cluster in a single frame scan. Instead, it appears as local point cloud clusters hit by varying numbers of line beams. Therefore, in a single frame point cloud, only the local point cloud clusters formed by each line beam scanning the trailer can be obtained, and multiple line beams result in a set of local point cloud clusters. ,in, Indicates the total number of local point cloud clusters; S3.

3. Based on the local point cloud cluster set, calculate the center point of each point cloud cluster and construct the center point set of the local point cloud cluster set; Local point cloud clusters The definition is as follows: In the formula, The first in the point cloud cluster One point, The index number of the points within the point cloud cluster ， The total number of points in the point cloud cluster. , and These represent the first and second points in the point cloud cluster, respectively. The horizontal, vertical, and height coordinates of each point; By calculating local point cloud clusters Construct the minimum and maximum values ​​along each coordinate axis. Align the bounding box with the axis of a local point cloud cluster to obtain the center point of the point cloud cluster. The calculation expression is as follows: in, and Indicates in The maximum and minimum values ​​of the horizontal coordinates of a local point cloud cluster. and Indicates in The maximum and minimum values ​​of the vertical coordinates of a local point cloud cluster. and Indicates in The maximum and minimum values ​​of the height direction coordinates of a local point cloud cluster are used to construct a set of center points based on the calculation expression for the center point of the point cloud cluster. ; S3.

4. Based on the set of center points of the local point cloud cluster, perform linear fitting based on principal component analysis and calculate the mean and covariance matrix of the center points. Then, perform eigenvalue decomposition on the covariance matrix and output the observation values ​​of the hinged corner point cloud. The mean of the center point The calculation expression is as follows: The covariance matrix The calculation expression is as follows: For covariance matrix Perform eigenvalue decomposition and extract the eigenvector corresponding to the largest eigenvalue. As the principal direction of the trailer in the tractor's coordinate system, where... , and These are the eigenvectors. x , y and z Quantity; The principal direction is converted into an observation of the hinge angle, and the hinge angle point cloud observation value is output. The expression is as follows: ； In the formula, It is the arctangent function; S3.5 Based on the IMU data of the truck head and the trailer, the relative angular velocity between the truck head IMU and the trailer IMU is calculated to construct a discrete kinematic model of the articulation angle and output the articulation angle estimate based on the IMU. Let the coordinate system of the vehicle's front lidar be... The coordinate system of the suspended lidar is Read the rotation from the gyroscope angular velocity of the axle's front end and trailer angular velocity Define the difference in relative angular velocity between the tractor and the trailer. The expression is: ； S3.

6. Based on the discrete kinematic model of the hinge angle, a point cloud hinge angle detection observation model is constructed by building the prediction equation of a one-dimensional Kalman filter, calculating the Kalman gain and updating the state to complete the construction of the hinge angle detection module.

5. A dual-laser SLAM positioning method for articulated vehicles according to claim 4, characterized in that, In S3.4, the expression for the discrete kinematic model of the hinge angle is: ； In the formula, For time intervals, For process covariance, express The estimated hinge angle at any given moment is defined as the angle from the trailer's orientation to the tractor's orientation as positive. The hinge angle at the previous moment. It follows a Gaussian distribution, and k is the time index.

6. The dual-laser SLAM positioning method for articulated vehicles according to claim 1, characterized in that, S4 specifically includes the following steps: S4.1, Global Coordinate System Based on Front Radar Odometer Global coordinate system of trailer radar odometer Establish an external parameter model between the vehicle front radar coordinate system and the trailer radar coordinate system; The external parameter model between the vehicle head radar coordinate system and the trailer radar coordinate system is specifically the transformation relationship between the global coordinate system of the trailer radar odometer and the global coordinate system of the vehicle head radar odometer. S4.2 Based on the vehicle front radar pose sequence, trailer radar pose sequence, predicted hinge angle and initial hinge angle, after calculating the predicted pose of the vehicle front radar, construct the residual function for the first stage of extrinsic parameter optimization, perform the first stage optimization and use Gauss-Newton iterative solution to output the initial value of the optimized extrinsic parameters. The predicted value of the tractor unit is calculated using a trailer radar odometer and motion constraints of the articulated vehicle. Then, an optimization function based on ICP matching is established using the predicted value and the odometer reading from the tractor unit radar to obtain the world system exoparameters. Trailer radar odometer provides information on trailer radar performance. The pose at time is ,pass Position at any given moment, trailer kinematics model and world exoparameters Calculate the predicted pose of the front radar The calculation formula is as follows: ； In the formula, express The estimated hinge angle at time [time]. The distance from the rotation center to the front radar. The distance from the rotation center to the trailer radar, for Trailer radar in key frames of time in coordinate system The vehicle's position is below; on the other hand, the front radar odometer operates independently to obtain the vehicle's position at any given time. observation pose Select The group-matched pose points are used to construct the residual function for the first-stage extrinsic parameter optimization between the predicted pose and the vehicle's odometer observations. The expression is as follows: ； In the formula, N is the total number of keyframes, and k is the time index. Given the logarithmic mapping from Lie groups to Lie algebras, by minimizing the aforementioned residual function, the nonlinear optimization problem can be transformed into a least-squares problem in the Lie algebra space, and solved iteratively using the Gauss-Newton method; the initial values ​​of the optimized extrinsic parameters are... The expression is as follows: ； in, The initial hinge angle; S4.3 Based on the optimized initial values ​​of extrinsic parameters, Scan-to-Map cross-radar point cloud matching is performed on the map established by the trailer radar point cloud and the truck front radar point cloud to obtain pose observation values. Then, the residual function of the second stage extrinsic parameter optimization is constructed, and the second stage optimization is performed. Gauss-Newton iteration is used to solve the problem to obtain the extrinsic parameter estimation results between the truck front radar and the trailer radar, thus completing the construction of the two-stage extrinsic parameter optimization module. Constructing the residual function for the second stage of extrinsic parameter optimization The expression is as follows: ； In the formula, for The pose observation value at time.

7. The dual-laser SLAM positioning method for articulated vehicles according to claim 1, characterized in that, S5 specifically includes the following steps: S5.1 Based on the vehicle front radar pose sequence and trailer radar pose sequence, construct system state optimization variables; The system state optimization variables include: the pose set variables of the front radar and the pose set variables of the trailer radar; Pose set variables of the front radar The expression is as follows: ； Pose set variables of trailer radar The expression is as follows: ； in, and These represent keyframes. The 3D poses of the vehicle-mounted radar and the trailer radar in their respective world coordinate systems at any given time, where N is the total number of keyframes. k For time index, For rigid body transformation in three-dimensional space; S5.2 Based on the front radar odometer, construct the front radar odometer constraint factor and the front radar odometer residual function; Front odometer factor The relative pose measurement of adjacent keyframes is provided by the front radar odometer, and the residual function of the front radar odometer is... The definition is as follows: ； The front radar odometer constraint factor constrains the continuity of the vehicle's front pose over time. S5.3 Based on the hinge angle detection module, construct the hinge angle kinematic constraint factor and the hinge angle kinematic error function; Hinged angle kinematic constraint factor The predicted pose of the front radar calculated by the articulation angle detection module is constructed with the predicted pose of adjacent key frames; Constructing the kinematic error function of the hinge angle The expression is: ； In the formula, The predicted pose of the front radar; S5.4 Based on the trailer radar odometer, construct the trailer radar odometer constraint factor and the trailer radar odometer residual function; Trailer radar odometer constraint factor The trailer radar odometer provides relative pose measurements between adjacent keyframes, and the trailer radar odometer residual function... Defined as: ； The constraint factor of the trailer radar odometer evolves continuously over time and is used to constrain the trailer's pose. S5.5 Based on the lidar odometry module, construct the cross-radar matching constraint factor and the residual function of the cross-radar matching constraint; Cross-radar matching constraint factor Specifically, when the trailer radar enters the area mapped by the tractor radar, a scan-to-map cross-radar point cloud matching is performed between the trailer radar point cloud and the map created by the tractor radar to obtain... Observation constraints of trailer under the cab map at all times The cross-radar matching constraint factor is constructed using the relative pose transformation between two adjacent frames, and the residual function of the cross-radar matching constraint is... Defined as: ； In the formula, As a reference for the world system, The pose to be optimized for the trailer radar in the coordinate system at time k. for Constant observation constraints of the trailer under the map at the front of the vehicle; S5.

6. Based on the vehicle-mounted radar odometer constraint factor, articulation angle kinematic constraint factor, trailer radar odometer constraint factor, and cross-radar matching constraint factor, a joint optimization objective function is constructed for joint optimization to complete the dual-laser SLAM localization method for articulated vehicles. The joint optimization objective function includes: the residual function of the vehicle front radar odometer, the kinematic error function of the articulation angle, the residual function of the trailer radar odometer, and the residual function of the cross-radar matching constraint.

8. A dual-laser SLAM positioning method for articulated vehicles according to claim 7, characterized in that, In step S5.6, the expression for the joint optimization objective function is as follows: ； In the formula, , and The information matrices represent three types of residuals: vehicular odometer, trailer odometer, and cross-radar matching. The matrix values ​​are proportional to the number of feature points matched from the point cloud. The information matrix corresponding to the kinematic constraints of the hinge angle is determined by the covariance matrix of the Kalman filter. For the pose set variables of the front radar, The pose set variables of the trailer radar.

9. A dual-laser SLAM positioning system for articulated vehicles, used to execute the dual-laser SLAM positioning method for articulated vehicles as described in any one of claims 1-8, characterized in that, It includes a lidar odometry module, a hinge angle detection module, a two-stage extrinsic parameter optimization module, and a factor graph back-end optimization module; The lidar odometer module is connected to the front lidar, the front IMU, the trailer lidar, and the trailer IMU, respectively. The articulation angle detection module is connected to the front lidar, the front IMU, and the trailer IMU, respectively; the two-stage extrinsic parameter optimization module is connected to the lidar odometer module and the articulation angle detection module, respectively. The factor graph backend optimization module is connected to the lidar odometer module, the hinge angle detection module, and the two-stage extrinsic parameter optimization module, respectively.

Images