Shield cutter changing robot motion control method based on closed-loop servo hydraulic control

Abstract

The invention relates to the technical field of motion control, and discloses a shield cutter changing robot motion control method based on closed-loop servo hydraulic control, which comprises the following steps: recording the start-stop times and the start-stop interval of a hydraulic motor; friction compensation is conducted on the hydraulic motor based on the start-stop times and the start-stop intervals, and the friction torque of the hydraulic motor is obtained; working parameters of the hydraulic motor are collected; comprehensive disturbance compensation is conducted on the hydraulic motor based on the working parameters, and the disturbance equivalent moment of the hydraulic motor is obtained; acquiring an actual rotation angle of the load; calculating a control voltage amount of a hydraulic motor based on the actual rotation angle, the friction torque and the disturbance equivalent torque of the load; and controlling the movement of the hydraulic motor load based on the control voltage amount. Through friction compensation and comprehensive disturbance compensation, accurate control over movement of the hydraulic motor is achieved.

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 cutter-changing robot based on closed-loop servo hydraulic control. Background Art

[0002] Currently, there are many deficiencies in the motion control technology of shield machines / TBM cutter-changing robots equipped with servo valve-controlled hydraulic motors. During the operation of the hydraulic motor of the cutter-changing robot, the friction between internal components will seriously affect its performance. It is difficult for the existing technology to accurately measure and compensate for the frictional torque, resulting in a decrease in motion control accuracy. When the cutter-changing robot is in different working conditions, the friction characteristics of the hydraulic motor are complex and variable, and are affected by the coupling of various factors such as oil temperature, load, and rotational speed. Traditional friction compensation methods cannot adapt to these changes in real time and accurately.

[0003] The hydraulic motor of the cutter-changing robot faces various interference factors in the actual working environment, such as motor speed fluctuations, joint angle changes, sealing degradation, power supply voltage fluctuations, etc. These factors are intertwined and jointly affect the output torque and motion stability of the hydraulic motor. The existing disturbance compensation technology cannot comprehensively handle these complex interferences, resulting in insufficient anti-interference ability of the system and difficulty in ensuring the 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 a hydraulic thruster of an operating underwater robot. This control simulation system transfers the control current to the electro-hydraulic servo valve transfer function model to obtain the servo valve spool displacement and transfers 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 torque; inputs the advance speed and feedback rotational speed of the propeller into the propeller torque and thrust coefficient calculation model to obtain the torque coefficient and thrust coefficient, and transfers them to the propeller thruster dynamics system model; the propeller thruster dynamics system model calculates the thrust and feedback rotational speed of the thruster. This invention has the advantage of stable output of the thrust of the hydraulic thruster when the sea current speed and the ROV motion speed change. However, this invention still has the problems raised in the background art of this application: it cannot comprehensively handle the interferences received by the motor, making it difficult to ensure the output accuracy of motion control.

[0005] The information disclosed in this background art section is only intended to enhance the overall understanding of the present invention and should not be regarded as an admission or any form of suggestion that this information constitutes prior art already known to those 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 prior art and provide a motion control method for a shield cutter-changing robot based on closed-loop servo hydraulic control. Through friction compensation and comprehensive disturbance compensation, precise control of the motion of the hydraulic motor is achieved.

[0007] To solve the above technical problems, the present invention provides the following technical solutions:

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

[0009] Record the start-stop times and start-stop intervals of the hydraulic motor; perform friction compensation on the hydraulic motor based on the start-stop times and start-stop intervals to obtain the frictional torque of the hydraulic motor;

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

[0011] Obtain the actual rotation angle of the load; calculate the control voltage of the hydraulic motor based on the actual rotation angle of the load, the frictional torque, and the equivalent disturbance torque;

[0012] Based on the control voltage, control the motion of the hydraulic motor load.

[0013] The above load includes joints driven by servo valve-controlled hydraulic motors in shield machines / TBM cutter-changing robots, such as joints that control the rotation of the manipulator of the cutter-changing robot.

[0014] As a preferred solution of the motion control method for a shield cutter-changing robot based on closed-loop servo hydraulic control according to the present invention, wherein: calculating the control voltage of the hydraulic motor based on the actual rotation angle of the load, the frictional torque, and the equivalent disturbance torque specifically includes:

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

[0016] Construct the load dynamics equation of the hydraulic motor;

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

[0018] Based on the load pressure, calculate the hydraulic oil flow rates in the two chambers of the hydraulic motor;

[0019] Based on the hydraulic oil flow rates in the two chambers of the hydraulic motor, calculate the spool opening of the servo valve of the hydraulic motor;

[0020] Based on the spool opening, calculate the control voltage of the hydraulic motor.

[0021] As a preferred solution of the motion control method of the shield tool-changing robot based on closed-loop servo hydraulic control according to the present invention, wherein: the obtaining of the actual rotation angle of the load specifically includes:

[0022] Obtain the current position and attitude of the end effector of the robotic arm; wherein, the position of the end effector is represented by three-dimensional coordinates; the attitude of the end effector is represented by a rotation matrix or Euler angles or quaternions.

[0023] Solve for the rotation angles of the joints of the robotic arm under the current position and attitude of the end effector through the inverse kinematics algorithm; 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 motion control method of the shield tool-changing robot based on closed-loop servo hydraulic control according to the present invention, wherein: the method for recording the start-stop times and start-stop intervals of the hydraulic motor is as follows:

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

[0026] Intercept a sequence segment of length m from the time series of the rotational speed of the hydraulic motor; m is a positive integer; the sequence segment contains the rotational speeds of the hydraulic motor at the m moments closest to the current moment; record the number of motor starts or stops in the sequence segment as the start-stop times;

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

[0028] As a preferred solution of the motion control method of the shield tool-changing robot based on closed-loop servo hydraulic control according to the present invention, wherein: the method for detecting the start and stop of the motor from the time series of the rotational speed of the hydraulic motor is as follows: If the rotational speed of the hydraulic motor gradually increases from 0 until it is greater than the start speed threshold, it is detected that the motor starts; if the rotational speed of the hydraulic motor gradually decreases to 0 and the duration of the rotational speed remaining at 0 is greater than the stop time threshold, it is detected that the motor stops.

[0029] As a preferred solution of the motion control method for the shield cutter-changing robot based on closed-loop servo hydraulic control described in the present invention, the method includes: performing friction compensation on the hydraulic motor based on the start-stop times and start-stop intervals to obtain the frictional 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] Collect the frictional torque of the hydraulic motor and the relative motion speed of the main contact surface under different working conditions through experiments; determine the model parameters of the LuGre model through parameter fitting;

[0032] Calculate the relative motion speed of the main contact surface based on the actual rotation angle of the load;

[0033] Input the relative motion speed of the main contact surface into the LuGre model, calculate and output the theoretical frictional torque of the hydraulic motor;

[0034] Correct the theoretical frictional torque based on the start-stop times and start-stop intervals to obtain the frictional torque of the hydraulic motor.

[0035] As a preferred solution of the motion control method for the shield cutter-changing robot based on closed-loop servo hydraulic control described in the present invention, the method includes: correcting the theoretical frictional torque based on the start-stop times and start-stop intervals to obtain the frictional torque of the hydraulic motor, specifically including:

[0036] Obtain the test data of the hydraulic motor through experiments and construct a test database; any piece of test data in the test database includes the start-stop times, start-stop intervals, frictional torque, and relative motion speed of the main contact surface of the hydraulic motor; calculate the theoretical frictional torque corresponding to each piece of test data based on the LuGre model;

[0037] Train a correction model based on the test database; the input of the correction model includes the theoretical frictional torque, start-stop times, and start-stop intervals of the hydraulic motor, and the output is the frictional torque of the hydraulic motor;

[0038] Input the theoretical frictional torque of the hydraulic motor into the trained correction model, calculate and output the frictional torque of the hydraulic motor.

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

[0040] Based on the working parameters, comprehensive disturbance compensation is performed on the hydraulic motor to obtain the equivalent disturbance torque of the hydraulic motor, specifically including:

[0041] Perform data cleaning, normalization processing, and encoding processing on the working parameters to obtain the feature vector of each working parameter;

[0042] Input the feature vector of each working parameter into the trained fuzzy neural network; the fuzzy neural network calculates and outputs the equivalent disturbance torque of the hydraulic motor.

[0043] As a preferred solution of the motion control method for the shield cutter-changing robot based on closed-loop servo hydraulic control described in the present invention, wherein: 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 an input variable;

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

[0046] The fuzzy inference 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 perform normalization processing on the activation degree of each fuzzy rule;

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

[0049] The output layer is used to output the equivalent disturbance torque.

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

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

[0052] Perform data cleaning, normalization processing, and encoding processing on the historical working parameters; divide the historical working parameters into a training set and a test set;

[0053] Construct a fuzzy neural network and perform parameter initialization;

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

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

[0056] Compared with the prior art, the beneficial effects achieved by the present invention are as follows:

[0057] By recording the start-stop times and intervals of the hydraulic motor and performing friction compensation based on this, the present application can effectively obtain a more accurate frictional torque, which helps to solve the performance problems caused by friction during the operation of the hydraulic motor and improve the stability and accuracy of the operation of the hydraulic motor.

[0058] Collecting various working parameters such as motor speed, load size, and hydraulic oil temperature, and using a fuzzy neural network for comprehensive disturbance compensation to obtain the disturbance equivalent torque can comprehensively consider various interference factors, effectively cope with complex working conditions, improve the anti-interference ability of the motor motion control, and ensure the stable control operation of the hydraulic motor 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 will briefly introduce the drawings required for the description of the embodiments. Obviously, the following drawings are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can be obtained based on these drawings. Among them:

[0060] Figure 1 is a flowchart of a motion control method for a shield cutter-changing robot based on closed-loop servo hydraulic control provided by the present invention;

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

[0062] Figure 3 is a flowchart of a method for obtaining the frictional torque of a hydraulic motor provided by the present invention;

[0063] Figure 4 is a flowchart of a training method for a fuzzy neural network provided by the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0064] The following will describe the technical solutions of the present invention in detail through the 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 solutions of the present invention, rather than limitations on the technical solutions of the present invention. Without conflict, the technical features in the embodiments of the present invention and the embodiments can be combined with each other.

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

[0066] Record the start-stop times and start-stop intervals of the hydraulic motor; perform friction compensation on the hydraulic motor based on the start-stop times and start-stop intervals to obtain the frictional torque of the hydraulic motor;

[0067] The method for recording the start-stop times and start-stop intervals of the hydraulic motor is as follows:

[0068] Continuously monitor the rotational speed of the hydraulic motor; sample the rotational speed of the hydraulic motor at a specified time interval and establish a time series of the rotational speed of the hydraulic motor; set the start speed threshold and stop time threshold of the rotational speed; detect motor start and motor stop from the time series of the rotational speed of the hydraulic motor, 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 rotational speed of the hydraulic motor is as follows: If the rotational speed of the hydraulic motor gradually increases from 0 until it is greater than the start speed threshold, then motor start is detected; if the rotational speed of the hydraulic motor gradually decreases to 0 and the duration during which the rotational speed remains 0 is greater than the stop time threshold, then motor stop is detected.

[0070] Intercept a sequence segment of length m from the time series of the rotational speed of the hydraulic motor; m is a positive integer; the sequence segment contains the rotational speeds of the hydraulic motor at the m moments closest to the current moment; record the number of motor starts or stops in the sequence segment as the start-stop times;

[0071] In this embodiment, the start speed threshold is preferably 50 rpm, the stop time threshold is 3 seconds, and the sequence segment length m = 30. Experiments show that this parameter setting can cover typical start-stop cycles. The start speed threshold is higher than the static friction critical rotational speed of the hydraulic motor, which can effectively avoid false triggering caused by system noise. Measured data shows that when the rotational speed exceeds 50 rpm, the hydraulic motor enters a stable dynamic friction state. Setting the stop time threshold to 3 seconds can ensure that short pauses are not misjudged as stop states and can detect real shutdowns in a timely manner to trigger friction compensation. The sequence segment length m = 30. When the sampling interval is 0.1 second, 30 data points can cover a 3-second duration, which is sufficient to capture the complete start-stop process; based on Weibull distribution analysis, 30 samples can achieve 90% confidence level (p < 0.1) for start-stop feature extraction. m can be adjusted according to the system response speed. In this embodiment, values that balance real-time performance and accuracy are preferably selected.

[0072] Extract the start time of the motor start closest to the current moment in the sequence segment and denote it as ; extract the start time of the motor stop closest to the current moment in the sequence segment and denote it as ; calculate and The time difference between them to obtain the start-stop interval. The formula is as follows:

[0073] ;

[0074] Among them, represents the start-stop interval of the hydraulic motor.

[0075] Referring to Figure 3 , based on the number of start-stops and the start-stop interval, perform friction compensation on the hydraulic motor to obtain the frictional 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 the frictional resistance inside the motor when the hydraulic motor rotates; in this embodiment, the contact surface between the piston and the inner surface of the cylinder barrel is preferably selected as the main contact surface. The piston makes reciprocating or rotational motion inside the cylinder barrel of the hydraulic motor, and there is relative motion between the piston and the inner surface of the cylinder barrel. Under the action of high-pressure hydraulic oil, the piston pushes the cylinder barrel to achieve the conversion of mechanical energy. During this process, the friction between the two has a significant impact on the performance of the hydraulic motor. Based on the structures of different hydraulic motors, the contact surfaces with relative motion such as between the vane and the inner surface of the stator, and between the spool and the valve body can also be selected as the main contact surface.

[0077] Collect the frictional torque of the hydraulic motor and the relative motion speed of the main contact surface under different working conditions through experiments; determine the model parameters of the LuGre model through parameter fitting; set a series of working parameters, conduct experiments on the hydraulic motor, and collect a large amount of experimental data; for example, use a torque sensor to measure the frictional torque output by the hydraulic motor, and use a speed sensor to measure the relative motion speed of the main contact surface; based on the experimental data, fit the parameters of the LuGre model through parameter identification methods such as the least squares method and the genetic algorithm.

[0078] Calculate the relative motion speed of the main contact surface based on the actual rotation angle of the load; first, clarify the transmission method and transmission ratio between the hydraulic motor and the load. The transmission methods include gear transmission, belt transmission, connecting rod transmission, etc. Then, according to the transmission relationship, calculate the angular velocity of the output shaft of the hydraulic motor from the actual rotation angle of the load. Finally, based on the angular velocity of the output shaft, combine the radius of the inner surface of the cylinder barrel and the length of the connecting rod between the piston and the output shaft to calculate the relative motion speed between the contact surfaces.

[0079] The above load includes the joints driven by servo valve-controlled hydraulic motors in shield machines / TBM tool-changing robots, such as the joints that control the rotation of the manipulator of the tool-changing robot.

[0080] Input the relative motion speed of the main contact surface into the LuGre model, calculate and output the theoretical frictional torque of the hydraulic motor;

[0081] Modify the theoretical frictional torque based on the start-stop times and start-stop intervals to obtain the frictional torque of the hydraulic motor. The method is as follows:

[0082] Obtain the test data of the hydraulic motor through experiments and construct a test database; any piece of test data in the test database includes the start-stop times, start-stop intervals, frictional torque, and relative movement speed of the main contact surface of the hydraulic motor; calculate the theoretical frictional torque corresponding to each piece of test data based on the LuGre model;

[0083] Train a correction model based on the test database; the input of the correction model includes the theoretical frictional torque, start-stop times, and start-stop intervals of the hydraulic motor, and the output is the frictional torque of the hydraulic motor;

[0084] Input the theoretical frictional torque of the hydraulic motor into the trained correction model, and calculate and output the frictional torque of the hydraulic motor.

[0085] The correction model is any one of the correction models such as a model based on linear compensation, a model based on exponential compensation, a model based on neural network, etc. This application preferably uses a correction model based on exponential compensation as the correction model, and the formula is as follows:

[0086] ;

[0087] Among them, is the theoretical frictional torque calculated by the LuGre model; n is the start-stop times; w is the weight coefficient; is the adjustment coefficient of the start-stop times; is the reference start-stop times; is the adjustment coefficient of the start-stop intervals; is the reference start-stop interval; w, 、 、 、 are all determined by parameter fitting. is regarded as the frictional torque in the ideal state;

[0088] During the movement of the hydraulic motor, an oil film will form on the main contact surface. The oil film plays a lubricating role and can reduce the frictional torque. When the oil film has sufficient thickness and viscosity, the frictional force mainly comes from the viscous resistance inside the oil film, and the viscous resistance is much smaller than the frictional force between the exposed contact surfaces. After the motor starts, it takes a certain time to re-establish a stable oil film. Therefore, the more start-stop times and the shorter the start-stop intervals, the worse the integrity and stability of the oil film, and the greater the frictional torque. represents the influence degree of the start-stop times on the theoretical frictional torque; when n is greater than , this part is greater than 1. As the number of start-stop cycles n increases, the frictional torque of the hydraulic motor tends to increase, reflecting the cumulative damage to the oil film on the main contact surface caused by frequent start-stops; represents the influence degree of the start-stop interval on the theoretical frictional torque. When is less than , this part is greater than 1. As the start-stop interval decreases, the frictional 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 the friction state with the start-stop conditions. The present invention adopts an improved LuGre model. By describing the friction process with dynamic state variables and combining the correction coefficients of the number of start-stops and the start-stop interval, the cumulative influence of the oil film state change on friction can be quantified. In the prior art, the LuGre model usually only compensates based on the instantaneous speed and does not consider the hysteresis effect of historical working conditions on the 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 frictional torque, reflecting the effect of insufficient oil film reconstruction; when starting and stopping at short intervals, the correction term also increases synchronously to capture the influence of the decrease in oil film stability.

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

[0091] The working 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 performance, environmental dust concentration, and power supply voltage fluctuation rate;

[0092] Each working parameter affects the resistance during the rotation of the hydraulic motor, thus contributing a certain value to the equivalent disturbance torque. Among them, the motor speed affects the air resistance, vibration interference, etc., thereby changing the equivalent disturbance torque; the joint angle affects the mechanical constraints on the hydraulic motor, thus affecting the equivalent disturbance torque; the load magnitude, such as the gravity of the tool, installation resistance, etc., directly affects the resistance that the hydraulic motor needs to overcome; the hydraulic oil temperature affects the viscosity of the hydraulic oil, and then generates different internal resistance interferences; the hydraulic oil contamination degree affects the friction force, leakage, etc. of the hydraulic components, thus affecting the equivalent disturbance torque; the hydraulic oil viscosity affects its lubrication performance and friction characteristics; the degree of mechanical wear includes the wear degree of key components such as gears and pistons. Based on the surface wear depth, the degree of mechanical wear is divided into several levels, such as mild wear, moderate wear, and severe wear, so as to measure its influence degree on the equivalent disturbance torque; the sealing performance is expressed by the volume of hydraulic oil leaked per minute; the leakage of hydraulic oil causes a change in the system pressure inside the motor, thus changing the equivalent disturbance torque; the environmental dust concentration affects the acceleration of component wear and the degree of oil circuit blockage, thus affecting the normal operation of the hydraulic motor; the power supply voltage fluctuation rate is quantified by the percentage of the maximum deviation value of the voltage to the rated voltage; the fluctuation of the power supply voltage affects the output power and rotational speed stability of the motor.

[0093] Based on the working parameters, comprehensive disturbance compensation is performed on the hydraulic motor to obtain the equivalent disturbance torque of the hydraulic motor, specifically including:

[0094] Perform data cleaning, normalization processing, and encoding processing on the working parameters to obtain the feature vector of each working parameter;

[0095] Input the feature vector of each working parameter into the trained fuzzy neural network; the fuzzy neural network calculates and outputs the equivalent disturbance 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; among them:

[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 degree of each working parameter; multiple fuzzy sets are defined for each input variable and output variable, where the output variable is the equivalent disturbance torque; the membership degree of each input working parameter belonging 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 the degree of mechanical wear include high and low, which respectively represent high degree of mechanical wear and low degree of mechanical wear.

[0099] The fuzzy inference layer calculates the activation degree of each fuzzy rule based on the membership degree of each working parameter; each node in the fuzzy inference layer corresponds to a fuzzy rule, and the output of each node is the activation degree 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 actual meaning of this fuzzy rule is: If the motor speed is relatively high, the load size is relatively large, and the hydraulic oil temperature is relatively high at the same time, then the disturbance equivalent torque is theoretically relatively large at this time.

[0100] The normalization layer is used to normalize the activation degree of each fuzzy rule; ensure that the sum of the activation degrees of all rules is 1;

[0101] The defuzzification layer calculates the exact value of the disturbance equivalent torque 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 degree method to obtain the specific value of the disturbance equivalent torque.

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

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

[0104] Collect the 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 processing, and encoding processing on the historical working parameters; divide the historical working parameters into a training set and a test set;

[0106] Construct a fuzzy neural network and perform parameter initialization; the parameters that need to be initialized include the center, width, and shape of the membership function, the weights of the fuzzy rules, the association parameters between fuzzy sets (including AND, OR), the connection weight values between layers in the fuzzy neural network, and the bias terms of each layer in the fuzzy neural network;

[0107] Train the fuzzy neural network on the training set; use the error backpropagation algorithm such as the BP algorithm or its improved algorithms, such as the momentum BP algorithm, the adaptive learning rate BP algorithm, etc. to train the fuzzy neural network.

[0108] Use the test set to evaluate and optimize the trained fuzzy neural network. Input the working parameters in the test set into the fuzzy neural network to obtain the predicted disturbance equivalent moment value of the fuzzy neural network, and compare it with the actual disturbance equivalent moment. Evaluation indicators such as mean square error and mean absolute error are used to evaluate the prediction accuracy of the fuzzy neural network. If the evaluation result is not satisfactory, it is necessary to adjust the network structure or increase the number of training iterations.

[0109] Obtain the actual rotation angle of the load; calculate the control voltage of the hydraulic motor based on the actual rotation angle, frictional torque, and disturbance equivalent moment of the load;

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

[0111] Obtain the current position and attitude of the end effector of the robotic arm; among them, the position of the end effector is represented by three-dimensional coordinates; the attitude of the end effector is represented by a rotation matrix or Euler angles or quaternions.

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

[0113] Refer to Figure 2 , and calculate the control voltage of the hydraulic motor based on the actual rotation angle, frictional torque, and disturbance equivalent moment 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 the displacement of the hydraulic motor are inherent parameters of the hydraulic motor and can be directly queried and obtained.

[0115] Construct the load dynamics equation of the hydraulic motor; in the embodiments of the present application, it is preferably to construct the load dynamics equation of the hydraulic motor based on Newton's second law, and the equation is as follows:

[0116] ;

[0117] Among them, represents the load pressure, that is, the pressure difference between the two chambers of the hydraulic motor; represents the displacement of the hydraulic motor; I represents the load moment of inertia of the hydraulic motor; represents the actual rotation angle of the load the second derivative with respect to time; represents the frictional torque; is the disturbance 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 parts on the right side of the above equation. Indicates the rotational inertia torque of the hydraulic motor, that is, the torque that needs to be applied to overcome inertia when the hydraulic motor rotates; frictional torque Specifically indicates the torque that needs to be applied to overcome friction when the hydraulic motor rotates; disturbance equivalent torque Specifically indicates the comprehensive equivalent torque of all other disturbance resistances when the hydraulic motor rotates, except for inertia and friction.

[0118] Input the actual rotation angle of the load, frictional torque, disturbance equivalent torque, load moment of inertia, and hydraulic motor displacement into the load dynamics equation, and 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 solution of this application. The solution of this application ultimately directly controls the control voltage of the hydraulic motor. Essentially, 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 known that when the actual rotation angle of the load, frictional 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, calculate the hydraulic oil flow rate in the two chambers of the hydraulic motor; describe the non-linear relationship between the load pressure and the hydraulic oil flow rate in the two chambers of the hydraulic motor through a non-linear equation, so as to calculate the hydraulic oil flow rate in the two chambers of the hydraulic motor according to the load pressure; this application embodiment preferably uses the flow continuity equation or Bernoulli equation to establish the non-linear equation, and other related parameters involved include but are not limited to the viscosity of the hydraulic oil, pipeline resistance, etc.

[0120] Based on the hydraulic oil flow rate in the two chambers of the hydraulic motor, calculate the spool opening of the hydraulic motor servo valve; obtain the characteristic curve of the hydraulic motor servo valve through experimental data or technical data provided by the manufacturer; determine the spool opening of the servo valve based on the hydraulic oil flow rate 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 rate in the two chambers and the spool opening of different types and specifications of hydraulic motors.

[0121] Based on the spool opening, calculate the control voltage of the hydraulic motor. Explore the electro-mechanical conversion characteristics of the servo valve through experiments, collect test data and fit the functional relationship between the control voltage and the spool opening, and consider the hysteresis and dead zone of the servo valve to correct the functional relationship between the control voltage and the spool opening, so as to accurately calculate the control voltage of the hydraulic motor when the spool opening is known.

[0122] Based on the control voltage quantity, control the movement of the hydraulic motor load. Input the control voltage quantity into the servo valve of the hydraulic motor. The servo valve controls the opening degree of its spool based on the control voltage quantity, thereby controlling the hydraulic oil flow rates in the two chambers of the hydraulic motor; the difference in the hydraulic oil flow rates in the two chambers of the hydraulic motor causes a pressure difference between the two chambers, and the pressure difference between the two chambers drives the load to move.

[0123] Those skilled in the art should understand that the embodiments of the present invention can be provided as a method, a system, or a computer program product. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present invention can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0124] The embodiments of the present invention have been described above in conjunction with the accompanying drawings. However, the present invention is not limited to the above specific embodiments. The above specific embodiments are merely illustrative and not restrictive. Under the inspiration of the present invention, those of ordinary skill in the art can also make many forms without departing from the purpose and scope of the present invention. All of these are within the protection scope of the present invention.

Claims

1. A motion control method for a shield cutter-changing robot based on closed-loop servo hydraulic control, characterized in that: It includes the following steps: Record the start-stop times and start-stop intervals of the hydraulic motor; perform friction compensation on the hydraulic motor based on the start-stop times and start-stop intervals to obtain the friction torque of the hydraulic motor; Collect the working parameters of the hydraulic motor; perform comprehensive disturbance compensation on the hydraulic motor based on the working parameters to obtain the equivalent disturbance torque of the hydraulic motor; Obtain the actual rotation angle of the load; calculate the control voltage of the hydraulic motor based on the actual rotation angle of the load, the friction torque, and the equivalent disturbance torque; Control the movement of the hydraulic motor load based on the control voltage.

2. The motion control method of 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 of the load, the friction torque, and the equivalent disturbance torque specifically includes: Obtain the load moment of inertia of the hydraulic motor and the displacement of the hydraulic motor; Construct the load dynamics equation of the hydraulic motor; Input the actual rotation angle of the load, the friction torque, the equivalent disturbance torque, the load moment of inertia, and the displacement of the hydraulic motor into the load dynamics equation, calculate and output the load pressure; Calculate the hydraulic oil flow rates in the two chambers of the hydraulic motor based on the load pressure; Calculate the spool opening of the servo valve of the hydraulic motor based on the hydraulic oil flow rates in the two chambers of the hydraulic motor; Calculate the control voltage of the hydraulic motor based on the spool opening.

3. A motion control method for a shield cutter-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 attitude of the end effector of the robotic arm; among them, the position of the end effector is represented by three-dimensional coordinates; the attitude of the end effector is represented by a rotation matrix or Euler angles or quaternions; Solve the rotation angles of the joints of the robotic arm at the current position and attitude of the end effector through the inverse kinematics algorithm; the actual rotation angle of any load is the rotation angle of the joint corresponding to the load.

4. The motion control method of a shield cutter-changing robot based on closed-loop servo hydraulic control according to claim 1, characterized in that: The method for recording the start-stop times and start-stop intervals of the hydraulic motor is as follows: Continuously monitor the rotational speed of the hydraulic motor; sample the rotational speed of the hydraulic motor at a specified time interval and establish a time series of the rotational speed of the hydraulic motor; set the start speed threshold and stop time threshold of the rotational speed; detect the start and stop of the motor from the time series of the rotational speed of the hydraulic motor, and record the start time of each motor start and the start time of each motor stop; Intercept a sequence segment with a length of m from the time series of the rotational speed of the hydraulic motor; m is a positive integer; the sequence segment contains the rotational speeds of the hydraulic motor at the m moments closest to the current moment; record the number of motor starts or stops in the sequence segment as the start-stop times; Extract the start time of the motor start closest to the current time in the sequence segment, denoted as ; extract the start time of the motor stop closest to the current time in the sequence segment, denoted as ; calculate and to obtain the start-stop interval 5. The motion control method of a shield cutter-changing robot based on closed-loop servo hydraulic control according to claim 4, characterized in that: The method for detecting the start and stop of the motor from the time series of the rotational speed of the hydraulic motor is as follows: If the rotational speed of the hydraulic motor gradually increases from 0 until it is greater than the start speed threshold, it is detected that the motor starts; if the rotational speed of the hydraulic motor gradually decreases to 0 and the duration of the rotational speed remaining at 0 is greater than the stop time threshold, it is detected that the motor stops.

6. The motion control method of a shield cutter-changing robot based on a closed-loop servo hydraulic control according to claim 5, characterized in that: Performing friction compensation on the hydraulic motor based on the start-stop times and start-stop intervals to obtain the friction torque of the hydraulic motor specifically includes: Select the main contact surface of the hydraulic motor and establish a LuGre model for the hydraulic motor; Collect the friction torque of the hydraulic motor and the relative movement speed of the main contact surfaces under different working conditions through experiments; determine the model parameters of the LuGre model through parameter fitting; Calculate the relative movement speed of the main contact surfaces based on the actual rotation angle of the load; Input the relative movement speed of the main contact surfaces into the LuGre model, calculate and output the theoretical friction torque of the hydraulic motor; Correct the theoretical friction torque based on the start-stop times and start-stop intervals to obtain the friction torque of the hydraulic motor.

7. The motion control method of a shield cutter-changing robot based on closed-loop servo hydraulic control according to claim 6, wherein: Correct the theoretical friction torque based on the start-stop times and start-stop intervals to obtain the friction torque of the hydraulic motor, specifically including: Obtain the test data of the hydraulic motor through experiments and construct a test database; any piece of test data in the test database includes the start-stop times, start-stop intervals, friction torque, and relative movement speed of the main contact surfaces of the hydraulic motor; calculate the theoretical friction torque corresponding to each piece of test data based on the LuGre model; Train a correction model based on the test database; the inputs of the correction model include the theoretical friction torque, start-stop times, and start-stop intervals of the hydraulic motor, 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, calculate and output the friction torque of the hydraulic motor.

8. A motion control method for a shield cutter-changing robot based on a closed-loop servo hydraulic control according to claim 7, characterized in that: The working parameters of the hydraulic motor include motor speed, joint angle, load size, hydraulic oil temperature, hydraulic oil contamination degree, hydraulic oil viscosity, mechanical wear degree, sealing performance, environmental dust concentration, power supply voltage fluctuation rate; Perform comprehensive disturbance compensation on the hydraulic motor based on the working parameters to obtain the equivalent disturbance torque of the hydraulic motor, specifically including: Perform data cleaning, normalization processing, and coding processing on the working parameters to obtain the feature vector of each working parameter; Input the feature vector of each working parameter into the trained fuzzy neural network; the fuzzy neural network calculates and outputs the equivalent disturbance torque of the hydraulic motor.

9. The motion control method of a shield cutter-changing robot based on closed-loop servo hydraulic control according to claim 8, 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; among them: The input layer is used to input the feature vector of each working parameter as an input variable; The fuzzification layer is used to calculate the membership degree of each working parameter; The fuzzy inference layer calculates the activation degree of each fuzzy rule based on the membership degree of each working parameter; The normalization layer is used to perform normalization processing on the activation degree of each fuzzy rule; The defuzzification layer calculates the exact value of the equivalent disturbance torque based on the activation degree of each fuzzy rule; The output layer is used to output the equivalent disturbance torque.

10. A motion control method for a shield cutter-changing robot based on a closed-loop servo hydraulic control, as described in claim 9, wherein: The training method of the fuzzy neural network is as follows: Collect the historical working parameters of the hydraulic motor; any piece of historical working parameter includes the value of each working parameter and the corresponding equivalent disturbance torque; Perform data cleaning, normalization processing, and coding processing on the historical working parameters; divide the historical working parameters into a training set and a test set; Construct a fuzzy neural network and perform parameter initialization; Train the fuzzy neural network on the training set; Use the test set to evaluate and optimize the trained fuzzy neural network.

Images