Automatic unhooking and hooking system based on stereoscopic vision and applied to AGV tractor

Abstract

The invention discloses an automatic unhooking and hooking system applied to an AGV tractor based on stereoscopic vision, and the system comprises a data collection module which obtains the operation state of the AGV, the characteristics of a traction saddle, and the position information of a traction hook; the data processing module is used for preprocessing data, denoising the point cloud, reconstructing a traction saddle three-dimensional model and extracting feature points; the position and posture calculation module is used for calculating the position and posture of the traction saddle, matching feature points, optimizing a graph structure and converting to an AGV coordinate system; the trajectory planning module is used for optimizing the motion trajectory and adjusting the pose of the AGV; the motion control module is used for analyzing a track, adjusting a path and optimizing unhooking in combination with feedback; and the task scheduling module plans a driving path and controls the AGV to execute unhooking. Through multi-sensor fusion, point cloud matching, trajectory optimization and intelligent control, AGV autonomous unhooking is achieved, the problems of low manual efficiency, high misoperation rate and the like are solved, unhooking precision is improved, manual intervention is reduced, safety and stability are enhanced, and AGV logistics is more efficient and reliable.

Description

Technical Field

[0001] The present invention relates to the field of stereoscopic vision technology, and in particular to an automatic unhooking system applied to an AGV tractor based on stereoscopic vision. Background Art

[0002] In the fields of industrial manufacturing and logistics transportation, AGVs (automated guided vehicles) and tractors are widely used in material handling and delivery operations, especially in scenarios such as warehousing, production workshops, and port terminals that require efficient material flow. The main task of the AGV tractor is to connect the material carts through a towing hook, automatically drive on the designated path, and complete material loading and unloading at the target site. However, when the material cart arrives at the target site, the towing hook must be separated from the AGV tractor so that the material cart can stop at the loading and unloading point for material handover. At present, most towing hook detachment methods still rely on manual operation, or require additional auxiliary equipment for detachment. This traditional method not only increases labor costs, but also leads to low work efficiency and poor reliability due to the uncontrollable factors of manual operation, and even poses a major safety hazard.

[0003] Existing methods for unhooking the towing hook generally include manual operation and mechanical assistance. The manual operation method relies on the operator to reach the material vehicle and manually pull or unlock the towing hook to separate the material vehicle from the towing vehicle. This method is not only labor-intensive, but also may cause operator fatigue in a continuous operation environment, increase the risk of misoperation, and affect work efficiency. At the same time, manual operation requires high skills from the operator, and the inconsistent operating levels of different personnel may lead to uncertainty in the unhooking time, which in turn affects the stability of material transportation. In addition, in certain harsh environments or high-risk areas, such as high temperature, high humidity, chemical transportation, etc., manual intervention in the unhooking operation of the towing hook will bring additional safety hazards.

[0004] Another common method is to use mechanical auxiliary equipment for unhooking, such as controlling the opening and closing of the towing hook through electric or hydraulic devices to achieve remote unhooking. Although this method reduces reliance on manual labor, it still has certain limitations. For example, the additional installation of mechanical devices increases equipment and maintenance costs, and the complex structure also increases the failure rate. If the unhooking mechanism mechanically jams or sensor failure occurs, it may cause unhooking failure, which in turn affects the continuous operation of the production line. In addition, mechanical unhooking systems usually rely on fixed tracks or specific stations, which limits their application scenarios and makes it difficult to meet the needs of flexible operations.

[0005] To address these challenges, intelligent sensing and automatic control technologies have been gradually applied to AGV systems in recent years to improve operational automation and efficiency. The rapid development of stereo vision technology, in particular, has enabled AGV tractors to perceive the spatial position of the towing hook and material cart in real time, providing a new technical path for automated unhooking. Traditional AGV positioning methods often rely on technologies such as magnetic navigation and lidar navigation, but these methods still have limitations for unhooking operations, which require high-precision docking. For example, magnetic navigation systems require the laying of magnetic strips, which reduces flexibility. While lidar can provide relatively accurate environmental modeling, it is susceptible to interference from obstacles in complex scenarios. In contrast, AGV tractors based on stereo vision can use multi-sensor data fusion to obtain real-time spatial position information of the towing hook, towing saddle, and material cart, enabling more precise automated unhooking.

[0006] However, the application of existing stereo vision solutions in the AGV tractor unhooking task still faces many challenges. First, in actual working conditions, there may be deviations in the parking positions of the AGV tractor and the material truck, resulting in unstable docking status between the towing hook and the traction saddle. Traditional monocular or binocular vision solutions are easily disturbed when dealing with complex lighting and dynamic environmental changes, affecting detection accuracy. Secondly, since the spatial position of the towing hook is constantly changing during the movement of the AGV tractor, the traditional fixed coordinate conversion method is difficult to accurately calculate the relative position of the towing hook, resulting in errors in the unhooking process. In addition, the existing automatic unhooking system lacks efficient trajectory optimization and control algorithms, making it difficult to complete accurate docking and stable unhooking within a limited time, affecting work efficiency.

[0007] Therefore, how to provide an automatic uncoupling system based on stereo vision for AGV tractors, so that it can achieve autonomous uncoupling of AGV tractors and material carts through high-precision posture perception, trajectory optimization and intelligent control, improve operational efficiency, reduce manual intervention costs, and adapt to changes in complex industrial environments, is an urgent problem that technicians in this field need to solve. Summary of the Invention

[0008] One purpose of the present invention is to propose an automatic unhooking system for AGV tractors based on stereo vision. The present invention makes full use of multi-sensor data fusion, point cloud recognition, deep learning optimization and motion trajectory planning technology, and describes in detail the algorithm and control strategy for the AGV tractor to accurately unhook the traction hook in a complex industrial environment. The system has the advantages of high unhooking accuracy, high docking efficiency, strong adaptability and full automation.

[0009] According to an embodiment of the present invention, an automatic unhooking system based on stereo vision and applied to an AGV tractor includes the following steps:

[0010] The data acquisition module is used to collect the operating status data of the AGV tractor, the geometric characteristics data of the traction saddle, and the spatial position information of the traction hook. It includes a ToF camera, an inertial measurement unit, and a depth sensor.

[0011] The data processing module is used to pre-process the collected data, remove noise from the cloud data, reconstruct the local three-dimensional model of the traction saddle, and extract feature point information;

[0012] The posture calculation module is used to calculate the posture information of the traction saddle, perform feature point matching and graph optimization, and convert it into the target docking point coordinates in the AGV tractor coordinate system;

[0013] The trajectory planning module is used to calculate the motion trajectory of the AGV tractor, optimize the path using an improved rapid exploration random tree algorithm, and adjust the position and motion direction of the AGV tractor based on the coordinates of the target docking point;

[0014] The motion control module is used to analyze the generated motion trajectory, adjust the movement path of the AGV tractor, and optimize the detachment process of the tractor hook in combination with feedback control;

[0015] The task scheduling module is used to plan the driving path of the AGV tractor and control the AGV tractor to move to the target site along the optimal path to perform the uncoupling task.

[0016] 2. A cable branch box integrated online monitoring system according to claim 1, characterized in that the modules are connected by the following method:

[0017] S1, collects the operating status data of the AGV tractor, the geometric feature data of the traction saddle, and the spatial position information of the traction hook, and integrates the multi-sensor data;

[0018] S2. Preprocess the multi-sensor data, remove noise from the point cloud data, reconstruct the local three-dimensional model of the traction saddle, and extract the feature point information of the traction saddle;

[0019] S3. Calculate the position information of the traction saddle, perform feature point matching and graph optimization, and convert it into the target docking point coordinates in the AGV tractor coordinate system;

[0020] S4. Based on the coordinates of the target docking point, the motion trajectory of the AGV tractor is calculated, and the improved rapid exploration random tree algorithm is used to optimize the path and adjust the position and driving direction of the AGV tractor;

[0021] S5. Analyze the motion trajectory, control the AGV tractor to adjust its position, and correct the motion trajectory based on visual feedback to align the tension support beam with the vertical guide hole of the traction saddle. Calculate the docking error and make fine adjustments.

[0022] S6. Based on the fine-tuned docking error, control the fork lift, adjust the force state of the towing hook, dynamically optimize the unhooking strategy using multi-sensor data, control the towing hook separation process, and detect the unhooking state;

[0023] S7. Based on the detection result of the unhooking state, the driving path of the AGV tractor is updated, the AGV tractor is controlled to leave the site along the optimized path, and the posture data of the unhooking process is recorded.

[0024] Optionally, S2 includes the following specific steps:

[0025] S21, receive the running status data of the AGV tractor, the geometric feature data of the traction saddle and the spatial position information of the traction hook, synchronize the multi-sensor data, and set the timestamps of the ToF camera, inertial measurement unit and depth sensor to be t tof , t imu and t depth , calculate the global time error:

[0026] Δt=max(t tof ,t imu ,t depth )-min(t tof ,t imu ,t depth );

[0027] Among them, t tof is the data acquisition timestamp of the ToF camera, t imu is the data acquisition timestamp of IMU, t depth is the data acquisition timestamp of the depth sensor, max(·) is the maximum value function, min(·) is the minimum value function, Δt is the maximum time difference of all sensor timestamps, and the linear interpolation method is used to align the data with different timestamps to generate the synchronized multi-sensor fusion dataset D sync ;

[0028] S22, from D sync The point cloud data of the traction saddle is extracted, and the voxel filtering method is used for noise reduction. The point cloud is uniformly sampled according to the grid size. The points in each voxel area are calculated by the mean to form the noise-reduced point cloud data, and the region growing algorithm is used for point cloud segmentation. The initial seed point p is selected. s , calculate the neighborhood point p n The normal vector and p s The angle between the normal vectors of :

[0029]

[0030] Among them, θ is the neighborhood point p n The normal vector and the initial seed point ps The angle between the normal vectors, arccos() is the inverse cosine of the inverse trigonometric function, N n is the neighborhood point p n Normal vector, N s is the initial seed point p s The normal vector of

[0031] S23, setting angle threshold θ th , when θ≤θ th When p n Classify them into the same area, repeat the iteration until all points are classified, and output the target point cloud area P of the traction saddle t ;

[0032] S24, based on P t Calculate the centroid of the target point cloud area, detect feature points using a method based on the curvature change rate, and extract the feature point information of the traction saddle:

[0033]

[0034] Among them, C t is the name of the centroid coordinate function of the target area, m is the total number of points in the area, p i ,p j is the target point cloud area P t The point in k i For point p i The curvature, N(p i ) is point p i The neighborhood point set, |N(p i )| is the number of points in the neighborhood point set, and the points whose curvature changes exceed the set threshold are extracted to form the feature point set F t ;

[0035] S25, based on P t and F t A local 3D model of the traction saddle is constructed, and the Poisson surface reconstruction method is used to solve the implicit function of the point cloud:

[0036]

[0037] Among them, f(x,y,z) is the reconstructed implicit surface function, is the gradient operator, δ(xp i ) represents the divergence function of the point cloud, m is the total number of point cloud points, and the local three-dimensional model M of the traction saddle is generated. t .

[0038] Optionally, S3 includes the following specific steps:

[0039] S31. Let the centroid of the traction saddle be represented by Ct (x c ,y c ,z c ), define the traction saddle local coordinate system C t (x′, y′, z′), establish the local coordinate system of the traction saddle, where p i (x i ,y i ,z i ) is the feature point set F t points in, m is the number of feature points, C t (x c ,y c ,z c ) is the centroid expression, the local coordinate system C t The origin of (x′, y′, z′) is set to C t , the direction of the coordinate axis is determined according to the distribution of feature points;

[0040] S32, select feature point set F t , calculate the initial position of the traction saddle and C t The spatial distribution of the feature point is constructed to form a distribution matrix

[0041]

[0042] in, is the relative position matrix of the feature points in the local coordinate system, (x c ,y c ,z c ) is the centroid coordinate, (x i ,y i ,z i ) is the coordinate of the feature point, n is the number of feature points;

[0043] S33, obtaining a pre-stored standard traction saddle feature point set F s , matching the local 3D model M of the traction saddle t , calculate F based on nearest neighbor matching t midpoint p i With F s midpoint p j The Euclidean distance between:

[0044]

[0045] Where D(i,j) is the set of traction saddle feature points F t midpoint p i With F s midpoint p j The Euclidean distance is selected, and the point pair with the smallest D(i,j) is selected as the matching pair to establish the point pair set Mf ;

[0046] S34, point pair set M f To observe the constraints, a graph G(V,E) based on pose optimization is established to solve the global rotation matrix and translation vector:

[0047]

[0048] Among them, R is the rotation matrix, T is the translation vector, and W ij is the weight matrix, p i is the current feature point of the traction saddle, p j For matching standard feature points, Gauss-Newton optimization algorithm is used to calculate R and T, and the rigid transformation matrix [R|T] is output;

[0049] S35, based on the rigid transformation matrix [R|T], the position of the traction saddle is transformed from the local coordinate system C t (x′, y′, z′) is converted to the AGV tractor coordinate system C agv (X,Y,Z), get the target docking point coordinates:

[0050] P agv =RF t +T;

[0051] Among them, P agv is the coordinate of the traction saddle feature point in the AGV tractor coordinate system, that is, the target docking point coordinate.

[0052] Optionally, S4 includes the following specific steps:

[0053] S41. Assume the initial position of the AGV tractor is P start , the target docking point coordinates are P agv , define the motion trajectory ξ as the start to P agv A collection of paths:

[0054] ξ={P0,P1,…,P n};

[0055] Where P0=P start , P n =P agv , P i (x i ,y i ,θ i ) is the intermediate state on the path, x i ,y i is the position coordinate of the AGV tractor in the path, θ i is the heading angle;

[0056] S42, set the search space to S, with P start As the root node, calculate the random sampling point P rand The closest point P in the current tree near The Euclidean distance of :

[0057]

[0058] Among them, d(P rand ,P near ) is the Euclidean distance, (x rand ,y rand ) is the coordinate of the random sampling point, (x near ,y near ) is the coordinate of the nearest point in the tree, along P near Towards P rand Expand step size Δd to generate a new node P new And add the path tree, repeat the iteration until P agv Join the path tree to form path ξ;

[0059] S43. Use the improved fast exploration random tree algorithm to optimize the path, and set the path cost function C(ξ) as the cumulative distance on the path:

[0060]

[0061] For each newly expanded node P new , search for the optimal parent node P in its neighborhood min ,satisfy:

[0062]

[0063] Among them, arg min is the variable value when the function takes the minimum value, N(P new ) is P new The set of neighboring nodes, update the path tree and optimize the path ξ;

[0064] S44, adjust the posture of the AGV tractor, calculate the posture adjustment amount of the AGV tractor based on the optimized path ξ, and set the current position as P cur , the target position is P agv , the adjustment amount ΔP is calculated as:

[0065] ΔP=P agv -P cur ;

[0066] Among them, ΔP(x Δ ,y Δ ,θ Δ ) represents the adjustment value of position and heading angle, and controls the AGV tractor to adjust its posture along the optimized path ξ so that it finally reaches P agv, complete the docking preparation.

[0067] Optionally, S5 includes the following specific steps:

[0068] S51, based on the posture adjustment amount ΔP of the AGV tractor, analyze the optimized motion trajectory and calculate the docking error, decomposing ΔP into the displacement deviation Δd and the heading angle deviation Δθ between the current position and the target position;

[0069] S52. Correct the motion trajectory based on visual feedback, call the ToF camera to detect the spatial posture of the traction saddle and the center position of the vertical guide hole, and calculate the posture error of the traction saddle based on the current trajectory point in the optimized path ξ:

[0070] ΔP t =R tof P hole +T tof -P opt ;

[0071] Among them, R tof is the rotation matrix detected by the ToF camera, T tof is the displacement vector detected by the ToF camera, P hole is the detection center coordinate of the vertical guide hole, P opt To optimize the current target trajectory point in the path ξ, ΔP t is the traction saddle relative to the optimized trajectory point P opt The pose error of

[0072] S53, adjust the AGV tractor posture and perform docking along the optimized path ξ, and calculate the adjusted control instructions:

[0073] U k =K p (Δd+ΔP t )+K θ Δθ+K ξ (ξ-P cur );

[0074] Among them, U k is the control instruction, K p is the position error gain coefficient, K θ is the angle error gain coefficient, K ξ is the path tracking gain coefficient, ξ-P cur Indicates the deviation of the current pose relative to the optimized path;

[0075] Calculate and fine-tune the final docking error:

[0076] ΔP f =P agv -P final ;

[0077] Where ΔP f is the final docking error of the AGV tractor, P final is the final posture of the AGV tractor after adjustment, if |ΔP f If |is less than the set threshold ∈, docking lock is performed; otherwise, the motion trajectory continues to be adjusted until the docking requirements are met.

[0078] Optionally, S6 includes the following specific steps:

[0079] S61, based on the fine-tuned docking error, control the fork lift. Assume that the current position of the fork of the AGV tractor is h cur , the target hook height is h goal , calculate the fork height error and calculate the control input based on the dynamic model:

[0080]

[0081] Among them, u h Fork control input, used to adjust the fork height, J f is the moment of inertia, indicating the inertia of the fork, c f is the damping coefficient, which represents the system's resistance to velocity changes, k f is the stiffness coefficient, which indicates the system's ability to recover from displacement, h is the current fork height, Δh=h goal -h cur is the fork height error, K p is the position error gain coefficient, K d is the differential gain coefficient, K i is the integral gain coefficient, which is used to adjust the dynamic response of the fork height control and output the corrected fork height h adj =h+u h ;

[0082] S62, adjust the height h based on the fork adj and the final docking error ΔP f , adjust the force state of the traction hook to gradually release the force and reach the zero force state. Suppose the current force vector of the traction hook is F cur , the target force state is zero force state F zero , calculate the force adjustment, and calculate the control input based on the dynamic equation:

[0083]

[0084] Among them, u θ It is the control input of the towing hook, used to adjust the force and rotation angle of the towing hook. his the moment of inertia, which indicates the inertia of the traction hook in rotational motion, c h is the damping coefficient, which represents the damping effect in rotational motion, k h is the stiffness coefficient, which indicates the recovery ability of the rotation angle, θ is the current rotation angle of the towing hook, θ zero is the rotation angle of the traction hook under zero force state, ΔF=F cur -F zero is the force adjustment, which represents the difference between the current force and the target force, and outputs the corrected rotation angle θ adj =θ+u θ ;

[0085] S63, detect the disconnection state and complete the separation, based on the corrected rotation angle θ adj Calculate the unhooking state, assuming the relative displacement of the towing hook during the unhooking process is D hook , the target complete detachment distance is D detach , calculate the dropout progress error ΔD, and optimize the state estimation based on the extended Kalman filter:

[0086]

[0087] in, is the optimal detachment state estimation after EKF optimization, X k is the current detachment state vector, including displacement and velocity, Z k is the sensor observation value, including the measured towing hook displacement and speed information, H is the observation matrix, which maps the system state to the measurement value, K is the Kalman gain matrix, which is used to optimize the state estimation, K θ is the rotation angle error gain coefficient, which is used to adjust the influence of the towing hook rotation error. If the value is less than the set threshold, the disconnection is determined to be complete.

[0088] The beneficial effects of the present invention are:

[0089] First, the present invention adopts stereo vision and multi-sensor fusion technology, which can perceive the spatial posture information of the AGV tractor, traction saddle and traction hook in real time, and optimize the recognition accuracy of the traction saddle through point cloud denoising and feature point matching, ensuring that the AGV tractor can accurately dock with the traction hook during the uncoupling process, thereby improving the stability and reliability of the system.

[0090] Secondly, the present invention introduces a feature point matching algorithm based on graph optimization to convert the spatial information of the traction saddle into the coordinate system of the AGV tractor, achieving high-precision target docking point positioning. At the same time, combined with the improved rapid exploration random tree algorithm, the motion trajectory of the AGV tractor is dynamically optimized to avoid the influence of trajectory deviation on the detachment accuracy and improve work efficiency.

[0091] Finally, in the process of controlling the unhooking of the towing hook, the present invention adopts a multi-sensor feedback mechanism, optimizes the unhooking state estimation through extended Kalman filtering, and adjusts the force state of the towing hook in combination with an adaptive sliding mode control strategy to ensure a smooth unhooking process and avoid mechanical jamming or accidental unhooking caused by uneven force, thereby improving the unhooking success rate, reducing manual intervention, and realizing the automated, intelligent, safe and efficient unhooking operation of the AGV tractor. BRIEF DESCRIPTION OF THE DRAWINGS

[0092] The accompanying drawings are used to provide a further understanding of the present invention and constitute a part of the specification. Together with the embodiments of the present invention, they are used to explain the present invention and do not constitute a limitation of the present invention. In the accompanying drawings:

[0093] Figure 1 This is a flow chart of a method for an automatic unhooking system for an AGV tractor based on stereo vision proposed by the present invention. DETAILED DESCRIPTION

[0094] The present invention will now be described in further detail with reference to the accompanying drawings, which are simplified schematic diagrams that illustrate the basic structure of the present invention in a schematic manner.

[0095] refer to Figure 1 , an automatic unhooking system for AGV tractors based on stereo vision, including:

[0096] The data acquisition module is used to collect the operating status data of the AGV tractor, the geometric characteristics data of the traction saddle, and the spatial position information of the traction hook. It includes a ToF camera, an inertial measurement unit, and a depth sensor.

[0097] The data processing module is used to pre-process the collected data, remove noise from the cloud data, reconstruct the local three-dimensional model of the traction saddle, and extract feature point information;

[0098] The posture calculation module is used to calculate the posture information of the traction saddle, perform feature point matching and graph optimization, and convert it into the target docking point coordinates in the AGV tractor coordinate system;

[0099] The trajectory planning module is used to calculate the motion trajectory of the AGV tractor, optimize the path using an improved rapid exploration random tree algorithm, and adjust the position and motion direction of the AGV tractor based on the coordinates of the target docking point;

[0100] The motion control module is used to analyze the generated motion trajectory, adjust the movement path of the AGV tractor, and optimize the detachment process of the tractor hook in combination with feedback control;

[0101] The task scheduling module is used to plan the driving path of the AGV tractor and control the AGV tractor to move to the target site along the optimal path to perform the uncoupling task.

[0102] 2. A cable branch box integrated online monitoring system according to claim 1, characterized in that the modules are connected by the following method:

[0103] S1, collects the operating status data of the AGV tractor, the geometric feature data of the traction saddle, and the spatial position information of the traction hook, and integrates the multi-sensor data;

[0104] S2. Preprocess the multi-sensor data, remove noise from the point cloud data, reconstruct the local three-dimensional model of the traction saddle, and extract the feature point information of the traction saddle;

[0105] S3. Calculate the position information of the traction saddle, perform feature point matching and graph optimization, and convert it into the target docking point coordinates in the AGV tractor coordinate system;

[0106] S4. Based on the coordinates of the target docking point, the motion trajectory of the AGV tractor is calculated, and the improved rapid exploration random tree algorithm is used to optimize the path and adjust the position and driving direction of the AGV tractor;

[0107] S5. Analyze the motion trajectory, control the AGV tractor to adjust its position, and correct the motion trajectory based on visual feedback to align the tension support beam with the vertical guide hole of the traction saddle. Calculate the docking error and make fine adjustments.

[0108] S6. Based on the fine-tuned docking error, control the fork lift, adjust the force state of the towing hook, dynamically optimize the unhooking strategy using multi-sensor data, control the towing hook separation process, and detect the unhooking state;

[0109] S7. Based on the detection result of the unhooking state, the driving path of the AGV tractor is updated, the AGV tractor is controlled to leave the site along the optimized path, and the posture data of the unhooking process is recorded.

[0110] In this embodiment, S2 includes the following specific steps:

[0111] S21, receive the running status data of the AGV tractor, the geometric feature data of the traction saddle and the spatial position information of the traction hook, synchronize the multi-sensor data, and set the timestamps of the ToF camera, inertial measurement unit and depth sensor to be t tof , t imu and t depth , calculate the global time error:

[0112] Δt=max(t tof ,t imu,t depth )-min(t tof ,t imu ,t depth );

[0113] Among them, t tof is the data acquisition timestamp of the ToF camera, t imu is the data acquisition timestamp of IMU, t depth is the data acquisition timestamp of the depth sensor, max(·) is the maximum value function, min(·) is the minimum value function, Δt is the maximum time difference of all sensor timestamps, and the linear interpolation method is used to align the data with different timestamps to generate the synchronized multi-sensor fusion dataset D sync ;

[0114] S22, from D sync The point cloud data of the traction saddle is extracted, and the voxel filtering method is used for noise reduction. The point cloud is uniformly sampled according to the grid size. The points in each voxel area are calculated by the mean to form the noise-reduced point cloud data, and the region growing algorithm is used for point cloud segmentation. The initial seed point p is selected. s , calculate the neighborhood point p n The normal vector and p s The angle between the normal vectors of :

[0115]

[0116] Among them, θ is the neighborhood point p n The normal vector and the initial seed point p s The angle between the normal vectors, arccos() is the inverse cosine of the inverse trigonometric function, N n is the neighborhood point p n Normal vector, N s is the initial seed point p s The normal vector of

[0117] S23, setting angle threshold θ th , when θ≤θ th When p n Classify them into the same area, repeat the iteration until all points are classified, and output the target point cloud area P of the traction saddle t ;

[0118] S24, based on P t Calculate the centroid of the target point cloud area, detect feature points using a method based on the curvature change rate, and extract the feature point information of the traction saddle:

[0119]

[0120] Among them, C tis the name of the centroid coordinate function of the target area, m is the total number of points in the area, p i ,p j is the target point cloud area P t The point in k i For point p i The curvature, N(p i ) is point p i The neighborhood point set, |N(p i )| is the number of points in the neighborhood point set, and the points whose curvature changes exceed the set threshold are extracted to form the feature point set F t ;

[0121] S25, based on P t and F t A local 3D model of the traction saddle is constructed, and the Poisson surface reconstruction method is used to solve the implicit function of the point cloud:

[0122]

[0123] Among them, f(x,y,z) is the reconstructed implicit surface function, is the gradient operator, δ(xp i ) represents the divergence function of the point cloud, m is the total number of point cloud points, and the local three-dimensional model M of the traction saddle is generated. t .

[0124] In this embodiment, S3 includes the following specific steps:

[0125] S31. Let the centroid of the traction saddle be represented by C t (x c ,y c ,z c ), define the traction saddle local coordinate system C t (x′, y′, z′), establish the local coordinate system of the traction saddle, where p i (x i ,y i ,z i ) is the feature point set F t points in, m is the number of feature points, C t (x c ,y c ,z c ) is the centroid expression, the local coordinate system C t The origin of (x′, y′, z′) is set to C t , the direction of the coordinate axis is determined according to the distribution of feature points;

[0126] S32, select feature point set F t , calculate the initial position of the traction saddle and C tThe spatial distribution of the feature point is constructed to form a distribution matrix

[0127]

[0128] in, is the relative position matrix of the feature points in the local coordinate system, (x c ,y c ,z c ) is the centroid coordinate, (x i ,y i ,z i ) is the coordinate of the feature point, n is the number of feature points;

[0129] S33, obtaining a pre-stored standard traction saddle feature point set F s , matching the local 3D model M of the traction saddle t , calculate F based on nearest neighbor matching t midpoint p i With F s midpoint p j The Euclidean distance between:

[0130]

[0131] Where D(i,j) is the set of traction saddle feature points F t midpoint p i With F s midpoint p j The Euclidean distance is selected, and the point pair with the smallest D(i,j) is selected as the matching pair to establish the point pair set M f ;

[0132] S34, point pair set M f To observe the constraints, a graph G(V,E) based on pose optimization is established to solve the global rotation matrix and translation vector:

[0133]

[0134] Among them, R is the rotation matrix, T is the translation vector, and W ij is the weight matrix, p i is the current feature point of the traction saddle, p j For matching standard feature points, Gauss-Newton optimization algorithm is used to calculate R and T, and the rigid transformation matrix [R|T] is output;

[0135] S35, based on the rigid transformation matrix [R|T], the position of the traction saddle is transformed from the local coordinate system C t (x′, y′, z′) is converted to the AGV tractor coordinate system C agv (X,Y,Z), get the target docking point coordinates:

[0136] P agv =RF t +T;

[0137] Among them, P agv is the coordinate of the traction saddle feature point in the AGV tractor coordinate system, that is, the target docking point coordinate.

[0138] In this embodiment, S4 includes the following specific steps:

[0139] S41. Assume the initial position of the AGV tractor is P start , the target docking point coordinates are P agv , define the motion trajectory ξ as the start to P agv A collection of paths:

[0140] ξ={P0,P1,…,P n};

[0141] Where P0=P start , P n =P agv , P i (x i ,y i ,θ i ) is the intermediate state on the path, x i ,y i is the position coordinate of the AGV tractor in the path, θ i is the heading angle;

[0142] S42, set the search space to S, with P start As the root node, calculate the random sampling point P rand The closest point P in the current tree near The Euclidean distance of :

[0143]

[0144] Among them, d(P rand ,P near ) is the Euclidean distance, (x rand ,y rand ) is the coordinate of the random sampling point, (x near ,y near ) is the coordinate of the nearest point in the tree, along P near Towards P rand Expand step size Δd to generate a new node P new And add the path tree, repeat the iteration until P agv Join the path tree to form path ξ;

[0145] S43. Use the improved fast exploration random tree algorithm to optimize the path, and set the path cost function C(ξ) as the cumulative distance on the path:

[0146]

[0147] For each newly expanded node P new , search for the optimal parent node P in its neighborhood min ,satisfy:

[0148]

[0149] Among them, arg min is the variable value when the function takes the minimum value, N(P new ) is P new The set of neighboring nodes, update the path tree and optimize the path ξ;

[0150] S44, adjust the posture of the AGV tractor, calculate the posture adjustment amount of the AGV tractor based on the optimized path ξ, and set the current position as P cur , the target position is P agv , the adjustment amount ΔP is calculated as:

[0151] ΔP=P agv -P cur ;

[0152] Among them, ΔP(x Δ ,y Δ ,θ Δ ) represents the adjustment value of position and heading angle, and controls the AGV tractor to adjust its posture along the optimized path ξ so that it finally reaches P agv , complete the docking preparation.

[0153] In this embodiment, S5 includes the following specific steps:

[0154] S51, based on the posture adjustment amount ΔP of the AGV tractor, analyze the optimized motion trajectory and calculate the docking error, decomposing ΔP into the displacement deviation Δd and the heading angle deviation Δθ between the current position and the target position;

[0155] S52. Correct the motion trajectory based on visual feedback, call the ToF camera to detect the spatial posture of the traction saddle and the center position of the vertical guide hole, and calculate the posture error of the traction saddle based on the current trajectory point in the optimized path ξ:

[0156] ΔP t =R tof P hole +T tof -P opt ;

[0157] Among them, R tofis the rotation matrix detected by the ToF camera, T tof is the displacement vector detected by the ToF camera, P hole is the detection center coordinate of the vertical guide hole, P opt To optimize the current target trajectory point in the path ξ, ΔP t is the traction saddle relative to the optimized trajectory point P opt The pose error of

[0158] S53, adjust the AGV tractor posture and perform docking along the optimized path ξ, and calculate the adjusted control instructions:

[0159] U k =K p (Δd+ΔP t )+K θ Δθ+K ξ (ξ-P cur );

[0160] Among them, U k is the control instruction, K p is the position error gain coefficient, K θ is the angle error gain coefficient, K ξ is the path tracking gain coefficient, ξ-P cur Indicates the deviation of the current pose relative to the optimized path;

[0161] Calculate and fine-tune the final docking error:

[0162] ΔP f =P agv -P final ;

[0163] Where ΔP f is the final docking error of the AGV tractor, P final is the final posture of the AGV tractor after adjustment, if |ΔP f If |is less than the set threshold ∈, docking lock is performed; otherwise, the motion trajectory continues to be adjusted until the docking requirements are met.

[0164] In this embodiment, S6 includes the following specific steps:

[0165] S61, based on the fine-tuned docking error, control the fork lift. Assume that the current position of the fork of the AGV tractor is h cur , the target hook height is h goal , calculate the fork height error and calculate the control input based on the dynamic model:

[0166]

[0167] Among them, u hFork control input, used to adjust the fork height, J f is the moment of inertia, indicating the inertia of the fork, c f is the damping coefficient, which represents the system's resistance to velocity changes, k f is the stiffness coefficient, which indicates the system's ability to recover from displacement, h is the current fork height, Δh=h goal -h cur is the fork height error, K p is the position error gain coefficient, K d is the differential gain coefficient, K i is the integral gain coefficient, which is used to adjust the dynamic response of the fork height control and output the corrected fork height h adj =h+u h ;

[0168] S62, adjust the height h based on the fork adj and the final docking error ΔP f , adjust the force state of the traction hook to gradually release the force and reach the zero force state. Suppose the current force vector of the traction hook is F cur , the target force state is zero force state F zero , calculate the force adjustment, and calculate the control input based on the dynamic equation:

[0169]

[0170] Among them, u θ It is the control input of the towing hook, used to adjust the force and rotation angle of the towing hook. h is the moment of inertia, which indicates the inertia of the traction hook in rotational motion, c h is the damping coefficient, which represents the damping effect in rotational motion, k h is the stiffness coefficient, which indicates the recovery ability of the rotation angle, θ is the current rotation angle of the towing hook, θ zero is the rotation angle of the traction hook under zero force state, ΔF=F cur -F zero is the force adjustment, which represents the difference between the current force and the target force, and outputs the corrected rotation angle θ adj =θ+u θ ;

[0171] S63, detect the disconnection state and complete the separation, based on the corrected rotation angle θ adj Calculate the unhooking state, assuming the relative displacement of the towing hook during the unhooking process is D hook , the target complete detachment distance is D detach , calculate the dropout progress error ΔD, and optimize the state estimation based on the extended Kalman filter:

[0172]

[0173] in, is the optimal detachment state estimation after EKF optimization, X k is the current detachment state vector, including displacement and velocity, Z k is the sensor observation value, including the measured towing hook displacement and speed information, H is the observation matrix, which maps the system state to the measurement value, K is the Kalman gain matrix, which is used to optimize the state estimation, K θ is the rotation angle error gain coefficient, which is used to adjust the influence of the towing hook rotation error. If the value is less than the set threshold, the disconnection is determined to be completed.

[0174] Example 1:

[0175] In order to verify the feasibility of the present invention in implementation, the present invention was applied to the intelligent logistics system of a large automobile manufacturing plant. The AGV tractor of the plant is responsible for transporting multiple material carts from the storage area to different production line sites, and automatically unhooking at the designated location to ensure that the materials are delivered on time. The average daily transportation task of the factory is large. The traditional manual unhooking method has problems such as low efficiency, high error rate, and high labor cost. Especially during peak hours, manual operation is prone to delays, affecting the production rhythm. To address this problem, the present invention uses stereo vision and automatic control technology to realize the intelligent unhooking operation of the AGV tractor, thereby improving the operation efficiency and system stability.

[0176] In this scenario, after arriving at the designated unloading site, each AGV tractor uses a ToF camera, lidar, and inertial navigation system to obtain real-time three-dimensional spatial information of the towing hook, towing saddle, and material cart. It then calculates the position information of the towing saddle by combining point cloud reconstruction and feature matching algorithms. After obtaining the spatial coordinates of the towing saddle, the AGV tractor adjusts its own motion trajectory based on a graph optimization algorithm to align the tension support beam with the vertical guide hole of the towing saddle. It then uses an improved Kalman filter algorithm to optimize the towing hook's unhooking state, and combines it with an adaptive sliding mode control algorithm to dynamically adjust the towing hook's force state so that it can be unhooked smoothly under minimal force, avoiding mechanical jamming or unhooking failures caused by uneven force. After completing the unhooking, the AGV tractor automatically adjusts its driving path, leaves the site, and proceeds to the next transportation task. At the same time, the system records the operating data of the entire unhooking process.

[0177] During the experimental testing phase, five different sites were selected for testing, including those with different ground materials, different lighting environments, and different parking accuracy requirements. Unhooking tests were conducted in high-precision navigation environments (error ≤ 5cm), medium-precision navigation environments (error ≤ 10cm), and low-precision navigation environments (error > 10cm), recording the AGV tractor's unhooking time, unhooking success rate, and unhooking error. The test period was 30 consecutive days, running 24 hours a day, for a total of 1,200 unhooking tests. The following is a statistical table of experimental test data:

[0178] Table 1 Comparison of test data of automatic detachment system

[0179]

[0180] As shown in Table 1, under high-precision navigation conditions, the automatic detachment system of the present invention has an average detachment time of 6.2 seconds, a 99.2% detachment success rate, and a detachment error within 1.8 cm. Under medium-precision navigation conditions, the detachment time increases slightly, to an average of 7.8 seconds, a 97.5% detachment success rate, and a detachment error within 2.9 cm. In low-precision navigation conditions, despite a decrease in the positioning accuracy of the AGV tractor, the present invention still maintains a 93.7% detachment success rate, an average detachment time of 9.4 seconds, and a detachment error within 4.6 cm. Compared to traditional manual detachment methods, the automatic detachment system reduces operation time by 78.5% and eliminates detachment failures caused by human error, enabling AGV tractors to complete transport tasks more stably and efficiently. Furthermore, monitoring of the equipment's operating status revealed that the automatic detachment system of the present invention had a system failure rate of less than 0.3% over a 30-day operation period, with no transport delays due to equipment jams.

[0181] As can be seen from the experimental data, the automatic detachment system of the present invention greatly improves operating efficiency compared to manual operation, maintains a high detachment success rate under different navigation accuracy environments, and has high operational stability, significantly reducing the risk of production stagnation due to equipment failure. Especially in high-precision navigation environments, the detachment error of the present invention is controlled within 2cm, ensuring the precise docking and stable detachment of the AGV tractor. In addition, the system maintains a low failure rate during long-term continuous operation, further verifying the reliability and adaptability of the present invention. Therefore, the present invention can effectively improve the intelligence level of AGV tractors, reduce the cost of manual intervention, and provide important technical support for the efficient operation of industrial intelligent logistics systems.

[0182] The above description is only a preferred specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any technician familiar with the technical field, within the technical scope disclosed by the present invention, who makes equivalent replacements or changes based on the technical solution and inventive concept of the present invention, should be covered by the scope of protection of the present invention.

Claims

1. An automatic unhooking system for AGV tractors based on stereo vision, characterized in that: include: The data acquisition module is used to collect the operating status data of the AGV tractor, the geometric characteristics data of the traction saddle, and the spatial position information of the traction hook. It includes a ToF camera, an inertial measurement unit, and a depth sensor. The data processing module is used to pre-process the collected data, remove noise from the cloud data, reconstruct the local three-dimensional model of the traction saddle, and extract feature point information; The posture calculation module is used to calculate the posture information of the traction saddle, perform feature point matching and graph optimization, and convert it into the target docking point coordinates in the AGV tractor coordinate system; The trajectory planning module is used to calculate the motion trajectory of the AGV tractor, optimize the path using an improved rapid exploration random tree algorithm, and adjust the position and motion direction of the AGV tractor based on the coordinates of the target docking point; The motion control module is used to analyze the generated motion trajectory, adjust the movement path of the AGV tractor, and optimize the detachment process of the tractor hook in combination with feedback control; The task scheduling module is used to plan the driving path of the AGV tractor and control the AGV tractor to move to the target site along the optimal path to perform the uncoupling task.

2. A cable branch box comprehensive online monitoring system according to claim 1, characterized in that: The modules are implemented as follows: S1, collects the operating status data of the AGV tractor, the geometric feature data of the traction saddle, and the spatial position information of the traction hook, and integrates the multi-sensor data; S2. Preprocess the multi-sensor data, remove noise from the point cloud data, reconstruct the local three-dimensional model of the traction saddle, and extract the feature point information of the traction saddle; S3. Calculate the position information of the traction saddle, perform feature point matching and graph optimization, and convert it into the target docking point coordinates in the AGV tractor coordinate system; S4. Based on the coordinates of the target docking point, the motion trajectory of the AGV tractor is calculated, and the improved rapid exploration random tree algorithm is used to optimize the path and adjust the position and driving direction of the AGV tractor; S5. Analyze the motion trajectory, control the AGV tractor to adjust its position, and correct the motion trajectory based on visual feedback to align the tension support beam with the vertical guide hole of the traction saddle. Calculate the docking error and make fine adjustments. S6. Based on the fine-tuned docking error, control the fork lift, adjust the force state of the towing hook, dynamically optimize the unhooking strategy using multi-sensor data, control the towing hook separation process, and detect the unhooking state; S7. Based on the detection result of the unhooking state, the driving path of the AGV tractor is updated, the AGV tractor is controlled to leave the site along the optimized path, and the posture data of the unhooking process is recorded.

3. The automatic unhooking system for AGV tractors based on stereo vision according to claim 2 is characterized in that: The S2 includes the following specific steps: S21, receive the running status data of the AGV tractor, the geometric feature data of the traction saddle and the spatial position information of the traction hook, synchronize the multi-sensor data, and set the timestamps of the ToF camera, inertial measurement unit and depth sensor to be t tof , t imu and t depth , calculate the global time error: Δt=max(t tof ,t imu ,t depth )-min(t tof ,t imu ,t depth ); Among them, t tof is the data acquisition timestamp of the ToF camera, t imu is the data acquisition timestamp of IMU, t depth is the data acquisition timestamp of the depth sensor, max(·) is the maximum value function, min(·) is the minimum value function, Δt is the maximum time difference of all sensor timestamps, and the linear interpolation method is used to align the data with different timestamps to generate the synchronized multi-sensor fusion dataset D sync ; S22, from D sync The point cloud data of the traction saddle is extracted, and the voxel filtering method is used for noise reduction. The point cloud is uniformly sampled according to the grid size. The points in each voxel area are calculated by the mean to form the noise-reduced point cloud data, and the region growing algorithm is used for point cloud segmentation. The initial seed point p is selected. s , calculate the neighborhood point p n The normal vector and p s The angle between the normal vectors of : Among them, θ is the neighborhood point p n The normal vector and the initial seed point p s The angle between the normal vectors, arccos() is the inverse cosine of the inverse trigonometric function, N n is the neighborhood point p n Normal vector, N s is the initial seed point p s The normal vector of S23, setting angle threshold θ th , when θ≤θ th When p n Classify them into the same area, repeat the iteration until all points are classified, and output the target point cloud area P of the traction saddle t ; S24, based on P t Calculate the centroid of the target point cloud area, detect feature points using a method based on the curvature change rate, and extract the feature point information of the traction saddle: Among them, C t is the name of the centroid coordinate function of the target area, m is the total number of points in the area, p i ,p j is the target point cloud area P t The point in k i For point p i The curvature, N(p i ) is point p i The neighborhood point set, |N(p i )| is the number of points in the neighborhood point set, and the points whose curvature changes exceed the set threshold are extracted to form the feature point set F t ; S25, based on P t and F t A local 3D model of the traction saddle is constructed, and the Poisson surface reconstruction method is used to solve the implicit function of the point cloud: Among them, f(x,y,z) is the reconstructed implicit surface function, is the gradient operator, δ(xp i ) represents the divergence function of the point cloud, m is the total number of point cloud points, and the local three-dimensional model M of the traction saddle is generated. t .

4. The automatic unhooking system for AGV tractors based on stereo vision according to claim 2 is characterized in that: The S3 includes the following specific steps: S31. Let the centroid of the traction saddle be represented by C t (x c ,y c ,z c ), define the traction saddle local coordinate system C t (x′, y′, z′), establish the local coordinate system of the traction saddle, where p i (x i ,y i ,z i ) is the feature point set F t points in, m is the number of feature points, C t (x c ,y c ,z c ) is the centroid expression, the local coordinate system C t The origin of (x′, y′, z′) is set to C t , the direction of the coordinate axis is determined according to the distribution of feature points; S32, select feature point set F t , calculate the initial position of the traction saddle and C t The spatial distribution of the feature point is constructed to form a distribution matrix in, is the relative position matrix of the feature points in the local coordinate system, (x c ,y c ,z c ) is the centroid coordinate, (x i ,y i ,z i ) is the coordinate of the feature point, n is the number of feature points; S33, obtaining a pre-stored standard traction saddle feature point set F s , matching the local 3D model M of the traction saddle t , calculate F based on nearest neighbor matching t midpoint p i With F s midpoint p j The Euclidean distance between: Where D(i,j) is the set of traction saddle feature points F t midpoint p i With F s midpoint p j The Euclidean distance is selected, and the point pair with the smallest D(i,j) is selected as the matching pair to establish the point pair set M f ; S34, point pair set M f To observe the constraints, a graph G(V,E) based on pose optimization is established to solve the global rotation matrix and translation vector: Among them, R is the rotation matrix, T is the translation vector, and W ij is the weight matrix, p i is the current feature point of the traction saddle, p j For matching standard feature points, Gauss-Newton optimization algorithm is used to calculate R and T, and the rigid transformation matrix [R|T] is output; S35, based on the rigid transformation matrix [R|T], the position of the traction saddle is transformed from the local coordinate system C t (x′, y′, z′) is converted to the AGV tractor coordinate system C agv (X,Y,Z), get the target docking point coordinates: P agv =RF t +T; Among them, P agv is the coordinate of the traction saddle feature point in the AGV tractor coordinate system, that is, the target docking point coordinate.

5. The automatic unhooking system for AGV tractors based on stereo vision according to claim 2 is characterized in that: The S4 includes the following specific steps: S41. Assume the initial position of the AGV tractor is P start , the target docking point coordinates are P agv , define the motion trajectory ξ as the start to P agv A collection of paths: ξ={P0,P1,…,P n }; Where P0=P start , P n =P agv , P i (x i ,y i ,θ i ) is the intermediate state on the path, x i ,y i is the position coordinate of the AGV tractor in the path, θ i is the heading angle; S42, set the search space to S, with P start As the root node, calculate the random sampling point P rand The closest point P in the current tree near The Euclidean distance of : Among them, d(P rand ,P near ) is the Euclidean distance, (x rand ,y rand ) is the coordinate of the random sampling point, (x near ,y near ) is the coordinate of the nearest point in the tree, along P near Towards P rand Expand step size Δd to generate a new node P new And add the path tree, repeat the iteration until P agv Join the path tree to form path ξ; S43. Use the improved fast exploration random tree algorithm to optimize the path, and set the path cost function C(ξ) as the cumulative distance on the path: For each newly expanded node P new , search for the optimal parent node P in its neighborhood min ,satisfy: Among them, argmin is the variable value when the function takes the minimum value, N(P new ) is P new The set of neighboring nodes, update the path tree and optimize the path ξ; S44, adjust the posture of the AGV tractor, calculate the posture adjustment amount of the AGV tractor based on the optimized path ξ, and set the current position as P cur , the target position is P agv , the adjustment amount ΔP is calculated as: ΔP=P agv -P cur ; Among them, ΔP(x Δ ,y Δ ,θ Δ ) represents the adjustment value of position and heading angle, and controls the AGV tractor to adjust its posture along the optimized path ξ so that it finally reaches P agv , complete the docking preparation.

6. The automatic unhooking system for AGV tractors based on stereo vision according to claim 2 is characterized in that: The S5 includes the following specific steps: S51, based on the posture adjustment amount ΔP of the AGV tractor, analyze the optimized motion trajectory and calculate the docking error, decomposing ΔP into the displacement deviation Δd and the heading angle deviation Δθ between the current position and the target position; S52. Correct the motion trajectory based on visual feedback, call the ToF camera to detect the spatial posture of the traction saddle and the center position of the vertical guide hole, and calculate the posture error of the traction saddle based on the current trajectory point in the optimized path ξ: ΔP t =R tof P hole +T tof -P opt ; Among them, R tof is the rotation matrix detected by the ToF camera, T tof is the displacement vector detected by the ToF camera, P hole is the detection center coordinate of the vertical guide hole, P opt To optimize the current target trajectory point in the path ξ, ΔP t is the traction saddle relative to the optimized trajectory point P opt The pose error of S53, adjust the AGV tractor posture and perform docking along the optimized path ξ, and calculate the adjusted control instructions: U k =K p (Δd+ΔP t )+K θ Δθ+K ξ (ξ-P cur ); Among them, U k is the control instruction, K p is the position error gain coefficient, K θ is the angle error gain coefficient, K ξ is the path tracking gain coefficient, ξ-P cur Indicates the deviation of the current pose relative to the optimized path; Calculate and fine-tune the final docking error: ΔP f =P agv -P final ; Where ΔP f is the final docking error of the AGV tractor, P final is the final posture of the AGV tractor after adjustment, if |ΔP f If |is less than the set threshold ∈, docking lock is performed; otherwise, the motion trajectory continues to be adjusted until the docking requirements are met.

7. The automatic unhooking system for AGV tractors based on stereo vision according to claim 2 is characterized in that: The S6 includes the following specific steps: S61, based on the fine-tuned docking error, control the fork lift. Assume that the current position of the fork of the AGV tractor is h cur , the target hook height is h goal , calculate the fork height error and calculate the control input based on the dynamic model: Among them, u h Fork control input, used to adjust the fork height, J f is the moment of inertia, indicating the inertia of the fork, c f is the damping coefficient, which represents the system's resistance to velocity changes, k f is the stiffness coefficient, which indicates the system's ability to recover from displacement, h is the current fork height, Δh=h goal -h cur is the fork height error, K p is the position error gain coefficient, K d is the differential gain coefficient, K i is the integral gain coefficient, which is used to adjust the dynamic response of the fork height control and output the corrected fork height h adj =h+u h ; S62, adjust the height h based on the fork adj and the final docking error ΔP f , adjust the force state of the traction hook to gradually release the force and reach the zero force state. Suppose the current force vector of the traction hook is F cur , the target force state is zero force state F zero , calculate the force adjustment, and calculate the control input based on the dynamic equation: Among them, u θ It is the control input of the towing hook, used to adjust the force and rotation angle of the towing hook. h is the moment of inertia, which indicates the inertia of the traction hook in rotational motion, c h is the damping coefficient, which represents the damping effect in rotational motion, k h is the stiffness coefficient, which indicates the recovery ability of the rotation angle, θ is the current rotation angle of the towing hook, θ zero is the rotation angle of the traction hook under zero force state, ΔF=F cur -F zero is the force adjustment, which represents the difference between the current force and the target force, and outputs the corrected rotation angle θ adj =θ+u θ ; S63, detect the disconnection state and complete the separation, based on the corrected rotation angle θ adj Calculate the unhooking state, assuming the relative displacement of the towing hook during the unhooking process is D hook , the target complete detachment distance is D detach , calculate the dropout progress error ΔD, and optimize the state estimation based on the extended Kalman filter: in, is the optimal detachment state estimation after EKF optimization, X k is the current detachment state vector, including displacement and velocity, Z k is the sensor observation value, including the measured towing hook displacement and speed information, H is the observation matrix, which maps the system state to the measurement value, K is the Kalman gain matrix, which is used to optimize the state estimation, K θ is the rotation angle error gain coefficient, which is used to adjust the influence of the towing hook rotation error. If the value is less than the set threshold, the disconnection is determined to be complete.

Images