Servo motion closed-loop control method and system of unhooking robot under harsh working conditions

Abstract

The application discloses a servo motion closed-loop control method and system of a hook removing robot under harsh working conditions, relates to the field of industrial robot control, and comprises the following steps: collecting a driving wheel servo torque signal to extract wheel-rail friction characteristics when the robot runs along a track, performing mutation identification to generate a compensation coefficient; collecting a carriage side point cloud to extract a carriage contour feature, performing deviation identification to generate a pose correction amount; performing speed feedforward compensation according to the compensation coefficient, the pose correction amount and a vehicle body attitude to generate a dynamic speed instruction; starting a grabbing mechanical arm after running to a preset position, taking the distance between the end and the vehicle ladder and the contact force as constraints to perform closed-loop correction on the speed instruction until reliable fixation; starting a hook removing mechanical arm, observing the hook removing state according to joint current and position signals and obtaining a result. The application solves the problem that the motion control of the existing hook removing robot cannot adaptively adjust the motion parameters thereof according to real-time working condition changes to maintain end execution precision and operation stability.

Description

Technical Field

[0001] This application relates to the field of industrial robot control, and in particular to a closed-loop control method and system for servo motion of a hook-unhooking robot under harsh working conditions. Background Technology

[0002] The accuracy of servo motion control for unhooking robots under harsh working conditions directly impacts the efficiency and safety of automated loading and unloading in railway freight transport. Current technologies for this type of robot generally employ open-loop control based on preset track parameters and fixed speed curves, or rely on single sensor feedback for local closed-loop adjustments. Specifically, this includes using track encoders for position calibration, triggering deceleration via limit switches, and using mechanical stops for end-effector positioning. However, these existing methods struggle to adapt to dynamic changes in actual working conditions when faced with sudden changes in wheel-rail adhesion coefficients, random shifts in carriage parking posture, and variations in dust and light intensity. This can easily lead to control failures such as drive wheel slippage, end-effector misalignment, and contact impacts.

[0003] At present, the motion control of unhooking robots in dynamic and changing environments faces the technical problem of difficulty in adaptively adjusting their motion parameters according to real-time changes in working conditions in order to maintain the accuracy of end-effector execution and operational stability. Summary of the Invention

[0004] This application provides a closed-loop control method and system for the servo motion of an unhooking robot under harsh working conditions. By acquiring signals and extracting wheel-rail friction features and carriage contour features, identifying friction abrupt changes and carriage deviations to generate compensation coefficients and correction amounts, performing speed feedforward compensation to generate dynamic speed commands, and then using distance and contact force as constraints to correct the speed commands in a closed loop until the end gripper is reliably fixed to the carriage ladder, the unhooking robot arm is then started and the unhooking status is observed to obtain the results. These technical means solve the technical problem of existing unhooking robots in dynamic and changing environments where it is difficult to adaptively adjust its motion parameters according to real-time changes in working conditions to maintain end-effector execution accuracy and operational stability. This achieves the technical effect of enabling the unhooking robot to maintain end-effector execution accuracy and operational stability under harsh working conditions.

[0005] This application provides a closed-loop control method for the servo motion of an uncoupling robot under harsh working conditions, comprising: during the uncoupling robot's movement along the track towards the carriage to be uncoupled, acquiring the servo torque feedback signal of the robot's active wheel drive motor and performing time-domain feature extraction to obtain wheel-rail contact friction feature parameters; acquiring point cloud data of the carriage side of the carriage to be uncoupled and performing plane fitting and edge extraction to obtain carriage contour feature parameters; identifying friction abrupt changes in the wheel-rail contact friction feature parameters and generating friction abrupt change compensation coefficients; identifying deviations in the carriage contour feature parameters and generating spatial pose correction amounts; and based on the friction abrupt change compensation coefficients, ... The spatial pose correction amount and the vehicle body attitude signal of the unhooking robot are used for velocity feedforward compensation to generate a dynamic speed command. After the unhooking robot runs to the preset common speed start position according to the dynamic speed command, it starts the common speed grasping manipulator. The real-time distance signal and contact force feedback signal between the end gripper and the train ladder are used as constraints to perform closed-loop correction on the dynamic speed command until the end gripper is reliably fixed to the train ladder, generating a fixation confirmation signal. According to the fixation confirmation signal, the unhooking execution manipulator is started, and the unhooking status is observed according to the current signal and position feedback signal of the joint servo motor to obtain the unhooking result.

[0006] In a possible implementation, the servo torque feedback signal of the active wheel drive motor of the unhooking robot is collected and its time-domain features are extracted to obtain wheel-rail contact friction characteristic parameters. The following processing is performed: the servo torque feedback signal sequence of the active wheel drive motor within a preset sliding time window is collected; the servo torque feedback signal sequence is filtered, the time-domain statistical features of the filtered signal sequence are extracted, and the time-domain statistical features are combined to form the wheel-rail contact friction characteristic parameters.

[0007] In a possible implementation, point cloud data of the side of the carriage to be unhooked is collected and subjected to planar fitting and edge extraction to obtain the carriage contour feature parameters. The following processing is also performed: based on the ambient light intensity and dust concentration values ​​at the time of data collection, a preset distortion degree mapping table is consulted to obtain the visual distortion level; when the visual distortion level is greater than a preset level threshold, the self-cleaning device of the unhooking robot's visual sensor is activated to perform a cleaning action and acquire secondary carriage side point cloud data; adaptive filtering is performed on the secondary carriage side point cloud data using the filtering intensity coefficient corresponding to the visual distortion level to obtain corrected carriage side point cloud data; the corrected carriage side point cloud data is then used to replace the original carriage side point cloud data for planar fitting and edge extraction.

[0008] In a possible implementation, friction abrupt changes are identified on the wheel-rail contact friction characteristic parameters to generate friction abrupt change compensation coefficients; deviations are identified on the car body contour characteristic parameters to generate spatial pose correction amounts, and the following processing is performed: the wheel-rail contact friction characteristic parameters are compared with preset normal operating reference characteristic parameters to calculate a characteristic deviation vector; when the magnitude of the characteristic deviation vector is greater than a preset magnitude threshold, the friction abrupt change type and friction abrupt change intensity are determined according to the direction and amplitude of the characteristic deviation vector; a preset compensation coefficient mapping relationship is queried according to the friction abrupt change type and the friction abrupt change intensity to obtain the friction abrupt change compensation coefficients; the car body contour characteristic parameters are matched with a preset standard car body template to calculate the lateral offset, longitudinal offset, and deflection angle of the car body to be uncoupled relative to the standard car body in three-dimensional space; and a spatial pose correction amount is generated according to the lateral offset, the longitudinal offset, and the deflection angle.

[0009] In a possible implementation, deviation identification is performed on the carriage contour feature parameters to generate a spatial pose correction amount, and the following processing is also performed: the carriage contour feature parameters are compared and matched with a preset standard carriage template to calculate the lateral and longitudinal dimension deviation values ​​of the carriage. When any dimension deviation value is greater than a preset dimension deviation tolerance threshold, a shape deviation type identifier is generated; a preset trajectory fault tolerance compensation mapping relationship is queried according to the shape deviation type identifier to obtain trajectory fault tolerance compensation parameters, and the trajectory fault tolerance compensation parameters are superimposed on the spatial pose correction amount.

[0010] In a possible implementation, speed feedforward compensation is performed based on the friction mutation compensation coefficient, the spatial pose correction amount, and the vehicle body attitude signal of the unhooking robot to generate a dynamic speed command. The following processing is then performed: the attitude deviation value between the vehicle body attitude signal and the preset reference attitude is obtained, and weighted fusion is performed by combining the friction mutation compensation coefficient and the spatial pose correction amount to generate a speed feedforward compensation value; the speed feedforward compensation value is limited to the interval defined by the difference between the preset upper and lower limits of the speed safety envelope and the current running speed value of the unhooking robot to obtain a limited speed feedforward compensation value; the limited speed feedforward compensation value is superimposed on the speed command of the servo speed loop to generate the dynamic speed command.

[0011] In a possible implementation, the real-time distance signal and contact force feedback signal between the end effector and the train ladder are used as constraints to perform closed-loop correction on the dynamic speed command until the end effector and the train ladder are reliably fixed, generating a fixing confirmation signal. The following processes are then performed: The current distance value between the end effector and the train ladder is calculated based on the real-time distance signal; the current distance value is matched with a preset deceleration mapping curve to obtain a basic deceleration correction amount; the contact force change rate is calculated based on the contact force feedback signal; when the contact force change rate is greater than a preset change rate threshold, an emergency deceleration correction amount is generated; the larger of the basic deceleration correction amount and the emergency deceleration correction amount is used as the final speed correction amount and superimposed on the dynamic speed command; when the speed deviation between the real-time distance signal, the contact force feedback signal, and the unhooking robot and the unhooking carriage satisfies the distance fluctuation condition, the effective contact force condition, and the speed synchronization condition respectively within a preset judgment time window, it is determined that the end effector and the train ladder are reliably fixed, and the fixing confirmation signal is generated; otherwise, the closed-loop correction of the dynamic speed command is maintained.

[0012] In a possible implementation, the unhooking status is observed based on the current signal and position feedback signal of the joint servo motor, the unhooking result is obtained, and the following processing is performed: the current signal is converted into a joint output torque estimate, and the position feedback signal is converted into a joint angular velocity estimate and a joint angular acceleration estimate; based on the joint output torque estimate, the joint angular velocity estimate, the joint angular acceleration estimate, and preset joint inertia parameters and joint damping parameters, inverse dynamics is solved, and the calculated external resistance estimate is used as the unhooking resistance estimate; based on the comparison result between the unhooking resistance estimate and the preset unhooking success threshold, the unhooking result is obtained.

[0013] In a possible implementation, the unhooking result is obtained based on the comparison between the estimated unhooking resistance value and the preset unhooking success threshold, and the following processing is performed: if the estimated unhooking resistance value is less than the preset unhooking success threshold, the unhooking is determined to be successful, and an unhooking completion signal is output; otherwise, it is determined whether the duration for which the estimated unhooking resistance value is greater than or equal to the preset unhooking success threshold is less than a preset time threshold; if yes, the estimated unhooking resistance value continues to be monitored; if no, a bidirectional alternating angular displacement disturbance command sequence is generated with the current joint angle position of the unhooking execution robot arm as the center, according to a preset jitter frequency and a preset jitter amplitude; according to the angular displacement disturbance command sequence, the unhooking execution robot arm is driven to perform reciprocating swings, and the estimated unhooking resistance value is continuously monitored; when the estimated unhooking resistance value is less than the preset unhooking success threshold, the swinging is stopped and the unhooking lifting operation is re-executed; if the estimated unhooking resistance value is still greater than or equal to the preset unhooking success threshold after a preset number of reciprocating swings, a manual intervention alarm signal is output.

[0014] This application also provides a closed-loop control system for the servo motion of a decoupling robot under harsh working conditions, including: a perception and feature extraction module, used to collect the servo torque feedback signal of the active wheel drive motor of the decoupling robot and perform time-domain feature extraction to obtain wheel-rail contact friction feature parameters during the process of the decoupling robot running along the track towards the carriage to be decoupled; collect point cloud data of the side of the carriage to be decoupled and perform plane fitting and edge extraction to obtain carriage contour feature parameters; a disturbance identification and pose calculation module, used to identify friction abrupt changes in the wheel-rail contact friction feature parameters and generate friction abrupt change compensation coefficients; identify deviations in the carriage contour feature parameters and generate spatial pose correction amounts; and a feedforward compensation and speed command generation module, used to generate speed commands based on the friction... The abrupt change compensation coefficient, the spatial pose correction amount, and the vehicle body posture signal of the unhooking robot are used for velocity feedforward compensation to generate a dynamic speed command; the common speed following and reliable fixing module is used to start the common speed grasping manipulator after the unhooking robot runs to the preset common speed start position according to the dynamic speed command, and use the real-time distance signal and contact force feedback signal between the end gripper and the train ladder as constraints to perform closed-loop correction on the dynamic speed command until the end gripper and the train ladder are reliably fixed, generating a fixing confirmation signal; the unhooking observation and result judgment module is used to start the unhooking execution manipulator according to the fixing confirmation signal, and observe the unhooking status according to the current signal and position feedback signal of the joint servo motor to obtain the unhooking result.

[0015] The proposed closed-loop control method and system for servo motion of a decoupling robot under harsh working conditions involves, firstly, during the robot's movement along the track towards the carriage to be decoupled, acquiring the servo torque feedback signal of the robot's drive motor and extracting its time-domain features to obtain wheel-rail contact friction characteristic parameters; secondly, acquiring point cloud data of the carriage's side surface and performing planar fitting and edge extraction to obtain carriage contour characteristic parameters; thirdly, identifying friction abrupt changes in the wheel-rail contact friction characteristic parameters to generate friction abrupt change compensation coefficients; and fourthly, identifying deviations in the carriage contour characteristic parameters to generate spatial pose correction amounts, and finally, compensating for the friction abrupt changes based on the corrections. The compensation coefficient, the spatial pose correction amount, and the vehicle body attitude signal of the unhooking robot are used for velocity feedforward compensation to generate a dynamic speed command. After the unhooking robot moves to a preset common-speed start position according to the dynamic speed command, it starts the common-speed gripping manipulator. The real-time distance signal and contact force feedback signal between the end effector gripper and the train ladder are used as constraints to perform closed-loop correction on the dynamic speed command until the end effector gripper is reliably fixed to the train ladder, generating a fixing confirmation signal. Finally, based on the fixing confirmation signal, the unhooking execution manipulator is started, and the unhooking status is observed based on the current signal and position feedback signal of the joint servo motor to obtain the unhooking result. Through the above process, the method and system proposed in this application achieve the technical effect of enabling the unhooking robot to maintain end effector execution accuracy and operational stability under harsh working conditions. Attached Figure Description

[0016] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings of the embodiments of the present invention will be briefly described below. Flowcharts are used in this application to illustrate the operations performed by the system according to the embodiments of the present application. It should be understood that the preceding or following operations are not necessarily performed precisely in sequence. Instead, various steps can be processed in reverse order or simultaneously as needed. Furthermore, other operations can be added to these processes, or one or more steps can be removed from these processes.

[0017] Figure 1 This is a flowchart illustrating the closed-loop control method for the servo motion of a hook-unhooking robot under harsh working conditions provided in this application embodiment.

[0018] Figure 2 A schematic diagram of the servo motion closed-loop control system for a hook-unhooking robot under harsh working conditions provided in this application embodiment.

[0019] Figure labeling: 10 Perception and feature extraction module, 20 Disturbance identification and pose calculation module, 30 Feedforward compensation and velocity command generation module, 40 Common velocity following and reliable fixing module, 50 Unhooking observation and result judgment module. Detailed Implementation

[0020] To further illustrate the technical means and effects of the present invention in achieving its intended purpose, the following detailed description of the specific implementation methods, structures, features, and effects of the present invention, in conjunction with the accompanying drawings and preferred embodiments, is provided below.

[0021] This application provides a closed-loop control method for the servo motion of a hook-unhooking robot under harsh working conditions, such as... Figure 1 As shown, the method includes: Step S100: During the process of the unhooking robot running along the track towards the carriage to be unhooked, the servo torque feedback signal of the active wheel drive motor of the unhooking robot is collected and time-domain feature extraction is performed to obtain wheel-rail contact friction feature parameters; the point cloud data of the side of the carriage to be unhooked is collected and plane fitting and edge extraction are performed to obtain the carriage contour feature parameters.

[0022] Specifically, the onboard industrial control computer of the unhooking robot reads the values ​​of the internal torque registers of the servo drivers of the drive wheel motors in real time via the EtherCAT bus at a set sampling frequency to obtain servo torque feedback signals. Simultaneously, a solid-state LiDAR installed on the side of the robot's body collects 3D point cloud data of the side of the carriage to be unhooked. After completing the acquisition of the above signals and data, on the one hand, time-domain statistical feature calculations are performed on the servo torque feedback signals, and the calculated statistical quantities are combined into wheel-rail contact friction characteristic parameters; on the other hand, random sampling consistency plane fitting and edge point extraction are performed on the point cloud data of the carriage side to obtain geometric feature parameters characterizing the carriage contour. These two types of parameters are used for friction mutation identification and spatial pose deviation identification, respectively, to address wheel-rail adhesion fluctuations and random deviations in carriage parking posture under harsh working conditions.

[0023] In one possible implementation, the servo torque feedback signal of the active wheel drive motor of the unhooking robot is acquired and its time-domain features are extracted to obtain wheel-rail contact friction characteristic parameters. Step S100 further includes step S110, acquiring the servo torque feedback signal sequence of the active wheel drive motor within a preset sliding time window. Specifically, during the operation of the unhooking robot, the on-board industrial control computer triggers a data acquisition interruption once with a fixed control cycle. Within each cycle, the industrial control computer reads the analog torque signal fed back by the servo driver of the active wheel drive motor, and obtains the digitized instantaneous torque value after analog-to-digital conversion. The industrial control computer maintains a first-in-first-out queue, the length of which is determined by the preset sliding time window. For example, if the preset sliding time window length is 0.5s and the sampling period is 10ms, then the queue length is 50. Whenever a new instantaneous torque value is read, the industrial control computer pushes the value to the tail of the queue and pops the oldest data from the head of the queue, thereby forming a servo torque feedback signal sequence covering the most recent 0.5s.

[0024] Step S120 involves filtering the servo torque feedback signal sequence, extracting the time-domain statistical features of the filtered signal sequence, and combining these features into the wheel-rail contact friction characteristic parameters. Specifically, the servo torque feedback signal sequence obtained in step S110 is subjected to amplitude limiting filtering and moving average filtering. The amplitude limiting filtering rule is as follows: calculate the difference between the newly added torque value and the torque value at the previous moment. If the absolute value of this difference exceeds the preset maximum allowable rate of change, the difference is truncated to the preset maximum allowable rate of change, and the truncated difference is added to the torque value at the previous moment to update the current torque value. The moving average filtering rule is as follows: calculate the arithmetic mean of all elements in the amplitude limiting filtered sequence, and use the difference between each element and the arithmetic mean as the filtered AC component. Then, subtract the AC component from the original sequence to obtain the smoothed torque signal sequence. Based on the filtered signal sequence, the following time-domain statistical characteristics are calculated: peak value (maximum value in the sequence); valley value (minimum value in the sequence); peak-to-peak value (difference between peak and valley values); absolute mean (arithmetic mean of the absolute values ​​of the sequence elements); effective value (square root of the sum of squares of the sequence elements divided by the sequence length); and variance (average of the squares of the differences between the sequence elements and their mean). These six statistical characteristics are arranged according to a preset feature vector order and combined into a six-dimensional vector, which represents the wheel-rail contact friction characteristic parameters. These parameters are used to quantitatively characterize the adhesion state and friction fluctuation of the current wheel-rail contact surface.

[0025] In one possible implementation, point cloud data of the side of the carriage to be unhooked is collected and subjected to planar fitting and edge extraction to obtain the carriage contour feature parameters. Step S100 further includes step S130, which queries a preset distortion level mapping table based on the ambient light intensity and dust concentration values ​​at the time of data collection of the carriage side point cloud data to obtain the visual distortion level. Specifically, in step S100, while triggering the lidar to collect the carriage side point cloud data, the current ambient light intensity value is obtained through an ambient light sensor installed on the top of the unhooking robot; the current dust concentration value is obtained through a dust concentration sensor installed on the side of the robot body. The on-board industrial control computer uses the ambient light intensity value and dust concentration value as a two-dimensional query index and inputs them into the distortion level mapping table pre-stored in the industrial control computer's memory. This mapping table is a two-dimensional table, with the horizontal axis divided into multiple ambient light intensity intervals and the vertical axis divided into multiple dust concentration intervals. Each intersecting cell stores a corresponding visual distortion level, which is an integer value. The larger the value, the more severe the point cloud distortion. The industrial control computer reads the corresponding visual distortion level by finding the intersection cell between the ambient light intensity value range and the dust concentration value range.

[0026] Step S140: When the visual distortion level is greater than a preset threshold, the self-cleaning device of the unhooking robot's visual sensor is activated to perform a cleaning action and acquire secondary point cloud data of the side of the vehicle compartment. Specifically, the visual distortion level obtained in step S130 is compared with the preset threshold. If the visual distortion level is less than or equal to the preset threshold, no cleaning action is required, and the point cloud data of the side of the vehicle compartment obtained in step S100 is directly used. If the visual distortion level is greater than the preset threshold, it is determined that the current visual sensor window is severely contaminated by dust or water mist, and the on-board industrial control computer sends a start command to the visual sensor self-cleaning device. The visual sensor self-cleaning device includes a high-pressure air nozzle and a windshield wiper. The steps for performing the cleaning action are as follows: the high-pressure air nozzle is turned on to spray compressed air onto the surface of the lidar window at a pressure of 0.6 MPa for 0.5 seconds; the windshield wiper is activated to wipe back and forth twice; the high-pressure air nozzle is turned on again to blow for 0.3 seconds. After the cleaning operation is completed, the industrial control computer re-triggers the lidar to collect point cloud data, and records the collected point cloud data as the secondary point cloud data of the side of the carriage.

[0027] Step S150: Adaptive filtering is performed on the secondary carriage side point cloud data using the filter intensity coefficient corresponding to the visual distortion level to obtain corrected carriage side point cloud data. The corrected carriage side point cloud data is then used to replace the original carriage side point cloud data for planar fitting and edge extraction. Specifically, based on the visual distortion level obtained in step S130, the corresponding filter intensity coefficient is queried from a preset filter parameter table. In this filter parameter table, each visual distortion level corresponds to a set of filter intensity coefficients, which include a spatial neighborhood radius value and a standard deviation threshold. For example, when the visual distortion level is 4, the corresponding spatial neighborhood radius is 0.03 meters, and the standard deviation threshold is 0.02 meters. Statistical filtering is performed on the secondary carriage side point cloud data: For each data point in the point cloud, neighboring data points are searched within the spatial neighborhood radius, centered on that point. The average spatial coordinates of all neighboring data points are calculated, and the standard deviation of the distance from each neighboring point to this average value is calculated. If the standard deviation corresponding to a point is greater than the standard deviation threshold, that point is identified as an outlier and removed from the point cloud. After performing the above judgment and removal operations on all data points, the remaining data points constitute the corrected point cloud data of the carriage side. This corrected point cloud data replaces the original carriage side point cloud data in step S100 and is used for plane fitting and edge extraction. Through the above adaptive filtering process, random noise in harsh visual environments is effectively suppressed, improving the accuracy and robustness of carriage contour feature extraction.

[0028] For example, suppose the unhooking robot is currently operating on a loading line in a mining area, the ambient light sensor measures an ambient light intensity of 18000 lx, and the dust concentration sensor measures a dust concentration of 12 mg / m³. 3The industrial control computer queries the distortion mapping table. In this table, the ambient light intensity range of 15000–20000 lx corresponds to a dust concentration range of 10–15 mg / m³. 3 The visual distortion level stored in the intersecting cells of the interval is 4, and the preset level threshold is 3. Since 4 > 3, it is determined that the lidar window is contaminated with dust, and the visual sensor self-cleaning device is activated to complete the cleaning operation. The self-cleaning execution process is as follows: the high-pressure nozzle blows the window with 0.6MPa air pressure for 0.5s, the wiper wipes back and forth twice, and then the high-pressure nozzle blows again for 0.3s; after cleaning, the point cloud data of the side of the carriage is collected again. A total of 9 sets of three-dimensional coordinate points (unit: m) are collected this time: point 1 (2.015, 0.502, 1.210), point 2 (2.018, 0.498, 1.215), point 3 (2.020, ... Points 0.505, 1.208, 4 (2.012, 0.495, 1.212), 5 (2.500, 0.510, 1.300), 6 (2.503, 0.508, 1.298), 7 (2.505, 0.515, 1.305), 8 (2.508, 0.502, 1.302), and 9 (3.210, 1.050, 1.860). The filtering parameters corresponding to visual distortion level 4 are: spatial neighborhood radius 0.03m, standard deviation threshold 0.02m. Taking point 1 as an example, points 2, 3, and 4 can be retrieved within its spatial neighborhood radius of 0.03m. The average spatial coordinates of the four points are 2.01625m in the X direction, 0.50000m in the Y direction, and 1.21125m in the Z direction. The overall standard deviation of the Euclidean distance sequence from each point to the center coordinate is approximately 0.0011m, which is less than the standard deviation threshold of 0.02m. Therefore, point 1 is retained. The judgment rules for points 2, 3, and 4 are the same as for point 1, and all are retained. For point 5, its neighborhood includes points 6, 7, and 8, with an average coordinate of (2.50400, 0.50875, 1.30125)m. The overall standard deviation of the distance sequence is approximately 0.0019m, which is still less than the judgment threshold of 0.02m. Therefore, points 5 to 8 are all retained. For point 9, a 0.03m spatial neighborhood was defined centered on this point. No neighboring points were found, making neighborhood standard deviation calculation impossible. This point is considered an outlier caused by dust interference in the mining area and was therefore removed. After adaptive filtering, eight effective corrected point clouds were retained, completely filtering out random noise while preserving the geometric features of the carriage's side profile, thus improving the accuracy of carriage pose extraction in harsh dusty environments. Detailed processing results for each data point are shown in Table 1.

[0029] Table 1

[0030] Step S200: Identify frictional abrupt changes in the wheel-rail contact friction characteristic parameters and generate frictional abrupt change compensation coefficients; identify deviations in the carriage contour characteristic parameters and generate spatial pose correction amounts.

[0031] Specifically, the onboard industrial control computer takes the wheel-rail contact friction characteristic parameters obtained in step S120 as input and compares them dimension-by-dimensionally with the pre-stored normal operation reference characteristic parameters measured on a standard dry rail surface. Based on the comparison results, it determines whether there is a frictional abrupt change. If so, it generates a frictional abrupt change compensation coefficient according to preset rules. Simultaneously, the industrial control computer performs iterative nearest-point registration between the car body contour characteristic parameters obtained in step S150 and a preset standard car body template, calculating the lateral offset, longitudinal offset, and deflection angle of the car body to be uncoupled relative to the standard car body in three-dimensional space. These three quantities are then combined into a spatial pose correction value. The frictional abrupt change compensation coefficient is used to compensate for driving force fluctuations caused by sudden changes in the wheel-rail adhesion coefficient, while the spatial pose correction value is used to compensate for end-gripper docking errors caused by deviations in the car body's parking posture.

[0032] In one possible implementation, friction abrupt changes are identified in the wheel-rail contact friction characteristic parameters to generate friction abrupt change compensation coefficients; deviations are identified in the carriage contour characteristic parameters to generate spatial pose correction amounts. Step S200 further includes step S210, comparing the wheel-rail contact friction characteristic parameters with preset normal operating reference characteristic parameters to calculate a feature deviation vector. Specifically, preset normal operating reference characteristic parameters are read from the on-board industrial control computer memory. These reference characteristic parameters are also a six-dimensional vector, and the meaning of each dimension is completely consistent with that of the wheel-rail contact friction characteristic parameters in step S120, corresponding to the peak reference value, valley reference value, peak-to-peak reference value, absolute mean reference value, effective value reference value, and variance reference value, respectively. The industrial control computer performs difference calculations on the wheel-rail contact friction characteristic parameters calculated in step S120 and the normal operating reference characteristic parameters in the corresponding dimensions, that is, subtracting the reference value of the corresponding dimension of the normal operating reference characteristic parameters from the value of each dimension of the wheel-rail contact friction characteristic parameters to obtain six differences. These six differences are arranged in their original order to form a feature deviation vector.

[0033] Step S220: When the magnitude of the feature deviation vector is greater than a preset magnitude threshold, the friction mutation type and friction mutation intensity are determined based on the direction and magnitude of the feature deviation vector. Specifically, the magnitude of the feature deviation vector obtained in step S210 is calculated, i.e., the sum of the squares of the values ​​of each dimension of the vector is calculated, and then the square root is taken. The calculated magnitude is compared with the preset magnitude threshold. If the magnitude is less than or equal to the preset magnitude threshold, the current wheel-rail contact state is determined to be normal, and there is no need to generate a friction mutation compensation coefficient. The friction mutation compensation coefficient is then set to 0. If the magnitude is greater than the preset magnitude threshold, a friction mutation is determined to have occurred. At this time, the direction of the feature deviation vector is further analyzed: the sign of each dimension of the vector is calculated. If the deviation values ​​of the peak, effective value, and variance dimensions are positive, and the deviation value of the valley dimension is negative, the friction mutation type is determined to be a sudden increase in the adhesion coefficient; conversely, if the deviation values ​​of the peak, effective value, and variance dimensions are negative, and the deviation value of the valley dimension is positive, the friction mutation type is determined to be a sudden decrease in the adhesion coefficient. The frictional mutation intensity is taken as the ratio of the magnitude of the characteristic deviation vector divided by a preset magnitude threshold, and is expressed as a dimensionless value.

[0034] Step S230: Based on the friction mutation type and friction mutation intensity, a preset compensation coefficient mapping relationship is queried to obtain the friction mutation compensation coefficient. Specifically, the on-board industrial control computer internally stores a compensation coefficient mapping relationship table. This table has a two-dimensional structure, with row indices representing friction mutation types (including sudden increases and decreases in adhesion coefficient) and column indices representing friction mutation intensity ranges. Each cell stores a friction mutation compensation coefficient value, ranging from -1.0 to +1.0, where a positive value indicates an increase in motor drive torque and a negative value indicates a decrease in motor drive torque. The industrial control computer queries this mapping table based on the friction mutation type and friction mutation intensity range determined in step S220 to retrieve the corresponding friction mutation compensation coefficient. This compensation coefficient is used to instantly correct the drive torque in the speed feedforward channel to offset speed fluctuations caused by friction mutations.

[0035] Step S240 involves matching the carriage contour feature parameters with a preset standard carriage template to calculate the lateral offset, longitudinal offset, and deflection angle of the carriage to be uncoupled relative to the standard carriage in three-dimensional space. Specifically, the preset standard carriage template is read from the on-board industrial control computer memory. This template is a three-dimensional point cloud model of the side of the standard carriage, with its coordinate system origin located on the central symmetry plane of the standard carriage. The iterative nearest-point algorithm is used to rigidly register the actual carriage contour feature parameters obtained in step S150 with the preset standard carriage template. The execution steps of the iterative nearest point algorithm are as follows: Initialize the transformation matrix as an identity matrix; transform the actual carriage contour point cloud according to the current transformation matrix; for each transformed point, search for the corresponding point with the closest Euclidean distance in the standard template point cloud to form a corresponding point pair; based on all corresponding point pairs, use singular value decomposition to solve for the optimal rotation matrix and optimal translation vector that minimize the mean square distance between corresponding point pairs; superimpose the optimal rotation matrix and optimal translation vector onto the current transformation matrix; repeat the above steps until the number of iterations reaches the preset maximum number of iterations or the change in mean square distance between two adjacent iterations is less than the preset convergence threshold. After registration convergence, extract the translation vector and the rotation angle around the vertical axis from the final transformation matrix. The X-direction component of the translation vector is denoted as the lateral offset, the Y-direction component as the longitudinal offset, and the rotation angle as the deflection angle. These three quantities respectively characterize the attitude deviation of the carriage to be uncoupled relative to the standard carriage in the lateral direction of the track, the longitudinal direction of the track, and the horizontal plane.

[0036] Step S250: Generate a spatial pose correction value based on the lateral offset, the longitudinal offset, and the deflection angle. Specifically, the lateral offset, longitudinal offset, and deflection angle obtained in step S240 are scaled according to a preset scaling factor to adapt to the motion range of the unhooking robot's actuator. Specifically, the spatial pose correction value is a six-dimensional vector. The first three dimensions are position correction values, corresponding to the X-direction, Y-direction, and Z-direction correction values ​​in the unhooking robot's base coordinate system, respectively. The X-direction correction value is equal to the lateral offset multiplied by the position scaling factor, the Y-direction correction value is equal to the longitudinal offset multiplied by the position scaling factor, and the Z-direction correction value is set to 0 (the deviation in the side height direction of the carriage is compensated by the subsequent contact force closed-loop). The last three dimensions are attitude correction values, corresponding to the rotation correction values ​​around the X-axis, Y-axis, and Z-axis, respectively. The rotation correction value around the Z-axis is equal to the deflection angle multiplied by the attitude scaling factor, and the rotation correction values ​​around the X-axis and Y-axis are set to 0. This spatial pose correction is used to adjust the target trajectory of the unhooking robot, so that the end effector gripper can accurately align with the position of the train ladder.

[0037] In one possible implementation, deviation identification is performed on the carriage contour feature parameters to generate spatial pose correction values. Step S200 further includes step S260, which compares and matches the carriage contour feature parameters with a preset standard carriage template to calculate the lateral and longitudinal dimension deviation values ​​of the carriage. When any dimension deviation value exceeds a preset dimension deviation tolerance threshold, a shape deviation type identifier is generated. Specifically, based on the iterative nearest point registration completed in step S240, the industrial control computer further compares the actual carriage contour feature parameters with the preset standard carriage template in terms of shape dimensions. Specifically, in the actual carriage contour point cloud, the outermost endpoints on both sides are searched along the lateral direction of the track, and the distance between the two points is calculated as the actual carriage width value; the farthest points at both ends are searched along the longitudinal direction of the track, and the distance between the two points is calculated as the actual carriage length value. At the same time, the standard width value and standard length value are read from the preset standard carriage template. The absolute value of the difference between the actual width value and the standard width value is calculated and recorded as the lateral dimension deviation value; the absolute value of the difference between the actual length value and the standard length value is calculated and recorded as the longitudinal dimension deviation value. The lateral and longitudinal dimensional deviations are compared with preset dimensional deviation tolerance thresholds. If the lateral dimensional deviation is greater than the preset tolerance threshold and the actual width is greater than the standard width, a deviation type label of "oversized carriage" is generated; if the lateral dimensional deviation is greater than the preset tolerance threshold and the actual width is less than the standard width, a deviation type label of "undersized carriage" is generated. Similarly, if the longitudinal dimensional deviation is greater than the preset tolerance threshold and the actual length is greater than the standard length, a label of "oversized carriage" is generated; if the longitudinal dimensional deviation is greater than the preset tolerance threshold and the actual length is less than the standard length, a label of "undersized carriage" is generated. If all dimensional deviations do not exceed the tolerance thresholds, the deviation type label is set to "normal shape".

[0038] Step S270: A preset trajectory tolerance compensation mapping relationship is queried based on the shape deviation type identifier to obtain trajectory tolerance compensation parameters. These parameters are then superimposed on the spatial pose correction amount. Specifically, the onboard industrial control computer's memory stores a trajectory tolerance compensation mapping table. This table uses the shape deviation type identifier as an index, with each index corresponding to a set of trajectory tolerance compensation parameters. The trajectory tolerance compensation parameter is a six-dimensional vector, and its dimensional meaning is completely consistent with the spatial pose correction amount in step S250. The industrial control computer queries this mapping table based on the shape deviation type identifier generated in step S260 and reads the corresponding six-dimensional compensation vector. For example, if the shape deviation type identifier is "excessively wide carriage," the X-direction correction value in the corresponding trajectory tolerance compensation parameter increases by 0.02m to guide the end gripper to shift outward; if the identifier is "too narrow carriage," the X-direction correction value decreases by 0.02m. After obtaining the trajectory fault tolerance compensation parameters, they are added to the corresponding dimension values ​​of the spatial pose correction amount generated in step S250 to obtain the updated spatial pose correction amount, which is used for velocity feedforward compensation calculation. By introducing dimensional deviation compensation, the unhooking robot can adapt to the unhooking operation requirements of deformable carriages.

[0039] For example, assuming the preset six-dimensional vector of normal operating baseline characteristic parameters is [12.5, 8.2, 4.3, 10.1, 11.3, 1.8], the currently calculated wheel-rail contact friction characteristic parameters are [18.7, 6.5, 12.2, 14.8, 16.9, 4.5]. The industrial control computer calculates the characteristic deviation vector, with the differences in each dimension being [6.2, -1.7, 7.9, 4.7, 5.6, 2.7]. The magnitude of this vector is the square root of the sum of the squares of each term, calculated to be approximately 12.825. The preset magnitude threshold is 5.0. Since 12.825 is greater than 5.0, a frictional abrupt change is determined. Analyzing the signs of each dimension, the peak, effective value, and variance deviations are positive, while the valley deviation is negative. Therefore, the frictional abrupt change type is determined to be a sudden increase in the adhesion coefficient. The frictional abrupt change intensity is 12.825 / 5.0 = 2.565. Querying the compensation coefficient mapping relationship, for the type of sudden increase in adhesion coefficient, the friction mutation compensation coefficient corresponding to the strength range of 2.0 to 3.0 is -0.35, indicating that the velocity feedforward command needs to be reduced by 35%. Meanwhile, in the identification of carriage size deviation, assuming the actual carriage width is 2.85m, the standard width is 2.80m, the lateral size deviation is 0.05m, and the preset size deviation tolerance threshold is 0.05m, the lateral size deviation value does not exceed the tolerance threshold, therefore the shape deviation type is identified as "normal shape," and the trajectory fault tolerance compensation parameter is a zero vector, not changing the spatial pose correction amount. The above key data is summarized in Table 2.

[0040] Table 2

[0041] Step S300: Based on the frictional change compensation coefficient, the spatial pose correction amount, and the vehicle body attitude signal of the unhooking robot, speed feedforward compensation is performed to generate dynamic speed commands.

[0042] Specifically, the onboard industrial control computer acquires the vehicle attitude signal of the unhooking robot through the inertial measurement unit. This signal includes the vehicle's pitch angle, roll angle, and yaw angle. The industrial control computer takes the updated spatial pose correction value from step S270, the friction mutation compensation coefficient obtained in step S230, and the vehicle attitude signal as inputs, and performs weighted fusion calculations to generate a speed feedforward compensation value. This speed feedforward compensation value is limited to a safe range and then superimposed on the original speed command of the servo speed loop to form the final dynamic speed command. This dynamic speed command is sent to the servo driver of the drive wheel motor via the EtherCAT bus, driving the robot to run at the corrected speed.

[0043] In one possible implementation, speed feedforward compensation is performed based on the friction mutation compensation coefficient, the spatial pose correction amount, and the vehicle body attitude signal of the unhooking robot to generate a dynamic speed command. Step S300 further includes step S310, obtaining the attitude deviation value between the vehicle body attitude signal and the preset reference attitude, and performing weighted fusion with the friction mutation compensation coefficient and the spatial pose correction amount to generate a speed feedforward compensation value. Specifically, the industrial control computer reads the vehicle body attitude signal fed back by the inertial measurement unit, including the current pitch angle, current roll angle, and current heading angle, and reads the preset reference attitude from the memory. This reference attitude is the standard attitude angle of the unhooking robot when it is unloaded on a horizontal straight track. The difference between the current pitch angle and the reference pitch angle is calculated and recorded as the pitch deviation angle; the difference between the current roll angle and the reference roll angle is calculated and recorded as the roll deviation angle; the difference between the current heading angle and the reference heading angle is calculated and recorded as the heading deviation angle. The pitch deviation angle, roll deviation angle, and heading deviation angle are combined to form an attitude deviation vector. The pitch and yaw angles in the attitude deviation vector are multiplied by preset attitude velocity compensation coefficients to obtain pitch velocity compensation components and yaw velocity compensation components, respectively. Simultaneously, the X-direction and Y-direction correction values ​​in the spatial attitude correction are multiplied by preset position velocity compensation coefficients to obtain lateral velocity compensation components and longitudinal velocity compensation components, respectively. These four velocity compensation components are added together to obtain the base velocity compensation value. The base velocity compensation value is then multiplied by the sum of the frictional abrupt change compensation coefficient obtained in step S230 and 1, i.e., base velocity compensation value × (1 + frictional abrupt change compensation coefficient), to obtain the final velocity feedforward compensation value. This fusion method allows the frictional abrupt change compensation coefficient to scale the velocity compensation amount calculated based on attitude and position deviations as a whole, thereby adjusting the compensation intensity during frictional abrupt changes.

[0044] Step S320: The speed feedforward compensation value is limited to the range defined by the difference between the preset upper and lower limits of the speed safety envelope and the current operating speed of the unhooking robot, respectively, to obtain the limited speed feedforward compensation value. Specifically, the industrial control computer reads the current operating speed of the unhooking robot, which is obtained by conversion from the encoder feedback of the drive wheel motor. The preset upper and lower limits of the speed safety envelope are read from the memory; these two values ​​represent the maximum and minimum operating speeds allowed for the unhooking robot under the current operating conditions, respectively. The difference between the upper limit of the speed safety envelope and the current operating speed is calculated and recorded as the positive compensable margin; the difference between the current operating speed and the lower limit of the speed safety envelope is calculated and recorded as the negative compensable margin. If the speed feedforward compensation value generated in step S310 is greater than the positive compensable margin, then the limiting speed feedforward compensation value is set to be equal to the positive compensable margin; if the speed feedforward compensation value is less than the negative number of the negative compensable margin, then the limiting speed feedforward compensation value is set to be equal to the negative number of the negative compensable margin; if the speed feedforward compensation value is between the above two boundaries, then the limiting speed feedforward compensation value is equal to the original speed feedforward compensation value. Through this limiting operation, it is ensured that the final speed command after superimposed compensation is always within the safe operating speed range, preventing overspeed or underspeed operation.

[0045] Step S330: The limited speed feedforward compensation value is superimposed on the speed command of the servo speed loop to generate the dynamic speed command. Specifically, the industrial control computer reads the basic speed command value from the current speed command register of the servo speed loop. This value is the original target speed without considering frictional abrupt changes and pose deviations. The limited speed feedforward compensation value obtained in step S320 is algebraically added to the basic speed command value, i.e., the new speed command value equals the basic speed command value plus the limited speed feedforward compensation value. The addition result is used as the dynamic speed command and written to the speed command register of the servo driver of the drive wheel motor via the EtherCAT bus. Subsequently, the PID controller inside the servo driver will drive the motor to run according to the dynamic speed command. The dynamic speed command is updated once in each control cycle, thereby realizing real-time feedforward compensation.

[0046] For example, assume that the pitch deviation angle in the current vehicle attitude signal is 2°, the yaw deviation angle is -1.5°, and the roll deviation angle is negligible. The attitude velocity compensation coefficient is 0.1 m / (s·°), then the pitch velocity compensation component is 0.2 m / s, and the yaw velocity compensation component is -0.15 m / s. Assume that the X-direction correction value in the spatial attitude correction is 0.04 m, the Y-direction correction value is -0.02 m, and the position velocity compensation coefficient is 0.2 s. -1The lateral velocity compensation component is 0.008 m / s, and the longitudinal velocity compensation component is -0.004 m / s. The base velocity compensation value is the sum of the above four items: 0.2 + (-0.15) + 0.008 + (-0.004) = 0.054 m / s. Assuming the friction mutation compensation coefficient is -0.35, the velocity feedforward compensation value = 0.054 × (1 - 0.35) = 0.0351 m / s. Assuming the current operating speed is 1.2 m / s, the upper limit of the velocity safety envelope is 1.5 m / s, and the lower limit of the velocity safety envelope is 0.5 m / s, the positive compensable margin is 0.3 m / s, the negative compensable margin is 0.7 m / s, and the velocity feedforward compensation value of 0.0351 m / s is less than 0.3 m / s and greater than -0.7 m / s. Therefore, the amplitude-limited velocity feedforward compensation value is 0.0351 m / s. Assuming the base speed command value is 1.0 m / s, the dynamic speed command is 1.0 + 0.0351 = 1.0351 m / s. Table 3 shows the data flow in this example.

[0047] Table 3

[0048] In step S400, after the unhooking robot runs to the preset common speed start position according to the dynamic speed command, it starts the common speed gripping robotic arm, uses the real-time distance signal and contact force feedback signal between the end gripper and the train ladder as constraints, and performs closed-loop correction on the dynamic speed command until the end gripper is reliably fixed to the train ladder, generating a fixing confirmation signal.

[0049] Specifically, the active wheel drive motor of the unhooking robot drives the robot along the track towards the carriage to be unhooked, based on the dynamic speed command generated in step S330. The onboard industrial control computer monitors the robot's position in real time, calculated by the wheel odometer. When the robot reaches the preset common-speed start position, the industrial control computer sends a start command to the common-speed gripping manipulator. The common-speed gripping manipulator begins to extend, and its end effector gradually approaches the train ladder. During the approach, a laser rangefinder installed on the end effector continuously outputs a real-time distance signal between the end effector and the train ladder; a thin-film pressure sensor installed on the inside of the end effector continuously outputs a contact force feedback signal between the end effector and the train ladder. The industrial control computer uses these two signals as constraints to perform closed-loop correction on the current dynamic speed command, specifically by reducing the speed command value to ensure that the end effector can make smooth contact without violent collision. When the real-time distance signal, the contact force feedback signal, and the speed deviation between the robot and the carriage simultaneously meet the preset reliable fixing conditions, the industrial control computer determines that the end effector has been reliably fixed to the train ladder and generates a fixing confirmation signal.

[0050] In one possible implementation, the real-time distance signal and contact force feedback signal between the end effector and the train ladder are used as constraints to perform closed-loop correction on the dynamic speed command until the end effector and the train ladder are reliably fixed, generating a fixing confirmation signal. Step S400 further includes step S410, calculating the current distance value between the end effector and the train ladder based on the real-time distance signal, and matching the current distance value with a preset deceleration mapping curve to obtain the basic deceleration correction amount. Specifically, the industrial control computer reads the real-time distance signal output by the laser rangefinder at fixed intervals. This signal is in mm and directly represents the current distance value between the end effector and the train ladder. The industrial control computer internally stores a preset deceleration mapping curve, which is a piecewise linear function, with the current distance value on the horizontal axis and the basic deceleration correction amount on the vertical axis. The basic rule of this curve is as follows: when the current distance value is greater than the first distance threshold, for example, 200mm, the basic deceleration correction is 0; when the current distance value is between the first distance threshold and the second distance threshold, for example, 50mm, the basic deceleration correction increases linearly from 0 to the maximum deceleration correction, for example, 0.5m / s; when the current distance value is less than the second distance threshold, the basic deceleration correction remains at the maximum deceleration correction. The industrial control computer substitutes the current distance value into this piecewise linear function to calculate the corresponding basic deceleration correction, which is a non-negative speed reduction value.

[0051] Step S420: Calculate the contact force change rate based on the contact force feedback signal. When the contact force change rate is greater than a preset change rate threshold, generate an emergency deceleration correction. Specifically, the industrial control computer reads the contact force feedback signal output by the diaphragm pressure sensor at the same cycle as in step S410. The industrial control computer maintains a contact force history value queue of length 5. Whenever a new contact force value is read, it is pushed to the end of the queue and the oldest value is popped. Calculate the difference between the current contact force value and the contact force value at the previous moment, and then divide it by the sampling period to obtain the contact force change rate. Compare the calculated contact force change rate with the preset change rate threshold. If the contact force change rate is greater than the preset change rate threshold, it is determined that the end gripper has experienced a rapid collision or impact with the train ladder, and the industrial control computer immediately generates an emergency deceleration correction, the value of which is fixed to the maximum allowable deceleration. If the contact force change rate is less than or equal to the preset change rate threshold, the emergency deceleration correction value is 0. This emergency deceleration correction is used to achieve rapid braking under sudden collision risk.

[0052] Step S430: The larger of the basic deceleration correction and the emergency deceleration correction is used as the final speed correction and added to the dynamic speed command. Specifically, the industrial control computer compares the basic deceleration correction obtained in step S410 and the emergency deceleration correction obtained in step S420, and takes the larger of the two values ​​as the final speed correction. This final speed correction is subtracted from the current dynamic speed command value to obtain the corrected dynamic speed command value. Simultaneously, to ensure that the corrected speed command is not lower than the preset minimum operating speed limit, the industrial control computer performs a lower limit limiting operation on the corrected dynamic speed command value; that is, if the corrected speed command value is less than the minimum operating speed limit, it is forcibly set to the minimum operating speed limit. The corrected and limited dynamic speed command value is sent to the servo driver for execution via the EtherCAT bus. By using the strategy of taking the larger value, a reasonable switch between normal approach deceleration and emergency collision braking can be achieved, ensuring operational safety.

[0053] Step S440: When the real-time distance signal, the contact force feedback signal, and the speed deviation between the uncoupling robot and the carriage to be uncoupled satisfy the distance fluctuation condition, the effective contact force condition, and the speed synchronization condition respectively within a preset judgment time window, it is determined that the end gripper is reliably fixed to the train ladder, and the fixing confirmation signal is generated; otherwise, the closed-loop correction of the dynamic speed command is maintained. Specifically, the industrial control computer continuously monitors three conditions within a preset judgment time window, for example, 0.5s. The distance fluctuation condition is defined as follows: within the judgment time window, the sampled value of the real-time distance signal is less than a preset fixed distance threshold, for example, 10mm, and the difference between the maximum and minimum values ​​is less than a preset fluctuation threshold, for example, 2mm. The effective contact force condition is defined as follows: within the judgment time window, the sampled value of the contact force feedback signal is greater than a preset minimum contact force threshold, for example, 50N, and the difference between the maximum and minimum values ​​is less than a preset contact force fluctuation threshold, for example, 10N. The speed synchronization condition is defined as follows: within the judgment time window, the absolute value of the difference between the current running speed of the uncoupling robot and the current running speed of the carriage to be uncoupled is less than a preset speed synchronization threshold, such as 0.02 m / s. The current running speed of the uncoupling robot is obtained through feedback from the active wheel encoder, and the current running speed of the carriage to be uncoupled is obtained wirelessly via a radar speedometer installed on the carriage or through a ground speed sensor beside the track. When all three conditions are simultaneously met at each sampling moment within the preset judgment time window, the industrial control computer determines that the end gripper has been reliably fixed to the train ladder and generates a fixed confirmation signal, which is a Boolean variable set to "true". If any condition is not met at any moment within the judgment time window, no fixed confirmation signal is generated, and the industrial control computer maintains the closed-loop correction operation in step S430, continuing to adjust the dynamic speed command until all conditions are simultaneously met and remain so throughout the entire time window.

[0054] Step S500: Based on the fixed confirmation signal, start the unhooking execution robot arm, observe the unhooking status based on the current signal and position feedback signal of the joint servo motor, and obtain the unhooking result.

[0055] Specifically, after receiving the fixed confirmation signal generated in step S440, the industrial control computer immediately sends a start command to the unhooking robot arm. The unhooking robot arm begins to move according to the preset unhooking trajectory, and its joint servo motor drivers send current and position feedback signals to the industrial control computer in real time. The industrial control computer collects these signals at a fixed sampling period and observes the unhooking status based on the joint dynamics model. That is, it estimates the external resistance through the current signal and determines whether the unhooking action is successfully completed by combining the position feedback signal, and finally outputs a result signal indicating whether the unhooking was successful or not.

[0056] In one possible implementation, the unhooking state is observed based on the current signal and position feedback signal of the joint servo motor to obtain the unhooking result. Step S500 further includes step S510, which converts the current signal into an estimated joint output torque value and the position feedback signal into estimated joint angular velocity and estimated joint angular acceleration values. Specifically, the industrial control computer reads the current signal of the servo driver of each joint of the unhooking execution robot arm, which is the combined effective value of the three-phase current. For each joint, the current signal is multiplied by the preset torque constant of the joint to obtain the estimated joint output torque value. At the same time, the industrial control computer reads the position feedback signal of each joint, which is the joint angle value output by the absolute encoder. The joint angle value is numerically differentiated, that is, the current angle value is subtracted from the previous angle value and then divided by the sampling period to obtain the estimated joint angular velocity value; the estimated joint angular velocity value is numerically differentiated again to obtain the estimated joint angular acceleration value. To suppress the high-frequency noise caused by differentiation, a low-pass filter with a cutoff frequency of 50Hz is applied to the angle sequence before differentiation processing.

[0057] Step S520: Based on the estimated joint output torque, estimated joint angular velocity, estimated joint angular acceleration, and preset joint inertia and damping parameters, inverse kinematics is performed, and the calculated estimated external resistance is used as the estimated unhooking resistance. Specifically, for each joint of the unhooking manipulator, the industrial control computer reads the preset inertia and preset damping parameters of that joint from the memory. The joint dynamics model adopts a standard second-order linear model, whose expression is: Estimated external resistance = Estimated joint output torque - Preset inertia parameter × Estimated joint angular acceleration - Preset damping parameter × Estimated joint angular velocity. The industrial control computer substitutes the estimated joint output torque, estimated joint angular velocity, and estimated joint angular acceleration obtained in step S510 into the above expression to calculate the estimated external resistance. This estimated external resistance characterizes the resistance from the coupler experienced by the end of the manipulator during the unhooking process, and is used as the estimated unhooking resistance. For multi-joint robotic arms, the estimated value of the unhooking resistance of the main joint bearing the main load can be selected as the criterion, or the estimated values ​​of the unhooking resistance of all joints can be weighted and summed.

[0058] Step S530: Obtain the unhooking result based on the comparison result between the estimated unhooking resistance value and the preset unhooking success threshold. Specifically, the industrial control computer compares the estimated unhooking resistance value calculated in step S520 with the preset unhooking success threshold read from the memory. Based on the comparison result, different unhooking result signals are output. This step achieves automatic determination of the unhooking state through resistance threshold judgment, avoiding the risk of failure of external vision sensors under harsh working conditions.

[0059] In one possible implementation, the uncoupling result is obtained based on the comparison between the estimated uncoupling resistance value and a preset uncoupling success threshold. Step S530 further includes step S531: if the estimated uncoupling resistance value is less than the preset uncoupling success threshold, uncoupling is determined to be successful, and an uncoupling completion signal is output. Specifically, the industrial control computer executes the comparison operation of step S530 in each sampling cycle. If the current estimated uncoupling resistance value is less than the preset uncoupling success threshold, it is determined that the uncoupling execution robot arm has completed the action of pulling up and disengaging the coupler pin, and uncoupling is successful. The industrial control computer sets the uncoupling result signal to "uncoupling successful" and generates an uncoupling completion signal, which is a Boolean variable and is set to "true". This signal can be sent to the ground dispatch center via a wireless communication module to inform that the uncoupling operation has been completed.

[0060] Step S532: Otherwise, determine whether the duration for which the estimated unhooking resistance value is greater than or equal to the preset unhooking success threshold is less than a preset time threshold; if so, continue monitoring the estimated unhooking resistance value. Specifically, if the condition in step S531 is not met, i.e., the estimated unhooking resistance value is greater than or equal to the preset unhooking success threshold, the industrial control computer starts a timer to record the duration of this state. The industrial control computer compares the duration recorded by the timer with the preset time threshold. If the duration is less than the preset time threshold, the industrial control computer does not take any additional action and continues to collect signals and calculate the estimated unhooking resistance value according to the process from steps S510 to S520, maintaining continuous monitoring of the unhooking state. This waiting mechanism is used to eliminate false threshold exceedance phenomena caused by instantaneous impacts or signal noise.

[0061] Step S533: If not, using the current joint angle position of the unhooking execution robot arm as the center, generate a bidirectional alternating angular displacement disturbance command sequence according to a preset jitter frequency and a preset jitter amplitude. Specifically, when step S532 determines that the duration for which the estimated unhooking resistance value is greater than or equal to the preset unhooking success threshold has reached a preset time threshold, the industrial control computer enters the active disturbance unhooking mode. By reading the current angle feedback value of the main joint of the unhooking execution robot arm, the current joint angle position is obtained, and this position is used as the disturbance center point. The preset jitter frequency and preset jitter amplitude are read from the memory. The industrial control computer generates a sinusoidal angular displacement disturbance sequence, the expression of which is: disturbance angle value = preset jitter amplitude × sine (2 × pi × preset jitter frequency × current time). The angle value generated by this sequence changes sinusoidally with time within the range of positive and negative preset jitter amplitudes, centered on the current joint angle position. The industrial control computer adds the current joint angle position to the disturbance angle value to generate an angular displacement disturbance command sequence. Each value in the sequence represents the target joint angle within a control cycle.

[0062] Step S534: Based on the angular displacement disturbance command sequence, drive the unhooking execution robot arm to perform reciprocating swings and continuously monitor the estimated unhooking resistance value. When the estimated unhooking resistance value is less than the preset unhooking success threshold, stop swinging and re-execute the unhooking lifting operation. Specifically, the industrial control computer sends the angular displacement disturbance command sequence generated in step S533 to the joint servo driver of the unhooking execution robot arm frame by frame via the EtherCAT bus, driving the robot arm to perform small-amplitude high-frequency reciprocating swings centered on the current joint angle position. During the swinging process, the industrial control computer continuously executes steps S510 to S520, calculating the estimated unhooking resistance value in real time. The industrial control computer compares the current estimated unhooking resistance value with the preset unhooking success threshold in real time. Once it detects that the estimated unhooking resistance value is less than the preset unhooking success threshold, it determines that the coupler jamming state has been resolved by disturbance. The industrial control computer immediately stops sending angular displacement disturbance commands, restores the joint angle command to the current joint angle position, and restarts the unhooking lifting operation process. This reciprocating oscillation can cause a slight relative displacement of the internal mechanical components of the stuck coupler, thereby reducing the resistance to uncoupling.

[0063] Step S535: If the estimated unhooking resistance value is still greater than or equal to the preset unhooking success threshold after a preset number of consecutive swings, a manual intervention alarm signal is output. Specifically, the industrial control computer maintains a swing counter to record the number of times the disturbance unhooking cycle constituted by steps S533 and S534 is executed. Each time a complete disturbance unhooking cycle is completed, i.e., from the triggering of step S533 until the swing stops due to the resistance dropping below the threshold, or after a preset shaking duration but the resistance does not drop below the threshold, the counter is incremented by 1. When the counter value reaches the preset number, if the estimated unhooking resistance value is still greater than or equal to the preset unhooking success threshold in the current sampling period, it is determined that the robotic arm cannot complete the unhooking through automatic disturbance. At this time, the industrial control computer sends a manual intervention alarm signal to the ground dispatch center via the wireless communication module. This alarm signal includes the current device number of the unhooking robot, the fault timestamp, and the estimated unhooking resistance value. Simultaneously, the industrial control computer controls the unhooking robotic arm to stop all movements and remain in a safe position, awaiting manual intervention.

[0064] This application's embodiments solve the technical problem of existing unhooking robots in dynamic and variable environments where it is difficult to adaptively adjust its motion parameters according to real-time working conditions to maintain end-effector execution accuracy and operational stability. By acquiring signals and extracting wheel-rail friction features and carriage contour features, identifying friction abrupt changes and carriage deviations to generate compensation coefficients and correction amounts, performing speed feedforward compensation to generate dynamic speed commands, and then using distance and contact force as constraints to correct the speed commands in a closed loop until the end gripper is reliably fixed to the carriage ladder, the unhooking robot arm is then started and the unhooking status is observed to obtain the results.

[0065] In the above text, refer to Figure 1 This paper describes in detail a closed-loop control method for the servo motion of a hook-unhooking robot under harsh working conditions according to an embodiment of the present invention. Next, reference will be made to... Figure 2 This invention describes a closed-loop control system for the servo motion of a hook-unhooking robot under harsh working conditions, according to an embodiment of the present invention.

[0066] The servo motion closed-loop control system for unhooking robots under harsh working conditions, according to embodiments of the present invention, addresses the technical problem of existing unhooking robots struggling to adaptively adjust their motion parameters based on real-time changes in working conditions to maintain end-effector accuracy and operational stability in dynamic and variable environments. This system enables the unhooking robot to maintain end-effector accuracy and operational stability under harsh working conditions. The servo motion closed-loop control system for unhooking robots under harsh working conditions includes: a perception and feature extraction module 10, a disturbance identification and pose calculation module 20, a feedforward compensation and speed command generation module 30, a common-speed following and reliable fixing module 40, and an unhooking observation and result determination module 50.

[0067] The perception and feature extraction module 10 is used to collect the servo torque feedback signal of the active wheel drive motor of the unhooking robot and perform time-domain feature extraction to obtain wheel-rail contact friction feature parameters during the process of the unhooking robot running along the track towards the unhooking carriage; collect the point cloud data of the side of the unhooking carriage and perform plane fitting and edge extraction to obtain the carriage contour feature parameters; the disturbance identification and pose calculation module 20 is used to identify friction abrupt changes in the wheel-rail contact friction feature parameters and generate friction abrupt change compensation coefficients; and to identify deviations in the carriage contour feature parameters to generate spatial pose correction amounts; the feedforward compensation and speed command generation module 30 is used to generate spatial pose correction amounts based on the friction abrupt change compensation coefficients and the spatial pose correction amounts. The system performs speed feedforward compensation on the positive and unhooking robot's vehicle posture signals to generate dynamic speed commands; the common speed following and reliable fixing module 40 is used to start the common speed grasping robot arm after the unhooking robot runs to the preset common speed start position according to the dynamic speed command, and uses the real-time distance signal and contact force feedback signal between the end gripper and the train ladder as constraints to perform closed-loop correction on the dynamic speed command until the end gripper and the train ladder are reliably fixed, generating a fixing confirmation signal; the unhooking observation and result judgment module 50 is used to start the unhooking execution robot arm according to the fixing confirmation signal, and observe the unhooking status according to the current signal and position feedback signal of the joint servo motor to obtain the unhooking result.

[0068] The specific configuration of the perception and feature extraction module 10 is described in detail below: As mentioned above, the servo torque feedback signal of the active wheel drive motor of the unhooking robot is collected and time-domain feature extraction is performed to obtain wheel-rail contact friction feature parameters. The perception and feature extraction module 10 may further include: a window sampling unit for collecting the servo torque feedback signal sequence of the active wheel drive motor within a preset sliding time window; and a filtering and combination unit for filtering the servo torque feedback signal sequence, extracting the time-domain statistical features of the filtered signal sequence, and combining the time-domain statistical features into the wheel-rail contact friction feature parameters.

[0069] The process involves collecting point cloud data of the side of the carriage to be unhooked and performing planar fitting and edge extraction to obtain the carriage contour feature parameters. The perception and feature extraction module 10 may further include: a distortion level query unit for querying a preset distortion degree mapping table based on the ambient light intensity and dust concentration values ​​at the time of data collection to obtain the visual distortion level; a self-cleaning trigger and re-collection unit for activating the self-cleaning device of the unhooking robot's vision sensor to perform a cleaning action and acquire secondary carriage side point cloud data when the visual distortion level is greater than a preset level threshold; and an adaptive filtering replacement unit for adaptively filtering the secondary carriage side point cloud data using the filtering intensity coefficient corresponding to the visual distortion level to obtain corrected carriage side point cloud data, and replacing the original carriage side point cloud data with the corrected carriage side point cloud data for planar fitting and edge extraction.

[0070] The disturbance identification and pose calculation module 20 is described in detail below: As mentioned above, it identifies frictional abrupt changes in the wheel-rail contact friction characteristic parameters and generates frictional abrupt change compensation coefficients; it identifies deviations in the carriage contour characteristic parameters and generates spatial pose correction amounts. The disturbance identification and pose calculation module 20 may further include: a deviation vector calculation unit for comparing the wheel-rail contact friction characteristic parameters with preset normal operating reference characteristic parameters and calculating a characteristic deviation vector; and a mutation type and intensity determination unit for determining the type and intensity of abrupt changes when the magnitude of the characteristic deviation vector exceeds a preset magnitude threshold, based on the... The direction and amplitude of the characteristic deviation vector determine the friction mutation type and friction mutation intensity; the compensation coefficient mapping query unit is used to query the preset compensation coefficient mapping relationship according to the friction mutation type and the friction mutation intensity to obtain the friction mutation compensation coefficient; the three-dimensional offset calculation unit is used to match the car body contour feature parameters with the preset standard car body template to calculate the lateral offset, longitudinal offset and deflection angle of the car body to be uncoupled relative to the standard car body in three-dimensional space; the pose correction amount synthesis unit is used to generate spatial pose correction amount according to the lateral offset, the longitudinal offset and the deflection angle.

[0071] The disturbance identification and pose calculation module 20 may further include: a size deviation calculation and type identification generation unit for comparing and matching the car body contour feature parameters with a preset standard car body template, calculating the lateral and longitudinal size deviation values ​​of the car body, and generating a shape deviation type identification when any size deviation value is greater than a preset size deviation tolerance threshold; and a fault tolerance compensation parameter superposition unit for querying a preset trajectory fault tolerance compensation mapping relationship based on the shape deviation type identification to obtain trajectory fault tolerance compensation parameters, and superimposing the trajectory fault tolerance compensation parameters into the spatial pose correction amount.

[0072] The detailed description of the specific configuration of the feedforward compensation and speed command generation module 30 is as follows: As mentioned above, speed feedforward compensation is performed based on the friction mutation compensation coefficient, the spatial pose correction amount, and the vehicle body posture signal of the unhooking robot to generate a dynamic speed command. The feedforward compensation and speed command generation module 30 may further include: a weighted fusion unit for obtaining the posture deviation value between the vehicle body posture signal and the preset reference posture, and performing weighted fusion with the friction mutation compensation coefficient and the spatial pose correction amount to generate a speed feedforward compensation value; a dynamic interval limiting unit for limiting the speed feedforward compensation value to the interval defined by the difference between the preset upper and lower limits of the speed safety envelope and the current running speed value of the unhooking robot, respectively, to obtain a limited speed feedforward compensation value; and a speed command superposition output unit for superimposing the limited speed feedforward compensation value onto the speed command of the servo speed loop to generate the dynamic speed command.

[0073] The detailed description of the specific configuration of the common speed following and reliable fixing module 40 is explained as follows: As mentioned above, the real-time distance signal and contact force feedback signal between the end gripper and the train ladder are used as constraints to perform closed-loop correction on the dynamic speed command until the end gripper and the train ladder are reliably fixed, generating a fixing confirmation signal. The common speed following and reliable fixing module 40 may further include: a basic deceleration correction amount generation unit used to calculate the current distance value between the end gripper and the train ladder based on the real-time distance signal, and match the current distance value with a preset deceleration mapping curve to obtain the basic deceleration correction amount; an emergency deceleration correction amount generation unit used to calculate the current distance value between the end gripper and the train ladder based on the contact force feedback signal. The contact force change rate is calculated. When the contact force change rate is greater than a preset change rate threshold, an emergency deceleration correction is generated. The final speed correction superposition unit is used to superimpose the larger of the basic deceleration correction and the emergency deceleration correction onto the dynamic speed command. The three-condition joint determination unit is used to determine that the end gripper is reliably fixed to the train ladder and generate the fixation confirmation signal when the speed deviation between the real-time distance signal, the contact force feedback signal, and the uncoupling robot and the carriage to be uncoupled meets the distance fluctuation condition, the effective contact force condition, and the speed synchronization condition respectively within a preset determination time window; otherwise, the closed-loop correction of the dynamic speed command is maintained.

[0074] The detailed description of the specific configuration of the hook-off observation and result determination module 50 is explained as follows: As mentioned above, the hook-off state is observed based on the current signal and position feedback signal of the joint servo motor to obtain the hook-off result. The hook-off observation and result determination module 50 may further include: a signal conversion unit for converting the current signal into a joint output torque estimate, and converting the position feedback signal into a joint angular velocity estimate and a joint angular acceleration estimate; a dynamic inverse solution unit for performing dynamic inverse solution based on the joint output torque estimate, the joint angular velocity estimate, the joint angular acceleration estimate, and preset joint inertia parameters and joint damping parameters, and using the calculated external resistance estimate as the hook-off resistance estimate; and a comparison and determination unit for obtaining the hook-off result based on the comparison result between the hook-off resistance estimate and the preset hook-off success threshold.

[0075] The unhooking result is obtained based on the comparison between the estimated unhooking resistance value and a preset unhooking success threshold. The comparison and determination unit may further include: a success determination and signal output subunit, used to determine unhooking success and output a unhooking completion signal if the estimated unhooking resistance value is less than the preset unhooking success threshold; a timeout determination subunit, used to determine whether the duration for which the estimated unhooking resistance value is greater than or equal to the preset unhooking success threshold is less than a preset time threshold; if so, to continue monitoring the estimated unhooking resistance value; and a jitter command generation subunit, used to execute the current joint of the robotic arm with the unhooking action. Centered on the angular position, a bidirectional alternating angular displacement disturbance command sequence is generated according to a preset jitter frequency and a preset jitter amplitude. The jitter execution and monitoring subunit is used to drive the unhooking execution robot arm to reciprocate according to the angular displacement disturbance command sequence, and continuously monitor the estimated value of the unhooking resistance. When the estimated value of the unhooking resistance is less than the preset unhooking success threshold, the swinging stops and the unhooking lifting operation is re-executed. The failure alarm subunit is used to output a manual intervention alarm signal if the estimated value of the unhooking resistance is still greater than or equal to the preset unhooking success threshold after a preset number of consecutive reciprocating swings.

[0076] The servo motion closed-loop control system for unhooking robots under harsh working conditions provided in the embodiments of the present invention can execute the servo motion closed-loop control method for unhooking robots under harsh working conditions provided in any embodiment of the present invention, and has the corresponding functional modules and beneficial effects of the execution method.

[0077] Although this application makes various references to certain modules in the system according to the embodiments of this application, any number of different modules can be used and run on user terminals and / or servers. The various units and modules included are only divided according to functional logic, but are not limited to the above division, as long as the corresponding functions can be achieved; in addition, the specific names of each functional unit are only for easy distinction between each other and are not used to limit the scope of protection of this invention.

[0078] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any way. Although the present invention has been disclosed above with reference to preferred embodiments, it is not intended to limit the present invention. Any person skilled in the art can make some modifications or alterations to the above-disclosed technical content to create equivalent embodiments without departing from the scope of the present invention. Any modifications, equivalent changes, and alterations made to the above embodiments based on the technical essence of the present invention without departing from the scope of the present invention shall still fall within the scope of the present invention.

Claims

1. A closed-loop control method for servo motion of a hook-unhooking robot under harsh working conditions, characterized in that, The method includes: During the process of the unhooking robot moving along the track towards the carriage to be unhooked, the servo torque feedback signal of the active wheel drive motor of the unhooking robot is collected and time-domain feature extraction is performed to obtain wheel-rail contact friction feature parameters; the point cloud data of the side of the carriage to be unhooked is collected and plane fitting and edge extraction are performed to obtain the carriage contour feature parameters. Friction abrupt change identification is performed on the wheel-rail contact friction characteristic parameters to generate friction abrupt change compensation coefficient; deviation identification is performed on the carriage contour characteristic parameters to generate spatial pose correction amount; Based on the frictional mutation compensation coefficient, the spatial pose correction amount, and the vehicle body attitude signal of the unhooking robot, speed feedforward compensation is performed to generate dynamic speed commands. After the unhooking robot runs to the preset common speed start position according to the dynamic speed command, it starts the common speed gripping robotic arm and uses the real-time distance signal and contact force feedback signal between the end gripper and the train ladder as constraints to perform closed-loop correction on the dynamic speed command until the end gripper is reliably fixed to the train ladder and a fixing confirmation signal is generated. Based on the fixed confirmation signal, the unhooking execution robot arm is started, and the unhooking status is observed based on the current signal and position feedback signal of the joint servo motor to obtain the unhooking result.

2. The closed-loop control method for servo motion of a hook-unhooking robot under harsh working conditions as described in claim 1, characterized in that, The servo torque feedback signal of the active wheel drive motor of the unhooking robot is collected and its time-domain features are extracted to obtain the wheel-rail contact friction characteristic parameters, including: Collect the servo torque feedback signal sequence of the active wheel drive motor within a preset sliding time window; The servo torque feedback signal sequence is filtered, and the time-domain statistical features of the filtered signal sequence are extracted. The time-domain statistical features are then combined to form the wheel-rail contact friction characteristic parameters.

3. The closed-loop control method for servo motion of a hook-unhooking robot under harsh working conditions as described in claim 1, characterized in that, The system collects point cloud data of the side of the car to be uncoupled and performs planar fitting and edge extraction to obtain the car contour feature parameters, and also includes: Based on the ambient light intensity and dust concentration values ​​at the time of data collection of the point cloud data on the side of the carriage, the visual distortion level is obtained by querying a preset distortion degree mapping table. When the visual distortion level is greater than a preset level threshold, the visual sensor self-cleaning device of the unhooking robot is activated to perform a cleaning action and acquire secondary point cloud data of the side of the carriage. The secondary carriage side point cloud data is adaptively filtered using the filtering intensity coefficient corresponding to the visual distortion level to obtain corrected carriage side point cloud data. The corrected carriage side point cloud data is then used to replace the original carriage side point cloud data for plane fitting and edge extraction.

4. The closed-loop control method for servo motion of a hook-unhooking robot under harsh working conditions as described in claim 1, characterized in that, Friction abrupt change identification is performed on the wheel-rail contact friction characteristic parameters to generate friction abrupt change compensation coefficient; Deviation identification is performed on the contour feature parameters of the carriage to generate spatial pose correction values, including: The wheel-rail contact friction characteristic parameters are compared with the preset normal operation reference characteristic parameters, and the characteristic deviation vector is calculated. When the magnitude of the feature deviation vector is greater than a preset magnitude threshold, the frictional mutation type and frictional mutation intensity are determined according to the direction and magnitude of the feature deviation vector. The friction mutation compensation coefficient is obtained by querying a preset compensation coefficient mapping relationship based on the friction mutation type and the friction mutation intensity. The contour feature parameters of the carriage are matched with a preset standard carriage template, and the lateral offset, longitudinal offset, and deflection angle of the carriage to be uncoupled relative to the standard carriage in three-dimensional space are calculated. Spatial pose correction is generated based on the lateral offset, the longitudinal offset, and the deflection angle.

5. The closed-loop control method for servo motion of a hook-unhooking robot under harsh working conditions as described in claim 1, characterized in that, The method further includes identifying deviations in the carriage contour feature parameters and generating spatial pose correction values, and also includes: The outline feature parameters of the carriage are compared and matched with a preset standard carriage template to calculate the lateral and longitudinal dimension deviation values ​​of the carriage. When any dimension deviation value is greater than the preset dimension deviation tolerance threshold, an outline deviation type identifier is generated. Based on the shape deviation type identifier, the preset trajectory fault tolerance compensation mapping relationship is queried to obtain the trajectory fault tolerance compensation parameters, and the trajectory fault tolerance compensation parameters are superimposed on the spatial pose correction amount.

6. The closed-loop control method for servo motion of a hook-unhooking robot under harsh working conditions as described in claim 1, characterized in that, Based on the frictional abrupt change compensation coefficient, the spatial pose correction amount, and the vehicle body attitude signal of the unhooking robot, velocity feedforward compensation is performed to generate dynamic velocity commands, including: The attitude deviation value between the vehicle body attitude signal and the preset reference attitude is obtained, and the friction mutation compensation coefficient and the spatial pose correction amount are weighted and fused to generate the velocity feedforward compensation value. The speed feedforward compensation value is limited to the range defined by the difference between the preset upper and lower limits of the speed safety envelope and the current running speed value of the unhooking robot, respectively, to obtain the limited speed feedforward compensation value. The limiting speed feedforward compensation value is superimposed on the speed command of the servo speed loop to generate the dynamic speed command.

7. The closed-loop control method for servo motion of a hook-unhooking robot under harsh working conditions as described in claim 1, characterized in that, Using the real-time distance signal and contact force feedback signal between the end gripper and the train ladder as constraints, the dynamic speed command is corrected in a closed loop until the end gripper is reliably fixed to the train ladder, generating a fixing confirmation signal, including: The current distance between the end gripper and the train ladder is calculated based on the real-time distance signal. The current distance is then matched with a preset deceleration mapping curve to obtain the basic deceleration correction amount. The contact force change rate is calculated based on the contact force feedback signal. When the contact force change rate is greater than a preset change rate threshold, an emergency deceleration correction amount is generated. The larger of the basic deceleration correction amount and the emergency deceleration correction amount is used as the final speed correction amount and superimposed on the dynamic speed command. When the real-time distance signal, the contact force feedback signal, and the speed deviation between the unhooking robot and the carriage to be unhooked satisfy the distance fluctuation condition, the effective contact force condition, and the speed synchronization condition respectively within the preset judgment time window, it is determined that the end gripper is reliably fixed to the train ladder, and the fixing confirmation signal is generated; otherwise, the closed-loop correction of the dynamic speed command is maintained.

8. The closed-loop control method for servo motion of a hook-unhooking robot under harsh working conditions as described in claim 1, characterized in that, The unhooking status is observed based on the current signal and position feedback signal of the joint servo motor, and the unhooking results are obtained, including: The current signal is converted into a joint output torque estimate, and the position feedback signal is converted into a joint angular velocity estimate and a joint angular acceleration estimate. Based on the estimated joint output torque, estimated joint angular velocity, estimated joint angular acceleration, and preset joint inertia parameters and joint damping parameters, the inverse dynamic solution is performed, and the calculated estimated external resistance is used as the estimated hook-off resistance. The unhooking result is obtained by comparing the estimated unhooking resistance value with the preset unhooking success threshold.

9. The closed-loop control method for servo motion of a hook-unhooking robot under harsh working conditions as described in claim 8, characterized in that, The unhooking result is obtained based on the comparison between the estimated unhooking resistance value and the preset unhooking success threshold, including: If the estimated unhooking resistance is less than the preset unhooking success threshold, the unhooking is determined to be successful, and an unhooking completion signal is output. Otherwise, determine whether the duration during which the estimated unhooking resistance value is greater than or equal to the preset unhooking success threshold is less than a preset time threshold; If so, continue monitoring the estimated resistance value for unhooking; If not, take the current joint angle position of the unhooking robotic arm as the center, and generate a bidirectional alternating angular displacement disturbance command sequence according to the preset jitter frequency and preset jitter amplitude; According to the angular displacement disturbance command sequence, the unhooking execution robot arm is driven to swing back and forth, and the estimated value of the unhooking resistance is continuously monitored. When the estimated value of the unhooking resistance is less than the preset unhooking success threshold, the swinging stops and the unhooking lifting operation is re-executed. If the estimated resistance value for unhooking is still greater than or equal to the preset unhooking success threshold after a preset number of consecutive reciprocating swings, a manual intervention alarm signal will be output.

10. A closed-loop servo motion control system for a hook-unhooking robot under harsh working conditions, characterized in that: The system is used to implement the closed-loop control method for servo motion of a hook-unhooking robot under harsh working conditions as described in any one of claims 1-9. The system includes: The perception and feature extraction module is used to collect the servo torque feedback signal of the active wheel drive motor of the unhooking robot and perform time-domain feature extraction to obtain wheel-rail contact friction feature parameters during the process of the unhooking robot running along the track towards the carriage to be unhooked; and to collect the point cloud data of the side of the carriage to be unhooked and perform plane fitting and edge extraction to obtain the carriage contour feature parameters. The disturbance identification and pose calculation module is used to identify frictional abrupt changes in the wheel-rail contact friction characteristic parameters and generate frictional abrupt change compensation coefficients; and to identify deviations in the carriage contour characteristic parameters and generate spatial pose correction amounts. The feedforward compensation and speed command generation module is used to perform speed feedforward compensation based on the frictional change compensation coefficient, the spatial pose correction amount, and the vehicle body posture signal of the unhooking robot, and generate dynamic speed commands. The common speed following and reliable fixing module is used to start the common speed grasping robot arm when the unhooking robot runs to the preset common speed start position according to the dynamic speed command. The module uses the real-time distance signal and contact force feedback signal between the end gripper and the train ladder as constraints to perform closed-loop correction on the dynamic speed command until the end gripper is reliably fixed to the train ladder, and generates a fixing confirmation signal. The hook removal observation and result determination module is used to start the hook removal execution robot arm according to the fixed confirmation signal, observe the hook removal status according to the current signal and position feedback signal of the joint servo motor, and obtain the hook removal result.

Images