SOFNN-based hybrid disturbance compensation method and system for multi-degree-of-freedom mechanical arm

Abstract

The embodiment of the invention discloses a multi-degree-of-freedom mechanical arm hybrid disturbance compensation method and system based on SOFNN, and the method comprises the steps: firstly constructing a composite disturbance model containing joint friction, load change and external environment disturbance component coupling action relation, and then collecting the position feedback data and torque output data of each joint of a mechanical arm in real time; calculating a dynamic residual error in the current motion state; inputting the dynamic residual error into a preset SOFNN network structure for disturbance estimation, and generating a composite disturbance estimation value containing the weight ratio of each disturbance component; a disturbance compensation control signal is generated according to the estimated value and expected trajectory parameters of the mechanical arm and used for counteracting the composite disturbance influence of all the joints; and finally, superposing the disturbance compensation control signal to an original trajectory tracking control instruction, and executing the integrated control instruction through a joint driver to realize accurate tracking of an expected trajectory.

Description

TECHNICAL FIELD

[0001] The embodiment of the application relates to the technical field of mechanical arm control, and in particular to a multi-degree-of-freedom mechanical arm hybrid disturbance compensation method and system based on a SOFNN. BACKGROUND

[0002] In the application field of multi-degree-of-freedom mechanical arms, such as aerospace, industrial automation equipment, robot technology, etc., the accurate tracking of the expected trajectory by the mechanical arm is crucial.

[0003] At present, the disturbance suppression strategy of the multi-degree-of-freedom mechanical arm mainly includes a disturbance attenuation method based on robust control and a hierarchical control framework combined with disturbance estimation. The method based on robust control, such as sliding mode control, control and model predictive control, etc., can cope with the bounded norm disturbance without relying on statistical prior assumptions, but there is an inherent trade-off between nominal performance and robustness, and the controller design is often too conservative in single-degree-of-freedom systems.

[0004] The hierarchical control method combined with disturbance estimation introduces a two-degree-of-freedom structure, estimates the external disturbance through a disturbance observer and feedback compensation, and the disturbance observer is divided into a model-based method and a data-driven method.

[0005] The model-based method, such as the nonlinear disturbance observer, the extended state observer, etc., has fast transient response capability, but the estimation error is significantly deteriorated when facing sudden or rapidly changing disturbances, and there is a trade-off between estimation accuracy and noise sensitivity, making it difficult to achieve stable high-precision disturbance estimation.

[0006] The data-driven method, such as the Koopman operator, neural network, etc., has an advantage in improving the steady-state disturbance suppression accuracy, but each has limitations, such as the Koopman operator may have a decrease in modeling accuracy due to limited observation function selection or insufficient training data coverage, and the neural network relies on a large amount of training data and has insufficient generalization ability.

[0007] Therefore, the existing technology has the problem of being difficult to balance fast response and stable high-precision in the disturbance estimation and compensation of the multi-degree-of-freedom mechanical arm. SUMMARY

[0008] The embodiment of the application provides a multi-degree-of-freedom mechanical arm hybrid disturbance compensation method and system based on a SOFNN.

[0009] In a first aspect, the embodiment of the application provides a multi-degree-of-freedom mechanical arm hybrid disturbance compensation method based on a SOFNN, applied to a multi-degree-of-freedom mechanical arm hybrid disturbance compensation system based on a SOFNN, and the method comprises the following steps. A composite disturbance model of the multi-degree-of-freedom robot arm is constructed, and the composite disturbance model includes a coupling relationship description of a joint friction disturbance component, a load change disturbance component and an external environment disturbance component; Real-time acquisition of position feedback data and torque output data of each joint of the robot arm, and calculation of a dynamic residual under a current motion state based on the position feedback data and the torque output data; The dynamic residual is input into a preset SOFNN network structure for disturbance estimation processing, and a composite disturbance estimation value including a weight proportion of each disturbance component is generated. According to the composite disturbance estimation value and the desired trajectory parameters of the robot arm, a disturbance compensation control signal is generated, which is used to offset the influence of the composite disturbance of each joint. The disturbance compensation control signal is superimposed on the original trajectory tracking control instruction of the robot arm, and the integrated control instruction is executed by the joint driver to realize accurate tracking of the desired trajectory.

[0010] In a second aspect, the embodiment of the present application provides a multi-degree-of-freedom robot arm hybrid disturbance compensation system based on SOFNN, comprising: A processor; A storage device having a computer program stored thereon, When the computer program is executed by the processor, the processor implements any of the multi-degree-of-freedom robot arm hybrid disturbance compensation methods based on SOFNN.

[0011] The embodiment of the present application provides a readable storage medium, and the readable storage medium stores a program or instructions, and the program or instructions are executed by a processor to implement the steps of the multi-degree-of-freedom robot arm hybrid disturbance compensation method based on SOFNN.

[0012] The multi-degree-of-freedom mechanical arm hybrid disturbance compensation method of the embodiment of the application significantly improves the tracking accuracy of the mechanical arm on the expected trajectory. By constructing a complex disturbance model containing the coupling relationship of joint friction, load change and external environmental disturbance components, the complex disturbance situation in the operation process of the mechanical arm is comprehensively and accurately described; the position feedback data and torque output data are collected in real time and the dynamics residual is calculated, so that the disturbance information in the current motion state of the mechanical arm can be captured in time; the dynamics residual is input into the preset SOFNN network structure for disturbance estimation processing, and a complex disturbance estimation value containing the weight proportion of each disturbance component is generated, which reflects the influence degree of different disturbance components on the motion of the mechanical arm in detail; the disturbance compensation control signal generated according to the complex disturbance estimation value and the expected trajectory parameters can counteract the influence of the complex disturbance of each joint; finally, the disturbance compensation control signal is superimposed into the original trajectory tracking control instruction, and the integrated control instruction is executed through the joint driver, so that the mechanical arm can effectively overcome various disturbances and realize accurate tracking of the expected trajectory.

[0013] Compared with the prior art, the method has significant advantages in comprehensiveness and accuracy of disturbance estimation and pertinence and effectiveness of compensation, and greatly improves the working performance and stability of the multi-degree-of-freedom mechanical arm in a complex environment. BRIEF DESCRIPTION OF DRAWINGS

[0014] Figure 1 A flowchart of a multi-degree-of-freedom mechanical arm hybrid disturbance compensation method based on SOFNN provided by the embodiment of the application.

[0015] Figure 2 A control architecture schematic diagram of a multi-degree-of-freedom mechanical arm hybrid disturbance compensation method based on SOFNN provided by the embodiment of the application.

[0016] Figure 3 A structure schematic diagram of an interval type 2 self-organizing fuzzy neural network IT2SOFNN provided by the embodiment of the application.

[0017] Figure 4 A schematic diagram of the basic structure of a multi-degree-of-freedom mechanical arm hybrid disturbance compensation system based on SOFNN provided by the embodiment of the application. DETAILED DESCRIPTION

[0018] In order to make the above-mentioned objects, features and advantages of the application more obvious and easy to understand, the embodiments of the application will be further described in detail below with reference to the drawings and specific embodiments.

[0019] Reference Figure 1As shown in the figure, it is a flow chart of a multi-degree-of-freedom robot arm hybrid disturbance compensation method based on SOFNN provided by the embodiment of the application, which can be applied to a multi-degree-of-freedom robot arm hybrid disturbance compensation system based on SOFNN. Please refer to Figure 1 , Figure 2 and Figure 3 The method can include steps 110-140.

[0020] Step 110: Construct a composite disturbance model of the multi-degree-of-freedom robot arm, which contains a coupling relationship description of joint friction disturbance components, load change disturbance components and external environment disturbance components.

[0021] In the multi-degree-of-freedom robot arm hybrid disturbance compensation scene, in order to more accurately process various disturbances of the robot arm during operation, it is necessary to construct a composite disturbance model, which comprehensively reflects the coupling relationship between the disturbance components of joint friction, load change and external environment. Because in actual operation, the three disturbances are not independent, they interact with each other, and jointly interfere with the operation of the robot arm. For example, joint friction will affect the movement speed of the robot arm, and the change of the movement speed may change the way the load change affects the robot arm, and the disturbance of the external environment may also exacerbate the change. Therefore, it is crucial to construct such a composite disturbance model for accurate analysis and compensation of the disturbance of the robot arm.

[0022] In an optional embodiment, the construction of the composite disturbance model of the multi-degree-of-freedom robot arm includes: Step 111: Establish a rigid body dynamics equation of the robot arm, which contains mathematical relationship expressions of inertia matrix items, Coriolis force and centrifugal force items and gravity items.

[0023] When constructing the composite disturbance model, first, the rigid body dynamics equation of the robot arm is established, which mainly contains mathematical relationship expressions of inertia matrix items, Coriolis force and centrifugal force items and gravity items. The inertia matrix items reflect the inertia characteristics of each part of the robot arm, which are related to the mass distribution and structure of the robot arm. The Coriolis force and centrifugal force items are related to the movement speed and acceleration of the robot arm, and their influence will be more obvious when the robot arm performs complex movements. The gravity item considers the effect of the earth's gravity on the robot arm, especially when the robot arm performs vertical movement, the influence of the gravity item cannot be ignored. By establishing the above rigid body dynamics equation, the movement law of the robot arm in the ideal state can be theoretically described. For example, in a six-degree-of-freedom robot arm, the rigid body dynamics equation can describe the relationship between the forces and torques received by each joint in different movement states.

[0024] Step 112: Introduce a nonlinear friction model into the rigid body dynamics equation, which includes a piecewise function description of the Coulomb friction component, viscous friction component, and static friction component.

[0025] After establishing the rigid body dynamics equation, the influence of joint friction on the motion of the robot arm needs to be considered. Joint friction is a complex nonlinear phenomenon that cannot be simply described by a linear model. Therefore, a nonlinear friction model is introduced, which includes a piecewise function description of the Coulomb friction component, viscous friction component, and static friction component. The Coulomb friction component is the constant friction force that the robot arm joint experiences when it starts to move, which is opposite to the direction of the movement speed. The viscous friction component is proportional to the movement speed, and as the movement speed increases, the viscous friction force also increases. The static friction component is the friction force that the robot arm joint experiences when it is stationary, and only when the external force overcomes the static friction force will the joint start to move. By describing these three components through a piecewise function, the nonlinear characteristics of joint friction can be more accurately reflected. For example, when the robot arm joint starts to move from a stationary state, the static friction component plays a major role; while in the process of movement, the Coulomb friction component and the viscous friction component jointly affect the movement of the joint.

[0026] Step 113: Based on the load variation range of the robot arm end effector, construct a mapping relationship function between the load disturbance coefficient and the joint torque variation.

[0027] The load variation of the robot arm end effector will cause disturbance to the motion of the robot arm. In order to accurately describe this disturbance, it is necessary to construct a mapping relationship function between the load disturbance coefficient and the joint torque variation based on the load variation range of the robot arm end effector. The load variation range refers to the interval between the maximum and minimum values of the load that the robot arm end effector experiences in different working scenarios. The load disturbance coefficient reflects the degree of influence of load variation on joint torque, while the joint torque variation refers to the change in the torque that the joint experiences due to load variation. By constructing the above mapping relationship function, the change in joint torque can be predicted according to the change in load, so as to better compensate for the disturbance caused by load variation. For example, when the robot arm end effector grasps objects of different weights, the required increase or decrease in torque for each joint can be calculated according to this mapping relationship function.

[0028] Step 114: Collect the dynamic response data of the robot arm under different external environments, and establish the correlation equation between the external environment disturbance component and the movement speed, acceleration through data fitting method.

[0029] Changes in the external environment can also interfere with the movement of the robot arm, such as temperature, humidity, air flow and other factors that can affect the performance of the robot arm. In order to analyze this interference, it is necessary to collect the dynamic response data of the robot arm under different external environments, which includes the movement speed, acceleration and the force and torque received by the joint of the robot arm under different temperature, humidity, air flow and other conditions. Through data fitting method, the correlation equation between external environment disturbance component and movement speed, acceleration is established. Data fitting method can find a suitable mathematical model according to the collected data, so that the model can as accurately as possible describe the relationship between external environment disturbance component and movement speed, acceleration. For example, under different temperatures, the movement speed and acceleration of the robot arm may change, and through data fitting, an equation can be obtained, which can predict the change of the movement speed and acceleration of the robot arm according to the change of temperature.

[0030] Step 115: coupling the nonlinear friction model, load disturbance mapping relationship function and external environment disturbance correlation equation to the rigid body dynamics equation to generate a composite disturbance model containing the interaction of multiple disturbance components.

[0031] After establishing the nonlinear friction model, load disturbance mapping relationship function and external environment disturbance correlation equation respectively, it is necessary to couple them into the rigid body dynamics equation established before to generate a composite disturbance model containing the interaction of multiple disturbance components. Because in actual situation, joint friction, load change and external environment are mutually influenced, they cannot be simply separated and processed. By coupling these models and equations, a more comprehensive and accurate composite disturbance model can be obtained, which can reflect the coupling relationship between the three disturbance components. For example, during the movement of the robot arm, joint friction will affect the movement speed and acceleration of the robot arm, and the change of movement speed and acceleration will also affect the disturbance of load change and external environment to the robot arm. Through the composite disturbance model, various disturbances received by the robot arm can be more accurately analyzed and compensated.

[0032] Step 120: real-time acquisition of position feedback data and torque output data of each joint of the robot arm, and calculation of the dynamic residual under the current motion state based on the position feedback data and the torque output data.

[0033] After the composite disturbance model is constructed, real-time acquisition of position feedback data and torque output data of each joint of the robot arm is required. The position feedback data can be obtained through the encoder installed on each joint, which reflects the current actual position of each joint of the robot arm. The torque output data can be acquired through the torque sensor, which represents the torque output of each joint in the current motion state. Based on the position feedback data and the torque output data, the dynamics residual in the current motion state can be calculated. The dynamics residual refers to the difference between the actual measured value and the theoretical value calculated according to the rigid body dynamics equation, which reflects the comprehensive influence of the unmodeled disturbance on the robot arm in the current motion state. For example, when the robot arm executes one of the tasks, by comparing the actual position and torque data with the theoretical calculation value, the dynamics residual can be obtained, which can be used for subsequent disturbance estimation and compensation.

[0034] As a preferred embodiment, the real-time acquisition of position feedback data and torque output data of each joint of the robot arm, and the calculation of dynamics residual in the current motion state based on the position feedback data and the torque output data, comprise: Step 121: Collect real-time position data through the encoder installed on each joint, and collect joint output torque data through the torque sensor, and establish a position-torque data sequence with a sampling period as the time reference.

[0035] In order to accurately acquire the position feedback data and torque output data of each joint of the robot arm, an encoder and a torque sensor need to be installed on each joint. The encoder can accurately measure the position of the joint, while the torque sensor can monitor the torque output by the joint in real time. When collecting data, a position-torque data sequence is established with a sampling period as the time reference. The sampling period refers to the time interval between adjacent two data collections, which can be reasonably selected according to the motion characteristics and control requirements of the robot arm. For example, for a robot arm with high motion speed, a shorter sampling period is required to ensure that the changes in joint position and torque can be captured in time. By establishing the above-mentioned position-torque data sequence, the motion state of the robot arm can be continuously monitored and analyzed.

[0036] Step 122: Numerical differentiation processing is performed on the position data sequence to calculate real-time angular velocity data and angular acceleration data of each joint, and the five-point central difference algorithm is used to reduce noise interference.

[0037] After obtaining the position data sequence, in order to further analyze the motion state of the robot arm, it is necessary to calculate the real-time angular velocity data and angular acceleration data of each joint, which can be realized by numerical differentiation processing on the position data sequence. Numerical differentiation processing is a method of approximating the derivative of a function through discrete data. In order to reduce noise interference, five-point central difference algorithm is adopted, which uses five adjacent points in the position data sequence to calculate the derivative, which can smooth the data to a certain extent and reduce the influence of noise. For example, for a position data sequence of a joint, the angular velocity and angular acceleration of the joint at each sampling time can be calculated by the five-point central difference algorithm.

[0038] Step 123: Substitute the position data, angular velocity data and angular acceleration data into the nominal dynamics equation of the robot arm to calculate the theoretical torque estimate.

[0039] After calculating the real-time position, angular velocity and angular acceleration data of each joint, these data are substituted into the nominal dynamics equation of the robot arm to calculate the theoretical torque estimate. The nominal dynamics equation is established under ideal conditions, which describes the relationship between the forces and torques received by each joint of the robot arm under different motion states. By substituting the actual position, angular velocity and angular acceleration data, the theoretical torque value that each joint should output under the current motion state can be obtained. For example, in a six-degree-of-freedom robot arm, the position, angular velocity and angular acceleration data of each joint are substituted into its nominal dynamics equation to calculate the theoretical torque estimate of each joint, which can be used as the basis for analyzing the difference between the actual torque and the theoretical torque.

[0040] Step 124: Extract the dynamic response component in the torque output data to remove the static bias and obtain the actual effective torque value.

[0041] The collected torque output data may contain static bias, which may be caused by sensor errors, mechanical structure imbalance and other factors, and does not reflect the actual disturbance of the robot arm during motion. Therefore, it is necessary to extract the dynamic response component in the torque output data to remove the static bias and obtain the actual effective torque value. The dynamic response component refers to the part of the torque output data that changes with the motion of the robot arm, which reflects the actual torque change of the robot arm during motion. For example, the static bias in the torque output data can be removed by filtering and other methods to obtain the actual effective torque value, which more accurately reflects the force acting on the robot arm during motion.

[0042] Step 125: Calculate the difference between the actual effective torque value and the theoretical torque estimation value, and define the difference as a dynamics residual in the current motion state, where the dynamics residual is used to represent the comprehensive influence of unmodeled disturbances.

[0043] After obtaining the actual effective torque value and the theoretical torque estimation value, the difference between them is calculated, and the difference is defined as a dynamics residual in the current motion state. The dynamics residual reflects the comprehensive influence of unmodeled disturbances on the manipulator in the current motion state, which can include the influence of joint friction, load changes, external environment, etc. Due to the complexity and uncertainty of these factors, they cannot be completely described in the nominal dynamics equation. By calculating the dynamics residual, the influence degree of these unmodeled disturbances can be quantified, providing a basis for subsequent disturbance estimation and compensation. For example, when the manipulator performs one of the tasks, if the dynamics residual is large, it means that the manipulator is subjected to serious unmodeled disturbances, and more accurate compensation is needed.

[0044] Step 130: Input the dynamics residual into a preset SOFNN network structure for disturbance estimation processing to generate a composite disturbance estimation value containing the weight proportion of each disturbance component.

[0045] After obtaining the dynamics residual, it is input into a preset SOFNN network structure for disturbance estimation processing. The SOFNN network structure is a self-adaptive fuzzy neural network with strong non-linear mapping ability and self-adaptive learning ability, which can accurately estimate complex disturbances. By inputting the dynamics residual into the SOFNN network structure, the network can learn and reason according to the input data to generate a composite disturbance estimation value containing the weight proportion of each disturbance component. The weight proportion of each disturbance component reflects the proportion of the joint friction disturbance component, the load change disturbance component, and the external environment disturbance component in the composite disturbance. For example, in one of the motion states, the weight proportion of the joint friction disturbance component may be 50%, the weight proportion of the load change disturbance component may be 30%, and the weight proportion of the external environment disturbance component may be 20%. The composite disturbance estimation value can more detailedly describe the disturbance situation of the manipulator.

[0046] In one exemplary embodiment, the inputting the dynamics residual into a preset SOFNN network structure for disturbance estimation processing to generate a composite disturbance estimation value containing the weight proportion of each disturbance component comprises: Step 131: Initialize the number of input layer nodes of the SOFNN network structure as the number of degrees of freedom of the manipulator, dynamically determine the number of hidden layer nodes through a fuzzy clustering algorithm, and the number of output layer nodes corresponds to the number of component types of the composite disturbance.

[0047] Before using the SOFNN network structure to estimate the disturbance, the network structure needs to be initialized. First, set the number of input layer nodes to the number of degrees of freedom of the robot arm. Because the dynamic residual is related to the motion state of each joint of the robot arm, the motion state of each joint can be used as an input variable, so the number of input layer nodes should be equal to the number of degrees of freedom of the robot arm. For example, for a six-degree-of-freedom robot arm, the number of input layer nodes is 6. The number of hidden layer nodes is determined dynamically by the fuzzy clustering algorithm. The fuzzy clustering algorithm can automatically divide the categories of data according to the distribution of input data, thereby determining the number of hidden layer nodes, so that the network structure can be more flexible and can adapt to different input data. The number of output layer nodes corresponds to the number of component types of the composite disturbance. In this scenario, the composite disturbance includes joint friction disturbance components, load change disturbance components, and external environmental disturbance components, so the number of output layer nodes is 3.

[0048] Step 132: Combine the dynamic residual with real-time motion state data of the robot arm to form an extended input vector, the real-time motion state data including normalized values of position, angular velocity, and angular acceleration.

[0049] In order to more comprehensively describe the motion state and disturbance of the robot arm, the dynamic residual is combined with real-time motion state data of the robot arm to form an extended input vector. The real-time motion state data includes normalized values of position, angular velocity, and angular acceleration. Normalization can convert data of different ranges to a unified range, so that the network can better process these data. For example, position data may have different value ranges between different joints, and normalization can convert them to the range of [0, 1]. Combining the dynamic residual with the normalized real-time motion state data can obtain an extended input vector containing more information, which can more accurately reflect the current motion state of the robot arm and the disturbance it receives, providing more abundant information for disturbance estimation of the SOFNN network structure.

[0050] Step 133: Calculate the membership function of the extended input vector through the fuzzy layer of the SOFNN to generate Gaussian membership values of each input variable, the center and width of the membership function being dynamically adjusted by the learning algorithm.

[0051] After inputting the extended input vector into the SOFNN network structure, it is first processed by the fuzzification layer. The main function of the fuzzification layer is to calculate the Gaussian membership value of each input variable. The Gaussian membership function is a commonly used membership function, which can describe the degree to which an input variable belongs to one of the fuzzy sets. By calculating the Gaussian membership value, the input variable can be converted from an exact value to a fuzzy value, thereby realizing fuzzy reasoning. The center and width of the membership function are dynamically adjusted by the learning algorithm. The learning algorithm can automatically adjust the center and width of the membership function according to the characteristics of the input data and the training of the network, so that the membership function can better adapt to the distribution of the input data. For example, when the distribution of the input data changes, the learning algorithm can adjust the center and width of the membership function, so that the calculation of the membership value is more accurate.

[0052] In one embodiment, the membership function calculation of the extended input vector by the fuzzification layer of the SOFNN generates Gaussian membership values of each input variable, including: Step 1331: Extract each dimensional feature component in the extended input vector, and determine the value range and distribution characteristics of each feature component.

[0053] Before performing the membership function calculation, it is necessary to extract each dimensional feature component in the extended input vector and determine the value range and distribution characteristics of each feature component. The extended input vector is a multi-dimensional vector, each dimension representing a feature component, such as kinetic residual, position, angular velocity, angular acceleration, etc. Different feature components may have different value ranges and distribution characteristics. For example, the value range of the kinetic residual may be between [-10, 10], while the value range of the position data may be between [0, 100]. By determining the value range and distribution characteristics of each feature component, it is possible to provide a basis for initializing the Gaussian membership function.

[0054] Step 1332: Initialize the center vector and width vector of the Gaussian membership function based on the distribution characteristics of each feature component, and the initial center vector is obtained from the training sample set by the K-means clustering algorithm.

[0055] After determining the value range and distribution characteristics of each feature component, the center vector and width vector of the Gaussian membership function are initialized based on these characteristics. The center vector represents the center position of the Gaussian membership function, and the width vector represents the width of the Gaussian membership function. The initial center vector is obtained from the training sample set by the K-means clustering algorithm. The K-means clustering algorithm is a commonly used clustering algorithm, which can divide the training sample set into different categories, and the center of each category is an initial center vector. Through the K-means clustering algorithm, the clustering center of the input data can be automatically found, making the initialization of the Gaussian membership function more reasonable. For example, for a training sample set of one of the feature components, the K-means clustering algorithm can divide it into 5 categories, and the center of each category is an initial center vector.

[0056] Step 1333: Calculate the Euclidean distance between each feature component and the corresponding center vector, and substitute the Euclidean distance into the Gaussian function expression to obtain the preliminary membership value.

[0057] After initializing the center vector and width vector of the Gaussian membership function, the Euclidean distance between each feature component and the corresponding center vector is calculated. Euclidean distance is a commonly used distance measurement method, which can measure the similarity between two vectors. Substitute the calculated Euclidean distance into the Gaussian function expression to obtain the preliminary membership value. The Gaussian function expression is a nonlinear function that can calculate the degree of a feature component belonging to one of the fuzzy sets according to the size of the Euclidean distance. For example, for a feature component and a center vector, calculate the Euclidean distance between them, and then substitute the distance into the Gaussian function expression to obtain the preliminary membership value of the feature component for the fuzzy set represented by the center vector.

[0058] Step 1334: Introduce a dynamic scaling factor to adaptively adjust the preliminary membership value, and the dynamic scaling factor is positively correlated with the standard deviation of the feature component.

[0059] In order to make the membership value more accurately reflect the actual situation of the feature component, a dynamic scaling factor is introduced to adaptively adjust the preliminary membership value. The dynamic scaling factor is positively correlated with the standard deviation of the feature component. The standard deviation reflects the dispersion degree of the feature component. When the standard deviation of the feature component is large, it means that the dispersion degree of the data is large, and a larger dynamic scaling factor is needed to adjust the membership value. Through the adjustment of the dynamic scaling factor, the membership value can better adapt to the distribution change of the feature component. For example, if the standard deviation of one of the feature components is large, the dynamic scaling factor will increase accordingly, thereby adjusting the preliminary membership value to a greater extent.

[0060] Step 1335: Normalizing the adjusted membership values to generate a Gaussian membership value matrix satisfying the convex fuzzy partition property.

[0061] After the preliminary membership values are dynamically adjusted, the adjusted membership values are normalized. The normalization can convert the membership values to the range of [0, 1] and make the sum of all membership values equal to 1. Through the normalization, a Gaussian membership value matrix satisfying the convex fuzzy partition property can be generated. The convex fuzzy partition property means that the boundary between fuzzy sets is smooth without jumps or mutations. The Gaussian membership value matrix described above can more accurately describe the degree to which the input variables belong to each fuzzy set and provide a more reliable basis for fuzzy rule reasoning.

[0062] Step 134: Performing a fuzzy rule matching operation in the rule reasoning layer of the SOFNN, calculating rule trigger strengths based on the membership values, and obtaining relative activation weights of each rule through normalization.

[0063] After the Gaussian membership value matrix is obtained, a fuzzy rule matching operation is performed in the rule reasoning layer of the SOFNN. Fuzzy rules are some conditional statements defined in advance and used to describe the relationship between input variables and output variables. Based on the Gaussian membership values, rule trigger strengths can be calculated. The rule trigger strength represents the degree to which one of the fuzzy rules is activated, which is related to the membership values of the input variables. Through normalization, the relative activation weights of each rule can be obtained. The relative activation weight represents the importance of each fuzzy rule among all rules, and the sum of the relative activation weights of all rules is 1. For example, at a certain time, the trigger strengths of different fuzzy rules are calculated according to the membership values of the input variables, and then their relative activation weights are obtained through normalization. These relative activation weights can be used for weighted summation calculation.

[0064] Step 135: Weighted summation of the relative activation weights and rule consequent parameters to generate estimated values of each disturbance component through the defuzzification layer, and the rule consequent parameters are updated using the recursive least squares method.

[0065] After obtaining the relative activation weights of each rule, they are weighted and summed with the rule consequent parameters. The rule consequent parameters are the coefficients of the output variables in the fuzzy rules, which determine the output results of the fuzzy rules. Through weighted summation, a comprehensive output result can be obtained. Then, the fuzzy output result is converted into an accurate numerical value through the defuzzification layer to generate the estimated values of each disturbance component. The rule consequent parameters are updated using the recursive least squares method. The recursive least squares method is an iterative algorithm that can continuously update the rule consequent parameters based on new input data and output data, making the output results of the network more accurate. For example, in each sampling period, the rule consequent parameters are updated using the recursive least squares method based on the current relative activation weights and output data, and then the estimated values of each disturbance component are recalculated.

[0066] In an optional embodiment, the rule consequent parameters are updated using the recursive least squares method, including: Step 1351: Establish an update target function for the rule consequent parameters, which is defined as the weighted combination of the sum of the squares of the disturbance estimation errors and the parameter penalty term.

[0067] When updating the rule consequent parameters using the recursive least squares method, an update target function for the rule consequent parameters is first established, which is defined as the weighted combination of the sum of the squares of the disturbance estimation errors and the parameter penalty term. The disturbance estimation error refers to the difference between the disturbance estimated value output by the network and the actual disturbance observation value. By minimizing the sum of the squares of the disturbance estimation errors, the output results of the network can be made closer to the actual values. The parameter penalty term is to prevent the rule consequent parameters from being too large and to avoid the occurrence of overfitting phenomenon. By combining these two parts through weighting, the estimation accuracy can be guaranteed while improving the generalization ability of the network. For example, in one sampling period, the sum of the squares of the disturbance estimation errors and the parameter penalty term are calculated, and then they are combined into the target function according to the pre-set weight.

[0068] Step 1352: Initialize the covariance matrix of parameter update and the forgetting factor, the value range of which is set according to the balance between convergence speed and estimation accuracy.

[0069] After the update target function is established, the covariance matrix and the forgetting factor for parameter update need to be initialized. The covariance matrix is used to describe the correlation between the rule consequent parameters, which can be used in the iterative process of recursive least squares. The value range of the forgetting factor is set according to the balance between convergence speed and estimation accuracy. The larger the forgetting factor, the slower the forgetting speed of the network to the past data, and the convergence speed may slow down, but the estimation accuracy may be higher; the smaller the forgetting factor, the faster the forgetting speed of the network to the past data, and the convergence speed may speed up, but the estimation accuracy may decrease. Therefore, the value range of the forgetting factor needs to be reasonably set according to the specific application scene and requirements. For example, in a scene with high estimation accuracy requirements, the value of the forgetting factor can be appropriately increased.

[0070] Step 1353: In each sampling period, calculate the parameter update increment according to the current rule activation weight and the disturbance estimation error, which is the difference between the composite disturbance estimation value and the actual disturbance observation value.

[0071] In each sampling period, calculate the parameter update increment according to the current rule activation weight and the disturbance estimation error. The disturbance estimation error is the difference between the composite disturbance estimation value and the actual disturbance observation value. Through the rule activation weight, the contribution degree of each rule to the parameter update can be determined, and the disturbance estimation error reflects the difference between the network output result and the actual value. According to these information, the update increment of the rule consequent parameter can be calculated. For example, in one of the sampling periods, the parameter update increment is calculated using the formula of recursive least squares according to the current rule activation weight and the calculated disturbance estimation error.

[0072] Step 1354: Weight and correct the parameter update increment using the covariance matrix and the forgetting factor to generate the corrected value of the rule consequent parameter.

[0073] After the parameter update increment is calculated, it is weighted and corrected using the covariance matrix and the forgetting factor. The covariance matrix can adjust the parameter update increment according to the correlation between the rule consequent parameters. The forgetting factor can control the influence degree of the past data on the current parameter update. Through the weighted correction, the corrected value of the rule consequent parameter can be generated. For example, the calculated parameter update increment is multiplied by the corresponding coefficients of the covariance matrix and the forgetting factor to obtain the corrected parameter update increment, and then it is added to the current rule consequent parameter to obtain the corrected value of the rule consequent parameter.

[0074] Step 1355: Add the corrected value to the current rule consequent parameter to complete the parameter update, and limit the value range of the parameter through the parameter constraint condition to avoid overfitting.

[0075] The modified value of the rule consequent parameter is superimposed to the current rule consequent parameter to complete the update of the parameter. In order to avoid the occurrence of overfitting phenomenon, the value range of the parameter is limited by the parameter constraint condition. The parameter constraint condition can be some preset upper and lower limits. When the updated parameter exceeds these ranges, it will be adjusted to within the range. For example, if the value range of one of the rule consequent parameters is limited to [0, 1], when the updated parameter is greater than 1, it will be adjusted to 1; when the updated parameter is less than 0, it will be adjusted to 0. In the above way, it can be ensured that the value of the rule consequent parameter is reasonable, and the generalization ability of the network is improved.

[0076] Step 136: Weight proportion analysis is performed on the estimated values of each disturbance component to calculate the percentage contribution of the friction disturbance component, the load disturbance component and the external environment disturbance component in the composite disturbance, and generate a composite disturbance estimate value containing component proportion information.

[0077] After obtaining the estimated values of each disturbance component, weight proportion analysis is performed. The percentage contribution of the friction disturbance component, the load disturbance component and the external environment disturbance component in the composite disturbance is calculated. By dividing the estimated value of each disturbance component by the total estimated value of the composite disturbance, the percentage contribution can be obtained. For example, if the estimated value of the friction disturbance component is A, the estimated value of the load disturbance component is B, the estimated value of the external environment disturbance component is C, and the total estimated value of the composite disturbance is A+B+C, then the percentage contribution of the friction disturbance component is A / (A+B+C). Combining these percentage contributions with the estimated values of each disturbance component, a composite disturbance estimate value containing component proportion information is generated. The above composite disturbance estimate value can more detailedly describe the disturbance situation of the robot arm, and provide a more accurate basis for disturbance compensation.

[0078] Step 140: A disturbance compensation control signal is generated according to the composite disturbance estimate value and the desired trajectory parameters of the robot arm, and the disturbance compensation control signal is used to offset the influence of the composite disturbance of each joint.

[0079] After obtaining the composite disturbance estimate value containing component proportion information, a disturbance compensation control signal is generated according to the estimate value and the desired trajectory parameters of the robot arm. The desired trajectory parameters of the robot arm describe the motion trajectory that the robot arm should follow in an ideal state, including position, velocity and acceleration information. The purpose of the disturbance compensation control signal is to offset the influence of the composite disturbance of each joint, so that the robot arm can more accurately track the desired trajectory. For example, if the composite disturbance estimate value indicates that the joint friction disturbance component is large, then the disturbance compensation control signal needs to increase the compensation force for joint friction accordingly to reduce its influence on the motion of the robot arm.

[0080] In an alternative design, the disturbance compensation control signal is generated according to the composite disturbance estimate and the desired trajectory parameters of the robot arm, including: Step 141: Analyze the desired trajectory parameters of the robot arm, and extract the position command sequence, velocity command sequence, and acceleration command sequence in the trajectory planning.

[0081] First, the desired trajectory parameters of the robot arm need to be analyzed, and the position command sequence, velocity command sequence, and acceleration command sequence in the trajectory planning need to be extracted. The position command sequence describes the positions that the robot arm should reach at different times, the velocity command sequence describes the velocities that the robot arm should have at different times, and the acceleration command sequence describes the accelerations that the robot arm should have at different times. These command sequences are the targets of the robot arm's motion, and by extracting these sequences, the desired motion state of the robot arm at each time can be determined. For example, in a complex task, the desired trajectory parameters of the robot arm may include multiple stages of motion, each with different position, velocity, and acceleration requirements. By analyzing these requirements, the corresponding command sequences can be extracted.

[0082] Step 142: Calculate the trajectory tracking error based on the position command sequence and the current position feedback data of the robot arm, which includes a three-dimensional vector of position error, velocity error, and acceleration error.

[0083] After the position command sequence is extracted, the trajectory tracking error is calculated based on the sequence and the current position feedback data of the robot arm. The trajectory tracking error includes a three-dimensional vector of position error, velocity error, and acceleration error. The position error is the difference between the current position of the robot arm and the desired position at the corresponding time in the position command sequence, the velocity error is the difference between the current velocity of the robot arm and the desired velocity at the corresponding time in the velocity command sequence, and the acceleration error is the difference between the current acceleration of the robot arm and the desired acceleration at the corresponding time in the acceleration command sequence. By calculating these errors, the difference between the actual motion state and the desired motion state of the robot arm can be quantified. For example, at a certain time, the current position of the robot arm is P1, and the desired position at that time in the position command sequence is P2, so the position error is P1-P2. Similarly, the velocity error and the acceleration error can be calculated, and they are combined into a three-dimensional vector to obtain the trajectory tracking error.

[0084] Step 143: Input the composite disturbance estimate and the trajectory tracking error into the disturbance compensator, and calculate the basic compensation torque through the feedforward control algorithm.

[0085] The composite disturbance estimate is input into a disturbance compensator, which calculates a base compensation torque using a feedforward control algorithm. Feedforward control is a model-based control method that uses known disturbance information and system models to calculate the required control input in advance. In this scenario, the composite disturbance estimate provides the disturbance information experienced by the robot arm, and the trajectory tracking error reflects the difference between the robot arm's actual motion and the desired motion. The disturbance compensator uses these inputs to calculate a base compensation torque using a feedforward control algorithm. The base compensation torque is designed to counteract some of the effects of the disturbance in advance, allowing the robot arm to more closely follow the desired trajectory. For example, based on the composite disturbance estimate and the trajectory tracking error, the disturbance compensator can calculate the additional torque that each joint needs to apply to compensate for the disturbance.

[0086] Step 144: Introduce a PID feedback regulation mechanism to dynamically modify the base compensation torque, with the proportional, integral, and derivative coefficients of the PID feedback regulation mechanism being dynamically adjusted based on the amplitude of the trajectory tracking error.

[0087] To further improve the effectiveness of disturbance compensation, a PID feedback regulation mechanism is introduced to dynamically modify the base compensation torque. PID feedback regulation is a classic control method that adjusts the control output through three components: proportional, integral, and derivative. The proportional coefficient is used to amplify the trajectory tracking error, allowing the system to respond quickly to changes in error; the integral coefficient is used to eliminate the steady-state error of the system, allowing the system to eventually reach the desired state; and the derivative coefficient is used to predict the trend of the error, taking measures in advance to suppress further increases in error. The proportional, integral, and derivative coefficients of the PID feedback regulation mechanism are dynamically adjusted based on the amplitude of the trajectory tracking error. When the amplitude of the trajectory tracking error is large, a larger proportional and integral coefficient is needed to speed up the response of the system; when the amplitude of the trajectory tracking error is small, a smaller proportional and integral coefficient is needed to reduce the overshoot of the system. For example, at a certain time, the amplitude of the trajectory tracking error is large, and the PID feedback regulation mechanism automatically increases the proportional and integral coefficients to speed up the adjustment of the robot arm.

[0088] As an optional embodiment, the proportional, integral, and derivative coefficients of the PID feedback regulation mechanism are dynamically adjusted based on the amplitude of the trajectory tracking error, including: Step 1441: Set a multi-section threshold interval for the trajectory tracking error, with each threshold interval corresponding to a set of reference values for the PID parameters.

[0089] To realize the dynamic adjustment of the proportional coefficient, integral coefficient and differential coefficient of the PID feedback adjustment mechanism according to the amplitude of the trajectory tracking error, a multi-section threshold interval of the trajectory tracking error is set. Each threshold interval corresponds to a set of reference values of PID parameters. For example, the amplitude of the trajectory tracking error can be divided into three threshold intervals: a small error interval, a medium error interval and a large error interval. Each interval corresponds to different PID parameter reference values. The small error interval may correspond to smaller proportional coefficient and integral coefficient to reduce the overshoot of the system; the large error interval may correspond to larger proportional coefficient and integral coefficient to speed up the response speed of the system.

[0090] Step 1442: Real-time monitoring of the current amplitude of the trajectory tracking error to determine the threshold interval to which the current amplitude belongs.

[0091] After setting the threshold interval and the corresponding PID parameter reference values, the current amplitude of the trajectory tracking error is monitored in real time. By comparing the current amplitude with the boundary values of each threshold interval, the threshold interval to which the current amplitude belongs is determined. For example, the position, velocity and acceleration data of the robot arm are collected in real time using sensors, the amplitude of the trajectory tracking error is calculated, and then it is determined which threshold interval it belongs to according to the amplitude.

[0092] Step 1443: When the error amplitude is in the first threshold interval, the first target proportional coefficient and the first target integral coefficient are used to speed up the response speed.

[0093] When the amplitude of the trajectory tracking error is in the first threshold interval, the first target proportional coefficient and the first target integral coefficient are used to speed up the response speed. The first threshold interval is usually the large error interval, in which larger proportional coefficient and integral coefficient are needed to quickly reduce the error. The first target proportional coefficient and the first target integral coefficient are determined according to the PID parameter reference values corresponding to this threshold interval. For example, if the proportional coefficient Kp1 and the integral coefficient Ki1 in the PID parameter reference values corresponding to the first threshold interval, then when the error amplitude is in this interval, Kp1 and Ki1 are used as the target proportional coefficient and the target integral coefficient to speed up the adjustment speed of the robot arm.

[0094] Step 1444: When the error amplitude is in the second threshold interval, the second target proportional coefficient and the second target integral coefficient are used, and the differential coefficient is introduced to suppress the overshoot.

[0095] When the amplitude of the trajectory tracking error is in the second threshold interval, the second target proportional coefficient and the second target integral coefficient are adopted, and a differential coefficient is introduced to suppress overshoot. The second threshold interval is usually the medium error interval, in which appropriate proportional coefficient and integral coefficient are needed to continue to reduce the error, while the differential coefficient is introduced to predict the trend of error change, and take measures in advance to suppress the further increase of error. The second target proportional coefficient and the second target integral coefficient are determined according to the PID parameter reference value corresponding to the threshold interval. For example, if the proportional coefficient in the PID parameter reference value corresponding to the second threshold interval is Kp2, the integral coefficient is Ki2, and the differential coefficient is Kd2, then when the error amplitude is in this interval, Kp2, Ki2 and Kd2 are adopted as the target proportional coefficient, the target integral coefficient and the differential coefficient.

[0096] Step 1445: When the error amplitude is in the third threshold interval, the third target proportional coefficient and the third target integral coefficient are adopted to eliminate the steady-state error.

[0097] When the amplitude of the trajectory tracking error is in the third threshold interval, the third target proportional coefficient and the third target integral coefficient are adopted to eliminate the steady-state error. The third threshold interval is usually the small error interval, in which smaller proportional coefficient and integral coefficient are needed to fine-tune the system and eliminate the steady-state error. The third target proportional coefficient and the third target integral coefficient are determined according to the PID parameter reference value corresponding to the threshold interval. For example, if the proportional coefficient in the PID parameter reference value corresponding to the third threshold interval is Kp3, and the integral coefficient is Ki3, then when the error amplitude is in this interval, Kp3 and Ki3 are adopted as the target proportional coefficient and the target integral coefficient to ensure that the robot arm can accurately track the desired trajectory.

[0098] Step 1446: Calculate the error amplitude change rate of the adjacent sampling period, and fine-tune the PID parameters according to the positive and negative directions of the change rate. When the change rate is positive, increase the proportional coefficient; when the change rate is negative, decrease the proportional coefficient.

[0099] In addition to adjusting the PID parameters according to the threshold interval to which the amplitude of the trajectory tracking error belongs, the error amplitude change rate of the adjacent sampling period is also calculated. The error amplitude change rate reflects the trend of error change, and the PID parameters are fine-tuned according to the positive and negative directions of the change rate. When the change rate is positive, it means that the error is increasing, and increasing the proportional coefficient can speed up the response of the system and reduce the error; when the change rate is negative, it means that the error is decreasing, and decreasing the proportional coefficient can reduce the overshoot of the system. For example, the difference between the error amplitude of the current sampling period and the error amplitude of the last sampling period is calculated, and then divided by the time interval of the sampling period to obtain the error amplitude change rate. If the change rate is positive, the current proportional coefficient is appropriately increased; if the change rate is negative, the current proportional coefficient is appropriately decreased.

[0100] Step 1447: Filtering the adjusted PID parameters through a smooth transition algorithm to suppress control signal jitter caused by parameter mutation.

[0101] The adjusted PID parameters are filtered through a smooth transition algorithm to suppress control signal jitter caused by parameter mutation. The smooth transition algorithm can make the changes of PID parameters more smooth, avoiding sudden changes of parameters to have a greater impact on the control signal. For example, a low-pass filter is used to filter the adjusted PID parameters, removing the high-frequency components therein, making the parameter changes more slow and stable, which can reduce the jitter of the control signal and improve the motion stability of the robot arm.

[0102] Step 145: Coordinate transformation of the corrected compensation torque from joint space to operational space according to the joint stiffness parameters of the robot arm, generating a disturbance compensation control signal that meets the input requirements of the driver.

[0103] After obtaining the corrected compensation torque, coordinate transformation is performed from joint space to operational space according to the joint stiffness parameters of the robot arm. Joint space describes the motion state of each joint of the robot arm, while operational space describes the motion state of the end effector of the robot arm in the Cartesian coordinate system. Through coordinate transformation, the compensation torque in joint space can be converted into control signal in operational space, so as to meet the input requirements of the driver. Joint stiffness parameters reflect the elastic properties of the joint, which play an important role in the coordinate transformation process. For example, according to the kinematic model of the robot arm and the joint stiffness parameters, the corrected compensation torque is converted into force and torque in operational space using coordinate transformation algorithm, generating a disturbance compensation control signal that meets the input requirements of the driver.

[0104] Step 146: Amplitude limiting processing is performed on the disturbance compensation control signal to ensure that the signal amplitude does not exceed the maximum output capability of the joint driver.

[0105] In order to ensure the safe operation of the joint driver, amplitude limiting processing is performed on the disturbance compensation control signal. The joint driver has its maximum output capability, and if the amplitude of the control signal exceeds this capability, the driver may be damaged. Therefore, through amplitude limiting processing, the signal amplitude is ensured not to exceed the maximum output capability of the joint driver. For example, an upper limit value and a lower limit value are set, and when the amplitude of the disturbance compensation control signal is greater than the upper limit value, it is adjusted to the upper limit value; when the amplitude of the disturbance compensation control signal is less than the lower limit value, it is adjusted to the lower limit value, thereby ensuring that the control signal is within the acceptable range of the driver and improving the reliability of the system.

[0106] Step 150: superimpose the disturbance compensation control signal to the original trajectory tracking control instruction of the robot arm, and execute the integrated control instruction through the joint driver to realize accurate tracking of the desired trajectory.

[0107] After generating the disturbance compensation control signal, it is superimposed to the original trajectory tracking control instruction of the robot arm. The original trajectory tracking control instruction is generated according to the desired trajectory planning of the robot arm, which describes the motion requirements of the robot arm in the ideal state. Superimposing the disturbance compensation control signal to the original control instruction can offset the influence of the disturbance on the robot arm, so that the robot arm can more accurately track the desired trajectory. By executing the integrated control instruction through the joint driver, the robot arm can adjust its motion according to the new control signal to realize accurate tracking of the desired trajectory. For example, the disturbance compensation control signal is superimposed with the original position loop control signal and velocity loop control signal to obtain the integrated control instruction, which is then sent to the joint driver, and the joint driver drives the robot arm to move according to the instruction.

[0108] In one example embodiment, the superimposition of the disturbance compensation control signal to the original trajectory tracking control instruction of the robot arm comprises: Step 151: extract the original trajectory tracking control instruction of the robot arm, which contains the position loop control signal and the velocity loop control signal of each joint.

[0109] Before signal superposition, the original trajectory tracking control instruction of the robot arm needs to be extracted. The original trajectory tracking control instruction contains the position loop control signal and the velocity loop control signal of each joint. The position loop control signal is used to control the position of each joint of the robot arm to reach the desired position, and the velocity loop control signal is used to control the motion speed of each joint of the robot arm to meet the desired speed requirement. By extracting these control signals, a basis for subsequent signal superposition can be provided. For example, the position loop control signal and the velocity loop control signal of each joint are obtained from the control system of the robot arm, which are usually generated by trajectory planning algorithm.

[0110] Step 152: time synchronization processing is performed on the disturbance compensation control signal and the trajectory tracking control instruction, and the disturbance compensation control signal and the trajectory tracking control instruction are superimposed in the same sampling period.

[0111] To ensure that the disturbance compensation control signal and the trajectory tracking control instruction can be accurately superimposed, time synchronization processing is performed on them. The time synchronization processing can ensure that the two signals are superimposed in the same sampling period, avoiding the inaccuracy of the control signal caused by time asynchronization. For example, the same clock signal is used to sample the disturbance compensation control signal and the trajectory tracking control instruction, ensuring that their time stamps are consistent at each sampling time. Then, in the same sampling period, the disturbance compensation control signal and the trajectory tracking control instruction are superimposed to obtain the integrated control signal.

[0112] Step 153: The disturbance compensation control signal and the trajectory tracking control instruction are fused by using a weighted superposition algorithm, and the weighting coefficient is dynamically adjusted according to the confidence of the composite disturbance estimate value.

[0113] The disturbance compensation control signal and the trajectory tracking control instruction are fused by using a weighted superposition algorithm. The weighting coefficient is dynamically adjusted according to the confidence of the composite disturbance estimate value. The confidence of the composite disturbance estimate value reflects the reliability of the estimate value. When the confidence is high, it means that the disturbance estimate is accurate, and at this time the weight of the disturbance compensation control signal can be increased. When the confidence is low, it means that the disturbance estimate may have some errors, and at this time the weight of the disturbance compensation control signal can be reduced and the weight of the original control instruction can be increased. For example, according to the error analysis and statistical information in the calculation process of the composite disturbance estimate value, the confidence is determined, and then the weighting coefficient is adjusted according to the confidence to weight and superimpose the disturbance compensation control signal and the trajectory tracking control instruction.

[0114] Step 154: When the confidence of the composite disturbance estimate value is higher than the pre-set confidence, the weight proportion of the disturbance compensation control signal is increased.

[0115] When the confidence of the composite disturbance estimate value is higher than the pre-set confidence, it means that the disturbance estimate is accurate, and at this time the weight proportion of the disturbance compensation control signal is increased. For example, if the pre-set confidence is 0.8, when the confidence of the composite disturbance estimate value is 0.9, the weighting coefficient of the disturbance compensation control signal is appropriately increased, so that the disturbance compensation control signal occupies a larger proportion in the integrated control signal, thereby more effectively offsetting the influence of the disturbance.

[0116] Step 155: When the confidence of the composite disturbance estimate value is lower than the pre-set confidence, the weight proportion of the disturbance compensation control signal is reduced and the weight proportion of the original control instruction is increased.

[0117] When the confidence of the composite disturbance estimation value is lower than the preset confidence, it indicates that the disturbance estimation may have some errors, at this time, the weight proportion of the disturbance compensation control signal is reduced and the weight proportion of the original control command is increased. For example, if the preset confidence is 0.8, when the confidence of the composite disturbance estimation value is 0.6, the weighting coefficient of the disturbance compensation control signal is appropriately reduced, and the weighting coefficient of the original control command is increased, so that the original control command occupies a larger proportion in the integrated control signal, to ensure the stability of the motion of the robot arm.

[0118] Step 156: Low-pass filtering the superimposed control signal to filter out high-frequency noise components, and the cutoff frequency of the low-pass filter is set according to the inherent frequency characteristics of the robot arm.

[0119] The superimposed control signal is low-pass filtered to filter out high-frequency noise components. High-frequency noise components may be caused by sensor errors, interference during signal transmission, and other factors, which can adversely affect the control signal and cause the robot arm to move unstably. The cutoff frequency of the low-pass filter is set according to the inherent frequency characteristics of the robot arm. The inherent frequency characteristics reflect the dynamics of the robot arm, and the cutoff frequency should be set at a suitable value to effectively filter out high-frequency noise without excessively affecting the useful components of the control signal. For example, according to the kinematic model and experimental data of the robot arm, the inherent frequency range is determined, and then the cutoff frequency of the low-pass filter is set at a value slightly higher than one of the values in the range.

[0120] Step 157: Convert the filtered control signal into a digital pulse signal and send it to the control interface of each joint driver through a bus communication protocol.

[0121] The filtered control signal is converted into a digital pulse signal and sent to the control interface of each joint driver through a bus communication protocol. The digital pulse signal is a signal form suitable for transmission in digital circuits, which can accurately represent the size and direction of the control signal. The bus communication protocol is a standard protocol for transmitting data between different devices, which can ensure that the control signal can be accurately and reliably transmitted to the control interface of each joint driver. For example, an analog-to-digital converter is used to convert the filtered analog control signal into a digital pulse signal, and then the digital pulse signal is sent to the control interface of each joint driver through a CAN bus, RS-485 bus, or other communication protocol. The joint driver drives the robot arm to move according to the received signal.

[0122] As a non-limiting example, the method further comprises: Step 210: Real-time monitoring of the disturbance compensation effect of each joint of the robot arm, and evaluating the compensation performance through the root mean square value of the trajectory tracking error.

[0123] To ensure the effectiveness of the disturbance compensation method, the disturbance compensation effect of each joint of the robot arm is monitored in real time. The compensation performance is evaluated by the root mean square value of the trajectory tracking error. The root mean square value of the trajectory tracking error reflects the average error level of the robot arm in a period of time, which can consider the influence of position error, velocity error and acceleration error. The smaller the root mean square value, the better the disturbance compensation effect, and the robot arm can track the desired trajectory more accurately. For example, in each sampling period, the trajectory tracking error of each joint of the robot arm is calculated, and then the error values in a period of time are squared, summed, averaged and square-rooted to obtain the root mean square value of the trajectory tracking error.

[0124] Step 220: When the root mean square value exceeds the allowed error range for a plurality of consecutive sampling periods, trigger the reconstruction mechanism of the SOFNN network structure; the reconstruction mechanism includes increasing the number of hidden layer nodes, adjusting the membership function parameters and resetting the rule consequent parameters.

[0125] When the root mean square value of the trajectory tracking error exceeds the allowed error range for a plurality of consecutive sampling periods, it indicates that the current SOFNN network structure may not be able to accurately estimate the disturbance, and the reconstruction mechanism of the SOFNN network structure needs to be triggered. The reconstruction mechanism includes increasing the number of hidden layer nodes, adjusting the membership function parameters and resetting the rule consequent parameters. Increasing the number of hidden layer nodes can improve the nonlinear mapping ability of the network, so that it can better adapt to complex disturbance conditions; adjusting the membership function parameters can make the fuzzy process more accurately reflect the distribution of input data; resetting the rule consequent parameters can reinitialize the output mapping relationship of the network. For example, if the root mean square value of the trajectory tracking error of 10 consecutive sampling periods exceeds the allowed error range, the reconstruction mechanism is triggered, the number of hidden layer nodes is increased, the center and width of the membership function are adjusted, the rule consequent parameters are reset, and then the network is retrained.

[0126] Step 230: Collect the reconstructed disturbance estimation data and compensation control signals, and establish an association model between the compensation effect and the network structure parameters.

[0127] After triggering the reconstruction mechanism of the SOFNN network structure, the reconstructed disturbance estimation data and compensation control signals are collected. By analyzing these data, an association model between the compensation effect and the network structure parameters is established. The association model can describe the relationship between the network structure parameters (such as the number of hidden layer nodes, membership function parameters, rule consequent parameters, etc.) and the compensation effect (such as the root mean square value of the trajectory tracking error). For example, by changing the network structure parameters, recording the disturbance estimation data and compensation control signals under different parameter combinations, and the corresponding compensation effect, and then using data fitting method to establish the association model.

[0128] Step 240: optimizing the learning rate of the SOFNN and the rule pruning threshold for deleting redundant rules with an activation frequency lower than a set value based on the correlation model.

[0129] Based on the correlation model of the established compensation effect and the network structure parameters, the learning rate of the SOFNN and the rule pruning threshold are optimized. The learning rate determines the step size of parameter update in the training process of the network, and a suitable learning rate can make the network converge to the optimal solution faster. The rule pruning threshold is used to delete redundant rules with an activation frequency lower than a set value, reducing the complexity of the network. For example, by analyzing the influence of different learning rates and rule pruning thresholds on the compensation effect through the correlation model, the learning rate and rule pruning threshold that can achieve the best compensation effect are selected.

[0130] Step 250: storing the optimized network structure parameters and control parameters into a non-volatile memory as initial configuration parameters for the next start-up.

[0131] The optimized network structure parameters and control parameters are stored in a non-volatile memory as initial configuration parameters for the next start-up. The non-volatile memory can retain data after power failure, so that the optimized parameters can be directly used for initialization when the robot arm is started next time, avoiding re-adjustment and training of parameters, and improving the start-up efficiency of the system. For example, the optimized parameters such as the number of hidden layer nodes, membership function parameters, rule consequent parameters, learning rate and rule pruning threshold are stored in the non-volatile memory such as EEPROM, Flash, etc. These parameters are read from the memory when starting next time to configure the SOFNN network structure and the control system.

[0132] The multi-degree-of-freedom robot arm hybrid disturbance compensation method of the embodiment of the application significantly improves the tracking accuracy of the robot arm on the desired trajectory. By constructing a complex disturbance model containing the coupling relationship of joint friction, load change and external environmental disturbance components, the complex disturbance situation during the operation of the robot arm is comprehensively and accurately described. Real-time acquisition of position feedback data and torque output data and calculation of dynamic residual error can timely capture the disturbance information under the current motion state of the robot arm. The dynamic residual error is input into the preset SOFNN network structure for disturbance estimation processing to generate a complex disturbance estimation value containing the weight proportion of each disturbance component. The estimation value reflects the influence degree of different disturbance components on the motion of the robot arm in detail. The disturbance compensation control signal generated based on the complex disturbance estimation value and the desired trajectory parameters can specifically offset the influence of the complex disturbance of each joint. Finally, the disturbance compensation control signal is superimposed into the original trajectory tracking control instruction, and the integrated control instruction is executed through the joint driver, so that the robot arm can effectively overcome various disturbances and achieve accurate tracking of the desired trajectory.

[0133] Compared with the prior art, the method has significant advantages in comprehensiveness and accuracy of disturbance estimation and pertinence and effectiveness of compensation, and greatly improves the working performance and stability of the multi-degree-of-freedom robot arm in a complex environment.

[0134] Referring to Figure 4 As shown in the figure, the figure is a schematic diagram of a basic structure of a multi-degree-of-freedom robot arm hybrid disturbance compensation system 200 based on a SOFNN provided by an embodiment of the application, and the multi-degree-of-freedom robot arm hybrid disturbance compensation system 200 based on the SOFNN comprises: a processor 201; a storage device 202, on which a computer program 2020 is stored; When the computer program 2020 is executed by the processor 201, the processor 201 implements any of the SOFNN-based multi-degree-of-freedom robot arm hybrid disturbance compensation methods.

[0135] On the basis of the above, a readable storage medium is provided, and the readable storage medium stores a program or instructions, and the program or instructions are executed by a processor to implement the steps of the above method.

[0136] It should be noted that the embodiments in the specification are described in a progressive manner, and each embodiment focuses on the difference from other embodiments, and the same or similar parts of each embodiment can be referred to each other. For the system or device disclosed in the embodiments, since it corresponds to the method disclosed in the embodiments, the description is relatively simple, and the relevant part can be referred to the method part.

Claims

1. A mixed disturbance compensation method for multi-degree-of-freedom manipulator based on SOFNN, characterized in that: The method comprises: Constructing a composite disturbance model of a multi-degree-of-freedom manipulator, wherein the composite disturbance model includes a description of the coupling relationship between a joint friction disturbance component, a load change disturbance component, and an external environment disturbance component; Collecting position feedback data and torque output data of each joint of the robotic arm in real time, and calculating the dynamic residual in the current motion state based on the position feedback data and the torque output data; The dynamic residual is input into a preset SOFNN network structure for disturbance estimation processing to generate a composite disturbance estimation value including the weight proportion of each disturbance component; generating a disturbance compensation control signal according to the composite disturbance estimation value and the desired trajectory parameter of the robotic arm, wherein the disturbance compensation control signal is used to offset the composite disturbance effect of each joint; The disturbance compensation control signal is superimposed on the original trajectory tracking control instruction of the robot arm, and the integrated control instruction is executed by the joint driver to achieve accurate tracking of the desired trajectory.

2. The hybrid disturbance compensation method for multi-degree-of-freedom manipulator based on SOFNN according to claim 1 is characterized in that: The composite disturbance model of the multi-degree-of-freedom manipulator is constructed, comprising: Establishing a rigid body dynamics equation for the robotic arm, wherein the rigid body dynamics equation includes a mathematical relationship expression of an inertia matrix term, a Coriolis force term, a centrifugal force term, and a gravity term; Introducing a nonlinear friction model into the rigid body dynamics equation, wherein the nonlinear friction model includes a piecewise function description of a Coulomb friction component, a viscous friction component, and a static friction component; Based on the load variation range of the end effector of the robotic arm, a mapping function between the load disturbance coefficient and the joint torque variation is constructed; Collect the dynamic response data of the manipulator under different external environments, and establish the correlation equation between the external environmental disturbance component and the motion speed and acceleration through data fitting method; The nonlinear friction model, the load disturbance mapping relationship function and the external environment disturbance correlation equation are coupled to the rigid body dynamics equation to generate a composite disturbance model including the interaction of multiple disturbance components.

3. The hybrid disturbance compensation method for multi-degree-of-freedom manipulator based on SOFNN according to claim 1 is characterized in that: The real-time acquisition of position feedback data and torque output data of each joint of the robotic arm, and calculation of the dynamic residual in the current motion state based on the position feedback data and the torque output data, includes: The encoders installed on each joint collect real-time position data, and the torque sensors collect joint output torque data, establishing a position-torque data sequence with the sampling period as the time reference. Performing numerical differentiation processing on the position data sequence to calculate real-time angular velocity data and angular acceleration data of each joint, wherein the numerical differentiation processing adopts a five-point central difference algorithm to reduce noise interference; Substituting the position data, angular velocity data, and angular acceleration data into a nominal dynamics model of the robotic arm to calculate a theoretical torque estimate; Extracting the dynamic response component from the torque output data and removing the static offset to obtain the actual effective torque value; The difference between the actual effective torque value and the theoretical torque estimate is calculated, and the difference is defined as a dynamic residual under the current motion state, where the dynamic residual is used to characterize the comprehensive influence of the unmodeled disturbance.

4. The hybrid disturbance compensation method for multi-degree-of-freedom manipulator based on SOFNN according to claim 1 is characterized in that: The dynamic residual is input into a preset SOFNN network structure for disturbance estimation processing to generate a composite disturbance estimation value including the weight proportion of each disturbance component, including: The number of input layer nodes of the initialized SOFNN network structure is the number of degrees of freedom of the manipulator, the number of hidden layer nodes is dynamically determined by the fuzzy clustering algorithm, and the number of output layer nodes corresponds to the number of component types of the composite disturbance; The dynamic residual and the real-time motion state data of the robot arm are combined into an extended input vector, wherein the real-time motion state data includes normalized values ​​of position, angular velocity and angular acceleration; The membership function of the extended input vector is calculated by the fuzzification layer of the SOFNN to generate a Gaussian membership value for each input variable, and the center and width of the membership function are dynamically adjusted by a learning algorithm; The fuzzy rule matching operation is performed in the rule reasoning layer of SOFNN, the rule triggering strength is calculated based on the membership value, and the relative activation weight of each rule is obtained through normalization processing; Performing a weighted summation of the relative activation weight and the rule consequent parameter, generating an estimated value of each disturbance component through a defuzzification layer, and updating the rule consequent parameter using a recursive least squares method; A weighted proportion analysis is performed on the estimated values ​​of each disturbance component, and the percentage contribution of the friction disturbance component, load disturbance component and external environment disturbance component in the composite disturbance is calculated to generate a composite disturbance estimate containing component proportion information.

5. The hybrid disturbance compensation method for multi-degree-of-freedom manipulator based on SOFNN according to claim 4 is characterized in that: The fuzzification layer of the SOFNN is used to calculate the membership function of the extended input vector to generate the Gaussian membership value of each input variable, including: Extracting each dimensional feature component in the extended input vector and determining the value range and distribution characteristics of each feature component; The center vector and width vector of the Gaussian membership function are initialized based on the distribution characteristics of each feature component. The initial center vector is obtained from the training sample set through the K-means clustering algorithm. Calculate the Euclidean distance between each feature component and the corresponding center vector, and substitute the Euclidean distance into the Gaussian function expression to obtain a preliminary membership value; Introducing a dynamic scaling factor to adaptively adjust the preliminary membership value, wherein the dynamic scaling factor is positively correlated with the standard deviation of the characteristic component; The adjusted membership values ​​are normalized to generate a Gaussian membership value matrix that satisfies the convex fuzzy partitioning characteristics.

6. The hybrid disturbance compensation method for multi-DOF manipulator based on SOFNN according to claim 4 is characterized in that: The rule consequent parameters are updated using a recursive least squares method, including: Establishing an objective function for updating the rule consequent parameters, wherein the objective function is defined as a weighted combination of the sum of squares of the disturbance estimation error and a parameter penalty term; Initialize the covariance matrix and forgetting factor of the parameter update, where the value range of the forgetting factor is set according to the balance between convergence speed and estimation accuracy; In each sampling period, the parameter update increment is calculated based on the current rule activation weight and the disturbance estimation error, where the disturbance estimation error is the difference between the composite disturbance estimate and the actual disturbance observation value; Using the covariance matrix and the forgetting factor to perform weighted correction on the parameter update increment to generate a correction value of the rule consequent parameter; The correction value is added to the consequent parameter of the current rule to complete the parameter update, and the parameter value range is limited by the parameter constraint condition to avoid overfitting.

7. The hybrid disturbance compensation method for multi-DOF manipulator based on SOFNN according to claim 1 is characterized in that: Generating a disturbance compensation control signal according to the composite disturbance estimation value and the desired trajectory parameter of the robotic arm includes: Analyze the desired trajectory parameters of the robot arm and extract the position instruction sequence, velocity instruction sequence and acceleration instruction sequence in trajectory planning; Calculating a trajectory tracking error based on the position instruction sequence and current position feedback data of the robotic arm, wherein the trajectory tracking error includes a three-dimensional vector of position error, velocity error, and acceleration error; The composite disturbance estimation value and the trajectory tracking error are input into the disturbance compensator, and the basic compensation torque is calculated by a feedforward control algorithm; A PID feedback regulation mechanism is introduced to dynamically correct the basic compensation torque, and the proportional coefficient, integral coefficient and differential coefficient of the PID feedback regulation mechanism are dynamically adjusted according to the amplitude of the trajectory tracking error; The corrected compensation torque is transformed from the joint space to the operation space according to the joint stiffness parameters of the manipulator to generate a disturbance compensation control signal that meets the input requirements of the driver; The disturbance compensation control signal is subjected to amplitude limiting processing so that the signal amplitude does not exceed the maximum output capacity of the joint driver.

8. The hybrid disturbance compensation method for multi-DOF manipulator based on SOFNN according to claim 7 is characterized in that: The proportional coefficient, integral coefficient, and differential coefficient of the PID feedback regulation mechanism are dynamically adjusted according to the amplitude of the trajectory tracking error, including: Set the multi-stage threshold interval of trajectory tracking error, each threshold interval corresponds to a set of reference values ​​of PID parameters; monitoring a current amplitude of the trajectory tracking error in real time, and determining a threshold interval to which the current amplitude belongs; When the error amplitude is within a first threshold range, a first target proportional coefficient and a first target integral coefficient are used to speed up the response speed; When the error amplitude is within the second threshold range, a second target proportional coefficient and a second target integral coefficient are adopted, and a differential coefficient is introduced to suppress overshoot; When the error amplitude is within a third threshold range, a third target proportional coefficient and a third target integral coefficient are used to eliminate the steady-state error; Calculate the error amplitude change rate between adjacent sampling periods and fine-tune the PID parameters according to the positive and negative directions of the change rate. Increase the proportional coefficient when the change rate is positive, and decrease the proportional coefficient when the change rate is negative. The adjusted PID parameters are filtered using a smooth transition algorithm to suppress control signal jitter caused by parameter mutations.

9. The hybrid disturbance compensation method for multi-DOF manipulator based on SOFNN according to claim 1 is characterized in that: The step of superimposing the disturbance compensation control signal onto the original trajectory tracking control instruction of the robotic arm includes: Extracting the original trajectory tracking control instructions of the robotic arm, wherein the trajectory tracking control instructions include the position loop control signal and the speed loop control signal of each joint; Performing time synchronization processing on the disturbance compensation control signal and the trajectory tracking control instruction, and superimposing the disturbance compensation control signal and the trajectory tracking control instruction within the same sampling period; A weighted superposition algorithm is used to fuse the disturbance compensation control signal with the trajectory tracking control command, and the weighting coefficient is dynamically adjusted according to the confidence level of the composite disturbance estimate; When the confidence level of the composite disturbance estimate is higher than the preset confidence level, the weight of the disturbance compensation control signal is increased; When the confidence level of the composite disturbance estimate is lower than the preset confidence level, the weight of the disturbance compensation control signal is reduced and the weight of the original control command is increased; The superimposed control signal is subjected to low-pass filtering to filter out high-frequency noise components, wherein the cutoff frequency of the low-pass filter is set according to the natural frequency characteristics of the robotic arm; The filtered control signal is converted into a digital pulse signal and sent to the control interface of each joint driver through the bus communication protocol.

10. A multi-degree-of-freedom manipulator hybrid disturbance compensation system based on SOFNN, characterized in that: include: processor; A storage device having a computer program stored thereon, wherein when the computer program is executed by the processor, the processor implements the multi-degree-of-freedom manipulator hybrid disturbance compensation method based on SOFNN as described in any one of claims 1-9.

Images