Anti-interference motion control method for ship rust removal mechanical arm by introducing jet flow counter-acting force

Abstract

The invention discloses a ship rust removal mechanical arm anti-interference motion control method introducing jet flow counter-acting force, and relates to the technical field of ship rust removal equipment control. Aiming at the problems of poor robustness, easy saturation of an integral term and poor dynamic response effect when a ship rust removal mechanical arm is faced with water jet reaction force and other interferences in traditional PID control, the invention provides an active disturbance rejection control (ADRC) scheme introducing jet reaction force. The method comprises the following steps: constructing a jet reaction force model, mapping the jet reaction force model into a joint torque compensation item based on a Jacobian matrix, and integrating the joint torque compensation item to an ADRC control law; an improved tracking differentiator, a nonlinear extended state observer (ESO) and a nonlinear state error feedback (NLSEF) module are designed, real-time estimation and compensation of internal and external disturbance of a system are achieved, and joint control performance is optimized. Experiments show that by means of the method, the joint angular displacement tracking error can be reduced by 10% or above, the adjusting time is shortened to 2 s from 10 s, the absolute value of the transient error is smaller than 0.3 rad, and the anti-interference capacity and the control precision of the mechanical arm are remarkably improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of ship rust removal equipment control, and particularly relates to a ship rust removal mechanical arm anti-interference motion control method introducing jet reaction force. BACKGROUND

[0002] During long-term sailing, the underwater hull part of a ship is continuously immersed in seawater, which will be seriously affected by electrochemical corrosion and marine biological attachment, resulting in the reduction of the thickness of the ship's steel plate and the decline of the structural strength, and even causing local perforation, fracture and other safety hazards. According to statistics, ship rust will increase the sailing resistance by 15%-30%, and the fuel consumption rate by 10%-20%. In addition, the rust of key components such as propellers and rudders will reduce the maneuvering performance and propulsion efficiency of the ship, shorten the service life of the ship, and even cause fuel or cargo leakage, which will cause serious pollution to the marine environment. Therefore, regular cleaning of the rust surface of the ship is a key link to ensure the safety of ship navigation and reduce operating costs. The rust removal mechanical arm equipped with a water jet rust removal device has become the mainstream equipment for ship rust removal operation due to its efficient and environmentally friendly rust removal characteristics.

[0003] During the actual operation of the ship rust removal mechanical arm, the water jet rust removal device will generate a significant water jet reaction force when spraying high-pressure water flow to remove rust. This reaction force will strongly interfere with the motion accuracy of the mechanical arm. At the same time, factors such as sea wave fluctuation, ship vibration, and the dynamic coupling and joint friction of the mechanical arm itself further increase the complexity of the system disturbance. Traditionally, the industry uses the PID (Proportional-Integral-Derivative, Control) control method to deal with the above disturbances. However, this method has obvious defects when facing the complex disturbance environment of the ship rust removal mechanical arm: first, the robustness is poor. When the mechanical arm parameters fluctuate due to changes in load, temperature, or are affected by external disturbances such as water jet reaction force and sea wave disturbance, the PID controller has difficulty in maintaining stable control performance, and even system oscillation may occur. Second, the integral term is easy to saturate. Due to the continuous action of the water jet reaction force, the integral term of the PID controller will continuously accumulate, resulting in excessive control output, joint overshoot and jitter of the mechanical arm, and the inability to achieve precise adjustment. Third, the dynamic response effect is poor. The PID controller balances the rapidity and overshoot through the linear combination of the proportional term, integral term and derivative term. However, the disturbance of the ship rust removal mechanical arm has time-varying and nonlinear characteristics, and the simple linear combination cannot meet the dynamic response demand, resulting in long adjustment time and large tracking error.

[0004] To solve the problem of anti-interference control of the mechanical arm, many researches have been carried out in the related field and various improvement schemes have been proposed. Comparative document 1 (CN201810425166.6) discloses an anti-interference iterative learning control method for a space mechanical arm system for capturing non-cooperative targets. The method estimates the external disturbance torque by designing a disturbance observer, combines robust H∞ control to suppress internal noise and observation error, and uses iterative learning control to correct trajectory tracking error. However, this method is aimed at the non-cooperative target capture scene of the space mechanical arm, and the disturbance mainly comes from the non-cooperative movement of the target spacecraft and the disturbance of the space environment. It does not consider specific disturbances such as water jet reaction force that are strongly related to the operation process, and the iterative learning control relies on multiple repeated operations to obtain error information, which is difficult to adapt to the uneven distribution of rust and dynamic adjustment of operation path in ship rust removal operation, and lacks real-time performance.

[0005] Comparative document 2 (CN201810984285.5) proposes a self-anti-interference control method, device and system, which uses parallel multiple self-anti-interference controllers to realize decoupling control of a multiple-input multiple-output system, and combines a dynamic feedforward model with an extended observer to compensate for deterministic disturbances and uncertain disturbances. Although this method introduces the core idea of self-anti-interference control, it is mainly used for general motion control of multi-joint industrial robots and does not model and compensate for the water jet reaction force of the ship rust removal mechanical arm. Moreover, the extended observer does not consider the mapping relationship between the end operation force of the mechanical arm and the joint torque, and when facing end-concentrated disturbances such as water jet reaction force, the disturbance estimation accuracy is low and the anti-interference effect is limited.

[0006] Comparative document 3 (CN202311683137.7) discloses a hydraulic mechanical arm adaptive integral robust control method based on friction compensation, which compensates for the nonlinear friction of the system by establishing a continuous friction model, designs an integral robust gain adaptive law to adjust the control gain online, and improves the tracking performance of the system. The core of this method is friction compensation and integral robust control, which focuses on solving the problems of friction nonlinearity and unmodeled disturbances of hydraulic mechanical arms, and does not involve modeling and compensation of water jet reaction force. Moreover, the control object of this method is a general mechanical arm driven by hydraulic pressure, and the coupling characteristics of the mechanical arm and the water jet device in ship rust removal operation are not considered, so it cannot be directly applied to the ship rust removal scene.

[0007] The comparative document 4 (CN202411019258.6) proposes a deep-sea hydraulic mechanical arm adaptive sliding mode control method based on disturbance observation, estimates the system disturbance and joint speed through an adaptive extended state observer, and realizes trajectory tracking in combination with an adaptive sliding mode controller. The method is aimed at the problems of parameter uncertainty and nonlinearity of the deep-sea hydraulic mechanical arm, but the disturbance observer mainly focuses on the ocean current disturbance and hydraulic system parameter change in the deep-sea environment, does not construct a special model for the water jet reaction force, and the sliding mode control is easy to introduce chattering phenomenon due to the sign function, which may cause unstable joint movement of the mechanical arm in ship rust removal operation and affect the rust removal precision.

[0008] In summary, the existing mechanical arm anti-interference control method does not consider the water jet reaction force due to the difference in application scenarios, or cannot balance the real-time performance and anti-interference performance due to the limitation of the control strategy, and it is difficult to meet the high-precision motion control requirements of the ship rust removal mechanical arm in a complex disturbance environment. Therefore, there is an urgent need for a ship rust removal mechanical arm anti-interference motion control method that can accurately model the water jet reaction force, real-time compensate for multiple source disturbances, and optimize the dynamic response performance. SUMMARY

[0009] The present application aims to overcome at least one of the above-mentioned defects of the prior art, and provides a ship rust removal mechanical arm anti-interference motion control method that introduces the jet reaction force, so as to optimize the joint control performance of the mechanical arm, realize effective compensation of the water jet reaction force, and solve the problem of insufficient anti-interference ability of the traditional PID control.

[0010] To solve the above problems, the present application proposes a self anti-interference control (ADRC) mechanical arm anti-interference motion compensation control method that introduces the jet reaction force, which estimates and compensates the internal and external disturbances of the system in real time, and improves the motion accuracy and robustness of the mechanical arm. In view of the interference problems such as water jet reaction force and environmental disturbance faced by the ship rust removal and cleaning mechanical arm during operation, specifically including: 1. Realize effective compensation of the water jet reaction force: based on the Jacobian matrix, the water jet reaction force is mapped to the joint torque compensation term and integrated into the ADRC control law, to reduce the influence of disturbance on the motion of the mechanical arm. 2. Solve the problem of insufficient anti-interference ability of the traditional PID control: by designing an active disturbance rejection controller (ADRC), using its nonlinear extended state observer (ESO) to estimate and compensate the internal and external disturbances of the system in real time, to improve the control accuracy and stability of the mechanical arm in a complex disturbance environment. 3. Optimize the joint control performance of the mechanical arm: by improving the state update law of the ESO and using the saturated nonlinear error feedback (NLSEF), the sharp peaks and chattering phenomenon in the initial control stage are suppressed, the adjustment time is shortened, and the dynamic response performance is improved.

[0011] The present application provides a ship rust removal mechanical arm anti-interference motion control method that introduces the jet reaction force, comprising the following steps: S1, construct a water jet reaction force compensation module: based on nozzle structure parameters and operation parameters, combining the law of conservation of momentum and Bernoulli equation to calculate the water jet reaction force, and using the principle of virtual work to establish a mechanical arm dynamics model; S2, design a tracking differentiator module: track the input signal to avoid the phenomenon of unsmooth or discontinuous system input; S3, build a nonlinear extended state observer: observe and compensate the unknown disturbance of the mechanical arm system in real time, and reduce the influence of disturbance on the performance of the mechanical arm system; S4, construct a nonlinear state error feedback control module: make the output of the mechanical arm system quickly track the set value, without the need to accumulate error through the integral element to eliminate steady-state error, and avoid slow dynamic response and integral saturation phenomenon; S5, integrate the modules of S1-S4 to build an ADRC control algorithm: map the water jet reaction force to the joint torque compensation item through the Jacobian matrix and integrate it into the ADRC control law to realize precise control of the active joint angular displacement of the mechanical arm.

[0012] In step S1, the calculation method of the water jet reaction force is: establish a water jet model reaction force model through the law of conservation of momentum, obtain the relationship between the jet thrust at the nozzle and the square term of flow rate according to Bernoulli equation, and obtain the reaction force of the jet F jet is: ; wherein A is the cross-sectional area of the nozzle, p is the density of the jet fluid, u 0 is the flow rate at the nozzle outlet.

[0013] Further, in step S1, the establishment method of the mechanical arm dynamics model is: define d ( t ) as all disturbance terms, then the mechanical arm dynamics model can be represented as: ; define the state variable x 1= q , , x 3= D ( t ), D ( t )=- M -1 d ( t ), the following state equation is obtained: ; Fluidic reaction force F jet The external force acting on the end of the mechanical arm can be expressed according to the principle of virtual work as: ; Wherein, δx is the virtual displacement of the end, δq is the virtual displacement of the joint space, τ j T is the generalized force or torque in the joint space; The size and direction of the fluidic reaction force always coincide with the three axes of the connecting rod, F jet Can be expressed as: ; At this time, the state equation of the mechanical arm and the dynamics model of the mechanical arm can be expressed as: ; .

[0014] Further, in the step S2, the basic mathematical model of the tracking differentiator is: ; Wherein: v 1 tracking input signal r , v 2 is the differential of v 1, k v is a characteristic parameter of the tracking signal, sat (·) is a linear saturation function, and its expression is: ; In the formula: δ is a constant, and sign(·) is a sign function.

[0015] The slope of the current point is used to approximate the function change of the next step by using the Euler method, and the continuous system is converted into a discrete form. The input signal arrangement transition and its differential signal can be expressed as: ; In the formula, r(k) is the input signal, the parameter kv is the speed factor, the parameter h is the filtering factor, and the size of the step T determines the calculation accuracy and the calculation cost. The nonlinear dynamic function fst(v1, v2, r, kv, h) is: ; Wherein, the threshold value α is: ; Wherein, the relationship between the parameters is: ; wherein kv and h are normal numbers.

[0016] Further, in the step S3, for a nonlinear uncertain controlled object of order n: , the nonlinear extended state observer is established as follows: ; wherein d0(t) represents an external disturbance, b is a control amplification coefficient, u(t) is a system control quantity, gi(·) represents a nonlinear function, x(t) is taken as an input of the nonlinear extended state observation, and each state variable zi(t) will track the extended state variable x(i-1)(t) respectively. In particular, zn+1(t) is an extended state quantity used for capturing unknown dynamics of the system, such as external disturbances and unmodeled parts of the cleaning robot.

[0017] Further, the model of the nonlinear extended state observer of the robot arm is: Discretization is performed to obtain: ; z1(k), z2(k) and z3(k) represent the output q, the derivative of the output and the disturbance of the system estimated by the nonlinear extended state observation model, and the observation error represents the difference between the observed value z1(k) and the real output q(k) of the system at a certain iteration, β1, β2 and β3 are observer gain parameters, and T is a time step.

[0018] Further, the robot arm includes three joints, and the nonlinear function fali(·) composed of three joint errors is designed as follows: ; wherein i=1, 2, 3; αi and δi are normal numbers, and the nonlinear extended state observation model of the robot arm after introducing the compensation of the fluidic reaction force is represented as: .

[0019] Further, in the step S4, the algorithm design of the nonlinear state error feedback is as follows: The robot arm system is designed as a second-order system, the input signal tracking differentiator is a second-order, and the nonlinear extended state observation model is a third-order. The closed-loop feedback error signal of the system obtained from the observation value is: The control quantity u0 of the system and the control rate τ are represented as: ; Wherein: KP, KD are constants, αP, αD, δP and δP are adjustable parameters, falP(·), falD(·) are nonlinear functions, defined as: ; Discretize the second-order nonlinear state error feedback model: ; Wherein, the adjustment amount u0(k) output by the nonlinear state error feedback model controller is obtained by the error and of the system through a nonlinear function, and the actual adjustment amount τ(k) is obtained after compensating the internal nonlinear error and disturbance of the system. Further, the observation error expression after introducing the jet reaction force compensation is: ; the disturbance term is: At this time, the control rate of the manipulator system is represented as: ; wherein, r3 is the differential of r2.

[0020] Further, in the step S5, the construction method of the ADRC control algorithm is as follows: The discretization iterative calculation algorithm of the manipulator system can be represented as:

[0021] The parameter variable input calculation algorithm can realize the control of the angular displacement of the active joint of the manipulator through programming.

[0022] Compared with the prior art, the beneficial effects of the present application are: 1) The anti-interference ability is significantly improved: By means of the nonlinear extended state observer, the water jet reaction force, the mechanical arm dynamics coupling and other disturbances are estimated and compensated in real time, so that the joint angular displacement tracking error is reduced by more than 10%.

[0023] 2) The system response speed is improved: After adding the jet reaction force compensation, the adjustment time of the ADRC is shortened from 10s to 2s under the disturbance of the jet reaction force not higher than 200N, and the initial peak amplitude is reduced by more than 15%.

[0024] 3) The motion control precision is improved: By using the saturated nonlinear error feedback instead of the traditional power function, the motion jitter phenomenon is reduced, and the absolute value of the transient error of the joint angular displacement is less than 0.3 rad. BRIEF DESCRIPTION OF DRAWINGS

[0025] Figure 1 It is a schematic diagram of water jet reaction force.

[0026] Figure 2 Tracking differentiator schematic diagram.

[0027] Figure 3 Control system block diagram based on fluidic compensation.

[0028] Figure 4 S-function execution process schematic diagram.

[0029] Figure 5 Output torque of each joint under disturbance.

[0030] Figure 6 Angular displacement of each active joint under water-free fluidic reaction force compensation.

[0031] Figure 7 Angular displacement of each active joint after water fluidic reaction force compensation.

[0032] Figure 8 Joint 1 angular displacement expected follow-up error after ADRC improvement.

[0033] Figure 9 Joint 1 observation state and state variable error after ADRC improvement.

[0034] Figure 10 Joint 1 observation state and state variable error after ADRC improvement.

[0035] Figure 11 Joint 1 disturbance and disturbance estimation error after ADRC improvement.

[0036] Figure 12 Joint 2 angular displacement expected follow-up error after ADRC improvement.

[0037] Figure 13 Joint 2 observation state and state variable error after ADRC improvement.

[0038] Figure 14 Joint 2 observation state and state variable error after ADRC improvement.

[0039] Figure 15 Joint 2 disturbance and disturbance estimation error after ADRC improvement.

[0040] Figure 16 Joint 3 angular displacement expected follow-up error after ADRC improvement.

[0041] Figure 17 Joint 3 observation state and state variable error after ADRC improvement.

[0042] Figure 18The error of the observation state and the state variable of the improved joint 3 of the ADRC.

[0043] Figure 19 The error of the disturbance and the disturbance estimation of the improved joint 3 of the ADRC. DETAILED DESCRIPTION

[0044] The drawings in the embodiments of the present application are used to describe the technical solutions in the embodiments of the present application in more detail. In the drawings, the same or similar notations represent the same or similar elements or elements with the same or similar functions throughout. The described embodiments are part of the embodiments of the present application, rather than all the embodiments. The embodiments described below by referring to the drawings are exemplary and are intended to explain the present application, and cannot be understood as a limitation of the present application. Based on the embodiments in the present application, all other embodiments obtained by a person of ordinary skill in the art without making creative efforts fall within the protection scope of the present application. The embodiments of the present application are described in detail below with reference to the drawings.

[0045] It should be noted that if the present application has any directionality indication (such as up, down, left, right, front, back, etc.), the directionality indication is only used to explain the relative position relationship, motion condition, etc. between components in a certain posture (as shown in the drawings), and if the certain posture changes, the directionality indication also changes accordingly.

[0046] In addition, if the present application has any description of “first”, “second”, etc., the description of “first”, “second”, etc. is only for the purpose of description, and cannot be understood as indicating or implying the relative importance of the indicated technical features or implicitly indicating the number of the indicated technical features. Therefore, the features limited by “first”, “second” can explicitly or implicitly include at least one of the features. In addition, the technical solutions of each embodiment can be combined with each other, but it must be based on the realization of a person of ordinary skill in the art, and when the combination of technical solutions contradicts each other or cannot be realized, it should be considered that the combination of technical solutions does not exist, and is not within the protection scope claimed by the present application.

[0047] Embodiment 1

[0048] The embodiment provides a ship rust removal mechanical arm anti-interference motion control method introducing a jet reaction force, and includes the following steps: S1, constructing a jet reaction force compensation module: based on nozzle structure parameters and operation parameters, combining the law of conservation of momentum and Bernoulli equation to calculate the size of water jet reaction force, and using the principle of virtual work to establish a mechanical arm dynamics model; The calculation method of the water jet reaction force size is: the water jet model reaction force model is established through the momentum conservation law, the jet thrust at the nozzle is obtained according to the Bernoulli equation, the jet reaction force is obtained in combination with Newton's third law, the jet reaction force is related to the square term of the flow velocity F jet For: ; wherein A is the nozzle cross-sectional area, p is the jet fluid density, u 0 is the flow velocity of the nozzle outlet.

[0049] The mechanical arm dynamics model is established by: Definition d ( t ) is all the interference terms, and the mechanical arm dynamics model can be represented as: ; Define the state variable x 1= q , , x 3= D ( t ), D ( t )=- M -1 d ( t ), the following state equation is obtained: ; The jet reaction force F jet The external force acting on the end of the mechanical arm can be represented according to the virtual work principle: ; Wherein, δx is the virtual displacement of the end, δq is the virtual displacement of the joint space, τ j T is the generalized force or torque in the joint space; The jet reaction force size and direction are shown in Figure 1 , the direction always coincides with the three-axis of the connecting rod, F jet can be represented as: ; At this time, the state equation of the mechanical arm and the mechanical arm dynamics model can be represented as: ; .

[0050] S2, design tracking differentiator module: track the input signal to avoid the phenomenon of system input not smooth or discontinuous; combined with Figure 2 As shown in the step S2, the basic mathematical model of the tracking differentiator is: ; Wherein: v 1 tracking input signal r , v 2 is the differential of 1, v k v characteristic parameters of the tracking signal, sat (·) is a linear saturation function, the expression is: ; In the formula: δ is a normal number, sign(·) is a sign function.

[0051] Using Euler method to describe the function change of the next step by the slope of the current point, the continuous system is converted into discrete form, and the input signal arrangement transition and its differential signal can be expressed as: ; In the formula, r(k) is the input signal, the parameter kv is the speed factor, the parameter h is the filter factor, the size of the step T determines the calculation accuracy and the calculation cost, the nonlinear dynamic function fst(v1, v2, r, kv, h) is: ; Wherein, the threshold α is: ; Wherein, the relationship between the parameters is: ; Wherein, kv and h are normal numbers.

[0052] S3, build nonlinear extended state observer: real-time observation and compensation of unknown disturbance of the manipulator system, reduce the influence of disturbance on the performance of the manipulator system; for an n-order nonlinear uncertain controlled object: , the nonlinear extended state observer is established as follows: ; ​In the formula, d0(t) represents an external disturbance, b is a control amplification coefficient, u(t) is a system control quantity, gi(·) represents a nonlinear function, x(t) is taken as an input of a nonlinear extended state observer, and each state variable zi(t) will track the extended state variable x(i-1)(t) respectively. In particular, zn+1(t) is an extended state variable used for capturing unknown dynamics of the system, such as external disturbances and unmodeled parts of the cleaning robot.

[0053] The model of the nonlinear extended state observer of the robot arm is as follows: Discretization is performed to obtain: ; z1(k), z2(k) and z3(k) represent the output q, the derivative of the output and the disturbance of the system estimated by the nonlinear extended state observer model respectively, and the observation error represents the difference between the observed value z1(k) and the actual output q(k) of the system at a certain iteration, β1, β2 and β3 are observer gain parameters, and T is a time step.

[0054] The robot arm includes three joints, and the nonlinear function fali(·) composed of three joint errors is designed as follows: ; wherein i=1, 2, 3; αi and δi are normal numbers, and the nonlinear extended state observer model of the robot arm after introducing the compensation of the fluidic reaction force is represented as: .

[0055] S4, a nonlinear state error feedback control module is constructed: the output of the robot arm system is made to track the set value quickly, without the need to accumulate errors through an integral element to eliminate steady-state errors, and dynamic response is avoided to be slow and integral saturation phenomenon occurs; In the step S4, the algorithm design of the nonlinear state error feedback is as follows: The robot arm system is designed as a second-order system, the input signal tracking differentiator is second-order, and the nonlinear extended state observer model is third-order. The closed-loop feedback error signal of the system obtained from the observation value is: The control quantity u0 of the system and the control rate τ are represented as: ; In the formula, KP and KD are constants, αP, αD, δP and δP are adjustable parameters, falP(·) and falD(·) are nonlinear functions, and are defined as: ; The second-order nonlinear state error feedback model is discretized: ; wherein the adjustment amount u0(k) outputted by the nonlinear state error feedback model controller is obtained by the error of the system and through a nonlinear function, and the actual adjustment amount τ(k) is obtained by the internal nonlinear error and disturbance of the system after compensation. Further, the expression of the observation error after introducing the jet reaction force compensation is: ; the disturbance term is: At this time, the control rate of the manipulator system is expressed as: ; in the formula, r3 is the differential of r2.

[0056] S5, the module integrating S1-S4 constructs the ADRC control algorithm: the jet reaction force is mapped to the joint torque compensation item through the Jacobian matrix and integrated into the ADRC control law, so as to realize the precise control of the active joint angular displacement of the manipulator. After adding the water jet compensation, the control rate of the system increases the water jet reaction force through the Jacobian determinant matrix vector transformation, the control amount τ of the closed-loop system increases the JMT transmission reaction force link, which can reduce the difference between the system output and the expected signal, and further reduce the control difficulty of the ADRC adjustment closed-loop system. The system block diagram is shown in Figure 3 .

[0057] The construction method of the ADRC control algorithm is as follows: The discrete iterative calculation algorithm of the manipulator system can be expressed as:

[0058] The input of the parameter variable into the calculation algorithm can realize the control of the active joint angular displacement of the manipulator through programming.

[0059] Embodiment 2

[0060] This embodiment is an implementation manner according to the method of embodiment 1.

[0061] 1. Simulation system construction: The basic functions of S-function are used for different stages of the discretization process such as initialization, update, output, state, etc. as shown in Figure 4 . The pseudo code execution process steps of the fal (·) function designed in ADRC are instantiated as shown in Table 1:

[0062] Further, the pseudo code of the specific discretization algorithm implementation of ESO is shown in Table 2:

[0063] 2. Reaction force compensation The expected angular displacement of each joint is set to [π / 3 rad π / 4 rad π / 6 rad], and the external reaction force of the system is set to 200 N. The torque of each joint of the cleaning robot under disturbance can be obtained by simulation. As shown in FIG. 6, when the water jet reaction force is transformed by the Jacobian determinant and then acts on the dynamic numerical theoretical model, the angular displacement output torque of each joint of the cleaning robot changes. It can be seen that the disturbance signal has an impact on the joint control of the system. Figure 4

[0064] As shown in FIG. 7, the angular displacement of each joint follows the expected value over time. As shown in FIG. 8, before the water jet reaction force compensation, the adjustment time of the ADRC is about 10 s, and the initial jitter peak of the follow-up curve is high. After the ADRC adds the reaction force compensation, the adjustment time is about 2 s, and the peak size of the initial follow-up curve is reduced. Figure 5 Figure 6~7

[0065] 3. ADRC motion control Based on the designed active disturbance rejection controller (ADRC), different types of expected signals and system total disturbances (including but not limited to mechanical vibration, sensor noise, and voltage fluctuation, etc.) are given to the three joints. The tracking of the angular displacement output of the first joint over time is analyzed. n y n , observer observed state z n1 , observer observed state z n2 and observer disturbance estimation value Mz n3 compared with the expected value r n , system state variable x n1 , system state variable x n2 and system total disturbance signal d ( t ) over time. The simulation step is set to T =0.01 s, the improved parameter ε is set to 0.82, δ is set to 1.05, the n δ n of the nonlinear extended state observer (ESO) of the ADRC of the first joint is set to (2.00, 2.00, 2.00), and other parameters are set as shown in Table 3. ​​​​​

[0066]

[0067] For joint 1, the nonlinear controller (NLSEF) parameters are set to K P =400, K D = 80.6, α P =0.95, α D =0.98, δ P =3.5, δ D =3.5.

[0068] Depend on Figure 8 As shown, the angular displacement of joint 1 y 1Can follow the expected signal better r 1. The peak and jitter phenomenon in the initial time period is not obvious, the error is small, and the adjustment time is less than 1 s. Figure 9 , the observed value of the state observer z 11 Can better observe state variables x 11 The difference is small. Figure 10 , the observed value of the state observer z 12 and observed state variables x 12 There is a small difference. Figure 11 , estimated system interference- Mz 13 Real interference with the system d ( t ) shows a small peak value in the initial stage, and there is no obvious oscillation phenomenon. The absolute value of the subsequent difference is less than 0.3 rad.

[0069] For joint 2, set the nonlinear controller (NLSEF) parameters K P =390, K D = 80. α P =0.95, α D =0.98, δ P =3.5, δ D =3.5.

[0070] Depend on Figure 12The angular displacement of joint 2 y 2 can better follow the desired signal r 2, initial period, no obvious jitter, small difference, adjustment time less than 1 s. By Figure 13 The observed value of the state observer z 21 The state variable can be better observed x 21 The value. By Figure 14 The observed value of the state observer z 22 The difference between the observed state variable x 22 Is small, and there is a jitter phenomenon in the initial stage. By Figure 15 The estimated system disturbance Mz 23 The difference between the estimated system disturbance d ( t ) and the true system disturbance is not obvious in the initial stage, and the subsequent error is small.

[0071] For joint 3, the nonlinear controller (NLSEF) parameters are set as K P =210, K D = 80, α P =0.95, α D =0.98, δ P =3.5, δ D =3.5.

[0072] The angular displacement of joint 3 Figure 16 3 is shown in y 3, the following curve has no obvious jitter, and the steady-state difference is small at each step change stage, and the average adjustment time is about 0.5 s. By r The observed value of the state observer Figure 17 31 The state variable can be better observed z 31 The value, the error is small. By x The observed value of the state observer Figure 18 32 The difference between the observed state variable z 32 Is large and oscillates greatly in the initial stage, but the duration is short. By x The estimated system disturbance Figure 19 Mz ​33 Difference between the system true disturbance d t The oscillation phenomenon in the initial stage is not severe, and there is no obvious peak phenomenon, and the subsequent difference is small.

[0073] The improved active disturbance rejection controller improves the peak and jitter phenomenon of the angular displacement output curve, especially for the 1 and 3 joints with high frequency of the expected signal change and high frequency of the system disturbance signal, and the error of the disturbance estimation value of the nonlinear state observer is smaller.

[0074] The above embodiments are only used to illustrate the technical solutions of the present application and are not limited. Although the present application has been described in detail with reference to the above preferred embodiments, those skilled in the art should understand that the technical solutions of the present application can be modified or replaced by equivalents without departing from the spirit and scope of the present application. Those skilled in the art can also make other changes within the spirit of the present application and use them in the design of the present application, as long as they do not deviate from the technical effects of the present application. These changes made in the spirit of the present application should be included in the scope of the present application.​

Claims

1. A method for controlling the anti-interference motion of a ship rust removal robot arm by introducing a jet reaction force, characterized in that: The following steps are involved: S1. Constructing a jet reaction force compensation module: Based on the nozzle structural parameters and operating parameters, the momentum conservation law and Bernoulli equation are combined to calculate the magnitude of the water jet reaction force, and the principle of virtual work is used to establish a dynamic model of the robotic arm; S2. Design a tracking differentiator module: track the input signal to avoid uneven or discontinuous system input; S3. Build a nonlinear extended state observer: observe and compensate for unknown disturbances of the robotic arm system in real time to reduce the impact of disturbances on the performance of the robotic arm system; S4. Construct a nonlinear state error feedback control module: This module enables the output of the robotic arm system to quickly track the set value without accumulating errors through the integral link to eliminate steady-state errors, thus avoiding slow dynamic response and integral saturation. S5. Integrate the modules of S1 to S4 to construct the ADRC control algorithm: map the jet reaction force into the joint torque compensation term through the Jacobian matrix and integrate it into the ADRC control law to achieve precise control of the angular displacement of the active joint of the robot arm.

2. The anti-interference motion control method for a ship rust removal manipulator arm that introduces jet reaction force according to claim 1 is characterized in that: In step S1, the calculation method of the magnitude of the water jet reaction force is as follows: the reaction force model of the water jet model is established by the law of conservation of momentum, and the jet thrust at the nozzle is related to the square term of the flow velocity according to the Bernoulli equation. Combined with Newton's third law, the reaction force of the jet is obtained. F jet for: ; in A is the nozzle cross-sectional area, ρ is the jet fluid density, u 0 is the flow rate at the nozzle outlet.

3. The anti-interference motion control method for a ship rust removal robot arm that introduces jet reaction force according to claim 1 is characterized in that: In step S1, the method for establishing the mechanical arm dynamics model is: definition d ( t ) are all interference terms, the dynamic model of the manipulator can be expressed as: ; Defining state variables x 1= q , , x 3= D ( t ), D ( t )=- M -1 d ( t ), we get the following equation of state: ; Jet reaction force F jet The external force acting on the end of the robotic arm can be expressed as follows according to the principle of virtual work: ; in, δx is the virtual displacement at the end, δq is the virtual displacement in the joint space, τ j T is the generalized force or moment in the joint space; The magnitude and direction of the jet reaction force always coincide with the three axes of the connecting rod. F jet It can be expressed as ； At this time, the state equation and dynamic model of the manipulator can be expressed as: ； 。 4. The anti-interference motion control method for a ship rust removal manipulator arm that introduces jet reaction force according to claim 1 is characterized in that: In step S2, the basic mathematical model of the tracking differentiator is: ； in: v 1 Tracking input signal r , v 2 Yes v The differential of 1, k v is the characteristic parameter of the tracking signal, sat (·) is a linear saturation function, and its expression is: ； Where: δ is a positive constant, sign (·) is the sign function; Euler method is used to approximate the slope of the current point to describe the function change of the next step, and the continuous system is transformed into a discrete form. The discrete algorithm is used to calculate the function of the given step size. T In the case of , the transition of the input signal and its differential signal can be expressed as: ； In the formula r(k) is the input signal, parameter k v is the speed factor, parameter h is the filter factor, step size T The size of determines the calculation accuracy and cost, nonlinear dynamic function fst(v 1 ,v 2 ,r,kv,h) for: ； Among them, the threshold α for: ； The relationship between the parameters is: ； in, k v and h Is a positive number.

5. The anti-interference motion control method for a ship rust removal manipulator arm that introduces jet reaction force according to claim 1 is characterized in that: In step S3, for a n Order nonlinear uncertain controlled object: , and establish its nonlinear extended state observer as follows: ； Where: d 0( t ) represents the external disturbance, b To control the amplification factor, u ( t ) is the system control quantity, g i (·) represents a nonlinear function, x ( t ) as the input of nonlinear expansion state observation, its state variables z i ( t ) will track the expanded state variables separately x (i-1) ( t ).

6. The anti-interference motion control method for a ship rust removal manipulator arm that introduces jet reaction force according to claim 5 is characterized in that: Model of the nonlinear extended state observer for the manipulator: Discretize it and get: ； z 1( k ), z 2( k )and z 3( k ) represent the output of the system estimated by the nonlinear extended state observation model q , the derivative of the output And extended state parameters such as disturbance, observation error Indicates the observed value at a certain iteration z 1( k ) and the actual output of the system q ( k ), β 1. β 2 and β 3 is the observer gain parameter, T is the time step.

7. The anti-interference motion control method for a ship rust removal manipulator arm that introduces jet reaction force according to claim 6 is characterized in that: The robot arm includes three joints, and the three joint errors constitute a nonlinear function fal i ( · ) is designed as follows: ； in, i =1, 2, 3; α i and δ i is a positive constant. After introducing the jet reaction force compensation, the nonlinear expansion state observation model of the manipulator is expressed as: 。 8. The anti-interference motion control method for a ship rust removal manipulator arm that introduces jet reaction force according to claim 1 is characterized in that: In step S4, the algorithm for constructing the nonlinear state error feedback is designed as follows: The manipulator system is designed as a second-order system, the input signal tracking differentiator is second-order, and the nonlinear extended state observation model is third-order. The closed-loop feedback error signal of the system obtained from the observation value is: Control quantity of the system u 0 and control rate τ Expressed as: ； Where: K P 、K D is a constant, α P 、 α D 、 δ P and δ P is an adjustable parameter, fal P (·), fal D (·) is a nonlinear function defined as: ； Discretize the second-order nonlinear state error feedback model: ； Among them, the regulation quantity output by the nonlinear state error feedback model controller is u 0( k ) due to the system error and The actual adjustment amount is obtained through nonlinear function τ ( k ) after the system internal nonlinear error and interference Received after compensation.

9. The anti-interference motion control method for a ship rust removal manipulator arm introducing a jet reaction force according to claim 8 is characterized in that: The observation error expression after introducing the jet reaction force compensation is: ; The interference terms are: , At this time, the control rate of the robotic arm system is expressed as: ; Where r3 is the differential of r2.

10. The anti-interference motion control method for a ship rust removal manipulator arm introducing a jet reaction force according to claim 9, characterized in that: In step S5, the construction method of the ADRC control algorithm is as follows: The discretized iterative calculation algorithm of the robotic arm system can be expressed as: ； Inputting parameter variables into the calculation algorithm can realize the control of the angular displacement of the active joint of the robot arm through programming; in T , time step; β1, β2, β3, ESO gain parameters; αi, δi (i=1, 2, 3), ESO positive constants; K P 、 K D 、 α P 、 α D 、 δ P 、 δ D , NLSEF parameters; r(t) ,input signal; d(t) , total system interference.

Images