A motion control method for shield tool changing robot based on closed-loop servo hydraulic control

Abstract

The present invention relates to the technical field of motion control and discloses a motion control method for a shield tool-changing robot based on closed-loop servo hydraulic control, comprising the following steps: recording the number of starts and stops and the start-stop intervals of a hydraulic motor; performing friction compensation on the hydraulic motor based on the start-stop number and start-stop interval to obtain the friction torque of the hydraulic motor; collecting the operating parameters of the hydraulic motor; performing comprehensive disturbance compensation on the hydraulic motor based on the operating parameters to obtain the disturbance equivalent torque of the hydraulic motor; obtaining the actual rotation angle of the load; calculating the control voltage of the hydraulic motor based on the actual rotation angle, friction torque, and disturbance equivalent torque of the load; and controlling the motion of the hydraulic motor load based on the control voltage. The present application achieves precise control of the motion of the hydraulic motor through friction compensation and comprehensive disturbance compensation.

Description

Technical Field

[0001] The present invention relates to the technical field of motion control, and in particular to a motion control method for a shield tool-changing robot based on closed-loop servo hydraulic control. Background Art

[0002] Currently, motion control technology for shield machine / TBM tool-changing robots equipped with servo-valve-controlled hydraulic motors has numerous shortcomings. During operation, friction between the internal components of the tool-changing robot's hydraulic motor can severely impact its performance. Existing technologies struggle to accurately measure and compensate for friction torque, resulting in reduced motion control accuracy. The friction characteristics of the hydraulic motor are complex and highly variable under varying operating conditions, influenced by a variety of factors, including oil temperature, load, and rotational speed. Traditional friction compensation methods are unable to accurately adapt to these changes in real time.

[0003] The hydraulic motor of a tool-changing robot faces a variety of interference factors in its actual working environment, including motor speed fluctuations, joint angle changes, seal degradation, and power supply voltage fluctuations. These factors intersect and affect the hydraulic motor's output torque and motion stability. Existing disturbance compensation technologies are unable to comprehensively handle these complex interferences, resulting in insufficient anti-interference capabilities and difficulty in ensuring output accuracy when facing complex working conditions.

[0004] For example, the Chinese patent with the authorization announcement number CN105425610B discloses a thrust control simulation system for the hydraulic propulsion of an operational underwater robot. The control simulation system transmits the control current to the electro-hydraulic servo valve transfer function model, obtains the servo valve spool displacement, and transmits it to the valve-controlled hydraulic motor transfer function model, outputs the oil pressure difference at both ends of the hydraulic motor to the motor displacement module, and outputs the torque; the propeller advance speed and feedback speed are input into the propeller torque and thrust coefficient calculation model to obtain the torque coefficient and thrust coefficient, which are transmitted to the propeller propeller dynamic system model; the propeller propeller dynamic system model calculates the thrust and feedback speed of the propeller. This invention has the advantage of stable thrust output of the hydraulic propeller when the ocean current speed and the ROV movement speed change. However, this invention still has the problem raised in the background technology of this application: it cannot fully handle the interference received by the motor, making it difficult to ensure the accuracy of the motion control output.

[0005] The information disclosed in this background section is only intended to enhance understanding of the overall background of the invention and should not be considered as an admission or any form of suggestion that the information constitutes the prior art already known to a person of ordinary skill in the art. Summary of the Invention

[0006] The technical problem to be solved by the present invention is to overcome the defects of the existing technology and provide a shield tool changing robot motion control method based on closed-loop servo hydraulic control, which realizes precise control of the hydraulic motor motion through friction compensation and comprehensive disturbance compensation.

[0007] In order to solve the above technical problems, the present invention provides the following technical solutions:

[0008] A motion control method for a shield tool-changing robot based on closed-loop servo hydraulic control comprises the following steps:

[0009] Recording the number of starts and stops and the start-stop intervals of the hydraulic motor; performing friction compensation on the hydraulic motor based on the number of starts and stops and the start-stop intervals to obtain the friction torque of the hydraulic motor;

[0010] Collecting the working parameters of the hydraulic motor; performing comprehensive disturbance compensation on the hydraulic motor based on the working parameters to obtain the disturbance equivalent torque of the hydraulic motor;

[0011] Obtaining the actual rotation angle of the load; calculating the control voltage of the hydraulic motor based on the actual rotation angle of the load, the friction torque, and the disturbance equivalent torque;

[0012] Based on the amount of control voltage, movement of the hydraulic motor load is controlled.

[0013] The above loads include joints driven by servo valve-controlled hydraulic motors in shield machine / TBM tool changing robots, such as the joints that control the rotation of the tool changing robot's mechanical arm.

[0014] As a preferred solution of the shield tool changing robot motion control method based on closed-loop servo hydraulic control described in the present invention, the control voltage of the hydraulic motor is calculated based on the actual rotation angle, friction torque, and disturbance equivalent torque of the load, specifically including:

[0015] Obtain the load moment of inertia and displacement of the hydraulic motor;

[0016] Construct the load dynamics equations for the hydraulic motor;

[0017] Input the load's actual rotation angle, friction torque, disturbance equivalent torque, load moment of inertia, and hydraulic motor displacement into the load dynamics equation to calculate and output the load pressure;

[0018] Calculating the hydraulic oil flow rates of the two chambers of the hydraulic motor based on the load pressure;

[0019] Calculating the valve core opening of the hydraulic motor servo valve based on the hydraulic oil flow rates of the two chambers of the hydraulic motor;

[0020] Based on the valve spool opening, a control voltage amount of the hydraulic motor is calculated.

[0021] As a preferred solution of the shield tool changing robot motion control method based on closed-loop servo hydraulic control described in the present invention, the actual rotation angle of the load is obtained, which specifically includes:

[0022] Get the current position and posture of the end effector of the robot arm; the position of the end effector is expressed in three-dimensional coordinates; the posture of the end effector is expressed in rotation matrix, Euler angle or quaternion.

[0023] The inverse kinematics algorithm is used to solve the rotation angles of each joint of the robotic arm under the current position and posture of the end effector; the actual rotation angle of any load is the rotation angle of the joint corresponding to the load.

[0024] As a preferred solution of the shield tool changing robot motion control method based on closed-loop servo hydraulic control described in the present invention, the method for recording the start and stop times and the start and stop intervals of the hydraulic motor is as follows:

[0025] Continuously monitor the speed of the hydraulic motor; sample the speed of the hydraulic motor at specified time intervals and establish a time series of the hydraulic motor speed; set a start speed threshold and a stop time threshold for the speed; detect motor starts and motor stops from the time series of the hydraulic motor speed, and record the start time of each motor start and the start time of each motor stop;

[0026] Extract a sequence segment of length m from the time series of the hydraulic motor speed, where m is a positive integer; the sequence segment includes the speeds of the hydraulic motor at the m moments closest to the current moment; and record the number of times the motor starts or stops in the sequence segment as the start-stop count.

[0027] Extract the start time of the motor start closest to the current moment in the sequence segment and record it as Extract the start time of the motor stop closest to the current moment in the sequence segment and record it as ;calculate and The time difference between them is used to obtain the start-stop interval.

[0028] As a preferred solution of the shield tool changing robot motion control method based on closed-loop servo hydraulic control described in the present invention, the method of detecting motor start and motor stop from the time series of the hydraulic motor speed is as follows: if the speed of the hydraulic motor gradually increases from 0 until it is greater than the starting speed threshold, the motor start is detected; if the speed of the hydraulic motor gradually decreases to 0, and the time for which the speed remains at 0 is greater than the stop time threshold, the motor stop is detected.

[0029] As a preferred solution of the shield tool changing robot motion control method based on closed-loop servo hydraulic control described in the present invention, wherein: friction compensation of the hydraulic motor is performed based on the start-stop number and the start-stop interval to obtain the friction torque of the hydraulic motor, specifically including:

[0030] Select the main contact surface of the hydraulic motor and establish a LuGre model for the hydraulic motor;

[0031] The friction torque of the hydraulic motor and the relative motion speed of the main contact surfaces under different working conditions are collected through experiments; the model parameters of the LuGre model are determined through parameter fitting;

[0032] Calculating the relative movement speed of the primary contact surface based on the actual rotation angle of the load;

[0033] Inputting the relative motion speed of the main contact surface into the LuGre model, calculating and outputting the theoretical friction torque of the hydraulic motor;

[0034] The theoretical friction torque is corrected based on the start-stop number and the start-stop interval to obtain the friction torque of the hydraulic motor.

[0035] As a preferred solution of the shield tool changing robot motion control method based on closed-loop servo hydraulic control described in the present invention, wherein: based on the start-stop number and the start-stop interval, the theoretical friction torque is corrected to obtain the friction torque of the hydraulic motor, specifically including:

[0036] Acquire test data of the hydraulic motor through experiments and construct a test database; any test data in the test database includes the number of starts and stops of the hydraulic motor, the start and stop interval, the friction torque, and the relative movement speed of the main contact surfaces; calculate the theoretical friction torque corresponding to each test data based on the LuGre model;

[0037] The correction model is trained based on the test database; the input of the correction model includes the theoretical friction torque of the hydraulic motor, the number of starts and stops, and the start and stop interval, and the output is the friction torque of the hydraulic motor;

[0038] The theoretical friction torque of the hydraulic motor is input into the trained correction model to calculate and output the friction torque of the hydraulic motor.

[0039] As a preferred solution of the shield tool changing robot motion control method based on closed-loop servo hydraulic control described in the present invention, the operating parameters of the hydraulic motor include motor speed, joint angle, load size, hydraulic oil temperature, hydraulic oil contamination, hydraulic oil viscosity, mechanical wear degree, sealing, environmental dust concentration, and power supply voltage fluctuation rate;

[0040] Performing comprehensive disturbance compensation on the hydraulic motor based on the working parameters to obtain the disturbance equivalent torque of the hydraulic motor specifically includes:

[0041] Performing data cleaning, normalization, and encoding on the working parameters to obtain a feature vector for each working parameter;

[0042] The characteristic vector of each working parameter is input into a trained fuzzy neural network; the fuzzy neural network calculates and outputs the disturbance equivalent torque of the hydraulic motor.

[0043] As a preferred solution of the shield tool changing robot motion control method based on closed-loop servo hydraulic control described in the present invention, the fuzzy neural network includes an input layer, a fuzzification layer, a fuzzy inference layer, a normalization layer, a defuzzification layer, and an output layer; wherein:

[0044] The input layer is used to input the feature vector of each working parameter as input variable;

[0045] The fuzzification layer is used to calculate the membership degree of each working parameter;

[0046] The fuzzy reasoning layer calculates the activation degree of each fuzzy rule based on the membership degree of each working parameter;

[0047] The normalization layer is used to normalize the activation level of each fuzzy rule;

[0048] The defuzzification layer calculates the exact value of the perturbation equivalent moment based on the activation degree of each fuzzy rule;

[0049] The output layer is used to output the perturbation equivalent moment.

[0050] As a preferred solution of the shield tool changing robot motion control method based on closed-loop servo hydraulic control described in the present invention, the training method of the fuzzy neural network is as follows:

[0051] Collect historical working parameters of the hydraulic motor; any historical working parameter includes the value of each working parameter and the corresponding disturbance equivalent torque;

[0052] Perform data cleaning, normalization, and encoding on historical working parameters; divide historical working parameters into training sets and test sets;

[0053] Construct fuzzy neural network and initialize parameters;

[0054] Train the fuzzy neural network on the training set;

[0055] Use the test set to evaluate and tune the trained fuzzy neural network.

[0056] Compared with the prior art, the present invention has the following beneficial effects:

[0057] This application records the number of starts and stops and the intervals between them, and performs friction compensation based on this, which can effectively obtain more accurate friction torque, help solve performance problems caused by friction during the operation of the hydraulic motor, and improve the stability and accuracy of the hydraulic motor's operation.

[0058] It collects various working parameters including motor speed, load size, hydraulic oil temperature, etc., and uses fuzzy neural network to perform comprehensive disturbance compensation to obtain disturbance equivalent torque. It can comprehensively consider various interference factors, effectively deal with complex working conditions, improve the anti-interference ability of motor motion control, and ensure that the hydraulic motor can be stably controlled and operated in different working environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following briefly introduces the drawings required for describing the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. Those skilled in the art can also derive other drawings based on these drawings without inventive effort. Among them:

[0060] Figure 1 A flow chart of a motion control method for a shield tool-changing robot based on closed-loop servo hydraulic control provided by the present invention;

[0061] Figure 2 A flow chart of a method for calculating the control voltage of a hydraulic motor provided by the present invention;

[0062] Figure 3 A flow chart of a method for obtaining the friction torque of a hydraulic motor provided by the present invention;

[0063] Figure 4 This is a flow chart of the fuzzy neural network training method provided by the present invention. DETAILED DESCRIPTION

[0064] The technical solution of the present invention is described in detail below through the accompanying drawings and specific embodiments. It should be understood that the embodiments of the present invention and the specific features in the embodiments are detailed descriptions of the technical solution of the present invention, rather than limitations on the technical solution of the present invention. In the absence of conflict, the embodiments of the present invention and the technical features in the embodiments can be combined with each other.

[0065] This embodiment introduces a shield tool changing robot motion control method based on closed-loop servo hydraulic control. Figure 1 , the method comprises the following steps:

[0066] Recording the number of starts and stops and the start-stop intervals of the hydraulic motor; performing friction compensation on the hydraulic motor based on the number of starts and stops and the start-stop intervals to obtain the friction torque of the hydraulic motor;

[0067] The method for recording the number of starts and stops and the start and stop intervals of the hydraulic motor is as follows:

[0068] Continuously monitor the speed of the hydraulic motor; sample the speed of the hydraulic motor at specified time intervals and establish a time series of the hydraulic motor speed; set a start speed threshold and a stop time threshold for the speed; detect motor starts and motor stops from the time series of the hydraulic motor speed, and record the start time of each motor start and the start time of each motor stop;

[0069] The method for detecting motor start and motor stop from the time series of the hydraulic motor speed is as follows: if the speed of the hydraulic motor gradually increases from 0 until it is greater than the starting speed threshold, the motor start is detected; if the speed of the hydraulic motor gradually decreases to 0, and the time for which the speed remains at 0 is greater than the stopping time threshold, the motor stop is detected.

[0070] Extract a sequence segment of length m from the time series of the hydraulic motor speed, where m is a positive integer; the sequence segment includes the speeds of the hydraulic motor at the m moments closest to the current moment; and record the number of times the motor starts or stops in the sequence segment as the start-stop count.

[0071] In this embodiment, the preferred starting speed threshold is 50 rpm, the stopping time threshold is 3 seconds, and the sequence segment length m = 30. Experiments have shown that this parameter setting can cover typical start-stop cycles. The starting speed threshold is higher than the static friction critical speed of the hydraulic motor, which effectively avoids false triggering due to system noise. Measured data shows that when the speed exceeds 50 rpm, the hydraulic motor enters a stable dynamic friction state. Setting the stopping time threshold to 3 seconds ensures that brief pauses are not mistaken for a stop state and enables timely detection of actual stoppages to trigger friction compensation. With a sequence segment length m = 30, when the sampling interval is 0.1 seconds, 30 data points can cover a 3-second period, which is sufficient to capture the complete start-stop process. Based on Weibull distribution analysis, 30 samples can achieve start-stop feature extraction with a 90% confidence level (p < 0.1). m can be adjusted based on the system response speed. In this embodiment, a value that balances real-time performance and accuracy is preferred.

[0072] Extract the start time of the motor start closest to the current moment in the sequence segment and record it as Extract the start time of the motor stop closest to the current moment in the sequence segment and record it as ;calculate and The time difference between the start and stop intervals is obtained by the following formula:

[0073] ;

[0074] in, Indicates the start and stop interval of the hydraulic motor.

[0075] Reference Figure 3 , performing friction compensation on the hydraulic motor based on the start-stop number and the start-stop interval to obtain the friction torque of the hydraulic motor, specifically including:

[0076] Select the main contact surface of the hydraulic motor and establish a LuGre model for the hydraulic motor; the main contact surface is the contact surface corresponding to the main source of friction resistance inside the motor when the hydraulic motor rotates; this embodiment preferably uses the contact surface between the piston and the inner surface of the cylinder as the main contact surface. The piston performs reciprocating or rotational motion in the cylinder of the hydraulic motor, and there is relative motion between the piston and the inner surface of the cylinder. Under the action of high-pressure oil, the piston pushes the cylinder to realize the conversion of mechanical energy. In this process, the friction between the two has a significant impact on the performance of the hydraulic motor. Based on the structure of different hydraulic motors, contact surfaces with relative motion such as between the blades and the inner surface of the stator, and between the valve core and the valve body can also be selected as the main contact surface.

[0077] Through experiments, the friction torque of the hydraulic motor and the relative movement speed of the main contact surfaces under different working conditions are collected; the model parameters of the LuGre model are determined through parameter fitting; a series of working parameters are set, and experiments are conducted on the hydraulic motor to collect a large amount of experimental data; for example, a torque sensor is used to measure the friction torque output by the hydraulic motor, and a speed sensor is used to measure the relative movement speed of the main contact surfaces; based on the experimental data, the parameters of the LuGre model are fitted through parameter identification methods such as the least squares method and genetic algorithm.

[0078] The relative velocity of the primary contact surfaces is calculated based on the load's actual rotational angle. First, the transmission method and transmission ratio between the hydraulic motor and the load are determined. Transmission methods include gear drive, belt drive, and connecting rod drive. Then, based on the transmission relationship, the angular velocity of the hydraulic motor's output shaft is calculated from the load's actual rotational angle. Finally, the relative velocity between the contact surfaces is calculated based on the output shaft's angular velocity, combined with the cylinder's inner surface radius and the connecting rod length between the piston and the output shaft.

[0079] The above loads include joints driven by servo valve-controlled hydraulic motors in shield machine / TBM tool changing robots, such as the joints that control the rotation of the tool changing robot's mechanical arm.

[0080] Inputting the relative motion speed of the main contact surface into the LuGre model, calculating and outputting the theoretical friction torque of the hydraulic motor;

[0081] The theoretical friction torque is corrected based on the start-stop number and the start-stop interval to obtain the friction torque of the hydraulic motor. The method is as follows:

[0082] Acquire test data of the hydraulic motor through experiments and construct a test database; any test data in the test database includes the number of starts and stops of the hydraulic motor, the start and stop interval, the friction torque, and the relative movement speed of the main contact surfaces; calculate the theoretical friction torque corresponding to each test data based on the LuGre model;

[0083] The correction model is trained based on the test database; the input of the correction model includes the theoretical friction torque of the hydraulic motor, the number of starts and stops, and the start and stop interval, and the output is the friction torque of the hydraulic motor;

[0084] The theoretical friction torque of the hydraulic motor is input into the trained correction model to calculate and output the friction torque of the hydraulic motor.

[0085] The correction model is any one of a linear compensation-based model, an exponential compensation-based model, a neural network-based model, etc. The present application prefers the exponential compensation-based correction model as the correction model, and the formula is as follows:

[0086] ;

[0087] in, is the theoretical friction torque calculated by the LuGre model; n is the number of starts and stops; w is the weight coefficient; is the adjustment coefficient of the number of starts and stops; is the reference start and stop times; is the adjustment coefficient of the start-stop interval; is the reference start-stop interval; w, 、 、 、 The values ​​are determined by parameter fitting. Considered as the friction torque under ideal conditions;

[0088] During the operation of a hydraulic motor, an oil film forms on the main contact surfaces. This oil film acts as a lubricant and reduces frictional torque. When the oil film is of sufficient thickness and viscosity, friction primarily arises from viscous resistance within the film, which is much smaller than the friction between the exposed contact surfaces. After the motor is started, it takes time to re-establish a stable oil film. Therefore, the more frequent the start-stop cycle and the shorter the intervals between them, the less integrity and stability of the oil film, and the greater the frictional torque. Indicates the degree of influence of the number of starts and stops on the theoretical friction torque; when n is greater than , this part is greater than 1. As the number of starts and stops n increases, the friction torque of the hydraulic motor tends to increase, reflecting the cumulative damage to the oil film on the main contact surface caused by frequent starts and stops; Indicates the degree of influence of the start-stop interval on the theoretical friction torque. Less than , this part is greater than 1, as the start-stop interval decreases, the friction torque of the hydraulic motor tends to increase.

[0089] Existing friction compensation models, such as the Stribeck model, can only characterize static friction characteristics, such as static friction, Coulomb friction and viscous friction. Its parameters are fixed values ​​and cannot reflect the dynamic changes of friction state with start-stop conditions. The present invention adopts an improved LuGre model to describe the friction process through dynamic state variables. Combined with the correction coefficients of the number of starts and stops and the start-stop intervals, it can quantify the cumulative impact of oil film state changes on friction. The LuGre model in the prior art is usually only based on instantaneous speed compensation, and does not consider the lag effect of historical conditions on friction characteristics. The present invention introduces an exponential correction term so that when starting and stopping frequently, the correction term automatically increases the predicted value of the friction torque to reflect the effect of insufficient reconstruction of the oil film; when starting and stopping at short intervals, the correction term is simultaneously increased to capture the impact of decreased oil film stability.

[0090] Collecting the working parameters of the hydraulic motor; performing comprehensive disturbance compensation on the hydraulic motor based on the working parameters to obtain the disturbance equivalent torque of the hydraulic motor;

[0091] The operating parameters of the hydraulic motor include motor speed, joint angle, load size, hydraulic oil temperature, hydraulic oil contamination, hydraulic oil viscosity, mechanical wear degree, sealing, ambient dust concentration, and power supply voltage fluctuation rate;

[0092] Each working parameter affects the resistance of the hydraulic motor during rotation, thereby making a certain contribution to the disturbance equivalent torque. Among them, motor speed changes the disturbance equivalent torque by affecting air resistance and vibration interference; joint angle affects the mechanical constraints of the hydraulic motor, thereby affecting the disturbance equivalent torque; load size, such as the weight of the tool and installation resistance, directly affects the resistance that the hydraulic motor needs to overcome; hydraulic oil temperature affects the viscosity of the hydraulic oil, which in turn produces different internal resistance and other interferences; hydraulic oil contamination affects the friction and leakage of hydraulic components, thereby affecting the disturbance equivalent torque; hydraulic oil viscosity affects its lubrication performance and friction characteristics; the degree of mechanical wear includes the wear of key components such as gears and pistons. The degree of mechanical wear is divided into several levels based on the surface wear depth, such as light wear, moderate wear, and heavy wear, to measure the degree of its impact on the disturbance equivalent torque; sealing is expressed as the volume of hydraulic oil leaked per minute; hydraulic oil leakage causes changes in the system pressure within the motor, thereby changing the disturbance equivalent torque; environmental dust concentration can affect the wear of accelerator components and the degree of oil circuit blockage, thereby affecting the normal operation of the hydraulic motor; the power supply voltage fluctuation rate is quantified as the percentage of the maximum voltage deviation to the rated voltage; power supply voltage fluctuation can affect the output power and speed stability of the motor.

[0093] Performing comprehensive disturbance compensation on the hydraulic motor based on the working parameters to obtain the disturbance equivalent torque of the hydraulic motor specifically includes:

[0094] Performing data cleaning, normalization, and encoding on the working parameters to obtain a feature vector for each working parameter;

[0095] The characteristic vector of each working parameter is input into a trained fuzzy neural network; the fuzzy neural network calculates and outputs the disturbance equivalent torque of the hydraulic motor.

[0096] The fuzzy neural network includes an input layer, a fuzzification layer, a fuzzy inference layer, a normalization layer, a defuzzification layer, and an output layer; wherein:

[0097] The input layer is used to input the feature vector of each working parameter as an input variable; the number of nodes in the input layer is equal to the number of working parameters;

[0098] The fuzzification layer is used to calculate the membership of each working parameter; multiple fuzzy sets are defined for each input variable and output variable, where the output variable is the disturbance equivalent moment; the membership of each input working parameter to each fuzzy set is calculated through membership functions such as triangular membership function and Gaussian membership function; for example, the fuzzy sets defined for the input variable of mechanical wear degree include high and low, which represent high and low mechanical wear degrees respectively.

[0099] The fuzzy inference layer calculates the activation level of each fuzzy rule based on the membership of each operating parameter. Each node in the fuzzy inference layer corresponds to a fuzzy rule, and the output of each node is the activation level of the corresponding fuzzy rule. A possible fuzzy rule is as follows: If the motor speed belongs to the fuzzy set "high", the load size belongs to the fuzzy set "large", and the hydraulic oil temperature belongs to the fuzzy set "high", then the value of the disturbance equivalent torque belongs to the fuzzy set "large". The practical meaning of this fuzzy rule is: if the motor speed is high, the load size is large, and the hydraulic oil temperature is high, then the disturbance equivalent torque is theoretically large.

[0100] The normalization layer is used to normalize the activation level of each fuzzy rule, ensuring that the sum of the activation levels of all rules is 1.

[0101] The defuzzification layer calculates the exact value of the disturbance equivalent moment based on the activation degree of each fuzzy rule; the defuzzification layer defuzzifies the activation degree of the fuzzy rule through algorithms such as the centroid method and the maximum membership method to obtain the specific value of the disturbance equivalent moment.

[0102] The output layer is used to output the perturbation equivalent moment; the number of nodes in the output layer is 1.

[0103] Reference Figure 4 , the training method of the fuzzy neural network is as follows:

[0104] Collect historical working parameters of the hydraulic motor; any historical working parameter includes the value of each working parameter and the corresponding disturbance equivalent torque;

[0105] Perform data cleaning, normalization, and encoding on historical working parameters; divide historical working parameters into training sets and test sets;

[0106] Construct a fuzzy neural network and initialize its parameters. The parameters that need to be initialized include the center, width, and shape of the membership function, the weight of the fuzzy rules, the association parameters between fuzzy sets (including AND and OR), the connection weight values ​​between each layer in the fuzzy neural network, and the bias terms of each layer in the fuzzy neural network.

[0107] The fuzzy neural network is trained in the training set; the fuzzy neural network is trained using an error back propagation algorithm such as a BP algorithm or an improved algorithm thereof, such as a momentum BP algorithm, an adaptive learning rate BP algorithm, etc.

[0108] Use the test set to evaluate and fine-tune the trained fuzzy neural network. Input the operating parameters from the test set into the fuzzy neural network to obtain the disturbance-equivalent moment predicted by the fuzzy neural network. Compare this with the actual disturbance-equivalent moment. Use metrics such as mean square error and mean absolute error to assess the prediction accuracy of the fuzzy neural network. If the evaluation results are unsatisfactory, adjust the network structure or increase the number of training iterations.

[0109] Obtaining the actual rotation angle of the load; calculating the control voltage of the hydraulic motor based on the actual rotation angle of the load, the friction torque, and the disturbance equivalent torque;

[0110] The obtaining of the actual rotation angle of the load specifically includes:

[0111] Get the current position and attitude of the end effector of the robot arm; the position of the end effector is expressed in three-dimensional coordinates; the attitude of the end effector is expressed in rotation matrix, Euler angle or quaternion.

[0112] The inverse kinematics algorithm is used to solve the rotation angles of each joint of the robotic arm under the current position and posture of the end effector; the actual rotation angle of any load is the rotation angle of the joint corresponding to the load.

[0113] Reference Figure 2 , calculating the control voltage of the hydraulic motor based on the actual rotation angle, friction torque, and disturbance equivalent torque of the load, specifically including:

[0114] Obtain the load moment of inertia and displacement of the hydraulic motor; both the load moment of inertia and displacement are inherent parameters of the hydraulic motor and can be directly queried and obtained.

[0115] Constructing a load dynamics equation for the hydraulic motor; The embodiment of the present application preferably constructs a load dynamics equation for the hydraulic motor based on Newton's second law, and the equation is as follows:

[0116] ;

[0117] in, Indicates the load pressure, which is the difference between the pressures in the two chambers of the hydraulic motor; Indicates the displacement of the hydraulic motor; I indicates the load moment of inertia of the hydraulic motor; Indicates the actual rotation angle of the load Second derivative with respect to time; represents the friction torque; is the perturbation equivalent moment. In the above equation, represents the driving torque of the hydraulic motor, that is, the theoretical output torque of the hydraulic motor. Based on Newton's second law, the driving torque is equal to the sum of the three torque components on the right side of the equation above. Indicates the rotational inertia moment of the hydraulic motor, that is, the torque that needs to be applied to overcome inertia when the hydraulic motor rotates; friction torque Specifically refers to the torque that the hydraulic motor needs to apply to overcome friction when rotating; disturbance equivalent torque Specifically, it represents the comprehensive equivalent torque of all disturbance resistances except inertia and friction when the hydraulic motor is rotating.

[0118] The actual rotation angle of the load, friction torque, disturbance equivalent torque, load moment of inertia, and hydraulic motor displacement are input into the load dynamics equation to calculate and output the load pressure; load pressure , that is, the pressure difference between the two chambers of the hydraulic motor is the power source for the rotation of the hydraulic motor, and is also the control target of the present application. The present application ultimately directly controls the control voltage of the hydraulic motor. In essence, it is to indirectly control the pressure difference between the two chambers of the hydraulic motor, thereby controlling the rotation of the hydraulic motor. Based on the load dynamics equation, it can be seen that when the actual rotation angle of the load, friction torque, disturbance equivalent torque, load moment of inertia, and hydraulic motor displacement are known, the load pressure can be calculated.

[0119] Based on the load pressure, the hydraulic oil flow rate of the two chambers of the hydraulic motor is calculated; the nonlinear relationship between the load pressure and the hydraulic oil flow rate of the two chambers of the hydraulic motor is described by a nonlinear equation, so that the hydraulic oil flow rate of the two chambers of the hydraulic motor is calculated according to the load pressure; the embodiment of the present application preferably uses the flow continuity equation or the Bernoulli equation to establish the nonlinear equation, and other related parameters involved include but are not limited to the viscosity of the hydraulic oil, pipeline resistance, etc.

[0120] The spool opening of the hydraulic motor servo valve is calculated based on the hydraulic oil flow in the two chambers of the hydraulic motor; a characteristic curve of the hydraulic motor servo valve is obtained through experimental data or technical data provided by the manufacturer; and the spool opening of the servo valve is determined based on the hydraulic oil flow in the two chambers of the hydraulic motor and the characteristic curve of the servo valve. The characteristic curve of the servo valve describes the corresponding relationship between the hydraulic oil flow in the two chambers and the spool opening of hydraulic motors of different types and specifications.

[0121] Based on the valve core opening, the control voltage of the hydraulic motor is calculated. The electrical-mechanical conversion characteristics of the servo valve are experimentally explored, and experimental data is collected to fit the functional relationship between the control voltage and the valve core opening. This functional relationship is corrected by taking into account the hysteresis and dead zone of the servo valve. This allows the control voltage of the hydraulic motor to be accurately calculated when the valve core opening is known.

[0122] The movement of the hydraulic motor load is controlled based on the control voltage. The control voltage is input to a servo valve of the hydraulic motor. The servo valve controls the opening of its valve core based on the control voltage, thereby controlling the flow of hydraulic oil in two chambers of the hydraulic motor. The difference in the flow of hydraulic oil in the two chambers of the hydraulic motor causes a pressure difference between the two chambers, which drives the load to move.

[0123] Those skilled in the art will appreciate that embodiments of the present invention may be provided as methods, systems, or computer program products. Thus, the present invention may take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to magnetic disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0124] The embodiments of the present invention are described above in conjunction with the accompanying drawings, but the present invention is not limited to the above-mentioned specific implementation methods. The above-mentioned specific implementation methods are merely illustrative and not restrictive. Under the guidance of the present invention, ordinary technicians in this field can also make many forms without departing from the purpose and scope of protection of the present invention, which are all protected by the present invention.

Claims

1. A motion control method for a shield tool-changing robot based on closed-loop servo hydraulic control, characterized by: The following steps are involved: Recording the number of starts and stops and the start-stop intervals of the hydraulic motor; performing friction compensation on the hydraulic motor based on the number of starts and stops and the start-stop intervals to obtain the friction torque of the hydraulic motor; specifically including: Select the main contact surface of the hydraulic motor and establish a LuGre model for the hydraulic motor; The friction torque of the hydraulic motor and the relative motion speed of the main contact surfaces under different working conditions are collected through experiments; the model parameters of the LuGre model are determined through parameter fitting; Calculating the relative movement speed of the primary contact surface based on the actual rotation angle of the load; Inputting the relative motion speed of the main contact surface into the LuGre model, calculating and outputting the theoretical friction torque of the hydraulic motor; The theoretical friction torque is corrected based on the start-stop number and the start-stop interval to obtain the friction torque of the hydraulic motor; specifically comprising: Acquire test data of the hydraulic motor through experiments and construct a test database; any test data in the test database includes the number of starts and stops of the hydraulic motor, the start and stop interval, the friction torque, and the relative movement speed of the main contact surfaces; calculate the theoretical friction torque corresponding to each test data based on the LuGre model; The correction model is trained based on the test database; the input of the correction model includes the theoretical friction torque of the hydraulic motor, the number of starts and stops, and the start and stop interval, and the output is the friction torque of the hydraulic motor; Input the theoretical friction torque of the hydraulic motor into the trained correction model to calculate and output the friction torque of the hydraulic motor; Collecting the working parameters of the hydraulic motor; performing comprehensive disturbance compensation on the hydraulic motor based on the working parameters to obtain the disturbance equivalent torque of the hydraulic motor; Obtaining the actual rotation angle of the load; calculating the control voltage of the hydraulic motor based on the actual rotation angle of the load, the friction torque, and the disturbance equivalent torque; Based on the amount of control voltage, movement of the hydraulic motor load is controlled.

2. The motion control method for a shield tool-changing robot based on closed-loop servo hydraulic control according to claim 1, characterized in that: Calculating the control voltage of the hydraulic motor based on the actual rotation angle, friction torque, and disturbance equivalent torque of the load specifically includes: Obtain the load moment of inertia and displacement of the hydraulic motor; Construct the load dynamics equations of the hydraulic motor; Input the load's actual rotation angle, friction torque, disturbance equivalent torque, load moment of inertia, and hydraulic motor displacement into the load dynamics equation to calculate and output the load pressure; Calculating the hydraulic oil flow rates of the two chambers of the hydraulic motor based on the load pressure; Calculating the valve core opening of the hydraulic motor servo valve based on the hydraulic oil flow rates of the two chambers of the hydraulic motor; Based on the valve spool opening, a control voltage amount of the hydraulic motor is calculated.

3. The motion control method for a shield tool-changing robot based on closed-loop servo hydraulic control according to claim 2, characterized in that: The obtaining of the actual rotation angle of the load specifically includes: Obtain the current position and posture of the end effector of the robotic arm; wherein the position of the end effector is represented by three-dimensional coordinates; the posture of the end effector is represented by a rotation matrix, Euler angles, or quaternions; The inverse kinematics algorithm is used to solve the rotation angles of each joint of the robotic arm under the current position and posture of the end effector; the actual rotation angle of any load is the rotation angle of the joint corresponding to the load.

4. The motion control method for a shield tool-changing robot based on closed-loop servo hydraulic control according to claim 1, characterized in that: The method for recording the number of starts and stops and the start and stop intervals of the hydraulic motor is as follows: Continuously monitor the speed of the hydraulic motor; sample the speed of the hydraulic motor at specified time intervals and establish a time series of the hydraulic motor speed; set a start speed threshold and a stop time threshold for the speed; detect motor starts and motor stops from the time series of the hydraulic motor speed, and record the start time of each motor start and the start time of each motor stop; Extract a sequence segment of length m from the time series of the hydraulic motor speed, where m is a positive integer; the sequence segment includes the speeds of the hydraulic motor at the m moments closest to the current moment; and record the number of times the motor starts or stops in the sequence segment as the start-stop count. Extract the start time of the motor start closest to the current moment in the sequence segment and record it as Extract the start time of the motor stop closest to the current moment in the sequence segment and record it as ;calculate and The time difference between them is used to obtain the start-stop interval.

5. The motion control method for a shield tool-changing robot based on closed-loop servo hydraulic control according to claim 4, characterized in that: The method for detecting motor start and motor stop from the time series of the hydraulic motor speed is as follows: if the speed of the hydraulic motor gradually increases from 0 until it is greater than the starting speed threshold, the motor start is detected; if the speed of the hydraulic motor gradually decreases to 0, and the time for which the speed remains at 0 is greater than the stopping time threshold, the motor stop is detected.

6. The motion control method for a shield tool-changing robot based on closed-loop servo hydraulic control according to claim 5, characterized in that: The operating parameters of the hydraulic motor include motor speed, joint angle, load size, hydraulic oil temperature, hydraulic oil contamination, hydraulic oil viscosity, mechanical wear degree, sealing, ambient dust concentration, and power supply voltage fluctuation rate; Performing comprehensive disturbance compensation on the hydraulic motor based on the working parameters to obtain the disturbance equivalent torque of the hydraulic motor specifically includes: Performing data cleaning, normalization, and encoding on the working parameters to obtain a feature vector for each working parameter; The characteristic vector of each working parameter is input into a trained fuzzy neural network; the fuzzy neural network calculates and outputs the disturbance equivalent torque of the hydraulic motor.

7. The motion control method for a shield tool-changing robot based on closed-loop servo hydraulic control according to claim 6, characterized in that: The fuzzy neural network includes an input layer, a fuzzification layer, a fuzzy inference layer, a normalization layer, a defuzzification layer, and an output layer; wherein: The input layer is used to input the feature vector of each working parameter as input variable; The fuzzification layer is used to calculate the membership degree of each working parameter; The fuzzy reasoning layer calculates the activation degree of each fuzzy rule based on the membership degree of each working parameter; The normalization layer is used to normalize the activation level of each fuzzy rule; The defuzzification layer calculates the exact value of the perturbation equivalent moment based on the activation degree of each fuzzy rule; The output layer is used to output the perturbation equivalent moment.

8. The motion control method for a shield tool-changing robot based on closed-loop servo hydraulic control according to claim 7, characterized in that: The training method of the fuzzy neural network is as follows: Collect historical working parameters of the hydraulic motor; any historical working parameter includes the value of each working parameter and the corresponding disturbance equivalent torque; Perform data cleaning, normalization, and encoding on historical working parameters; divide historical working parameters into training sets and test sets; Construct fuzzy neural network and initialize parameters; Train the fuzzy neural network on the training set; Use the test set to evaluate and tune the trained fuzzy neural network.

Images