Method for robot abnormal behavior detection and response based on motion control constraints

Abstract

The present application relates to the technical field of multi-joint robot motion control and fault monitoring, in particular to a robot abnormal behavior detection and response method based on motion control constraints, based on robot kinematics and dynamics parameters, a multi-dimensional dynamic coupling constraint manifold model is constructed by fusing Jacobian linkage constraints and Lagrange dynamics constraints, the actual state of the joint is calculated in real time to deviate from the comprehensive residual of the constraint manifold; then the effective constraint violation event is verified and confirmed through space-time joint filtering, and the constraint violation characteristic vector is extracted; then the characteristic vector is matched with each standard characteristic template in the fault mode library, and the diagnosis conclusion of the fault type, position and stage is output; finally, the corresponding control adjustment and work order pushing are executed according to the fault type-response strategy hierarchical mapping table, without additional hardware, the servo system feedback data is reused, the false positive rate can be effectively reduced, the accurate fault positioning and hierarchical safety response are realized, and the production continuity and equipment operation safety are considered.

Description

Technical Field

[0001] This invention relates to the field of motion control and fault monitoring technology for multi-joint robots, specifically to a method for detecting and responding to abnormal robot behavior based on motion control constraints. Background Technology

[0002] In most existing industrial robots operating in continuous scenarios such as welding, handling, and assembly, motion safety monitoring often relies on pre-set hard physical limit thresholds for each joint, such as maximum permissible speed, maximum permissible torque, and joint position limits. When the measured value of a joint exceeds the corresponding set threshold, the control system determines an anomaly and triggers a shutdown protection. While this method can detect hard faults that have developed into explicit over-limits, such as joint runaway or instantaneous overload caused by external impact, it fails to detect gradual anomalies in their early stages, such as early wear of the reducer, slow drift of the encoder signal, and gradual increase in transmission backlash. In these early stages, the individual measured values ​​of each joint are often still within their respective permissible ranges. Therefore, threshold comparisons cannot detect these potential anomalies, causing the industrial robot to continue operating under a seemingly normal state until the fault worsens to the point of hard over-limit, triggering an alarm. By this time, the equipment may have already suffered irreversible damage or caused quality defects in the manufactured products.

[0003] Furthermore, most existing anomaly detection methods, upon detecting anomalies, only respond with a single emergency stop, failing to distinguish the root cause and severity of the anomaly. This can lead to the disruption of the entire production process due to minor, recoverable disturbances. A few technical solutions attempt to introduce fault diagnosis models, but the diagnostic basis mostly relies on external signals collected by additional vibration or acoustic emission sensors, rather than directly analyzing the anomaly patterns from the robot's own motion control constraints. This fails to fully utilize the torque, speed, and position feedback data already available in the servo drive system.

[0004] This has resulted in most existing technical solutions lacking the ability to characterize anomalies and provide graded responses from the perspective of underlying physical linkages, making it difficult for current robots to achieve an effective balance between operational safety and production continuity when facing progressive performance degradation. Summary of the Invention

[0005] To address the aforementioned shortcomings of existing technologies, this invention provides a method for detecting and responding to abnormal robot behavior based on motion control constraints, which can effectively solve the problem of difficulty in detecting and classifying latent anomalies in the prior art.

[0006] To solve the above-mentioned technical problems, the present invention adopts the following technical solution: The present invention provides a method for detecting and responding to abnormal robot behavior based on motion control constraints, including:

[0007] Step 1: Obtain the kinematic and dynamic parameters of the target robot, construct a multi-dimensional dynamic coupling constrained manifold model, collect real-time state data of each joint of the target robot in each control cycle and substitute it into the constrained manifold model, calculate the residual value of the current actual state point deviating from the constrained manifold, and obtain the comprehensive deviation quantification value of the target robot's multi-axis linkage relationship from the normal state.

[0008] Step 2: Perform spatiotemporal joint filtering on the comprehensive deviation quantification value. In the time dimension, set an adaptive anomaly judgment threshold based on the historical statistical characteristics of the residuals and verify the duration of continuous exceedance. In the spatial dimension, verify the correlation consistency between kinematic residuals and dynamic residuals, confirm effective constraint violation events, and freeze the joint state data within a specified time window before and after the effective constraint violation event, and extract the constraint violation feature vector from it.

[0009] Step 3: Construct a fault mode library. The fault mode library contains standard feature templates corresponding to various typical faults. Each standard feature template in the fault mode library is matched and scored one by one. The matching score between the constraint violation feature vector and each known fault type is quantified to confirm the preliminary diagnosis. Finally, the diagnosed fault type, fault location, fault stage and confidence level are output.

[0010] Step 4: Construct a fault type-response strategy hierarchical mapping table. Based on the preliminary diagnosis, determine the response level and quantitative parameter adjustment amount corresponding to the diagnosed fault type, and execute the corresponding control commands and push maintenance work orders to the target robot.

[0011] Preferably, the construction process of the multidimensional dynamically coupled constrained manifold model specifically includes:

[0012] Obtain the factory-preset parameter table and dynamic parameters of each joint of the target robot. The parameter table includes the link length, link torsion angle, joint offset and joint rotation range between adjacent joint coordinate systems. The dynamic parameters include the mass, center of mass position and inertia tensor of each link.

[0013] Based on the parameter table, a kinematic model of the target robot is constructed, and the Jacobian matrix between the end effector velocity and the velocities of each joint is derived.

[0014] Based on the aforementioned dynamic parameters, a dynamic model of the target robot is constructed, and the regression matrix and dynamic parameter vector between the torque of each joint and the joint position, angular velocity, and angular acceleration are derived.

[0015] The kinematic linkage relationship and the dynamic coupling relationship are integrated into a unified multidimensional dynamic coupling constraint manifold model. The multidimensional dynamic coupling constraint manifold model represents the mathematical relationship that must be satisfied simultaneously between the position, velocity and torque of each joint in the target robot's operating state.

[0016] Preferably, the calculation process of the residual value of the current actual state point deviating from the constrained manifold specifically includes:

[0017] Within each control cycle, the measured position vector, measured angular velocity vector, and measured angular acceleration vector of each joint of the target robot at the current moment are collected, and the measured torque vector of each joint of the target robot and the actual velocity vector of the end effector obtained by forward kinematics calculation are obtained.

[0018] The current joint position vector is substituted into the real-time calculation process of the Jacobian matrix to obtain the value of the Jacobian matrix at the current time. The current measured joint position vector, measured angular velocity vector, and measured angular acceleration vector are substituted into the real-time calculation process of the regression matrix to obtain the value of the regression matrix at the current time.

[0019] The real-time calculation results and the collected measured data are substituted into the multidimensional dynamic coupling constraint manifold model to calculate the kinematic linkage residuals and dynamic coupling residuals respectively. Combined with the preset weight coefficients, the comprehensive deviation quantification value of the target robot under the current operating state is calculated.

[0020] Preferably, the process for determining effective constraint violation events specifically includes:

[0021] The quantized value of the overall deviation of each control cycle is stored in a circular buffer of a set length. The circular buffer stores the residual history sequence corresponding to multiple control cycles.

[0022] The mean and standard deviation are calculated based on the residual historical sequence, and an adaptive anomaly detection threshold is calculated in combination with a preset sensitivity coefficient.

[0023] If the quantized value of the overall deviation of the current period exceeds the adaptive anomaly judgment threshold, the duration counter is started. If the overall residual scalar falls back to below the adaptive anomaly judgment threshold, the counter is cleared. When the cumulative value of the counter reaches the preset minimum duration period, the time dimension condition is satisfied.

[0024] Obtain the kinematic linkage residual norm sequence and the dynamic coupling residual norm sequence for multiple consecutive control cycles before the current time, respectively, and calculate the Pearson correlation coefficient between the two. If both types of residual norms increase simultaneously and are positively correlated, it is determined that the spatial dimension condition is satisfied. Only when both the temporal and spatial dimension conditions are satisfied simultaneously is it confirmed that a valid constraint violation event has occurred.

[0025] Preferably, the process of extracting the constraint violation feature vector specifically includes:

[0026] Based on the confirmed valid constraint violation event, the state data of all joints within each set time period before and after the valid constraint violation event is confirmed are frozen, forming a feature extraction time window.

[0027] The kinematic linkage residuals and dynamic coupling residuals of each control cycle within the feature extraction time window are expanded according to the joint dimension. The absolute value of the residual peak value of each joint within the time window is calculated. Joints whose absolute value of the residual peak value exceeds the corresponding component judgment threshold are marked as constraint violation joints, and the corresponding numbers are recorded to the violation linkage relationship number set.

[0028] For each marked joint, the peak values ​​of the corresponding kinematic residual components and dynamic residual components are retrieved within the time window. The maximum number of consecutive control cycles in which the comprehensive residual continuously exceeds the component judgment threshold is calculated and converted into the duration of constraint violation.

[0029] The moment when the sign of the angular velocity of each marked joint changes is retrieved as the moment of motion reversal. The time offset between the event confirmation moment and the most recent moment of motion reversal is calculated, and the constraint violation feature vector is assembled.

[0030] Preferably, the construction method of the fault mode library specifically includes:

[0031] The statistical analysis includes data on various confirmed fault cases during the historical operation of the target robot, as well as labeled data obtained from fault injection experiments on similar robots. The fault types include reducer wear, gear backlash, encoder signal drift, joint bearing loosening, external force collision, and servo drive command tampering. The labeled data for each fault type contains a complete record of the constraint violation feature vector corresponding to the time of the fault occurrence.

[0032] Statistical analysis is performed on all labeled samples for each fault type to extract common patterns in constraint violation features of each fault type. Standard feature templates corresponding to each fault type are defined, and the structure of the standard feature templates corresponds to the constraint violation feature vectors.

[0033] Calculate the corresponding reference amplitude center and allowable deviation radius for each numerical range field in the standard feature template.

[0034] By summarizing the standard feature templates, reference amplitude centers, and permissible deviation radii corresponding to each fault type, a fault mode library is obtained.

[0035] Preferably, the specific process of quantifying the matching score between the constraint violation feature vector and each known fault type includes:

[0036] Before comparison, feature dimension alignment verification is performed to check whether there is an intersection between the set of violated linkage relationship numbers in the constraint violation feature vector and the set of joint numbers involved in the standard feature template corresponding to each fault type in the fault mode library.

[0037] If the intersection of the two is empty, the matching score for this fault type is zero. If there is an intersection, the joints in the intersection are extracted as valid comparison joints for quantitative calculation.

[0038] For each field, the measured value of each effective alignment joint in the constraint violation feature vector is compared with the reference amplitude center and allowable deviation radius of the corresponding field in the template corresponding to the target fault type. The deviation degree corresponding to each field is calculated, and each deviation degree is substituted into the scoring function to obtain the normalized field matching score.

[0039] For each valid matching joint, the sum of the individual matching scores of its corresponding fields is divided by the total number of fields to obtain the comprehensive matching score of a single joint. The sum of the comprehensive matching scores of all valid matching joints is divided by the total number of valid matching joints to obtain the matching score between the constraint violation feature vector and the corresponding fault type.

[0040] Preferably, the process of constructing the fault type-response strategy hierarchical mapping table specifically includes:

[0041] Obtain the safety operation specifications and production requirements corresponding to the target robot, and classify the response level into four levels according to the severity from low to high: early warning speed reduction, constraint replanning and degraded operation, controlled soft shutdown and emergency power failure.

[0042] The four levels are matched with each fault type in the fault mode library, based on the degree of impact of the fault type on the operational safety of the target robot and the window of time available for handling after the fault occurs.

[0043] Define a corresponding quantitative control parameter adjustment scheme for each response level, as well as a corresponding maintenance work order push type. Among them, early warning deceleration and constraint replanning degraded operation correspond to early warning work orders, and controlled soft shutdown and emergency power outage correspond to alarm work orders.

[0044] Preferably, the specific process of modifying control commands and pushing maintenance work orders to the target robot includes:

[0045] Based on the diagnostic conclusion report, the pre-built fault type-response strategy hierarchical mapping table is queried using the confirmed fault type as the index to determine the response level corresponding to the fault type. If the diagnostic conclusion is an unknown anomaly, controlled soft shutdown is adopted as the response strategy.

[0046] Based on the queried response level, execute the corresponding quantitative control parameter adjustment plan and push the corresponding maintenance work order. After confirming that the triggered response action has been completed, record the complete processing process of this abnormal event from detection and diagnosis to response in the target robot's operation log.

[0047] The technical solution provided by this invention has the following advantages compared with the known prior art:

[0048] 1. By combining the linkage relationship of joint movement speed and the coupling relationship of torque and force to establish the mathematical boundary for normal operation, rather than just looking at torque or position error alone, it helps to detect small faults that develop slowly, such as wear of reducer and increased gear clearance, earlier.

[0049] 2. When judging anomalies, we should consider whether the anomaly has lasted for a period of time and whether the kinematic and dynamic anomaly signals have increased synchronously. This helps to filter out false anomalies such as electromagnetic interference and instantaneous impacts, thereby reducing false alarms.

[0050] 3. The entire detection process uses the position, speed and torque data that the target robot's servo drive already collects. No additional sensors or other data acquisition devices are required. It can be used directly on the existing robot control system, which helps reduce hardware costs and installation process.

[0051] 4. Fault diagnosis relies on matching the physical features corresponding to different faults, rather than on black-box models. This helps to identify which joint is malfunctioning and what type of fault it is. It also eliminates the need to collect a large amount of fault data in advance to train the model, thus reducing the deployment process.

[0052] 5. Different handling methods are used for different types and severity of faults. For minor faults, production can be continued at a reduced speed and downgraded level, while for serious faults, the machine can be safely shut down. It is not necessary to shut down the machine directly when an abnormality is discovered, which helps to ensure production efficiency while ensuring safety. Attached Figure Description

[0053] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0054] Figure 1 This is a schematic diagram of the implementation steps of the present invention. Detailed Implementation

[0055] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are within the scope of protection of the present invention.

[0056] The present invention will be further described below with reference to embodiments.

[0057] Please see Figure 1 A method for detecting and responding to abnormal robot behavior based on motion control constraints, comprising at least:

[0058] Step 1: Based on the kinematic and dynamic parameters of the target robot, construct a multidimensional dynamically coupled constraint manifold model that integrates Jacobian linkage constraints and Lagrangian dynamic constraints to describe the linkage relationship between the position, velocity, and torque of each joint of the target robot during normal operation.

[0059] In each control cycle, real-time state data of each joint of the target robot is collected and substituted into the constrained manifold model to calculate the residual value of the current actual state point deviating from the constrained manifold, which serves as the original signal for anomaly detection to characterize whether the multi-axis linkage relationship of the target robot has been destroyed.

[0060] In one specific embodiment, the construction process of the multidimensional dynamic coupling constraint manifold model specifically includes: obtaining the parameter table preset by the target robot and the dynamic parameters of each joint. The parameter table includes the link length, link torsion angle, joint offset and joint rotation angle range between adjacent joint coordinate systems. The dynamic parameters include the mass, center of mass position and inertia tensor of each link.

[0061] Based on the parameter table, a kinematic model of the target robot is constructed. The Jacobian matrix between the end effector velocity and the velocities of each joint is derived using the forward kinematic equations. This derivation process is common knowledge in the field of robot kinematics and will not be elaborated here.

[0062] Based on the aforementioned dynamic parameters, a dynamic model of the target robot is constructed, and the regression matrix between the joint torques and joint positions, angular velocities, and angular accelerations is derived using Lagrangian mechanics. and dynamic parameter vector The following relation is satisfied: ,in This represents the measured torque vector fed back by the servo driver corresponding to each joint.

[0063] The kinematic linkage and dynamic coupling are integrated into a unified multidimensional dynamic coupling constraint manifold model. This model characterizes the mathematical relationships that must be simultaneously satisfied between the position, velocity, and torque of each joint of the target robot during operation. Specifically, it is manifested as follows: , This is represented as the actual velocity vector of the target robot's end effector. The vector formed by the measured positions of each joint. and These are expressed as joint angular velocity and joint angular acceleration, respectively, which are the first derivatives of the joint angle with respect to time and the first derivatives of the joint angular velocity with respect to time. It is represented as a vector composed of the torque values ​​collected and fed back in real time by the servo drives of each joint of the target robot. Substitute the Jacobian matrix calculated in real time by the kinematic model into the current joint position. This refers to the vector of dynamic parameters determined in advance through parameter identification experiments.

[0064] It should be noted that each joint refers to all the active joints involved in the current task performed by the target robot. For example, when a six-axis welding robot performs a spot welding task, each joint refers to the six rotary joints of the robot. Their numbers and kinematic / dynamic parameters are directly obtained from the parameter table and servo drive configuration file preset by the target robot at the factory.

[0065] In one specific embodiment, the calculation of the residual value of the current actual state point deviating from the constrained manifold is specifically constituted by: in each control cycle, the measured joint position vector at the current moment is collected in real time by the servo drive of each joint of the target robot. Measured angular velocity vector and measured angular acceleration vector ,in, for The first derivative with respect to time, for The second derivative with respect to time.

[0066] It also obtains the measured torque vectors of each joint of the target robot and the actual velocity vectors of the end effector obtained from the forward kinematics solution. The joint position vector acquired at the current moment Substitute the Jacobian matrix The real-time calculation process obtains the Jacobian matrix value at the current moment, and the measured joint position vector at the current moment is then used. Measured angular velocity vector and measured angular acceleration vector Substitute the regression matrix The real-time calculation process yields the regression matrix value at the current moment.

[0067] The real-time solution results and the collected measured data are substituted into the constructed multidimensional dynamically coupled constrained manifold model to calculate the kinematic linkage residuals. and dynamic coupling residual The specific calculation formula is as follows: .

[0068] Based on dynamic linkage residual and dynamic coupling residual Combined with preset weighting coefficients Through the formula: The comprehensive deviation of the actual multi-axis linkage relationship from the normal linkage relationship defined by the multi-dimensional dynamic coupling constraint manifold model under the current operating state of the target robot is calculated and quantified. .

[0069] It should be noted that the above data all come from the existing sensor feedback channels of the target robot control system, and no additional hardware is required.

[0070] The value is set based on the confidence level of the dynamic parameter vector obtained from the parameter identification experiment. When the torque feedback accuracy is high and the identification residual of the dynamic parameter vector is less than 5%, Set the value to 1.0 to ensure that the dynamic residual and kinematic residual are equally weighted in the comprehensive residual calculation; when the torque feedback accuracy is generally low and the identification residual of the dynamic parameter vector is between 5% and 15%, Take a value of 0.3 to 0.5.

[0071] Step 2: Perform spatiotemporal joint filtering on the comprehensive residual scalar output from Step 1. In the time dimension, set an adaptive threshold based on the historical statistical characteristics of the residual and require it to exceed a specified duration. In the spatial dimension, verify the consistency of the correlation between the kinematic residual and the dynamic residual to confirm the occurrence of effective constraint violation events.

[0072] After confirming a valid constraint violation event, the joint state data within a specified time window before and after the event is frozen, and the constraint violation feature vector is extracted from it, including the number of the violated linkage, the peak amplitude of each residual component, the duration, and the relative offset with the moment of motion reversal.

[0073] It should be noted that the joint status data includes, but is not limited to: the measured position, measured angular velocity, measured angular acceleration, measured torque of each joint, current feedback value of the servo driver, joint temperature, and position tracking error.

[0074] In one specific embodiment, the process for determining the occurrence of an effective constraint violation event specifically includes: based on the comprehensive residual scalar of each control cycle. Store the comprehensive residual scalar of each control cycle to a set length. The circular buffer stores the corresponding The residual history sequence of each control period ( The value is preset and determined based on the ratio between the target robot control cycle and the duration of typical anomaly development.

[0075] Based on the residual history sequence, calculate the mean corresponding to the residual history sequence. and standard deviation (Using existing conventional calculation methods), based on the mean and standard deviation corresponding to the residual historical sequence, combined with a preset sensitivity coefficient. Through the formula: The current comprehensive residual scalar is calculated to determine the value. Adaptive anomaly determination threshold for whether an anomaly is constituted .

[0076] If the current period's comprehensive residual scalar If the adaptive anomaly detection threshold is exceeded, a duration counter is activated, and the anomaly is checked in each subsequent control cycle. Whether it exceeds consecutively ,like Falling back to Then, the counter will be reset to zero. When the cumulative value of the counter reaches the preset minimum duration period, the time dimension condition is determined to be met.

[0077] The preset minimum duration of cycles is determined based on the maximum transient adjustment time of the target robot servo system.

[0078] Obtain the kinematic linkage residual norm at the current moment. and dynamic coupling residual norm Calculate the Pearson correlation coefficient between the two, and based on the Pearson correlation coefficient, determine whether the two exhibit a synchronously increasing correlation characteristic. and If both components increase and are positively correlated, the spatial dimension condition is satisfied; if only one residual component increases while the other remains stable, the spatial dimension condition is marked as not satisfied.

[0079] A valid constraint violation event is confirmed only if both the time dimension condition and the space dimension condition are met simultaneously; if neither condition is met, no action is taken.

[0080] It should be noted that the process of calculating the Pearson correlation coefficient is as follows: Take the kinematic linkage residual norm sequence and the dynamic coupling residual norm sequence for several consecutive control cycles prior to the current time, respectively, and calculate the linear correlation between them using the standard Pearson product-moment correlation coefficient formula, obtaining a correlation coefficient value between -1 and 1. The method for determining whether the two exhibit a synchronously increasing correlation characteristic is as follows: when the correlation coefficient value is greater than a preset correlation threshold and the current mean of both residual norm sequences exceeds their respective baseline levels during stable operation, it is determined that the two exhibit a synchronously increasing correlation characteristic.

[0081] The sensitivity coefficient is determined during the deployment and debugging phase of the target robot. Specifically, the target robot is controlled to repeatedly run along a preset standard test trajectory in an unloaded state for several cycles. The overall residual scalar sequence is recorded, and the maximum value of this sequence is taken. The known slightest abnormal operating condition is then reproduced on a similar robot, and the minimum value of the overall residual scalar at this point is recorded. The value that satisfies this condition is then selected. An integer value that is exactly greater than the maximum value and less than the minimum value of the comprehensive residual scalar is used as the sensitivity coefficient. If there are multiple integer values ​​that meet the conditions, the median value is taken.

[0082] In one specific embodiment, the process of extracting the constraint violation feature vector specifically includes: when a valid constraint violation event is confirmed to have occurred, freezing the pre- and post-defined time periods centered on the moment the valid constraint violation event is confirmed. The state data of all joints within the structure are used to obtain a width that is twice the width of the structure. The feature extraction time window.

[0083] The The value is set as an integer multiple of the target robot's control cycle.

[0084] Within the feature extraction time window, the violated linkage relationship number is evaluated. Specifically, the kinematic linkage residual and dynamic coupling residual of each control cycle within the feature extraction time window are expanded according to the joint dimension. For each joint, the absolute value of the residual peak value of each joint within the feature extraction time window is calculated. If the absolute value of the residual peak value of a certain joint within the feature extraction time window exceeds the component judgment threshold corresponding to that joint, then the joint is marked as a constraint violation joint, and the number corresponding to that joint is recorded in the set of violation linkage relationship numbers.

[0085] For each joint in the set of violated linkage numbers, the peak values ​​of its kinematic residual components and dynamic residual components are searched and recorded within the feature extraction time window. The corresponding peak data are directly retrieved from the intermediate results generated within the feature extraction time window in the residual calculation process of step one.

[0086] For each marked joint, within the feature extraction time window, the maximum number of consecutive control cycles in which the comprehensive residual corresponding to each joint continuously exceeds the component determination threshold is counted, and then multiplied by the control cycle time interval to obtain the duration of constraint violation for each joint.

[0087] Within the feature extraction time window, the moment when the sign of the angular velocity of each marked joint changes is retrieved, and this moment is taken as the motion reversal moment. The time offset between the confirmation moment of the effective constraint violation event and the most recent motion reversal moment is calculated and recorded.

[0088] All the above extraction results are assembled into a structured constraint violation feature vector, which includes the set of violated linkage relationship numbers, the peak value of the kinematic residual of each joint, the peak value of the dynamic residual of each joint, the duration of the constraint violation of each joint, and the time offset between the confirmation time of the effective constraint violation event of each joint and the time of the most recent motion reversal.

[0089] It should be noted that the component determination threshold is obtained by amortizing the adaptive anomaly determination threshold according to the number of joint degrees of freedom of the target robot, that is, by dividing the adaptive anomaly determination threshold by the total number of joints of the target robot, and the result is the value of the component determination threshold.

[0090] Step 3: Pre-build a fault mode library and define a standard feature template for each typical fault, which includes the set of numbers of the violated linkage relationship corresponding to the fault, the peak amplitude range of each residual component, the duration range, and the relative offset feature with the moment of motion reversal.

[0091] The constraint violation feature vector output in step two is matched with each standard feature template in the fault mode library one by one to calculate the score, and the matching score between the constraint violation feature vector and each known fault type is quantified.

[0092] The fault mode with the highest score that exceeds the preset matching threshold is selected as the preliminary diagnostic conclusion, and the final output is the confirmed fault type, fault location, fault stage and confidence level.

[0093] In one specific embodiment, the fault mode library is constructed as follows: statistical data on various fault cases that have been confirmed during the historical operation of the target robot, and labeled data obtained on a robot of the same type as the target robot through fault injection experiments. The fault types include reducer wear, gear backlash, encoder signal drift, joint bearing loosening, external force collision and servo drive command tampering. The labeled data for each fault type contains a complete record of the constraint violation feature vector corresponding to the time when the fault occurred.

[0094] Statistical analysis is performed on all labeled samples for each fault type to extract common patterns in constraint violation features of each fault type. Based on this, standard feature templates corresponding to each fault type are defined. The structure of the standard feature templates corresponds to the constraint violation feature vectors output in step two.

[0095] Using this as a reference amplitude center for each numerical range field in the standard feature template, the distance metric for matching score calculation in subsequent steps is calculated. The minimum and maximum values ​​of the corresponding fields are added together, and the sum is divided by 2 to obtain the reference amplitude center for each field. Each field is one of the five fields contained in the constraint violation feature vector.

[0096] Based on the minimum and maximum values ​​of each field, the minimum value is subtracted from the maximum value of each field, and the result of the difference is divided by 2 to obtain the allowable deviation radius for each field.

[0097] By summarizing the standard feature templates defined for each fault type, the reference amplitude center of each field, and the allowable deviation radius, a fault mode library is obtained.

[0098] In one specific embodiment, the specific process of quantifying the matching score between the constraint violation feature vector and each known fault type is as follows: based on the constraint violation feature vector output in step two, compare it sequentially with the standard feature template corresponding to each fault type in the fault mode library.

[0099] Before comparison, feature dimension alignment verification is performed. Specifically, it checks whether there is an intersection between the set of violated linkage relationship numbers in the constraint violation feature vector and the set of involved joint numbers in the standard feature template corresponding to each fault type in the fault mode library. For the target fault type, if the intersection is empty, the matching score of the target fault type in the template is directly recorded as 0. If there is an intersection, the joints in the intersection are extracted as valid comparison joints and quantitatively calculated.

[0100] The specific process of quantitative calculation is as follows: For each field, the measured value of each effective comparison joint in the constraint violation feature vector is compared with the reference amplitude center and allowable deviation radius of the field corresponding to the target fault type in the template. The deviation degree corresponding to each field is calculated. The calculation method is as follows: the measured value of each effective comparison joint in the constraint violation feature vector is calculated by subtracting the reference amplitude center of each field. The absolute value of the result of the subtraction calculation is calculated. Finally, the result of the absolute value calculation is divided by the allowable deviation radius corresponding to each field to obtain the deviation degree corresponding to each effective comparison joint in each field.

[0101] The deviation of each field corresponding to each valid alignment joint is substituted into the scoring function, specifically: ,in For normalized field matching scores, For a single valid alignment joint, the deviation is defined on a single field. It is represented as the preset attenuation coefficient (which is uniformly set according to the dispersion of each fault type sample in the fault mode library).

[0102] For each valid comparison joint, the individual matching scores of each field corresponding to each valid comparison joint are summed, and then the sum is divided by the total number of fields corresponding to each valid comparison joint (the total number of fields corresponding to each valid comparison joint is the same) to obtain the comprehensive matching score corresponding to each valid comparison joint; the comprehensive matching scores corresponding to each valid comparison joint are summed, and then the sum is divided by the total number of valid comparison joints to obtain the matching score between the constraint violation feature vector and the target fault type in the template. The matching score between the constraint violation feature vector and each known fault type is calculated in this way.

[0103] In one specific embodiment, the specific output process of fault type, fault location, fault stage and confidence level includes: based on the matching score between the constraint violation feature vector and each known fault type, retrieving the highest matching score and the corresponding known fault type number, and simultaneously recording the second highest matching score and the corresponding known fault type number.

[0104] The highest matching score is compared with a preset matching confirmation threshold. The preset matching confirmation threshold is determined based on the distribution of internal matching scores of various fault samples in the fault mode library, and the value is usually between 0.7 and 0.85. If the highest matching score is lower than the preset matching confirmation threshold, it is determined that the current constraint violation feature vector cannot form a valid match with any known fault type, the diagnostic conclusion is output as "unknown anomaly", and it directly enters the waiting state. Otherwise, it enters the discrimination verification.

[0105] The discrimination ratio between the highest and second-highest matching scores is verified by calculating the difference between the highest and second-highest matching scores and dividing the result by the highest matching score.

[0106] The discrimination ratio is compared with a preset discrimination threshold. The preset discrimination threshold is set according to the degree of feature overlap between different fault types in the fault mode library, and the typical value is 0.1 to 0.2. If the discrimination ratio is less than the preset discrimination threshold, it indicates that there are two or more fault types with similar interpretability of the current feature vector, thereby triggering redundancy verification.

[0107] The redundancy check process is as follows: All competing fault types whose differences from the highest matching score are within a preset discrimination threshold are retrieved from the fault mode library. These competing fault types are then compared with the corresponding sets of violated linkage relationship numbers to determine if a joint is simultaneously attributed to two mutually exclusive physical mechanisms. This means determining whether the values ​​of each field in the constraint violation feature vector of the joint can be explained by both faults simultaneously. If such a conflict exists, the fault mode upstream in the chain of motion control constraints is selected as the sole explanation based on the principle of prioritizing the physical causal chain of fault propagation, excluding downstream derived modes, and finally retaining a conflict-free confirmed fault type.

[0108] If the verification passes, the determined fault type is obtained. Based on the determined fault type, a diagnostic conclusion report is generated, including: fault type name, fault location and specific joint number involved in the set of violated linkage relationship numbers, fault stage and diagnostic confidence level. The fault stage is determined based on the duration of constraint violation and the relative position of the residual peak amplitude relative to the range in the fault type template, and is divided into three levels: early, middle and severe.

[0109] The diagnostic confidence level is calculated by multiplying the highest matching score by the ratio of the highest matching score to the second-highest matching score.

[0110] Step 4: Pre-construct a fault type-response strategy hierarchical mapping table, defining the corresponding response level, quantitative control parameter adjustment scheme and maintenance early warning trigger conditions for each diagnosable fault type. The response level is divided into four levels in order of severity from low to high: early warning speed reduction, constraint replanning degraded operation, controlled soft shutdown and emergency power failure.

[0111] Based on the diagnostic conclusions output in step three, the mapping table is queried according to the confirmed fault type and fault stage to determine the corresponding response level and quantitative parameter adjustment amount, and the corresponding control command modification and maintenance work order are pushed to the target robot accordingly.

[0112] It should be noted that the diagnosable fault types refer to the fault types that have been predefined and stored in the fault mode library with standard feature templates. That is, the set of fault types that this solution can identify and output by constraining the violation of feature pattern matching, specifically including reducer wear, gear backlash, encoder signal drift, joint bearing loosening, external force collision and servo drive command tampering.

[0113] In one specific embodiment, the process of constructing the fault type-response strategy hierarchical mapping table includes: obtaining the safety operation specifications and production requirements corresponding to the target robot, and classifying the response level into four levels in order of severity from low to high: early warning deceleration, which is suitable for early minor anomalies with minimal impact on production tasks; constraint replanning degraded operation, which is suitable for progressive anomalies that can continue to complete the current task by reducing the performance boundary; controlled soft shutdown, which is suitable for faults that have affected control accuracy but have not yet endangered the surrounding safety; and emergency power outage, which is suitable for sudden fatal anomalies that may cause equipment damage or personal injury.

[0114] The four corresponding levels are matched with each fault type in the fault mode library. The matching criteria are the degree of impact of a fault type on the operational safety of the target robot and the window of time available for handling after the occurrence of the fault type: For reducer wear and gear backlash, since the fault develops slowly and does not lead to loss of control in the short term, the corresponding constraint replanning and degraded operation is performed; For encoder signal drift and joint bearing loosening, since the control accuracy is affected but the controlled motion can still be maintained, the corresponding controlled soft shutdown is performed; For external force collision and servo drive command tampering, since it may cause uncontrollable motion instantly, the corresponding emergency power cut is performed.

[0115] The adjustment scheme corresponding to the warning speed reduction is to reduce the maximum allowable operating speed of the affected joint to a specified percentage of the original nominal value (the value is determined by the fault stage output in step three: 0.7 when the fault stage is early, 0.5 when the fault stage is mid-stage, and 0.3 when the fault stage is severe).

[0116] The adjustment scheme corresponding to the constraint replanning degradation operation is to reduce the maximum allowable acceleration of the affected joint from the original nominal value to the derated value, and use the derated value as the new hard constraint boundary to replan the subsequent motion trajectory online. The derated value is determined by obtaining the performance degradation coefficient corresponding to the constraint replanning degradation (directly determined based on the fault stage output in step three; 0.8 when the fault stage is early, 0.6 when it is mid-stage, and 0.4 when it is severe), multiplying it with the original nominal value, and the result is the derated value.

[0117] The adjustment scheme corresponding to controlled soft stop is to linearly reduce the target speed of the affected joint to zero according to the preset deceleration slope, maintain the attitude after reaching the position and cut off the servo enable.

[0118] The adjustment plan for emergency power failure is to immediately cut off the servo enable of all joints and trigger the mechanical brake. The response delay shall not exceed the preset limit time (determined by the sum of the maximum response delay of the target robot's servo drive system and the inherent action time of the mechanical brake device. It is necessary to ensure that the total time from the triggering of the diagnostic conclusion to the cutting off of power to all joints and the completion of the mechanical brake does not exceed the maximum allowable stop time specified in the target robot's safety certification standard).

[0119] Warning-based speed reduction and constraint-based replanning and degraded operation require pushing warning work orders to the upper-level management system, which include the faulty joint number, the remaining safe running time for recommended shutdown and inspection, and the level of maintenance urgency; controlled soft shutdown and emergency power outage require pushing alarm work orders that include fault details and the response actions already performed at the same time as the response is triggered.

[0120] In a specific embodiment, the specific process of executing corresponding control command modification and maintenance work order push for the target robot includes: according to the diagnostic conclusion report, using the confirmed fault type as the index, querying the fault type-response strategy hierarchical mapping table pre-built in step four, determining the response level corresponding to the fault type, and if the diagnostic conclusion is "unknown anomaly", that is, the highest matching score in step three has not reached the matching confirmation threshold, then controlled soft shutdown is adopted as a conservative response strategy.

[0121] Based on the queried response level, the control parameter adjustment plan is executed. Finally, it is confirmed that the triggered response action has been completed, and the complete processing process from detection and diagnosis to response of this abnormal event is recorded in the target robot's operation log.

[0122] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions will not cause the essence of the corresponding technical solutions to deviate from the protection scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for detecting and responding to abnormal robot behavior based on motion control constraints, characterized in that, include: Step 1: Obtain the kinematic and dynamic parameters of the target robot, construct a multi-dimensional dynamic coupling constrained manifold model, collect real-time state data of each joint of the target robot in each control cycle and substitute it into the constrained manifold model, calculate the residual value of the current actual state point deviating from the constrained manifold, and obtain the comprehensive deviation quantification value of the target robot's multi-axis linkage relationship from the normal state. Step 2: Perform spatiotemporal joint filtering on the comprehensive deviation quantification value. In the time dimension, set an adaptive anomaly judgment threshold based on the historical statistical characteristics of the residuals and verify the duration of continuous exceedance. In the spatial dimension, verify the correlation consistency between kinematic residuals and dynamic residuals, confirm effective constraint violation events, and freeze the joint state data within a specified time window before and after the effective constraint violation event, and extract the constraint violation feature vector from it. Step 3: Construct a fault mode library. The fault mode library contains standard feature templates corresponding to various typical faults. Each standard feature template in the fault mode library is matched and scored one by one. The matching score between the constraint violation feature vector and each known fault type is quantified to confirm the preliminary diagnosis. Finally, the diagnosed fault type, fault location, fault stage and confidence level are output. Step 4: Construct a fault type-response strategy hierarchical mapping table. Based on the preliminary diagnosis, determine the response level and quantitative parameter adjustment amount corresponding to the diagnosed fault type, and execute the corresponding control commands and push maintenance work orders to the target robot.

2. The method for detecting and responding to abnormal robot behavior based on motion control constraints according to claim 1, characterized in that, The construction process of a multidimensional dynamically coupled constrained manifold model specifically includes: Obtain the factory-preset parameter table and dynamic parameters of each joint of the target robot. The parameter table includes the link length, link torsion angle, joint offset and joint rotation range between adjacent joint coordinate systems. The dynamic parameters include the mass, center of mass position and inertia tensor of each link. Based on the parameter table, construct the kinematic model of the target robot and derive the Jacobian matrix between the end effector velocity and the velocities of each joint. Based on the aforementioned dynamic parameters, a dynamic model of the target robot is constructed, and the regression matrix and dynamic parameter vector between the torque of each joint and the joint position, angular velocity and angular acceleration are derived. The kinematic linkage relationship and the dynamic coupling relationship are integrated into a unified multidimensional dynamic coupling constraint manifold model. The multidimensional dynamic coupling constraint manifold model represents the mathematical relationship that must be satisfied simultaneously between the position, velocity and torque of each joint in the target robot's operating state.

3. The method for detecting and responding to abnormal robot behavior based on motion control constraints according to claim 2, characterized in that, The calculation process for the residual value of the current actual state point deviating from the constrained manifold specifically includes: Within each control cycle, the measured position vector, measured angular velocity vector, and measured angular acceleration vector of each joint of the target robot at the current moment are collected, and the measured torque vector of each joint of the target robot and the actual velocity vector of the end effector obtained by forward kinematics calculation are obtained. The current joint position vector is substituted into the real-time calculation process of the Jacobian matrix to obtain the value of the Jacobian matrix at the current time. The current measured joint position vector, measured angular velocity vector, and measured angular acceleration vector are substituted into the real-time calculation process of the regression matrix to obtain the value of the regression matrix at the current time. The real-time calculation results and the collected measured data are substituted into the multidimensional dynamic coupling constraint manifold model to calculate the kinematic linkage residuals and dynamic coupling residuals respectively. Combined with the preset weight coefficients, the comprehensive deviation quantification value of the target robot under the current operating state is calculated.

4. The method for detecting and responding to abnormal robot behavior based on motion control constraints according to claim 3, characterized in that, The process for determining effective constraint violation events specifically includes: The overall deviation quantization value of each control cycle is stored in a circular buffer of a set length, and the circular buffer stores the residual history sequence corresponding to multiple control cycles; The mean and standard deviation are calculated based on the residual history sequence, and the adaptive anomaly detection threshold is calculated in combination with the preset sensitivity coefficient. If the quantized value of the overall deviation of the current period exceeds the adaptive anomaly judgment threshold, the duration counter is started. If the overall residual scalar falls back to below the adaptive anomaly judgment threshold, the counter is cleared. When the cumulative value of the counter reaches the preset shortest duration period, the time dimension condition is satisfied. Obtain the kinematic linkage residual norm sequence and the dynamic coupling residual norm sequence for multiple consecutive control cycles before the current time, respectively, and calculate the Pearson correlation coefficient between the two. If both types of residual norms increase simultaneously and are positively correlated, it is determined that the spatial dimension condition is satisfied. Only when both the temporal and spatial dimension conditions are satisfied simultaneously is it confirmed that a valid constraint violation event has occurred.

5. The method for detecting and responding to abnormal robot behavior based on motion control constraints according to claim 4, characterized in that, The process of extracting constraint violation feature vectors specifically includes: Based on the confirmed valid constraint violation event, the state data of all joints within each set time period before and after the valid constraint violation event is confirmed is frozen, and a feature extraction time window is formed, with the time when the valid constraint violation event is confirmed as the center. The kinematic linkage residual and dynamic coupling residual of each control cycle within the feature extraction time window are expanded according to the joint dimension. The absolute value of the residual peak value of each joint within the time window is calculated. Joints whose absolute value of the residual peak value exceeds the corresponding component judgment threshold are marked as constraint violation joints, and the corresponding numbers are recorded to the violation linkage relationship number set. For each marked joint, the peak values ​​of the corresponding kinematic residual components and dynamic residual components are retrieved within the time window. The maximum number of consecutive control cycles in which the comprehensive residual continuously exceeds the component judgment threshold is calculated and converted into the duration of constraint violation. The moment when the sign of the angular velocity of each marked joint changes is retrieved as the moment of motion reversal. The time offset between the event confirmation moment and the most recent moment of motion reversal is calculated, and the constraint violation feature vector is assembled.

6. The method for detecting and responding to abnormal robot behavior based on motion control constraints according to claim 5, characterized in that, The specific methods for constructing the fault mode library include: The statistical analysis includes data on various confirmed fault cases in the historical operation of the target robot, as well as labeled data obtained on similar robots through fault injection experiments. The fault types include reducer wear, gear backlash, encoder signal drift, joint bearing loosening, external force collision, and servo drive command tampering. The labeled data for each fault type contains a complete record of the constraint violation feature vector corresponding to the time of the fault occurrence. Statistical analysis is performed on all labeled samples for each fault type to extract common patterns in constraint violation features of each fault type, and standard feature templates corresponding to each fault type are defined. The structure of the standard feature templates corresponds to the constraint violation feature vectors. Calculate the corresponding reference amplitude center and allowable deviation radius for each numerical range field in the standard feature template; By summarizing the standard feature templates, reference amplitude centers, and permissible deviation radii corresponding to each fault type, a fault mode library is obtained.

7. The method for detecting and responding to abnormal robot behavior based on motion control constraints according to claim 6, characterized in that, The specific process of quantifying the matching score between the constraint violation feature vector and each known fault type includes: Before comparison, feature dimension alignment verification is performed to check whether there is an intersection between the set of violated linkage relationship numbers in the constraint violation feature vector and the set of joint numbers involved in the standard feature template corresponding to each fault type in the fault mode library. If the intersection of the two is empty, the matching score for this fault type is zero. If there is an intersection, the joints in the intersection are extracted as effective comparison joints for quantitative calculation. For each field, the measured value of each effective alignment joint in the constraint violation feature vector is compared with the reference amplitude center and allowable deviation radius of the corresponding field in the template corresponding to the target fault type. The deviation degree corresponding to each field is calculated, and each deviation degree is substituted into the scoring function to obtain the normalized field matching score. For each valid matching joint, the sum of the individual matching scores of its corresponding fields is divided by the total number of fields to obtain the comprehensive matching score of a single joint. The sum of the comprehensive matching scores of all valid matching joints is divided by the total number of valid matching joints to obtain the matching score between the constraint violation feature vector and the corresponding fault type.

8. The method for detecting and responding to abnormal robot behavior based on motion control constraints according to claim 7, characterized in that, The specific output process for fault type, fault location, fault stage, and confidence level includes: Based on the matching score between the constraint violation feature vector and each known fault type, the highest matching score and the corresponding fault type number are retrieved, and the second highest matching score and the corresponding fault type number are recorded. The highest matching score is compared with the preset matching confirmation threshold. If it is lower than the matching confirmation threshold, the diagnostic conclusion is output as unknown anomaly; otherwise, the discrimination test is performed. Calculate the discrimination ratio between the highest matching score and the second highest matching score, compare the discrimination ratio with a preset discrimination threshold, and trigger redundancy check if the discrimination ratio is less than the discrimination threshold. A diagnostic conclusion report is generated based on the confirmed fault type, including the fault type name, fault location, fault stage, and diagnostic confidence level. The fault stage is determined based on the duration of constraint violation and the relative position of the residual peak amplitude within the template range.

9. The method for detecting and responding to abnormal robot behavior based on motion control constraints according to claim 8, characterized in that, The process of constructing the fault type-response strategy hierarchical mapping table specifically includes: Obtain the safety operation specifications and production requirements corresponding to the target robot, and classify the response level into four levels according to the severity from low to high: early warning speed reduction, constraint replanning and degraded operation, controlled soft shutdown and emergency power failure. The four levels are matched with each fault type in the fault mode library, based on the degree of impact of the fault type on the operational safety of the target robot and the window of time available for handling after the fault occurs. Define a corresponding quantitative control parameter adjustment scheme for each response level, as well as a corresponding maintenance work order push type. Among them, early warning deceleration and constraint replanning degraded operation correspond to early warning work orders, and controlled soft shutdown and emergency power outage correspond to alarm work orders.

10. The method for detecting and responding to abnormal robot behavior based on motion control constraints according to claim 9, characterized in that, The specific process of modifying control commands and pushing maintenance work orders to the target robot includes: Based on the diagnostic conclusion report, the pre-built fault type-response strategy hierarchical mapping table is queried using the confirmed fault type as the index to determine the response level corresponding to the fault type. If the diagnostic conclusion is an unknown anomaly, controlled soft shutdown is adopted as the response strategy. Based on the queried response level, execute the corresponding quantitative control parameter adjustment plan and push the corresponding maintenance work order. After confirming that the triggered response action has been completed, record the complete processing process of this abnormal event from detection and diagnosis to response in the target robot's operation log.

Images