SPMT autonomous splicing alignment control method and system based on 3D vision

Abstract

A 3D vision-based autonomous splicing alignment control method and system for SPMT (Special Purpose Vehicle) involves the field of general control or adjustment systems. The method includes: establishing a splicing coordinate system with the center of the splicing pin holes of a reference vehicle as the target origin; driving the vehicle to be spliced ​​to approach the target origin and acquiring 3D point cloud data of the reference vehicle; identifying pin hole features in the 3D point cloud data and calculating the six-dimensional deviation of the vehicle to be spliced ​​relative to the splicing coordinate system; decoupling the six-dimensional deviation to determine the planar motion control quantity and the suspension adjustment control quantity; sending the planar motion control quantity to the walking and steering system to correct the planar pose; sending the suspension adjustment control quantity to the hydraulic suspension unit to correct the vertical attitude of the frame; and issuing a locking command when all six-dimensional deviations converge within a preset alignment tolerance range to lock the vehicle and complete the splicing alignment. Implementing this application can improve docking accuracy and reduce the risk of structural damage to the SPMT during splicing alignment.

Description

Technical Field

[0001] This application relates to the general field of control or regulation systems, and more particularly to a method and system for SPMT autonomous splicing alignment control based on 3D vision. Background Technology

[0002] Self-Propelled Modular Transporters (SPMTs) form transport arrays by mechanically connecting the end lugs and pins of multiple modules to carry components and complete transfer operations. During assembly, the corresponding pin holes of two adjacent SPMTs must be coaxial before the pin can be inserted. The alignment accuracy of the pin holes directly affects the overall structural rigidity and load transfer uniformity of the array.

[0003] In the related vehicle assembly operations, a vision-guided automatic docking scheme was adopted. This scheme equips the vehicles with positioning sensors and vision cameras. As the two vehicles approach each other, the control system acquires images through the vision cameras and calculates the planar positional deviation between the two vehicles. This planar positional deviation is converted into planar motion commands and sent to the vehicle's drive system. The system controls the vehicles to perform planar movements such as forward, backward, and turning on the ground. Through continuous planar displacement adjustments, the assembly ends of the two vehicles are brought closer together and docking is completed.

[0004] However, when SPMT operates in complex terrain environments, uneven settlement of the frame caused by ground undulations and heavy loads can lead to vertical spatial misalignment between the splicing pin holes of the two vehicles. Related technologies only possess the ability to perceive and correct deviations in the planar dimension. When the vertical misalignment exceeds the allowable range of the mechanical chamfer of the pin hole, the pin insertion will experience hard interference, posing a risk of structural damage. Summary of the Invention

[0005] This application provides a 3D vision-based method and system for autonomous splicing and alignment control of SPMT, which can improve the docking accuracy and reduce the risk of structural damage to SPMT during splicing and alignment.

[0006] Firstly, this application provides a 3D vision-based SPMT autonomous stitching alignment control method, applied to a control system. The method includes: establishing a stitching coordinate system with the center of the stitching pin hole of a reference vehicle as the target origin; driving the vehicle to be stitched to approach the target origin until the distance is less than a preset distance threshold; acquiring three-dimensional point cloud data of the reference vehicle using a 3D vision camera on the vehicle to be stitched; identifying pin hole features in the three-dimensional point cloud data; calculating the six-dimensional deviation of the vehicle to be stitched relative to the stitching coordinate system; the six-dimensional deviation includes translational deviation along three orthogonal translation axes and angular deviation around three orthogonal rotation axes; and adjusting the six-dimensional deviation... The quantities are decoupled, with the degree of freedom component corresponding to the ground plane motion in the six-dimensional deviation quantity being used as the planar motion control quantity, and the degree of freedom component corresponding to the vertical displacement and attitude angle in the six-dimensional deviation quantity being used as the suspension adjustment control quantity. The planar motion control quantity is sent to the travel and steering system to drive the vehicle to be spliced ​​to correct its planar posture. The suspension adjustment control quantity is sent to the hydraulic suspension unit to adjust the extension length of each set of suspension cylinders of the vehicle to be spliced ​​and the reference vehicle to correct the vertical posture of the frame. When all six-dimensional deviation quantities converge to the preset alignment tolerance range, a locking command is issued to lock the frame posture of the vehicle to be spliced ​​and the reference vehicle to complete the splicing alignment.

[0007] In the above embodiments, the control system decouples the six-dimensional deviation and controls the planar motion and the vertical attitude of the suspension respectively, thereby achieving all-round posture correction of six degrees of freedom in space. This eliminates the vertical misalignment caused by ground undulations, improves the coaxiality of pin holes and docking accuracy, and reduces the risk of structural interference during the assembly process.

[0008] In conjunction with some embodiments of the first aspect, in some embodiments, the step of identifying pin hole features in three-dimensional point cloud data and calculating the six-dimensional deviation of the vehicle to be stitched relative to the stitching coordinate system specifically includes: acquiring wheel odometer data of the vehicle to be stitched and the initial relative pose of the vehicle to be stitched and the reference vehicle; determining a three-dimensional bounding box in the three-dimensional point cloud data and capturing local point cloud data based on the wheel odometer data and the initial relative pose; extracting edge features and cylindrical surface features in the local point cloud data, and fitting pin hole features based on the edge features and cylindrical surface features; and calculating the six-dimensional deviation of the vehicle to be stitched relative to the stitching coordinate system based on the pin hole features.

[0009] In the above embodiments, the control system effectively filters out background interference point clouds by combining odometer data with the initial pose to define a three-dimensional bounding box, thereby improving the accuracy and computational efficiency of pin hole feature extraction and ensuring the reliability of deviation calculation.

[0010] In conjunction with some embodiments of the first aspect, in some embodiments, the steps of sending planar motion control quantities to the walking and steering system to drive the vehicle to be stitched to correct its planar pose, and sending suspension adjustment control quantities to the hydraulic suspension unit to adjust the extension length of each group of suspension cylinders of the vehicle to be stitched and the reference vehicle to correct the vertical attitude of the chassis, are executed synchronously and in parallel. While being executed synchronously and in parallel, the method further includes: acquiring the change in the extension length of each group of suspension cylinders in the hydraulic suspension unit; calculating the pose change compensation parameters of the 3D vision camera based on the change in extension length and the chassis kinematic model of the vehicle to be stitched; and performing coordinate system correction on the continuously acquired three-dimensional point cloud data based on the pose change compensation parameters, and updating the six-dimensional deviation.

[0011] In the above embodiments, the control system eliminates the interference of the vehicle frame attitude adjustment process on the visual measurement reference by synchronously performing planar and vertical adjustments and compensating for camera pose changes based on the suspension extension amount, thus ensuring the closed-loop control accuracy during the continuous adjustment process.

[0012] In conjunction with some embodiments of the first aspect, in some embodiments, the step of sending planar motion control quantities to the travel and steering system to drive the vehicle to be stitched to correct its planar pose specifically includes: acquiring the planar distance deviation of the vehicle to be stitched relative to the target origin; when the planar distance deviation is not less than a preset fine-tuning distance threshold, sending a continuous speed control command to the travel and steering system to drive the vehicle to be stitched closer to the target origin; when the planar distance deviation is less than the fine-tuning distance threshold, sending a micro-motion control command to the travel and steering system to drive the vehicle to be stitched to perform micro-motion displacement until the planar distance deviation converges; the micro-motion control command is a low-speed continuous creep command or a staggered walking command.

[0013] In the above embodiments, the control system adopts a segmented speed control strategy, which ensures approach efficiency at long distances and prevents overshoot through micro-motion control at close distances, thereby improving the stability of planar pose correction and the final alignment accuracy.

[0014] In conjunction with some embodiments of the first aspect, in some embodiments, before issuing a locking command to lock the frame posture of the vehicle to be spliced ​​and the reference vehicle to complete the splicing alignment when all six-dimensional deviations converge to the preset alignment tolerance range, the method further includes: obtaining the mechanical chamfer parameters of the splicing pin holes of the reference vehicle and the vehicle to be spliced; calculating a first translational tolerance and a first yaw angle tolerance along the pin insertion direction, and a second translational tolerance perpendicular to the pin insertion direction, based on the mechanical chamfer parameters; the first translational tolerance being greater than the second translational tolerance; and determining the preset alignment tolerance range based on the first translational tolerance, the first yaw angle tolerance, and the second translational tolerance.

[0015] In the above embodiments, the control system dynamically sets the anisotropic alignment tolerance based on the mechanical chamfer parameters, so that the tolerance range matches the actual assembly physical constraints, avoiding over-adjustment or assembly interference caused by a single tolerance.

[0016] In conjunction with some embodiments of the first aspect, in some embodiments, the step of determining the preset alignment tolerance range based on the first translation tolerance, the first yaw angle tolerance, and the second translation tolerance specifically includes: obtaining the attitude drift rates of the vehicle to be spliced ​​and the reference vehicle in the locked state; calculating the expected drift margin for each dimension based on the attitude drift rate and the preset pin assembly allowance time; determining the first effective translation tolerance, the effective yaw angle tolerance, and the second effective translation tolerance corresponding to the first translation tolerance, the first yaw angle tolerance, and the second translation tolerance, respectively, based on the expected drift margin; and determining the preset alignment tolerance range based on the first effective translation tolerance, the effective yaw angle tolerance, and the second effective translation tolerance.

[0017] In the above embodiments, the control system introduces attitude drift rate for tolerance margin compensation, ensuring that within the assembly time window after locking, the slight creep of the frame attitude will not cause the pin hole misalignment to exceed the assembly limit, thereby improving the assembly success rate.

[0018] In conjunction with some embodiments of the first aspect, in some embodiments, after identifying pin hole features in the three-dimensional point cloud data and calculating the six-dimensional deviation of the vehicle to be stitched relative to the stitching coordinate system, the method further includes: acquiring the six-dimensional deviation over multiple consecutive control cycles; calculating the frame oscillation convergence rate of the vehicle to be stitched based on the change amplitude and attenuation trend of the six-dimensional deviation over multiple consecutive control cycles; determining a gain attenuation coefficient based on the frame oscillation convergence rate when the frame oscillation convergence rate is lower than a preset stability threshold; and adjusting the control gain of the driving steering system and the hydraulic suspension unit based on the gain attenuation coefficient to update the planar motion control quantity and the suspension adjustment control quantity.

[0019] In the above embodiments, the control system monitors the oscillation convergence rate of the chassis and adaptively adjusts the control gain, thereby suppressing system oscillations caused by the coupling of multiple actuators under heavy load conditions and enhancing the dynamic stability of the alignment process.

[0020] In a second aspect, embodiments of this application provide a control system comprising: one or more processors and a memory; the memory being coupled to the one or more processors, the memory being used to store computer program code, the computer program code including computer instructions, the one or more processors invoking the computer instructions to cause the control system to perform the method described in the first aspect and any possible implementation thereof.

[0021] Thirdly, embodiments of this application provide a computer-readable storage medium including instructions that, when executed on a control system, cause the control system to perform the method described in the first aspect and any possible implementation thereof.

[0022] Fourthly, embodiments of this application provide a computer program product containing instructions that, when the computer program product is run on a control system, cause the control system to perform the method described in the first aspect and any possible implementation thereof.

[0023] Understandably, the control system provided in the second aspect, the computer storage medium provided in the third aspect, and the computer program product provided in the fourth aspect are all used to execute the methods provided in the embodiments of this application. Therefore, the beneficial effects they can achieve can be referred to the beneficial effects in the corresponding methods, and will not be repeated here.

[0024] One or more technical solutions provided in the embodiments of this application have at least the following technical effects or advantages: 1. Due to the decoupling of the six-dimensional deviation, the planar motion control quantity is sent to the travel and steering system, and the suspension adjustment control quantity is sent to the hydraulic suspension unit. Therefore, the control system can synchronously drive the vehicle's planar movement and suspension lifting, effectively solving the problem that existing technologies can only adjust the planar posture and cannot eliminate vertical misalignment, and achieving high-precision alignment of six degrees of freedom in space.

[0025] 2. By using wheel odometer data and initial relative pose to determine the 3D bounding box in the 3D point cloud data and extract the features of the local point cloud data, the control system can accurately remove background interference data, effectively solving the problem of feature extraction being easily interfered with in complex environments in existing technologies, and realizing stable recognition of pin hole features and efficient calculation of deviation.

[0026] 3. Because the system adopts the feature of calculating the differentiated translational tolerance and yaw angle tolerance along the pin insertion direction and the vertical direction based on the mechanical chamfer parameters, the control system establishes a judgment boundary that conforms to the physical assembly characteristics. This effectively solves the problem of long adjustment time or easy hard interference caused by using uniform tolerance in the existing technology, and realizes flexible alignment control that takes into account both efficiency and safety. Attached Figure Description

[0027] Figure 1 This is a schematic diagram of a topology architecture of the control system in an embodiment of this application; Figure 2 This is a flowchart illustrating a SPMT autonomous splicing alignment control method based on 3D vision in an embodiment of this application. Figure 3This is another flowchart illustrating the SPMT autonomous splicing alignment control method based on 3D vision in the embodiments of this application; Figure 4 This is a schematic diagram of the physical device structure of a control system in an embodiment of this application. Detailed Implementation

[0028] The terminology used in the following embodiments of this application is for the purpose of describing particular embodiments only and is not intended to be limiting of this application. As used in the specification of this application, the singular expressions “a,” “an,” “the,” “the,” and “this” are intended to include the plural expressions as well, unless the context clearly indicates otherwise. It should also be understood that the term “and / or” as used in this application refers to any or all possible combinations including one or more of the listed items.

[0029] Hereinafter, the terms "first" and "second" are used for descriptive purposes only and should not be construed as implying or suggesting relative importance or implicitly indicating the number of indicated technical features. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature, and in the description of the embodiments of this application, unless otherwise stated, "multiple" means two or more.

[0030] The SPMT (Self-Propelled Modular Transporter) involved in this application is a small, electric modular transporter for indoor operations, also classified as a heavy-duty AGV in the industry. This type of equipment consists of multiple independent transport modules mechanically rigidly connected by pin holes and pin shafts on end lugs, forming a transport array to carry heavy-tonnage components for transfer operations. Each SPMT module has an independent hydraulic drive system for travel and steering, and a multi-axis independent hydraulic suspension system. The hydraulic drive system for travel and steering is used to drive the vehicle to perform forward, backward, lateral, and crab-like planar movements on the ground. The multi-axis independent hydraulic suspension system is used to independently adjust the vertical height of each support point of the frame, thereby changing the pitch angle, roll angle, and overall lifting amount of the frame.

[0031] During the splicing process, the pin holes at the ends of two adjacent SPMT modules must be spatially coaxial before the pin can be inserted. The alignment accuracy of the pin holes directly determines the overall structural rigidity and load uniformity of the array. In complex ground environments, local undulations in the ground, uneven tire compression, and differential settlement of the frame caused by heavy loads can all lead to vertical height differences, pitch deflection, and lateral misalignment of the splicing pin holes of two adjacent SPMT modules.

[0032] In this application, the control system acquires three-dimensional point cloud data of the stitching area in real time through a 3D vision camera, calculates the six-dimensional spatial deviation of the pin hole of the vehicle to be stitched relative to the reference vehicle, and decouples the six-dimensional deviation into planar motion control quantity and suspension adjustment control quantity, respectively driving the walking and steering system and the hydraulic suspension unit to perform attitude correction in coordination, eliminating pin hole misalignment within the six degrees of freedom in space, and achieving high-precision alignment.

[0033] In this application, the splicing coordinate system is a three-dimensional rectangular coordinate system established with the geometric center point of the splicing pin hole at the end of the reference vehicle as the origin, the pin axis direction as the Y-axis, the longitudinal alignment direction of the two vehicles as the X-axis, and the vertical direction as the Z-axis. The six-dimensional deviation refers to the linear translation deviation of the center of the splicing pin hole of the vehicle to be spliced ​​relative to the origin of the splicing coordinate system in the directions of the three orthogonal translation axes (X-axis, Y-axis, and Z-axis) and the angular deviation around the three orthogonal rotation axes (roll angle around the X-axis, pitch angle around the Y-axis, and yaw angle around the Z-axis).

[0034] The X-axis is defined along the longitudinal direction of the two vehicles, the Y-axis along the lateral direction, and the Z-axis along the vertical direction. Planar motion control quantities are the degrees of freedom components in the six-dimensional deviation quantities corresponding to ground plane motion, including X-axis translational deviation, Y-axis translational deviation, and yaw angle deviation. Suspension adjustment control quantities are the degrees of freedom components in the six-dimensional deviation quantities corresponding to vertical displacement and attitude angle, including Z-axis translational deviation, roll angle deviation, and pitch angle deviation. The travel and steering system refers to the hydraulic actuator assembly in the SPMT module responsible for driving the steering and travel of each wheel group. The hydraulic suspension unit refers to the independent hydraulic cylinder group distributed at each axis position of the SPMT frame; each group of cylinders can independently extend and retract to change the frame height of the corresponding support point. The 3D vision camera is a structured light or time-of-flight ranging camera installed at the splicing end of the vehicle to be stitched, used to acquire three-dimensional point cloud data of the splicing area of ​​the reference vehicle.

[0035] Please see Figure 1 This is a schematic diagram of a topology architecture of the control system in an embodiment of this application. Figure 1 The diagram illustrates the hierarchical architecture of the control system, which consists of a perception layer, a decision layer, and an execution layer. It also includes a human-machine interface unit as an interface for external command input and status feedback.

[0036] The perception layer includes a global localization unit and a 3D vision camera. The global localization unit uses 3D laser SLAM localization to calculate the global pose data of the vehicle to be stitched in real time and transmits the pose data to the control system of the decision layer. The 3D vision camera is installed at the stitching end of the vehicle to be stitched and collects the 3D point cloud data of the stitching area of ​​the reference vehicle in real time, transmitting the 3D point cloud data to the control system of the decision layer.

[0037] The decision-making layer is the control system, namely the vehicle controller (VCU). The control system receives pose data and 3D point cloud data from the perception layer, and executes control logic such as six-dimensional deviation calculation, deviation decoupling processing, segmented speed control strategy, suspension linkage attitude adjustment algorithm, oscillation suppression gain adjustment, and lock-up determination. Based on the decoupling results, the control system outputs two types of control variables to the execution layer: planar motion control variables and suspension adjustment control variables.

[0038] The execution layer includes the travel and steering system and the hydraulic suspension unit. The hydraulic suspension unit comprises the hydraulic suspension units of the vehicle to be assembled and the reference vehicle. The control system transmits suspension adjustment control quantities to the hydraulic suspension unit of the reference vehicle via an inter-vehicle communication interface (such as a wireless communication module or wired CAN bus interconnection). The travel and steering system receives planar motion control quantities from the control system and drives the vehicle to be assembled to perform forward, backward, lateral, and crab-like planar movements on the ground to correct its planar posture. The hydraulic suspension unit receives suspension adjustment control quantities from the control system and changes the height of each support point of the frame by adjusting the extension length of the suspension cylinders on each axle in zones, thereby correcting the vertical height difference, pitch angle, and roll angle of the frame.

[0039] The human-machine interface unit communicates bidirectionally with the control system. Operators issue work instructions (such as starting the splicing alignment mode) to the control system through the human-machine interface unit, and the control system provides feedback on the current alignment status information to the operator through the human-machine interface unit.

[0040] In the above architecture, the global positioning unit transmits pose data down to the control system, the 3D vision camera transmits 3D point cloud data down to the control system, the control system transmits planar motion control quantities down to the walking and steering system, the control system transmits suspension adjustment control quantities down to the hydraulic suspension unit, and the human-machine interaction unit and the control system conduct bidirectional transmission of commands and status.

[0041] The method provided in this embodiment is described in detail below. Please refer to [link / reference]. Figure 2 This is a flowchart illustrating a SPMT autonomous splicing alignment control method based on 3D vision in an embodiment of this application.

[0042] S201. Establish a splicing coordinate system with the center of the splicing pin hole of the reference vehicle as the target origin. Drive the vehicle to be spliced ​​to approach the target origin until the distance is less than a preset distance threshold. Then, collect the three-dimensional point cloud data of the reference vehicle through the 3D vision camera on the vehicle to be spliced.

[0043] In this context, the reference vehicle refers to the SPMT module that remains stationary during the stitching operation, serving as a alignment reference. The vehicle to be stitched refers to the SPMT module that needs to move towards the reference vehicle and complete the pin hole alignment. The target origin is the geometric center point of the stitching pin hole at the end of the reference vehicle. The stitching coordinate system is a three-dimensional Cartesian coordinate system established with the target origin as the origin and the pin axis direction as one coordinate axis. The preset distance threshold refers to the maximum working distance determined by the effective measurement range of the 3D vision camera. When the distance between the vehicle to be stitched and the target origin is less than this threshold, the 3D vision camera can acquire effective three-dimensional point cloud data.

[0044] Specifically, after the stitching operation starts, the control system establishes a stitching coordinate system with the geometric center point of the stitching pin hole at the end of the reference vehicle as the origin. The control system acquires the real-time global pose of the vehicle to be stitched through the global positioning unit, plans the travel trajectory with the target origin as the navigation endpoint, and issues drive commands to the walking and steering system to make the vehicle to be stitched continuously approach the reference vehicle. The control system continuously monitors the Euclidean distance between the vehicle to be stitched and the target origin. When this distance is less than a preset distance threshold, the control system triggers the 3D vision camera to start acquiring three-dimensional point cloud data of the stitching end area of ​​the reference vehicle.

[0045] In some embodiments, the vehicle to be stitched can be driven to approach the target origin in multiple ways: Optionally, the control system obtains the real-time pose of the vehicle to be stitched through a global positioning unit, performs path planning with the target origin of the stitching coordinate system as the endpoint, issues continuous speed commands to the walking and steering system along the planned path to drive the vehicle to be stitched, and continuously compares the distance between the vehicle and the target origin with a preset distance threshold during the driving process; Optionally, the control system uses the wheel odometer data of the vehicle to be stitched combined with the initial manual coarse positioning pose to perform dead reckoning, uses the reckoned pose as feedback to drive the vehicle to approach the target origin, and switches to 3D vision camera acquisition mode when the reckoned distance is less than the preset distance threshold. It is understood that other positioning methods can also be used to guide the vehicle to be stitched to approach the target origin, which is not limited here.

[0046] S202. Identify the pin hole features in the 3D point cloud data and calculate the six-dimensional deviation of the vehicle to be spliced ​​relative to the splicing coordinate system.

[0047] The pin hole features refer to the cylindrical profile and end face edge information of the pin hole extracted through geometric analysis of point cloud data, including the center coordinates, axial direction vector, and hole diameter parameters. The six-dimensional deviations include translational deviation Δx along the X-axis, translational deviation Δy along the Y-axis, translational deviation Δz along the Z-axis, yaw deviation Δyaw around the Z-axis, roll deviation Δroll around the X-axis, and pitch deviation Δpitch around the Y-axis.

[0048] Specifically, after receiving 3D point cloud data acquired by a 3D vision camera, the control system locates the pin hole region within the point cloud data and extracts the geometric features of the pin hole. The control system obtains the pin hole's axial direction and center position by performing cylindrical surface fitting on the point cloud of the pin hole region, and determines the pin hole's plane normal vector by combining this with end-face edge fitting. Using the fitted pin hole center coordinates and axial direction as the target pose in the reference vehicle stitching coordinate system, the control system performs a difference calculation with the current pose of the vehicle to be stitched, obtaining a six-dimensional deviation.

[0049] In some embodiments, the identification of pin hole features and the calculation of six-dimensional deviation can be achieved in several ways: Optionally, the control system performs normal vector estimation on the 3D point cloud data, extracts edge point sets through the normal vector abrupt change region, and then performs RANSAC circle fitting and cylindrical surface fitting on the edge point sets to obtain the center and axis direction of the pin hole. Based on the calibration transformation matrix between the camera coordinate system and the stitching coordinate system, the center pose is transformed to the stitching coordinate system before calculating the six-dimensional deviation. Optionally, the control system performs Euclidean clustering on the 3D point cloud data to separate independent point cloud clusters in the pin hole region, performs least-squares cylindrical surface fitting on these point cloud clusters to obtain the axis parameters and center coordinates, and then combines the 3D vision camera extrinsic parameter calibration matrix to transform the results to the stitching coordinate system to calculate the six-dimensional deviation. It is understood that other point cloud feature extraction and coordinate transformation methods can also be used to calculate the six-dimensional deviation, which is not limited here.

[0050] S203. Decouple the six-dimensional deviation, and use the degree of freedom component of the six-dimensional deviation corresponding to the ground plane motion as the plane motion control quantity, and use the degree of freedom component of the six-dimensional deviation corresponding to the vertical displacement and attitude angle as the suspension adjustment control quantity.

[0051] Decoupling refers to the process of grouping and mapping the deviations of the six degrees of freedom in space according to the motion capabilities of the actuators. Planar motion control quantities include X-axis translational deviation Δx, Y-axis translational deviation Δy, and yaw deviation Δyaw, which are corrected by the travel and steering system through vehicle planar motion. Suspension adjustment control quantities include Z-axis translational deviation Δz, roll deviation Δroll, and pitch deviation Δpitch, which are corrected by the hydraulic suspension unit through the extension and retraction of cylinders on each axis.

[0052] Specifically, after obtaining the six-dimensional deviations, the control system groups and processes these deviations according to the kinematic constraints of the SPMT module. The travel and steering system has the ability to perform translational and yaw rotational movements in the ground plane, but it does not have the ability to change the vertical height of the chassis. The hydraulic suspension unit has the ability to independently adjust the support height of each axis, and can change the vertical rise and fall, pitch and roll angles of the chassis through differentiated extension and retraction, but it does not have the ability to drive the vehicle to translate. Based on the above kinematic constraints, the control system assigns Δx, Δy, and Δyaw to the travel and steering system as planar motion control quantities, and assigns Δz, Δroll, and Δpitch to the hydraulic suspension unit as suspension adjustment control quantities.

[0053] After the control system completes the calculation of the six-dimensional deviation, the decoupling process specifically includes: the control system representing the six-dimensional deviation as a six-element vector. The first three components represent translational deviations, and the last three components represent angular deviations. The control system maintains a pre-configured 6×6 decoupling assignment matrix M. Each row of this matrix corresponds to the input weight of an actuator channel. The channel row corresponding to the travel and steering system has non-zero weight values ​​(usually 1) only in the columns corresponding to Δx, Δy, and Δyaw, and zero in the columns corresponding to Δz, Δroll, and Δpitch. Conversely, the channel row corresponding to the hydraulic suspension unit has non-zero weight values ​​only in the columns corresponding to Δz, Δroll, and Δpitch, and zero in the columns corresponding to Δx, Δy, and Δyaw. The control system maps the six-dimensional deviation vector E to the actuator control vector U through matrix multiplication U=M·E, where the first three components of U... The last three components constitute the planar motion control quantity. This constitutes the suspension adjustment control quantity. The reason for using matrix allocation instead of simple index extraction is that there are weak coupling relationships between certain deviation dimensions in actual engineering (for example, when the vehicle moves laterally, the tire side slip will cause a small change in the yaw angle). The non-zero weights of the allocation matrix can be fine-tuned according to the coupling coefficient of the calibration test to achieve feedforward compensation.

[0054] For example, if calibration tests show that every 1 mm of Y-axis lateral displacement causes an additional 0.002° yaw angle offset, then a coupling compensation weight of 0.002 can be set in the Δy column of the yaw channel of the travel steering system in the allocation matrix. This will cause the yaw angle control quantity sent to the travel steering system to have a feedforward correction term of 0.002·Δy added to the original Δyaw. After decoupling, the output planar motion control quantity and suspension adjustment control quantity enter their respective independent control loops, driving the corresponding actuators to move without interference.

[0055] S204. Send the planar motion control quantity to the walking and steering system to drive the vehicle to be spliced ​​to correct its planar posture.

[0056] In this context, planar pose refers to the combination of the vehicle's position coordinates (X, Y) and yaw angle on the ground plane. Planar pose correction refers to the process by which the control system issues motion commands to the travel and steering system to cause the vehicle to perform displacement and steering on the ground plane, thereby bringing the deviation components in the planar motion control quantities close to zero.

[0057] Specifically, the control system converts the decoupled planar motion control quantities (Δx, Δy, Δyaw) into a motion command format executable by the travel and steering system, including the steering angle and travel speed parameters of each wheel set. The control system determines the vehicle's motion mode (forward, lateral, or crabbing) based on the combined direction of Δx and Δy, and determines the additional steering correction based on Δyaw. These parameters are then sent to the hydraulic valve groups and hydraulic motors of each wheel set in the travel and steering system, driving the vehicle to be assembled to perform corresponding displacement and steering movements on the ground, continuously reducing planar deviation. Optionally, the control system can also correct each planar deviation component sequentially in the order of yaw priority followed by translation.

[0058] S205. The suspension adjustment control quantity is sent to the hydraulic suspension unit to adjust the extension length of each set of suspension cylinders of the vehicle to be spliced ​​and the reference vehicle in order to correct the vertical attitude of the frame.

[0059] The vertical attitude of the chassis refers to the combined state of the chassis's height in the vertical direction and its pitch and roll angles around the horizontal axis. Each set of suspension cylinders refers to an independent hydraulic support cylinder distributed at each axis position of the SPMT (Special Purpose Motor). The extension length of each set of cylinders can be controlled independently. Extension length refers to the amount by which the piston rod of the suspension cylinder extends relative to the cylinder barrel; its change directly alters the chassis height at the corresponding support point.

[0060] Specifically, the control system converts the decoupled suspension adjustment control quantities (Δz, Δroll, Δpitch) into target extension length change commands for each group of suspension cylinders. Based on the chassis geometry and the spatial layout of the cylinders on each axle, the control system assigns Δz to equal extension / retraction commands for each group of cylinders, Δroll to differential extension / retraction commands for the left and right cylinders, and Δpitch to differential extension / retraction commands for the front and rear axle cylinders. These commands are then sent to the electromagnetic proportional valve groups of the hydraulic suspension units of the vehicle to be spliced ​​and the reference vehicle, driving the corresponding cylinders to extend / retract by the specified amount, thus aligning the pin holes at the splicing ends of the two vehicles in the vertical direction.

[0061] Among them, the control system will control the suspension adjustment amount When converting the command to change the target extension length of each group of suspension cylinders, the specific steps are as follows: Assuming the SPMT module's frame has N axes (e.g., N=4), with one group of suspension cylinders distributed on each side of each axis, there are a total of 2N groups of cylinders. The control system establishes a frame geometric model, using the frame's geometric center as a reference point. The lateral coordinate of the i-th group of cylinders relative to this reference point is... The vertical axis is .

[0062] The control system calculates the required change in extension length for the i-th group of hydraulic cylinders based on the small-angle linearization assumption. When Δpitch and Δroll are small angles, it is approximately equal to The physical meaning of the above formula is: when the six-dimensional deviation represents the deviation (current value minus target value) of the current pose of the vehicle to be stitched relative to the target pose, the hydraulic cylinder needs to perform a compensation action opposite to the direction of the deviation to eliminate the deviation. Wherein, Drive all hydraulic cylinders to extend and retract in the opposite direction by the same amount to eliminate the vertical height difference; The pitch angle deviation is eliminated by generating a front-to-back difference in the reverse extension and retraction through the different longitudinal positions of each cylinder. The roll angle deviation is eliminated by generating left-right differences and reversing the extension / retraction based on the different lateral positions of each cylinder. The control system will then adjust the above 2N cylinders accordingly. The values ​​are respectively sent as the target extension length increments of the corresponding oil cylinders to the control channels of each electromagnetic proportional valve of the hydraulic suspension unit.

[0063] For example, for an SPMT module with 4 axes and a total of 8 suspension cylinders, if the current suspension adjustment control values ​​are Δz = +3mm (meaning the current chassis is 3mm higher than the target), Δpitch = +0.1° (approximately 0.00175rad, meaning the current front end is higher than the target), and Δroll = -0.05° (approximately -0.000873rad), and the longitudinal coordinates of each axis are x = [-1.5m, -0.5m, +0.5m, +1.5m], and the lateral coordinates are y = [-0.8m, +0.8m], then the target change of the left cylinder of the first axis is: , This means the cylinder needs to retract by 1.073mm. Physically, this means the overall frame is 3mm too high and needs to be lowered (cylinder retraction). However, this cylinder is located at the rear (x=-1.5m) and the front is too high (Δpitch>0), so the required descent at the rear is less than the overall descent. Calculations show a net retraction of 1.073mm at this position. Other cylinders are calculated using the same formula. The electromagnetic proportional valves in each channel of the hydraulic suspension unit control the valve core opening and oil flow direction based on the received target change value, driving the corresponding cylinder piston rod to extend or retract to the target position.

[0064] S206. When all six-dimensional deviations converge to the preset alignment tolerance range, a locking command is issued to lock the frame posture of the vehicle to be spliced ​​and the reference vehicle to complete the splicing alignment.

[0065] The preset alignment tolerance range refers to the threshold set consisting of the maximum allowable residual values ​​of the deviation components in each dimension. When all six dimensions of deviation components are less than the corresponding threshold, the alignment accuracy is determined to meet the pin assembly requirements. The lock-up command refers to the control signal issued by the control system to the hydraulic suspension unit to cut off the oil inlet and outlet passages of each suspension cylinder, so that the pistons of each cylinder are in a volume-locked state, preventing the frame height and attitude from changing under external disturbances.

[0066] Specifically, after updating the six-dimensional deviation in each control cycle, the control system compares each dimension's deviation component with the corresponding threshold within the preset alignment tolerance range. When all six dimensions' deviation components are less than their respective thresholds, the control system determines that alignment is complete and issues a locking command to the hydraulic suspension units of the vehicle to be assembled and the reference vehicle. Upon receiving the locking command, the hydraulic suspension unit closes the inlet and outlet valves of each suspension cylinder, sealing the hydraulic oil volume within the cylinders, thus maintaining the vehicle's height and attitude in their current state. Simultaneously, the control system issues a parking brake command to the travel and steering system, locking the vehicle's planar position and ensuring the pin holes remain coaxial during pin assembly. The control system can lock the vehicle's planar position by issuing a locking signal to the valve group of the hydraulic suspension unit to close the oil inlet and outlet passages of each cylinder and by issuing a parking brake command to the travel and steering system.

[0067] It should be noted that, in combination Figure 2 As shown in the process, the method of this application has a step loop with the control cycle as the cycle, that is: step S201 is the pre-start stage of the loop. In this stage, the control system establishes the stitching coordinate system and drives the vehicle to be stitched to approach from a distance to within the effective working distance of the 3D vision camera. This stage is executed once and does not participate in the subsequent cycle.

[0068] When the distance between the vehicle to be stitched and the target origin is less than the preset distance threshold, the control system triggers the 3D vision camera to start continuous acquisition and enters a periodic closed-loop control cycle.

[0069] Within each control cycle (typical cycle length is 100ms to 200ms, depending on the frame rate of the 3D vision camera and the computing power of the control system), steps S202 to S205 constitute the processing sequence of the loop: The control system first executes step S202 to perform pin hole feature recognition and six-dimensional deviation calculation on a newly acquired frame of 3D point cloud data from the 3D vision camera in this cycle; then executes step S203 to decouple the calculated six-dimensional deviation, separating the planar motion control quantity and the suspension adjustment control quantity; then executes steps S204 and S205 simultaneously to send the planar motion control quantity to the walking and steering system to drive the vehicle to correct its planar pose, and at the same time send the suspension adjustment control quantity to the hydraulic suspension unit to correct the vertical attitude of the frame.

[0070] After steps S204 and S205 are completed, the actuator of this control cycle is completed, and the control system then enters the starting position of the next control cycle to re-execute step S202, collect a new frame of point cloud data and recalculate the six-dimensional deviation, forming a closed-loop iterative structure of "collection → calculation → decoupling → execution → re-collection".

[0071] Step S206 is the termination and exit step of the loop: After calculating the current six-dimensional deviation in step S202 during each control cycle, the control system synchronously compares the deviation components of each dimension with the corresponding thresholds in the preset alignment tolerance range. If the deviation component of any dimension exceeds the corresponding threshold, the loop continues to run, proceeding to step S203 and subsequent steps; if the deviation components of all six dimensions are less than the corresponding thresholds, the control system determines that the alignment is complete, exits the loop, and executes the locking process of step S206, that is, it sends a locking command to the hydraulic suspension unit to close the oil circuits of each cylinder, and sends a parking brake command to the travel and steering system to lock the vehicle position, completing the splicing alignment.

[0072] Steps S201 to S206 above describe the basic implementation process of the SPMT autonomous splicing alignment control method based on 3D vision. This process achieves pin hole spatial alignment through six-dimensional deviation decoupling and coordinated control of planar motion and suspension adjustment. However, in actual SPMT splicing operation scenarios, the above basic process still faces further engineering implementation problems in terms of pin hole feature recognition accuracy, planar motion control stability, and the rationality of alignment tolerance determination. For example, the three-dimensional point cloud data collected by the 3D vision camera contains a large number of vehicle structure surfaces and ground background point clouds unrelated to pin holes. If pin hole feature search is performed on the entire scene point cloud, the calculation time will exceed the control cycle requirements and is prone to misidentification. Another example is that when the planar pose correction approaches the target position, the continuous speed control command is prone to overshoot oscillation due to system inertia, requiring a segmented control strategy to balance approach efficiency and final accuracy. Furthermore, if the preset alignment tolerance range is set with a fixed uniform value, it fails to reflect the differentiated constraints of the pin hole mechanical chamfer on the allowable deviation in each direction, which may lead to overly strict or lenient alignment judgment.

[0073] To address the above issues, the following will be discussed in conjunction with... Figure 3 The illustrated embodiment provides a more detailed description of the control flow for each stage. Please refer to [link / reference]. Figure 3 This is another flowchart illustrating the SPMT autonomous splicing alignment control method based on 3D vision in this application embodiment.

[0074] Combination Figure 3 The process shown is as follows: Figure 3 The detailed control cycle is in Figure 2 Based on the basic loop frame, four refinement and enhancement processes are embedded in the loop body: pin hole feature recognition is refined into bounding box truncation and geometric fitting subsequence (S302-S305); camera pose compensation is performed synchronously during suspension adjustment; oscillation convergence rate determination and gain adaptive adjustment are performed after deviation calculation; and tolerance dynamic determination based on chamfer geometry and drift margin is performed before convergence determination (S309-S311).

[0075] Figure 3 Each control cycle operates in the following sequence: The control system first reads the point cloud data of the current frame and the feedback values ​​of each sensor, and performs coordinate system compensation and correction on the point cloud according to the change in the extension length of the suspension cylinder in the previous cycle. Then, step S302 is executed sequentially to obtain the odometer increment and initial relative pose, step S303 to construct a three-dimensional bounding box to extract local point cloud, step S304 to extract edge and cylindrical surface features and fit pin hole features, and step S305 to calculate the six-dimensional deviation of this cycle and store it in the sliding window. Next, the oscillation convergence rate is calculated on the sliding window data, and the control gain is updated if necessary; Then, step S306 is executed to decouple and output the planar motion control quantity and the suspension adjustment control quantity; Then, simultaneously execute steps S307 (including segmented speed control strategy) and S308 to send instructions to the travel steering system and hydraulic suspension unit; Finally, the six-dimensional deviation of this cycle is compared dimension by dimension with the effective tolerance threshold determined in steps S309-S311. That is, if any dimension exceeds the threshold, the cycle continues to the next cycle. If all dimensions converge, the cycle is exited and the locking is performed in step S312 to complete the splicing and alignment.

[0076] S301. Establish a splicing coordinate system with the center of the splicing pin hole of the reference vehicle as the target origin. Drive the vehicle to be spliced ​​to approach the target origin until the distance is less than a preset distance threshold. Then, collect the three-dimensional point cloud data of the reference vehicle through the 3D vision camera on the vehicle to be spliced.

[0077] Refer to step S201, which will not be repeated here.

[0078] S302. Obtain the wheel odometer data of the vehicle to be spliced ​​and the initial relative pose between the vehicle to be spliced ​​and the reference vehicle.

[0079] Among them, wheel odometer data refers to the wheel rotation angle and speed information accumulated by the encoders installed in each wheel set of the vehicle to be stitched. After conversion using wheel diameter parameters, the cumulative travel distance and heading change of the vehicle can be obtained. Initial relative pose refers to the known spatial positional relationship between the vehicle to be stitched and the reference vehicle before the stitching operation starts. This positional relationship is recorded and determined by the global positioning unit at the end of the approach phase, including the three-dimensional translation vector and three-dimensional rotation angle between the two vehicles.

[0080] Specifically, in step S301, while the 3D vision camera starts acquiring data, the control system reads the cumulative pulse count values ​​of the encoders of each wheel group of the vehicle to be stitched and converts them into wheel odometer data. Simultaneously, the control system obtains the relative pose of the two vehicles recorded at the end of the approach phase from the global positioning unit as the initial relative pose. The wheel odometer data is used to estimate the incremental displacement of the vehicle to be stitched between two adjacent acquisition frames in subsequent point cloud processing, and the initial relative pose is used to map the point cloud data in the 3D vision camera coordinate system to the estimated position region in the stitching coordinate system.

[0081] In some embodiments, wheel odometer data and initial relative pose can be acquired in multiple ways: Optionally, the control system periodically reads the encoder pulse accumulation value from each wheel group drive controller via the CAN bus, calculates the linear displacement increment of each wheel group using the wheel diameter coefficient, and then calculates the overall translational and heading increments of the vehicle as wheel odometer data using a multi-wheel differential model. Simultaneously, it reads the pose coordinates output by the global positioning unit in the last frame of the approach phase as the initial relative pose. Optionally, the control system acquires the absolute position values ​​of each wheel group servo encoder at a microsecond-level synchronization cycle via the EtherCAT bus, calculates the cumulative displacement of the vehicle in the stitched coordinate system using a forward kinematics algorithm as wheel odometer data, and obtains the initial relative pose by latching the output value of the global positioning unit when the control system switches from the navigation approach phase to the visual fine-tuning phase. It is understood that other sensor combinations can also be used to acquire vehicle motion increment data and initial relative pose information between the two vehicles; this is not limited here. Under heavy load conditions, to quantify tire elastic deformation, the control system can use a pre-calibrated equivalent rolling radius parameter instead of the nominal wheel diameter for odometer conversion.

[0082] S303. Based on the wheel odometer data and the initial relative pose, determine the three-dimensional bounding box in the three-dimensional point cloud data and capture the local point cloud data.

[0083] The 3D bounding box refers to a cuboid spatial region centered on the estimated pin hole location and extending along three coordinate axes in a stitched coordinate system. Local point cloud data refers to a subset of point clouds contained within the 3D bounding box, which retains only valid measurement points around the pin hole.

[0084] Specifically, the control system determines the estimated position of the pin hole of the reference vehicle in the 3D vision camera coordinate system based on the initial relative pose. Then, it incrementally corrects this estimated position using wheel odometer data to obtain the updated position of the pin hole area in the camera coordinate system at the current moment. Using this updated position as the center, the control system expands to both sides by a preset half-length along each of the three coordinate axes, constructing a 3D bounding box. The control system traverses the coordinates of each point in the 3D point cloud data, retaining points whose coordinate values ​​fall within the boundary of the 3D bounding box, and removing background and interference points outside the bounding box, obtaining local point cloud data containing only measurement data near the pin hole area.

[0085] In some embodiments, the 3D bounding box and local point cloud data can be determined and extracted in several ways: Optionally, the control system uses the translation vector in the initial relative pose to determine the estimated center point coordinates of the pin hole in the camera coordinate system, uses the incremental displacement in the wheel odometer data within the most recent control cycle to correct the center point coordinates, and then constructs an axis-aligned bounding box with a preset size value as the geometric center using the corrected center point. The point cloud within the bounding box is extracted using a point-by-point coordinate judgment method. Optionally, the control system inputs the initial relative pose and wheel odometer data into an extended Kalman filter for pose fusion estimation, constructs an oriented bounding box (OBB) centered on the estimated pin hole position of the fused output, and aligns the axes of this oriented bounding box with the coordinate axes of the stitching coordinate system. After coordinate rotation transformation, point-by-point judgment is performed to extract the local point cloud. It is understood that other spatial region division and point cloud extraction methods can also be used to obtain the local point cloud data of the pin hole region, which is not limited here.

[0086] S304. Extract edge features and cylindrical surface features from the local point cloud data, and fit the pin hole features based on the edge features and cylindrical surface features.

[0087] Edge features refer to the set of point cloud boundary lines formed in local point cloud data due to abrupt changes in depth or normal vectors between the pin hole end face and the surrounding frame surface. Cylindrical surface features refer to the cylindrical geometric parameters obtained by fitting a group of points distributed on the inner wall surface of the pin hole in the local point cloud data, including the cylinder axis direction vector, the coordinates of a point on the axis, and the cylinder radius. Pin hole features are a complete geometric description of the pin hole obtained by combining the hole opening plane position determined by edge features with the inner wall geometric parameters determined by cylindrical surface features, including the three-dimensional coordinates of the pin hole center, the hole axis direction, and the hole diameter.

[0088] Specifically, the control system estimates the normal vector of the local point cloud data obtained in step S303, performs a neighborhood consistency check on the normal vector direction of each point, and marks points whose normal vector direction changes by more than a preset angle threshold as edge candidate points. The control system performs clustering processing on all edge candidate points, grouping edge points belonging to the same connected region into one group, which constitutes the edge feature. The control system performs RANSAC cylindrical surface model fitting on the non-edge point group located in the inner wall region of the pin hole in the local point cloud data, iteratively solving for the cylindrical surface parameters that maximize the number of inner points, and obtains the cylindrical surface feature. The control system combines the hole opening plane position determined by the edge feature and the axis direction and radius determined by the cylindrical surface feature, and obtains the three-dimensional coordinates of the pin hole center through simultaneous constraint solution, which, together with the axis direction vector and radius parameters, constitute the pin hole feature.

[0089] In some embodiments, edge features and cylindrical surface features can be extracted and fitted to pin hole features in various ways: Optionally, the control system calculates the surface curvature value of each point in the local point cloud data, extracts high curvature points with curvature values ​​exceeding a preset curvature threshold as edge feature point sets, performs least-squares cylindrical surface fitting on the subset of remaining low curvature point groups that falls within the estimated pin hole depth range to obtain cylindrical surface features, and then performs cross-validation between the major and minor axes of the ellipse obtained by fitting the edge feature point sets and the cylindrical surface axis to output the pin hole features; Optionally, the control system uses a region growing method to segment the connected region of the inner wall of the pin hole from the local point cloud data, performs Hough transform on the point groups in this region to detect cylindrical surface parameters to obtain cylindrical surface features, and simultaneously performs B-spline curve fitting on the boundary points of the grown region to obtain edge features, and fuses the geometric parameters of the two to output the pin hole features. It is understood that other geometric feature extraction and fitting methods can also be used to obtain pin hole feature parameters, which are not limited here.

[0090] S305. Based on the pin hole characteristics, calculate the six-dimensional deviation of the vehicle to be spliced ​​relative to the splicing coordinate system.

[0091] The six-dimensional deviation calculation in this step is the spatial pose deviation obtained by transforming the pin hole feature parameters (three-dimensional coordinates of the center and the axis direction vector) obtained in step S304 from the 3D vision camera coordinate system to the stitching coordinate system and performing a difference calculation with the pose of the target origin.

[0092] Specifically, the control system uses the calibration extrinsic parameter matrix of the 3D vision camera relative to the vehicle frame to transform the center coordinates and axis direction vector of the pin hole feature from the camera coordinate system to the vehicle body coordinate system. The control system then uses the wheel odometer data and initial relative pose from step S302 to map the pin hole pose in the vehicle body coordinate system to the stitching coordinate system. The control system calculates the linear distance difference between the mapped pin hole center coordinates and the target origin of the stitching coordinate system in the X, Y, and Z axes, obtaining three translational deviation components. The control system calculates the angular deviation between the mapped pin hole axis direction vector and each reference axis direction of the stitching coordinate system, decomposing it into rotational angle differences around the three coordinate axes, obtaining three angular deviation components. These six components constitute a six-dimensional deviation quantity.

[0093] In some embodiments, coordinate system transformation and deviation calculation can be implemented in multiple ways: Optionally, the control system performs matrix concatenation operations on the camera extrinsic parameter matrix and the incremental pose transformation matrix calculated by the odometer to obtain a direct transformation matrix from the camera coordinate system to the stitched coordinate system. The coordinates of the pin hole center are then processed through this transformation matrix to obtain coordinate values ​​in the stitched coordinate system, and the difference between these values ​​and the target origin is calculated to obtain the translation deviation. The rotation components of the transformation matrix are then decomposed with the target attitude rotation matrix to obtain three Euler angle deviations. Optionally, the control system uses quaternions to represent the rotation transformation relationship, converting the rotation relationship from the camera coordinate system to the stitched coordinate system into a unit quaternion multiplication chain. The pin hole axis direction vector is rotated using quaternions and then subjected to vector cross product and dot product operations with the target axis direction to decompose the rotation angle deviations of each axis. It is understood that other coordinate transformation and deviation decomposition methods can also be used to calculate the six-dimensional deviation, which is not limited here.

[0094] S306. Decouple the six-dimensional deviation, and use the degree of freedom component of the six-dimensional deviation corresponding to the ground plane motion as the plane motion control quantity, and use the degree of freedom component of the six-dimensional deviation corresponding to the vertical displacement and attitude angle as the suspension adjustment control quantity.

[0095] Refer to step S203, which will not be repeated here.

[0096] S307. Send the planar motion control quantity to the walking and steering system to drive the vehicle to be spliced ​​to correct its planar posture.

[0097] Refer to step S204, which will not be repeated here.

[0098] The following provides a further explanation of the strategy for issuing and executing planar motion control quantities in step S307.

[0099] In step S307, the control system sends the planar motion control quantity to the travel and steering system to drive the vehicle to be spliced ​​to correct its planar pose. During this correction process, when the planar distance between the vehicle to be spliced ​​and the target origin is large, using a higher speed to approach can improve work efficiency; when the planar distance is reduced to close to the target position, the hydraulic actuator of the travel and steering system has a response delay and motion inertia, and if continuous speed control is still used, it will cause overshoot oscillation.

[0100] To address this issue, in some embodiments, the control system performs segmented speed control during the planar pose correction process. Specifically, the control system acquires the planar distance deviation of the vehicle to be stitched relative to the target origin. When the planar distance deviation is not less than a preset fine-tuning distance threshold, a continuous speed control command is issued to the travel and steering system to drive the vehicle to be stitched closer to the target origin. When the planar distance deviation is less than the fine-tuning distance threshold, a micro-motion control command is issued to the travel and steering system to drive the vehicle to be stitched to perform micro-motion displacement until the planar distance deviation converges. This micro-motion control command is a low-speed continuous creep command or a staggered walking command.

[0101] Among them, planar distance deviation refers to the Euclidean distance between the current planar coordinates of the vehicle to be stitched and the target origin on the ground plane (XY plane). Fine-tuning distance threshold refers to the distance boundary value that distinguishes between long-distance continuous approach mode and short-distance fine-tuning mode. This threshold is determined based on the hydraulic response delay of the travel and steering system and the vehicle's motion inertia parameters. Continuous speed control command refers to a motion command with a continuous speed target value issued by the control system to the travel and steering system, under which the vehicle travels at a continuous speed. Fine-motion control command refers to a small displacement motion command issued by the control system to the travel and steering system, including two forms: low-speed continuous creep command and clutch advance command. Low-speed continuous creep command refers to a motion command that travels continuously at an extremely low constant speed (e.g., 1mm / s to 5mm / s). Clutch advance command refers to a pulse-like motion command that issues a fixed small displacement amount (e.g., 0.5mm to 2mm) each time, after which the vehicle stops and waits for the next command.

[0102] Specifically, within each control cycle, the control system calculates the planar distance deviation based on the X-axis deviation Δx and Y-axis deviation Δy in the current six-dimensional deviations, i.e. The control system compares the planar distance deviation with a preset fine-tuning distance threshold. When the planar distance deviation is not less than the fine-tuning distance threshold, the control system calculates a continuous speed control command with the target origin direction as the target speed direction and a target speed magnitude proportional to the distance deviation, and sends it to the travel and steering system to drive the vehicle to continuously approach the target origin. When the planar distance deviation is less than the fine-tuning distance threshold, the control system switches to micro-motion control mode, generating micro-motion control commands based on the current deviation direction and remaining deviation amount, and sends them to the travel and steering system. In low-speed continuous creep mode, the control system issues extremely low speed target values ​​to make the vehicle move slowly, and adjusts the creep direction according to the updated deviation amount in each control cycle. In staggered creep mode, the control system issues displacement commands of fixed step length in each control cycle. After the vehicle executes the displacement of that step length, it stops and waits for the control system to collect a new frame of point cloud data and update the deviation amount before deciding whether to continue issuing the next command.

[0103] In some embodiments, the segmented speed control strategy can be implemented in several ways: Optionally, the control system uses trapezoidal speed planning to generate continuous speed control commands in the long-distance segment. The speed curve includes an acceleration segment, a constant speed segment, and a deceleration segment. When the planar distance deviation drops to a fine-tuning distance threshold, the speed planning enters the deceleration segment and switches to a micro-motion control mode after dropping to zero at the threshold. In the micro-motion control mode, the control system drives the vehicle to gradually approach the target position with a fixed-step staggered walking command. Optionally, the control system uses an exponentially decaying speed curve as the speed target value of the continuous speed control command in the long-distance segment, so that the vehicle speed decreases smoothly as the distance decreases. When the speed drops below a preset creep speed threshold, it switches to a low-speed continuous creep mode to continuously micro-motion until the planar distance deviation converges to the alignment tolerance range. It is understood that other segmented speed planning strategies can also be used to achieve accurate planar pose correction, which is not limited here.

[0104] S308. The suspension adjustment control quantity is sent to the hydraulic suspension unit to adjust the extension length of each set of suspension cylinders of the vehicle to be spliced ​​and the reference vehicle in order to correct the vertical attitude of the frame.

[0105] Refer to step S205, which will not be repeated here.

[0106] During the execution of steps S307 and S308 above, planar motion correction and vertical suspension adjustment are performed synchronously and in parallel. However, when the hydraulic suspension unit adjusts the extension length of each set of cylinders, the changes in the frame height and attitude will cause the 3D vision camera installed at the stitching end to produce corresponding position and attitude offsets. If this offset is not compensated, the coordinate reference of the subsequent frame point cloud data will drift, resulting in systematic errors in the calculation results of the six-dimensional deviation.

[0107] To address this issue, in some embodiments, the control system performs real-time compensation for the pose changes of the 3D vision camera during the synchronous parallel execution of planar motion correction and suspension adjustment. Specifically, the control system acquires the extension length changes of each group of suspension cylinders in the hydraulic suspension unit; calculates the pose change compensation parameters of the 3D vision camera based on the extension length changes and the chassis kinematic model of the vehicle to be stitched; and performs coordinate system correction on the continuously acquired 3D point cloud data based on the pose change compensation parameters, and updates the six-dimensional deviation.

[0108] The change in extension length refers to the difference between the current extension length of each group of suspension cylinders and the extension length at the start of the attitude adjustment action, which is obtained in real time by displacement sensors installed on each cylinder. The chassis kinematic model is a mathematical model describing the spatial displacement relationship of each point on the SPMT frame under the condition of varying extension lengths of the suspension cylinders on each axle. This model takes the extension length of each cylinder as input and outputs the changes in three-dimensional coordinates and three-dimensional attitude angles of any specified point on the frame (including the mounting position of the 3D vision camera). The pose change compensation parameters refer to the three-dimensional translational and three-dimensional rotational increments of the 3D vision camera mounting point calculated based on the chassis kinematic model.

[0109] Specifically, in each control cycle, the control system reads the current values ​​of the displacement sensors of each group of suspension cylinders and calculates the difference between these values ​​and the recorded values ​​at the start of the attitude adjustment action to obtain the change in the extension length of each group of cylinders. The control system inputs this change into the chassis kinematic model and, based on the installation coordinates and support direction of each cylinder on the frame, calculates the overall vertical lift, pitch angle change, and roll angle change of the frame. Then, based on the position vector of the 3D vision camera mounting point relative to the origin of the frame coordinate system, it maps the frame attitude change into the three-dimensional translational and rotational increments of the camera mounting point, i.e., the pose change compensation parameters. The control system converts these compensation parameters into a homogeneous transformation matrix and performs an inverse transformation operation on the three-dimensional point cloud data acquired by the 3D vision camera in the current frame, correcting the point cloud coordinates to the coordinate reference corresponding to the camera pose at the start of the attitude adjustment action. Based on the corrected point cloud data, the control system re-executes pin hole feature recognition and six-dimensional deviation calculation, outputting the updated six-dimensional deviation for decoupling and control command generation in the next control cycle.

[0110] The control system calculates the 3D vision camera pose change compensation parameters based on the change in the extension length of each group of suspension cylinders and performs coordinate system correction on the point cloud data, specifically including: The control system records the reference values ​​of each set of hydraulic cylinder displacement sensors at the moment the suspension attitude adjustment action is initiated. The current value is read in each subsequent control cycle. And calculate the change. The control system uses the changes in all 2N cylinders as input vectors. Input the chassis kinematics model.

[0111] The calculations in the chassis kinematic model include: firstly, calculating the vertical lift of the frame reference point based on the changes in each cylinder. Pitch angle change and the change in roll angle The specific method is to analyze the overdetermined system of equations. Perform a least squares solution (when the number of cylinders is greater than 3, the equation is overdetermined, and the optimal estimate can be obtained through least squares); then, based on the known coordinates of the 3D vision camera mounting point in the vehicle frame coordinate system... Calculate the three-dimensional translational increment of the camera mounting point caused by changes in the vehicle frame attitude:

[0112] Wherein, the X component The X-component represents the displacement of the camera mounting point along the X-axis caused by the pitch and rotation of the chassis; the Y-component... This indicates the displacement of the camera mounting point along the Y-axis caused by the tilting and rotation of the chassis; the Z component... This refers to the overall height adjustment of the chassis. This refers to the additional vertical displacement caused by pitch rotation at the camera's longitudinal position. This refers to the additional vertical displacement caused by the tilt rotation at the camera's lateral position. Simultaneously, it refers to the camera's three-dimensional rotation increment. (The yaw angle is not affected by the suspension adjustment).

[0113] The control system will and The combination forms a 4×4 homogeneous transformation matrix. For each point cloud coordinate point acquired in the current frame Perform inverse transformation This process restores the coordinates of the point to the coordinate reference corresponding to the camera's pose at the start of the attitude adjustment. The corrected point cloud data is then input into the pin hole feature recognition and six-dimensional deviation calculation stages. The output deviation does not include spurious components introduced by the camera's own displacement, ensuring the accuracy of the closed-loop control.

[0114] During the closed-loop control execution in steps S307 and S308, the synchronized actions of the travel steering system and the hydraulic suspension unit trigger a dynamic response in the chassis. Because the SPMT module has a large moment of inertia and suspension elasticity under heavy load conditions, abrupt changes in control commands may excite low-frequency oscillations in the six degrees of freedom of the chassis. When the decay rate of this oscillation is too slow, the six-dimensional deviation will fluctuate continuously within the continuous control cycle and fail to converge stably, affecting alignment efficiency and final accuracy.

[0115] To address this issue, in some embodiments, the control system adaptively adjusts the control gain to suppress chassis oscillation. Specifically, the control system acquires the six-dimensional deviation over multiple consecutive control cycles and calculates the chassis oscillation convergence rate of the vehicle to be assembled based on the amplitude and attenuation trend of the six-dimensional deviation over multiple consecutive control cycles. When the chassis oscillation convergence rate is lower than a preset stability threshold, a gain attenuation coefficient is determined based on the chassis oscillation convergence rate. The control gain of the driving and steering system and the hydraulic suspension unit is adjusted based on the gain attenuation coefficient to update the planar motion control quantity and the suspension adjustment control quantity.

[0116] The chassis oscillation convergence rate is a numerical indicator characterizing the rate at which the amplitude of the six-dimensional deviation decays within a continuous control cycle. The control system extracts the peak envelope from the time series of the six-dimensional deviation over multiple consecutive control cycles and calculates the amplitude decay ratio between adjacent peaks; this ratio is the chassis oscillation convergence rate. A higher convergence rate indicates rapid oscillation decay and a stable system; a lower convergence rate indicates slow oscillation decay and an underdamped system. The preset stability threshold is the dividing line for determining whether the system oscillation is within the normal decay range; values ​​below this threshold indicate that the oscillation has not converged effectively. The gain attenuation coefficient is a coefficient used to multiply the current control gain to reduce the output force of the actuator. Its value ranges from 0 to 1; the smaller the value, the greater the gain reduction. Control gain refers to the proportional coefficient in the control system that maps the deviation to the amplitude of the actuator control command, including the speed gain of the travel and steering system and the flow gain of the hydraulic suspension unit.

[0117] Specifically, the control system maintains a sliding window buffer for a six-dimensional deviation sequence over N consecutive control cycles (N being a preset window length, such as 20 to 50 cycles). The control system extracts local maxima and minima from each dimension of the deviation component in this sequence to form a peak sequence. It calculates the amplitude ratio between two adjacent peaks in the same direction and takes a weighted average of the amplitude ratios for all dimensions to obtain the overall chassis oscillation convergence rate. The control system compares this convergence rate with a preset stability threshold. If the convergence rate is lower than the stability threshold, it indicates that the current control gain is causing an overreaction in the system. The control system calculates a gain attenuation coefficient based on the difference between the convergence rate and the stability threshold using a preset mapping function. The control system multiplies the speed gain of the current driving and steering system and the flow gain of the hydraulic suspension unit by the gain attenuation coefficient to obtain an updated control gain. Based on the updated control gain, it recalculates the planar motion control quantity and the suspension adjustment control quantity and sends them to the corresponding actuators. After gain attenuation, the output force of the actuators decreases, the amplitude of the chassis dynamic response decreases accordingly, and the oscillation gradually converges. When the convergence rate rises above the stability threshold, the control system gradually restores the gain to its initial value to maintain adjustment efficiency.

[0118] The control system calculates the chassis oscillation convergence rate and determines the gain attenuation coefficient accordingly. Specifically, the control system maintains a sliding window of length N (e.g., N=30). Each control cycle stores the component values ​​of the current six-dimensional deviation in the corresponding positions within the window. When the window is full, it updates the window using a first-in, first-out (FIFO) method. For the deviation time series of each dimension within the window, the control system performs peak detection: it iterates through the 2nd to N-1th sampling points in the sequence. If the absolute value of the deviation at a certain sampling point is simultaneously greater than the absolute values ​​of the deviations of its two adjacent sampling points, then that point is marked as a local peak.

[0119] Suppose that the absolute values ​​of the peak values ​​detected in a certain dimension are arranged in chronological order as follows: (k≥2), the control system calculates the adjacent peak ratio sequence. Take the average decay ratio of this dimension as .when This indicates that the amplitude of the oscillation in that dimension is decreasing. The smaller the value, the faster the decay; when This indicates that the oscillation in that dimension has not decayed or has even diverged. The control system calculates a weighted average of the average decay ratios across all six dimensions according to the weight of the deviation amplitude in each dimension, yielding the overall frame oscillation convergence rate. ,in The weight coefficients for each dimension and The convergence rate CR ranges from negative infinity to 1; a larger CR value indicates faster system convergence. The control system compares CR with a preset stability threshold. (like ) Comparison, if If the system is in an underdamped oscillation state, the control gain needs to be reduced.

[0120] The control system calculates the gain attenuation coefficient based on the difference between the convergence rate and the threshold. ,in To set a minimum allowable gain attenuation limit (e.g., 0.3), preventing the gain from dropping too low and causing excessively sluggish system response, the control system adjusts the speed proportional gain of the current driving and steering system. and the flow ratio gain of the hydraulic suspension unit Multiply by λ to obtain the updated gain. and The planar motion control and suspension adjustment control values ​​for the current cycle are recalculated using the updated gain. For example, if the absolute values ​​of the three consecutive peak values ​​of the Y-axis deviation within the current sliding window are 2.1mm, 1.9mm, and 1.85mm, then the average attenuation ratio in that dimension is... The corresponding dimensional convergence contribution is This indicates that the decay of this dimension is slow. If the six dimensions are weighted and averaged... ,but The control system reduces both speed gain and flow gain to 50% of their original values, halving the actuator output amplitude in the next cycle to suppress oscillations. When subsequent window data shows that CR rises above 0.3, the control system... Gradually restore the gain to its original value.

[0121] S309. Obtain the mechanical chamfer parameters of the splicing pin holes of the reference vehicle and the vehicle to be spliced.

[0122] The mechanical chamfering parameters refer to the geometric dimensions of the guide chamfer at the pin hole opening, including the chamfer angle and chamfer depth. They may also include the pin hole diameter, pin diameter, and effective axial depth of the pin hole. The chamfer angle is the angle between the chamfered surface at the pin hole opening and the pin hole axis. The chamfer depth is the projected length of the chamfered surface along the pin hole axis. The pin hole diameter, pin diameter, and effective axial depth of the pin hole are the mating geometric parameters required to determine the pin's deflection limit within the hole. The function of the mechanical chamfer is to provide a guide cone surface when the pin is inserted, allowing the pin to slide into the pin hole along the chamfered surface even with minor deviations.

[0123] Specifically, the control system reads the chamfer angle and chamfer depth values ​​corresponding to the splicing pin hole models of the reference vehicle and the vehicle to be spliced ​​from the pre-stored SPMT module mechanical parameter database. This parameter database is entered by engineers based on the pin hole machining drawings during the system deployment phase. After obtaining the mechanical chamfer parameters of the pin holes on both sides, the control system uses them as the basis for calculating the allowable deviations in each direction in the subsequent step S310.

[0124] In some embodiments, the mechanical chamfering parameters of the splicing pin holes can be obtained in several ways: Optionally, the control system reads the chamfering angle and chamfering depth parameters of the pin hole model corresponding to the vehicle number index from a locally stored mechanical parameter database; Optionally, the control system performs conical fitting on the point cloud of the pin hole opening area using a 3D vision camera, and calculates the chamfering angle and chamfering depth parameters based on the fitted conical half angle and conical height to accommodate the situation where there is a deviation between the actual chamfering size and the design value after pin hole wear. It is understood that other parameter acquisition methods can also be used to obtain the mechanical chamfering parameters of the pin holes, which are not limited here.

[0125] S310. Based on the mechanical chamfer parameters, calculate the first translational tolerance and the first yaw angle tolerance along the pin insertion direction, and the second translational tolerance perpendicular to the pin insertion direction.

[0126] The first translational tolerance refers to the maximum allowable translational deviation along the pin insertion direction (i.e., the X-axis direction of the splicing coordinate system). The first yaw angle tolerance refers to the maximum allowable yaw angle deviation around the vertical axis (i.e., the Z-axis of the splicing coordinate system). The second translational tolerance refers to the maximum allowable translational deviation perpendicular to the pin insertion direction (i.e., the Y-axis and Z-axis directions of the splicing coordinate system). The reason why the first translational tolerance is greater than the second translational tolerance is that: along the pin insertion direction, the chamfered conical surface provides a longer guide stroke, allowing for a larger axial positional deviation; perpendicular to the insertion direction, the deviation is directly reflected in the radial clearance occupied between the pin and the hole wall, resulting in a smaller guide margin.

[0127] Specifically, the control system calculates the tolerances in each direction based on the chamfer angle α and chamfer depth d obtained in step S309. The first translational tolerance along the pin insertion direction is equal to the chamfer depth d, within which the pin can slide along the chamfered conical surface without radial interference. The second translational tolerance perpendicular to the insertion direction is equal to the chamfer depth d multiplied by the tangent of the chamfer angle α. This value represents the maximum guiding displacement provided by the chamfered surface in the radial direction. The first yaw angle tolerance is calculated based on the pin hole diameter, pin diameter, and pin hole axial depth parameters, determining the maximum yaw angle value corresponding to the pin's oscillation within the hole until it contacts the hole wall. Where D is the diameter of the pin hole. Where is the pin diameter, and L is the effective axial depth of the pin hole.

[0128] The process of establishing anisotropic alignment tolerances based on mechanical chamfering parameters and deducting drift allowances in the control system specifically includes: the control system first obtains the pin hole chamfering angle α from the parameter database (e.g., ...). and chamfer depth (like ), and the diameter D of the pin hole (e.g. ), pin diameter (like ) and the effective axial depth L of the pin hole (e.g. ).

[0129] Along the pin insertion direction (X-axis), the chamfered conical surface provides a depth of [missing information] in the axial direction. Within the guide stroke, the front end of the pin can slide naturally along the conical surface, thus the first translational tolerance... Perpendicular to the insertion direction (Y-axis and Z-axis), the radial guiding displacement provided by the chamfered conical surface is equal to the radial projection component of the conical surface, i.e., the second translational tolerance. First yaw angle tolerance This indicates the maximum permissible deflection angle of the pin within the hole, determined by the geometric limit when the pin is tilted within the hole until both ends contact the opposite sides of the hole wall: .

[0130] The above three tolerance values ​​reflect the anisotropic characteristics: This reflects the physical fact that the tolerance along the insertion direction is much larger than the radial tolerance.

[0131] The control system then performs drift margin deduction: it reads the combined attitude drift rates of the two vehicles in the locked state from the parameter table, assuming the drift rate in the X-axis direction. Y / Z axis drift rate Yaw angle drift rate Allowable time for pin assembly The expected drift margins for each dimension are as follows: , , .

[0132] The effective tolerance value is the original tolerance minus the drift allowance: First effective translation tolerance Second effective translation tolerance Effective yaw angle tolerance .

[0133] The control system constructs the final preset alignment tolerance range based on the above effective tolerance values: the six-dimensional tolerance vector is... Among them, the roll angle threshold and pitch angle threshold Depend on Divide by the longitudinal distance between the centers of the two pin holes (like The conversion yields: .

[0134] In some embodiments, tolerances in each direction can be calculated in multiple ways: Optionally, the control system establishes a three-dimensional geometric interference model between the pin hole and the pin shaft, and iteratively solves for the limit displacements of the pin shaft when it translates and rotates in each direction until it makes geometric contact with the chamfered surface or the hole wall, using these as the tolerance values ​​in each direction; alternatively, the control system uses analytical geometric formulas to directly calculate the geometric limit displacements in each direction as the tolerance values ​​based on the chamfer angle, chamfer depth, pin hole diameter, and pin shaft diameter. It is understood that other geometric analysis methods can also be used to calculate the permissible deviation values ​​in each direction, which are not limited here. In practical applications, the control system can apply a safety reduction factor of 0.8 to 0.9 to the above tolerance values ​​to cope with chamfer wear.

[0135] S311. Determine the preset alignment tolerance range based on the first translation tolerance, the first yaw angle tolerance, and the second translation tolerance.

[0136] The preset alignment tolerance range refers to a six-dimensional tolerance vector composed of independent thresholds for each dimension. In this vector, the threshold in the X-axis direction is the first translation tolerance, the thresholds in the Y-axis and Z-axis directions are the second translation tolerance, the threshold in the yaw angle dimension is the first yaw angle tolerance, and the thresholds in the roll angle and pitch angle dimensions are calculated based on the geometric relationship between the second translation tolerance and the axial length of the pin hole.

[0137] Specifically, the control system assigns the first translational tolerance calculated in step S310 as the threshold value of the X-axis component in the six-dimensional tolerance vector. The control system assigns the second translational tolerance as the threshold values ​​of the Y-axis and Z-axis components in the six-dimensional tolerance vector. The control system assigns the first yaw angle tolerance as the threshold value of the yaw angle component in the six-dimensional tolerance vector. For the threshold values ​​of the roll and pitch angle components, the control system calculates the corresponding angle threshold values ​​by dividing the second translational tolerance by the longitudinal distance between the centers of the two pin holes. These six threshold values ​​together constitute a preset alignment tolerance range, used in step S312 to determine the convergence of the six-dimensional deviation.

[0138] In some embodiments, the preset alignment tolerance range can be determined in several ways: Optionally, the control system directly assembles the tolerance values ​​in each direction into a six-dimensional threshold vector, with each dimension's threshold set independently and unrelated, forming an axis-aligned cuboid tolerance domain; Optionally, the control system establishes coupling constraint relationships between the tolerances in each dimension, using a multi-dimensional ellipsoid as the tolerance boundary. Convergence is determined when the weighted norm of the six-dimensional deviation is less than the ellipsoid radius, and the radii of each axis of the ellipsoid are determined by the first translation tolerance, the second translation tolerance, and the first yaw angle tolerance, respectively. It is understood that other tolerance domain construction methods can also be used to determine the preset alignment tolerance range, which is not limited here.

[0139] In step S311, the control system determines a preset alignment tolerance range based on the first translation tolerance, the first yaw angle tolerance, and the second translation tolerance. However, the above tolerance values ​​are calculated based on the static geometric parameters of the pin hole and do not take into account the creep drift that may occur in the frame attitude during the period from locking to the actual completion of pin assembly. When the SPMT module is in the suspension locking state, slight leakage of the internal seals of the hydraulic cylinder and tire rubber creep will cause the frame attitude to drift continuously at a slow rate. If the tolerance setting does not reserve a drift margin, the frame drift may cause the actual deviation to exceed the guide limit of the pin hole chamfer during the time window from locking to pin insertion. To address this issue, in some embodiments, the control system compensates for the drift margin within the tolerance range. Specifically, the control system acquires the attitude drift rates of the vehicle to be assembled and the reference vehicle in the locked state; calculates the expected drift margin for each dimension based on the attitude drift rate and a preset allowable pin assembly time; determines the first effective translation tolerance, effective yaw angle tolerance, and second effective translation tolerance corresponding to the first translation tolerance, the first yaw angle tolerance, and the second translation tolerance, respectively, based on the expected drift margin; and determines the preset alignment tolerance range based on the first effective translation tolerance, the effective yaw angle tolerance, and the second effective translation tolerance.

[0140] The attitude drift rate refers to the rate at which the attitude deviations of the vehicle in each dimension change over time when the hydraulic suspension is locked, measured in mm / s or ° / s. It is calibrated by the control system through continuous monitoring of statistical data on the changes in deviations in each dimension after locking during each operation. The allowable time for pin assembly refers to the maximum permissible time window from the issuance of the locking command by the control system to the completion of the pin insertion operation by the operator or robot. It is determined by the operation procedure and pre-stored in the control system parameter table. The expected drift margin refers to the maximum cumulative increase in deviations in each dimension due to attitude drift within the allowable time for pin assembly, equal to the product of the attitude drift rate and the allowable time for pin assembly. The first effective translation tolerance is the remaining value after subtracting the expected drift margin along the pin insertion direction from the first translation tolerance. The effective yaw angle tolerance is the remaining value after subtracting the expected drift margin in the yaw angle dimension from the first yaw angle tolerance. The second effective translation tolerance is the remaining value after subtracting the expected drift margin perpendicular to the insertion direction from the second translation tolerance.

[0141] Specifically, the control system reads the attitude drift rate calibration values ​​of the vehicle to be spliced ​​and the reference vehicle in the locked state from the parameter table for each dimension. The control system takes the superposition value of the drift rates of the two vehicles in the same dimension as the comprehensive drift rate of that dimension, because the drift directions of the two vehicles may be opposite, thus making the deviation growth rate between the pin holes of the two vehicles the sum of the two. The control system multiplies the comprehensive drift rate of each dimension by the allowable time for pin assembly to obtain the expected drift margin for each dimension. The control system subtracts the expected drift margin in the X-axis direction from the first translation tolerance calculated in step S310 to obtain the first effective translation tolerance; subtracts the expected drift margin in the yaw angle direction from the first yaw angle tolerance to obtain the effective yaw angle tolerance; and subtracts the expected drift margins in the Y-axis and Z-axis directions from the second translation tolerance to obtain the second effective translation tolerance. The control system replaces the original tolerance value in step S311 with the above effective tolerance values ​​to reconstruct the preset alignment tolerance range.

[0142] In some embodiments, the expected drift margin and effective tolerance can be calculated and determined in several ways: Optionally, the control system employs a linear drift model, directly multiplying the combined drift rate of each dimension by the allowable assembly time of the pin to obtain the expected drift margin, and then linearly subtracting this margin from the corresponding original tolerance value to obtain the effective tolerance value; Optionally, the control system employs an exponential decay drift model, fitting the exponential curve parameters of the drift amount of each dimension changing with time based on historical calibration data, substituting the allowable assembly time of the pin into the exponential curve equation to obtain the expected drift margin, and then subtracting this margin from the original tolerance value to obtain the effective tolerance value. It is understood that other drift modeling and margin calculation methods can also be used to determine the effective tolerance, and this is not limited here.

[0143] S312. When all six-dimensional deviations converge to the preset alignment tolerance range, a locking command is issued to lock the frame posture of the vehicle to be spliced ​​and the reference vehicle to complete the splicing alignment.

[0144] Refer to step S206, which will not be repeated here.

[0145] In this embodiment, a 3D vision camera is used to collect three-dimensional point cloud data of the splicing area in real time and calculate the six-dimensional spatial deviation of the vehicle to be spliced ​​relative to the reference vehicle's pin holes. After decoupling the six-dimensional deviation, the planar motion control quantity is sent to the driving and steering system, and the suspension adjustment control quantity is sent to the hydraulic suspension unit for coordinated closed-loop adjustment. Therefore, the control system can synchronously correct the planar position deviation and vertical attitude deviation between the vehicle to be spliced ​​and the reference vehicle within the six degrees of freedom in space. This effectively solves the problem in the prior art that only has the ability to perceive and correct planar dimensional deviations and motion, but cannot eliminate vertical misalignment caused by ground undulations and heavy-load settlement. It achieves high-precision coaxial alignment of the splicing pin holes of the SPMT module in three-dimensional space, reduces the risk of structural interference during the pin assembly process, and improves the alignment accuracy and assembly success rate of the splicing operation.

[0146] The control system in the embodiments of this application is described below from the perspective of hardware processing. Please refer to [link / reference needed]. Figure 4 This is a schematic diagram of the physical device structure of a control system in an embodiment of this application.

[0147] It should be noted that, Figure 4 The structure of the control system shown is merely an example and should not impose any limitations on the functionality and scope of use of the embodiments of this application.

[0148] like Figure 4 As shown, the control system includes a CPU 401, which can perform various appropriate actions and processes according to a program stored in ROM 402 or a program loaded into RAM 403 from storage section 408, such as executing the methods described in the above embodiments. RAM 403 also stores various programs and data required for system operation. The CPU 401, ROM 402, and RAM 403 are interconnected via bus 404. I / O interface 405 is also connected to bus 404.

[0149] The following components are connected to I / O interface 405: input section 406 including audio input devices, push-button switches, etc.; output section 407 including a liquid crystal display (LCD) and audio output devices, indicator lights, etc.; storage section 408 including a hard disk, etc.; and communication section 409 including a network interface card such as a LAN (Local Area Network) card, modem, etc. Communication section 409 performs communication processing via a network such as the Internet. Drive 410 is also connected to I / O interface 405 as needed. Removable media 411, such as a disk, optical disk, magneto-optical disk, semiconductor memory, etc., are installed on drive 410 as needed so that computer programs read from them can be installed into storage section 408 as needed.

[0150] Specifically, according to embodiments of this application, the processes described above with reference to the flowcharts can be implemented as computer software programs. For example, embodiments of this application include a computer program product comprising a computer program carried on a computer-readable medium, the computer program including a computer program for performing the methods shown in the flowcharts. In such embodiments, the computer program can be downloaded and installed from a network via communication section 409, and / or installed from removable medium 411. When the computer program is executed by CPU 401, it performs the various functions defined in this application.

[0151] The flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of systems, methods, and computer program products according to various embodiments of this application. Each block in a flowchart or block diagram may represent a module, segment, or portion of code, which contains one or more executable instructions for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions indicated in the blocks may occur in a different order than those shown in the drawings.

[0152] Specifically, the control system of this embodiment includes a processor and a memory. The memory stores a computer program. When the computer program is executed by the processor, it implements the SPMT autonomous splicing alignment control method based on 3D vision provided in the above embodiment.

[0153] In another aspect, this application also provides a computer-readable storage medium, which may be included in the control system described in the above embodiments; or it may exist independently and not assembled into the control system. The storage medium carries one or more computer programs, which, when executed by a processor of the control system, cause the control system to implement the 3D vision-based SPMT autonomous stitching alignment control method provided in the above embodiments.

[0154] The above embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit it. Although this application 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 of the technical features. Such 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 this application.

[0155] As used in the above embodiments, depending on the context, the term "when..." can be interpreted as meaning "if...", "after...", "in response to determining...", or "in response to detecting...". Similarly, depending on the context, the phrase "when determining..." or "if (the stated condition or event) is detected" can be interpreted as meaning "if determining...", "in response to determining...", "when (the stated condition or event) is detected", or "in response to detecting (the stated condition or event)".

Claims

1. A method for autonomous splicing and alignment control of SPMT based on 3D vision, characterized in that, Applied to a control system, the method includes: A splicing coordinate system is established with the center of the splicing pin hole of the reference vehicle as the target origin. The vehicle to be spliced ​​is driven to approach the target origin until the distance is less than a preset distance threshold. Then, the 3D point cloud data of the reference vehicle is collected by the 3D vision camera on the vehicle to be spliced. Identify the pin hole features in the three-dimensional point cloud data and calculate the six-dimensional deviation of the vehicle to be stitched relative to the stitching coordinate system; the six-dimensional deviation includes translational deviation along three orthogonal translation axes and angular deviation around three orthogonal rotation axes; The six-dimensional deviation is decoupled, and the degree of freedom component corresponding to the ground plane motion in the six-dimensional deviation is used as the plane motion control quantity, and the degree of freedom component corresponding to the vertical displacement and attitude angle in the six-dimensional deviation is used as the suspension adjustment control quantity. The planar motion control quantity is sent to the walking and steering system to drive the vehicle to be spliced ​​to correct its planar posture. The suspension adjustment control quantity is sent to the hydraulic suspension unit to adjust the extension length of each set of suspension cylinders of the vehicle to be spliced ​​and the reference vehicle in order to correct the vertical attitude of the frame. When all six-dimensional deviations converge to within the preset alignment tolerance range, a locking command is issued to lock the frame orientation of the vehicle to be spliced ​​and the reference vehicle to complete the splicing alignment.

2. The method according to claim 1, characterized in that, The step of identifying pin hole features in the 3D point cloud data and calculating the six-dimensional deviation of the vehicle to be stitched relative to the stitching coordinate system specifically includes: Acquire the wheel odometer data of the vehicle to be spliced ​​and the initial relative pose of the vehicle to be spliced ​​and the reference vehicle; Based on the wheel odometer data and the initial relative pose, a three-dimensional bounding box is determined in the three-dimensional point cloud data and local point cloud data is captured. Edge features and cylindrical surface features are extracted from the local point cloud data, and the pin hole features are obtained by fitting the edge features and cylindrical surface features. Based on the pin hole features, the six-dimensional deviation of the vehicle to be spliced ​​relative to the splicing coordinate system is calculated.

3. The method according to claim 1, characterized in that, The steps of sending the planar motion control quantity to the travel and steering system to drive the vehicle to be spliced ​​to correct its planar posture, and the steps of sending the suspension adjustment control quantity to the hydraulic suspension unit to adjust the extension length of each set of suspension cylinders of the vehicle to be spliced ​​and the reference vehicle to correct the vertical posture of the frame, are executed synchronously and in parallel. In addition to the synchronous parallel execution, the method further includes: Obtain the change in the extension length of each group of suspension cylinders in the hydraulic suspension unit; Based on the change in the extension length and the chassis kinematic model of the vehicle to be spliced, calculate the pose change compensation parameters of the 3D vision camera; The coordinate system of the continuously acquired 3D point cloud data is corrected according to the pose change compensation parameters, and the six-dimensional deviation is updated.

4. The method according to claim 1, characterized in that, The step of sending the planar motion control quantity to the walking and steering system to drive the vehicle to be spliced ​​to correct its planar posture specifically includes: Obtain the planar distance deviation of the vehicle to be spliced ​​relative to the target origin; When the planar distance deviation is not less than a preset fine-tuning distance threshold, a continuous speed control command is sent to the walking and steering system to drive the vehicle to be spliced ​​closer to the target origin. When the planar distance deviation is less than the fine-tuning distance threshold, a micro-motion control command is issued to the walking and steering system to drive the vehicle to be spliced ​​to perform micro-motion displacement until the planar distance deviation converges; the micro-motion control command is a low-speed continuous creep command or a step-by-step advance command.

5. The method according to claim 1, characterized in that, Before the step of issuing a locking command to lock the frame orientation of the vehicle to be spliced ​​and the reference vehicle to complete the splicing alignment when all six-dimensional deviations converge to within the preset alignment tolerance range, the method further includes: Obtain the mechanical chamfer parameters of the splicing pin holes of the reference vehicle and the vehicle to be spliced; Based on the mechanical chamfer parameters, calculate the first translational tolerance and the first yaw angle tolerance along the pin insertion direction, and the second translational tolerance perpendicular to the pin insertion direction; the first translational tolerance is greater than the second translational tolerance; The preset alignment tolerance range is determined based on the first translation tolerance, the first yaw angle tolerance, and the second translation tolerance.

6. The method according to claim 5, characterized in that, The step of determining the preset alignment tolerance range based on the first translation tolerance, the first yaw angle tolerance, and the second translation tolerance specifically includes: Obtain the attitude drift rates of the vehicle to be spliced ​​and the reference vehicle in the locked state; Based on the attitude drift rate and the preset pin assembly allowance time, calculate the expected drift margin for each dimension. Based on the expected drift margin, determine the first effective translation tolerance, effective yaw angle tolerance, and second effective translation tolerance corresponding to the first translation tolerance, the first yaw angle tolerance, and the second translation tolerance, respectively; The preset alignment tolerance range is determined based on the first effective translation tolerance, the effective yaw angle tolerance, and the second effective translation tolerance.

7. The method according to any one of claims 1 to 6, characterized in that, After the steps of identifying pin hole features in the three-dimensional point cloud data and calculating the six-dimensional deviation of the vehicle to be stitched relative to the stitching coordinate system, the method further includes: The six-dimensional deviation is obtained over multiple consecutive control cycles, and the frame oscillation convergence rate of the vehicle to be spliced ​​is calculated based on the change amplitude and decay trend of the six-dimensional deviation over multiple consecutive control cycles. When the frame oscillation convergence rate is lower than the preset stability threshold, the gain attenuation coefficient is determined based on the frame oscillation convergence rate. The control gain of the travel steering system and the hydraulic suspension unit is adjusted according to the gain attenuation coefficient to update the planar motion control quantity and the suspension adjustment control quantity.

8. A control system, characterized in that, The control system includes: one or more processors and a memory; the memory is coupled to the one or more processors, the memory is used to store computer program code, the computer program code including computer instructions, and the one or more processors call the computer instructions to cause the control system to perform the method as described in any one of claims 1-7.

9. A computer-readable storage medium comprising instructions, characterized in that, When the instruction is executed on the control system, it causes the control system to perform the method as described in any one of claims 1-7.

10. A computer program product, characterized in that, When the computer program product is run on the control system, the control system performs the method as described in any one of claims 1-7.

Images