A method for anti-interference motion control of a ship rust removal mechanical arm introducing fluid reaction force

Abstract

The application discloses a kind of ship rust removal mechanical arm anti-interference motion control methods of introducing jet reaction force, it is related to ship rust removal equipment control technical field.The method is aimed at the poor robustness of traditional PID control when ship rust removal mechanical arm faces water jet reaction force and other disturbances, integral term is easy to saturate, and the problem of poor dynamic response effect, proposes to introduce jet reaction force active disturbance rejection control (ADRC) scheme.Through the construction of jet reaction force model, it is mapped to joint torque compensation item based on Jacobian matrix and integrated into ADRC control law;Improved tracking differentiator, nonlinear extended state observer (ESO) and nonlinear state error feedback (NLSEF) module are designed, the real-time estimation and compensation of internal and external disturbances in the system are realized, and the joint control performance is optimized.Experiments show that the method can reduce the joint angular displacement tracking error by more than 10%, the adjustment time is shortened from 10s to 2s, the absolute value of transient error is less than 0.3 rad, and the anti-interference ability and control precision of the mechanical arm are significantly improved.

Description

Technical Field

[0001] This invention relates to the field of ship rust removal equipment control technology, specifically to an anti-interference motion control method for a ship rust removal robotic arm that incorporates jet reaction force. Background Technology

[0002] During long-term voyages, the underwater hull of ships, continuously immersed in seawater, suffers severe electrochemical corrosion and marine organism adhesion, leading to thinning of the hull steel plates, decreased structural strength, and even safety hazards such as localized perforations and fractures. Statistics show that ship corrosion increases navigation resistance by 15%-30% and fuel consumption by 10%-20%. Furthermore, corrosion of critical components such as propellers and rudders reduces ship maneuverability and propulsion efficiency, shortens the ship's lifespan, and, if rust causes hull damage, can lead to fuel or cargo leaks, causing serious pollution to the marine environment. Therefore, regularly cleaning rusted surfaces is crucial for ensuring safe navigation and reducing operating costs. Rust-removing robotic arms equipped with waterjet rust removal devices have become the mainstream equipment for ship rust removal operations due to their high efficiency and environmentally friendly rust removal characteristics.

[0003] During the actual operation of the ship rust removal robotic arm, the water jet rust removal device generates a significant water jet reaction force when spraying high-pressure water to remove rust. This reaction force strongly interferes with the movement accuracy of the robotic arm. At the same time, factors such as wave fluctuations, ship vibration, and the dynamic coupling and joint friction of the robotic arm itself further exacerbate the disturbance complexity of the system. Traditionally, the industry has mostly used PID (Proportional-Integral-Derivative) control methods to deal with the above-mentioned disturbances. However, this method has obvious shortcomings when facing the complex disturbance environment of marine rust removal robotic arms: First, it has poor robustness. When the robotic arm parameters fluctuate due to load changes, temperature effects, or external disturbances such as water jet reaction force and wave disturbances, the PID controller struggles to maintain stable control performance and may even experience system oscillations. Second, the integral term is prone to saturation. Due to the continuous action of the water jet reaction force, the integral term of the PID controller will accumulate continuously, leading to excessive control output, causing overshoot and jitter of the robotic arm joints, and making precise adjustment impossible. Third, it has poor dynamic response. The PID controller balances the system's speed and overshoot through a linear combination of proportional, integral, and derivative terms. However, the disturbances of marine rust removal robotic arms have time-varying and nonlinear characteristics. Simple linear combinations are insufficient to meet dynamic response requirements, resulting in long settling times and large tracking errors.

[0004] To address the problem of anti-interference control for robotic arms, numerous studies have been conducted and various improvement schemes have been proposed. Reference document 1 (CN201810425166.6) discloses an anti-interference iterative learning control method for a space robotic arm system used for capturing non-cooperative targets. This method estimates external disturbance torque by designing an interference observer, combines robust H∞ control to suppress internal noise and observation errors, and uses iterative learning control to correct trajectory tracking errors. However, this method is designed for non-cooperative target capture scenarios of space robotic arms, where interference mainly originates from the non-cooperative motion of the target spacecraft and disturbances in the space environment. It does not consider specific disturbances strongly correlated with the operation process, such as the reaction force of water jets. Furthermore, iterative learning control relies on multiple repeated operations to obtain error information, making it difficult to adapt to the needs of uneven rust distribution and dynamic adjustment of the operation path in ship rust removal operations, resulting in insufficient real-time performance.

[0005] Reference document 2 (CN201810984285.5) proposes an active disturbance rejection control method, device, and system. It employs a parallel multi-active disturbance rejection controller to achieve decoupled control of a multi-input multi-output system, combining a dynamic feedforward model and an extended observer to compensate for deterministic and uncertain disturbances, respectively. While this method incorporates the core concept of active disturbance rejection control, it is primarily applied to the general motion control of multi-joint industrial robots. It does not specifically model and compensate for the water jet reaction force of a ship rust removal robot arm, and its extended observer does not consider the mapping relationship between the end-effector force and joint torque. Therefore, when facing concentrated end-effector disturbances such as water jet reaction force, the disturbance estimation accuracy is low, and the anti-interference effect is limited.

[0006] Prior art document 3 (CN202311683137.7) discloses an adaptive integral robust control method for a hydraulic manipulator based on friction compensation. This method compensates for nonlinear friction in the system by establishing a continuous friction model and designs an adaptive law for integral robust gain to adjust the control gain online, thereby improving the system's tracking performance. The core of this method lies in friction compensation and integral robust control, focusing on solving the frictional nonlinearity and unmodeled disturbance problems of the hydraulic manipulator. However, it does not address the modeling and compensation of the water jet reaction force, and its controlled object is a hydraulically driven general-purpose manipulator. It does not consider the coupling characteristics between the manipulator and the water jet device in ship rust removal operations, making it unsuitable for direct application in ship rust removal scenarios.

[0007] Reference document 4 (CN202411019258.6) proposes an adaptive sliding mode control method for deep-sea hydraulic manipulators based on disturbance observation. This method estimates system disturbances and joint velocities using an adaptive extended state observer, and then uses an adaptive sliding mode controller to achieve trajectory tracking. While this method addresses the parameter indeterminacy and nonlinearity of deep-sea hydraulic manipulators, its disturbance observer primarily focuses on ocean current disturbances and hydraulic system parameter variations in the deep-sea environment. It does not construct a specific model for the water jet reaction force, and the sliding mode control is prone to chattering due to the sign function, which may lead to unstable joint movement of the manipulator during ship rust removal operations, affecting rust removal accuracy.

[0008] In summary, existing anti-interference control methods for robotic arms either fail to consider the reaction force of water jets due to differences in application scenarios, or their control strategies are limited and cannot balance real-time performance and anti-interference capabilities, making it difficult to meet the high-precision motion control requirements of marine rust removal robotic arms in complex and disturbed environments. Therefore, there is an urgent need for an anti-interference motion control method for marine rust removal robotic arms that can accurately model the reaction force of water jets, compensate for multi-source disturbances in real time, and optimize dynamic response performance. Summary of the Invention

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

[0010] To address the aforementioned problems, this invention proposes an Active Disturbance Rejection Control (ADRC) method for compensating for disturbances in a robotic arm by incorporating jet reaction force. This method improves the robotic arm's motion accuracy and robustness by real-time estimation and compensation of internal and external disturbances. Specifically addressing the disturbances encountered by a ship rust removal and cleaning robotic arm during operation, such as water jet reaction force and environmental disturbances, the method includes: 1. Effective compensation for water jet reaction force: Mapping the water jet reaction force into joint torque compensation terms based on the Jacobian matrix and integrating it into the ADRC control law to reduce the impact of disturbances on the robotic arm's motion. 2. Solving the problem of insufficient anti-disturbance capability of traditional PID control: By designing an Active Disturbance Rejection Controller (ADRC), utilizing its nonlinear extended state observer (ESO) to estimate and compensate for internal and external disturbances in real time, improving the control accuracy and stability of the robotic arm in complex disturbance environments. 3. Optimizing the joint control performance of the robotic arm: By improving the ESO's state update law and employing saturated nonlinear error feedback (NLSEF), suppressing spikes and jitter in the initial control phase, shortening the settling time, and improving dynamic response performance.

[0011] This invention provides an anti-interference motion control method for a ship rust removal robotic arm that incorporates jet reaction force, comprising the following steps:

[0012] S1. Construct a jet reaction force compensation module: Based on the nozzle structural parameters and operating parameters, calculate the magnitude of the water jet reaction force by combining the law of conservation of momentum and Bernoulli's equation, and establish a dynamic model of the robotic arm using the principle of virtual work.

[0013] S2. Design a tracking differentiator module: track the input signal to avoid uneven or discontinuous input to the system.

[0014] S3. Build a nonlinear extended state observer: to observe and compensate for unknown disturbances in the robotic arm system in real time, and reduce the impact of disturbances on the performance of the robotic arm system.

[0015] S4. Construct a nonlinear state error feedback control module: This enables the output of the robotic arm system to quickly track the set value, eliminating the need to accumulate errors through an integral element to eliminate steady-state errors, and avoiding slow dynamic response and integral saturation.

[0016] S5. Integrating modules from S1 to S4 to construct the ADRC control algorithm: The jet reaction force is mapped to a joint torque compensation term through the Jacobian matrix and integrated into the ADRC control law to achieve precise control of the active joint angular displacement of the robotic arm.

[0017] In step S1, the method for calculating the magnitude of the water jet reaction force is as follows: A water jet reaction force model is established using the law of conservation of momentum. Based on Bernoulli's equation, the jet thrust at the nozzle is related to the square of the velocity term. Combining this with Newton's third law, the reaction force of the jet is obtained. ;

[0018] in A The nozzle cross-sectional area is... ρ For the density of the jet fluid, u 0 It is the flow rate at the nozzle outlet.

[0019] Furthermore, in step S1, the method for establishing the dynamic model of the robotic arm is as follows:

[0020] definition d ( t If ) represents all disturbance terms, then the dynamic model of the robotic arm can be expressed as:

[0021] ;

[0022] Define state variables x 1= q , , x 3= D ( t ), D ( t )=- M-1 d ( t The following state equations are obtained:

[0023] ;

[0024] Jet reaction force F jet The external force acting on the end effector of the robotic arm can be expressed as follows according to the principle of virtual work:

[0025] ;

[0026] in, δx It is the virtual displacement at the end. δq It is a virtual displacement in the joint space. τ j T It is a generalized force or torque in the joint space;

[0027] The magnitude and direction of the jet reaction force always coincide with the three axes of the connecting rod. F jet It can be represented as:

[0028] ;

[0029] At this point, the state equation and dynamic model of the robotic arm can be expressed as follows:

[0030] ;

[0031] .

[0032] Furthermore, in step S2, the basic mathematical model of the tracking differentiator is:

[0033] ;

[0034] in: v 1. Tracking input signal r , v 2 is v The differential of 1, k v To track the characteristic parameters of the signal, sat (·) is a linear saturation function, and its expression is:

[0035] ;

[0036] In the formula: δ is a positive constant, and sign(·) is the sign function.

[0037] Using the Euler method, the slope at the current point is used to approximate the function change in the next step, transforming the continuous system into a discrete form. Using a discrete algorithm with a given step size T, the transition of the input signal and its differential signal can be expressed as:

[0038] ;

[0039] In the formula, r(k) is the input signal, kv is the velocity factor, h is the filter factor, and the step size T determines the calculation accuracy and computational cost. The nonlinear dynamic function fst(v1,v2,r,kv,h) is:

[0040] ;

[0041] Among them, threshold a for:

[0042] ;

[0043] The relationships between the parameters are as follows:

[0044] ;

[0045] Where kv and h are positive constants.

[0046] Furthermore, in step S3, for an nth-order nonlinear uncertain controlled object: The nonlinear extended state observer is established as follows:

[0047] ;

[0048] In the formula: d0(t) represents the external disturbance, b is the control amplification coefficient, u(t) is the system control quantity, gi(·) represents the nonlinear function, and x(t) is used as the input for the nonlinear extended state observation. Each of its state variables zi(t) will track the extended state variables x(i-1)(t) respectively. In particular, zn+1(t) is the extended state quantity used in active disturbance rejection control to capture unknown dynamics of the system, such as external disturbances and the unmodeled parts of the cleaning robot arm.

[0049] Furthermore, a model for the nonlinear extended state observer of the robotic arm:

[0050] Discretize the data to obtain:

[0051] ;

[0052] z1(k), z2(k), and z3(k) represent the system output q, the output derivative, and the output value, respectively, estimated by the nonlinear extended state observation model. And extended state parameters such as disturbances, observation errors This represents the difference between the observed value z1(k) and the system's true output q(k) in a certain iteration, where β1, β2, and β3 are the observer gain parameters, and T is the time step.

[0053] Furthermore, the robotic arm includes three joints, and the nonlinear function fali(·) formed by the errors of the three joints is designed in the following form:

[0054] ;

[0055] Where i = 1, 2, 3; αi and δi are positive constants. The observation model of the nonlinear expansion state of the robotic arm after introducing jet reaction force compensation is expressed as:

[0056] .

[0057] Furthermore, in step S4, the algorithm design for constructing the nonlinear state error feedback is as follows:

[0058] The robotic arm 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 observations is: The control quantity u0 and control law τ of the system are expressed as: ;

[0059] In the formula: KP and KD are constants, αP, αD, and δ D and δ P For adjustable parameters, falP(·) and falD(·) are nonlinear functions, defined as follows:

[0060] ;

[0061] Discretize the second-order nonlinear state error feedback model:

[0062] ;

[0063] In this context, the adjustment quantity u0(k) output by the nonlinear state error feedback model controller is determined by the system error. and The actual adjustment amount τ(k) is obtained through a nonlinear function, after passing through the system's internal nonlinear error. With interference It was obtained after compensation.

[0064] Furthermore, after incorporating jet reaction force compensation, the expression for the observation error is: The interference items are: At this point, the control rate of the robotic arm system is expressed as:

[0065] In the formula, r3 is the differential of r2.

[0066] Furthermore, in step S5, the method for constructing the ADRC control algorithm is as follows:

[0067] The discretized iterative calculation algorithm for the robotic arm system can be expressed as:

[0068] By inputting parameter variables into the calculation algorithm, the active joint angular displacement of the robotic arm can be controlled through programming.

[0069] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0070] 1) Significantly improved anti-interference capability:

[0071] By using a nonlinear extended state observer to estimate and compensate for disturbances such as water jet reaction force and robotic arm dynamic coupling in real time, the tracking error of joint angular displacement is reduced by more than 10%.

[0072] 2) Improved system response speed:

[0073] After adding jet reaction force compensation, the settling time of ADRC is shortened from 10s to 2s under jet reaction force disturbance of no more than 200N, and the initial peak amplitude is reduced by more than 15%.

[0074] 3) Improved motion control precision:

[0075] By replacing the traditional power function with saturated nonlinear error feedback, motion jitter is reduced, and the absolute value of the transient error of joint angular displacement is less than 0.3 rad. Attached Figure Description

[0076] Figure 1 This is a schematic diagram of the reaction force of a water jet.

[0077] Figure 2 This is a schematic diagram of a tracking differentiator.

[0078] Figure 3 This is a block diagram of a control system based on jet compensation.

[0079] Figure 4 This is a diagram illustrating the execution process of an S-function.

[0080] Figure 5 The output torque of each joint is determined by the disturbance.

[0081] Figure 6 This is to ensure that the angular displacement of each active joint follows the reaction force compensation of the waterless jet.

[0082] Figure 7 The angular displacement of each active joint follows the reaction force of the water jet after compensation.

[0083] Figure 8 The expected following error of joint 1 angular displacement after ADRC improvement.

[0084] Figure 9 Errors in the observed state and state variables of joint 1 after ADRC improvement.

[0085] Figure 10 Errors in the observed state and state variables of joint 1 after ADRC improvement.

[0086] Figure 11 The interference and interference estimation error of joint 1 after ADRC improvement.

[0087] Figure 12 The expected following error of the joint 2 angular displacement after ADRC improvement.

[0088] Figure 13 Errors in the observed state and state variables of joint 2 after ADRC improvement.

[0089] Figure 14 Errors in the observed state and state variables of joint 2 after ADRC improvement.

[0090] Figure 15 The interference and interference estimation error of joint 2 after ADRC improvement.

[0091] Figure 16 The expected following error of the joint 3-angle displacement after ADRC improvement.

[0092] Figure 17 Errors in the observed state and state variables of joint 3 after ADRC improvement.

[0093] Figure 18 Errors in the observed state and state variables of joint 3 after ADRC improvement.

[0094] Figure 19 The interference and interference estimation error of joint 3 after ADRC improvement. Detailed Implementation

[0095] The accompanying drawings illustrate the technical solutions of the embodiments of the present invention in more detail. Throughout the drawings, the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions. The described embodiments are some, but not all, embodiments of the present invention. The embodiments described below with reference to the accompanying drawings are exemplary and intended to explain the present invention, and should not be construed as limiting the present invention. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are within the scope of protection of the present invention. The embodiments of the present invention will now be described in detail with reference to the accompanying drawings.

[0096] It should be noted that if the embodiments of this application involve directional indicators (such as up, down, left, right, front, back, etc.), the directional indicators are only used to explain the relative positional relationship and movement of the components in a certain specific posture (as shown in the figure). If the specific posture changes, the directional indicators will also change accordingly.

[0097] Furthermore, if the embodiments of this application involve descriptions such as "first" or "second," these descriptions are for descriptive purposes only and should not be construed as indicating or implying their relative importance or implicitly specifying the number of technical features indicated. Therefore, features defined with "first" or "second" may explicitly or implicitly include at least one of those features. Additionally, the technical solutions of various embodiments can be combined with each other, but this must be based on the ability of those skilled in the art to implement them. If the combination of technical solutions is contradictory or impossible to implement, it should be considered that such a combination of technical solutions does not exist and is not within the scope of protection claimed in this application.

[0098] Example 1

[0099] This embodiment provides an anti-interference motion control method for a ship rust removal robotic arm that incorporates jet reaction force, including the following steps:

[0100] S1. Construct a jet reaction force compensation module: Based on the nozzle structural parameters and operating parameters, calculate the magnitude of the water jet reaction force by combining the law of conservation of momentum and Bernoulli's equation, and establish a dynamic model of the robotic arm using the principle of virtual work.

[0101] The method for calculating the magnitude of the water jet reaction force is as follows: A water jet reaction force model is established using the law of conservation of momentum. Based on Bernoulli's equation, the jet thrust at the nozzle is related to the square of the flow velocity. Combining this with Newton's third law, the reaction force of the jet is obtained. F jet for: ;in A The nozzle cross-sectional area is... ρ For the density of the jet fluid, u 0 It is the flow rate at the nozzle outlet.

[0102] The method for establishing the dynamic model of the robotic arm is as follows:

[0103] definition d ( t If ) represents all disturbance terms, then the dynamic model of the robotic arm can be expressed as:

[0104] ;

[0105] Define state variablesx 1= q , , x 3= D ( t ), D ( t )=- M -1 d ( t The following state equations are obtained:

[0106] ;

[0107] Jet reaction force F jet The external force acting on the end effector of the robotic arm can be expressed as follows according to the principle of virtual work:

[0108] ;

[0109] in, δx It is the virtual displacement at the end. δq It is a virtual displacement in the joint space. τ j T It is a generalized force or torque in the joint space;

[0110] The magnitude and direction of the jet reaction force are as follows: Figure 1 As shown, its direction always coincides with the three axes of the connecting rod. F jet It can be represented as:

[0111] ;

[0112] At this point, the state equation and dynamic model of the robotic arm can be expressed as follows:

[0113] ;

[0114] .

[0115] S2. Design a tracking differentiator module: track the input signal to avoid uneven or discontinuous input to the system; combined with... Figure 2 As shown, in step S2, the basic mathematical model of the tracking differentiator is:

[0116] ;

[0117] in: v 1. Tracking input signal r , v 2 is v The differential of 1, k vTo track the characteristic parameters of the signal, sat (·) is a linear saturation function, and its expression is:

[0118] ;

[0119] In the formula: δ is a positive constant, and sign(·) is the sign function.

[0120] Using the Euler method, the slope at the current point is used to approximate the function change in the next step, transforming the continuous system into a discrete form. Using a discrete algorithm with a given step size T, the transition of the input signal and its differential signal can be expressed as:

[0121] ;

[0122] In the formula, r(k) is the input signal, kv is the velocity factor, h is the filter factor, and the step size T determines the calculation accuracy and computational cost. The nonlinear dynamic function fst(v1,v2,r,kv,h) is:

[0123] ;

[0124] Among them, threshold a for:

[0125] ;

[0126] The relationships between the parameters are as follows:

[0127] ;

[0128] Where kv and h are positive constants.

[0129] S3. Construct a nonlinear extended state observer: This allows for real-time observation and compensation of unknown disturbances in the robotic arm system, reducing the impact of disturbances on the system's performance. For an nth-order nonlinear uncertain controlled object: The nonlinear extended state observer is established as follows:

[0130] ;

[0131] In the formula: d0(t) represents the external disturbance, b is the control amplification coefficient, u(t) is the system control quantity, gi(·) represents the nonlinear function, and x(t) is used as the input for the nonlinear extended state observation. Each of its state variables zi(t) will track the extended state variables x(i-1)(t) respectively. In particular, zn+1(t) is the extended state quantity used in active disturbance rejection control to capture unknown dynamics of the system, such as external disturbances and the unmodeled parts of the cleaning robot arm.

[0132] Model for the nonlinear extended state observer of the robotic arm:

[0133] Discretize the data to obtain:

[0134] ;

[0135] z1(k), z2(k), and z3(k) represent the system output q, the output derivative, and the output value, respectively, estimated by the nonlinear extended state observation model. And extended state parameters such as disturbances, observation errors This represents the difference between the observed value z1(k) and the system's true output q(k) in a certain iteration, where β1, β2, and β3 are the observer gain parameters, and T is the time step.

[0136] The robotic arm comprises three joints, and the nonlinear function fali(·) formed by the errors of the three joints is designed in the following form:

[0137] ;

[0138] Where i = 1, 2, 3; αi and δi are positive constants. The observation model of the nonlinear expansion state of the robotic arm after introducing jet reaction force compensation is expressed as:

[0139] .

[0140] S4. Construct a nonlinear state error feedback control module: This enables the output of the robotic arm system to quickly track the set value, eliminating the need to accumulate errors through an integral element to eliminate steady-state errors, and avoiding slow dynamic response and integral saturation.

[0141] In step S4, the algorithm design for constructing the nonlinear state error feedback is as follows:

[0142] The robotic arm 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 observations is: The control quantity u0 and control law τ of the system are expressed as: ;

[0143] In the formula: KP and KD are constants, αP, αD, and δ D and δ P For adjustable parameters, falP(·) and falD(·) are nonlinear functions, defined as follows:

[0144] ;

[0145] Discretize the second-order nonlinear state error feedback model:

[0146] ;

[0147] In this context, the adjustment quantity u0(k) output by the nonlinear state error feedback model controller is determined by the system error. and The actual adjustment amount τ(k) is obtained through a nonlinear function, after considering the system's internal nonlinear error. With interference It was obtained after compensation.

[0148] Furthermore, after incorporating jet reaction force compensation, the expression for the observation error is: The interference items are: At this point, the control rate of the robotic arm system is expressed as:

[0149] In the formula, r3 is the differential of r2.

[0150] S5. Integrating modules S1~S4 to construct the ADRC control algorithm: The jet reaction force is mapped to a joint torque compensation term through a Jacobian matrix and integrated into the ADRC control law to achieve precise control of the active joint angular displacement of the robotic arm. After adding water jet compensation, the control power of the system increases. After the water jet reaction force is transformed by the Jacobian determinant matrix vector transformation, the control quantity τ of the closed-loop system adds a JMT link to transmit the reaction force, which can reduce the difference between the system output and the desired signal, thereby reducing the control difficulty of the ADRC adjustment closed-loop system. The system block diagram is as follows. Figure 3 As shown.

[0151] The method for constructing the ADRC control algorithm is as follows:

[0152] The discretized iterative calculation algorithm for the robotic arm system can be expressed as:

[0153] By inputting parameter variables into the calculation algorithm, the active joint angular displacement of the robotic arm can be controlled through programming.

[0154] Example 2

[0155] This embodiment is an implementation of the method according to Embodiment 1.

[0156] 1. Simulation system construction:

[0157] The basic functions of S-function are used for different stages of the discretization process, such as... Figure 4 As shown, this includes initialization, update, output, and status. The ADRC design... fal (·) The instantiation steps of the function pseudocode execution process are shown in Table 1:

[0158] Table 1 fal (·) Function pseudocode

[0159]

[0160] Furthermore, the pseudocode for the specific discretization algorithm implementation of ESO is shown in Table 2:

[0161] Table 2 Pseudocode of ESO Function

[0162]

[0163] 2. Jet reaction force compensation

[0164] With the desired angular displacement of each joint set to [π / 3 rad π / 4 rad π / 6 rad], and the external jet reaction force of the system set to 200 N, simulation can obtain the disturbance torque of each joint of the cleaning robot arm. For example... Figure 4 As shown, when the reaction force of the water jet is vector-transformed through the Jacobian determinant and applied to the dynamic numerical theoretical model, the output torque of the angular displacement of each joint of the cleaning robot arm changes. It can be seen that the interference signal affects the joint control of the system.

[0165] like Figure 5 During the time-varying process of the angular displacement of each joint following the expected value, combined with Figures 6-7 As shown, before the ADRC was compensated for the reaction force of the water jet, the adjustment time was about 10 seconds and the initial jitter peak of the follow curve was relatively high. After the ADRC was compensated for the reaction force, the adjustment time was about 2 seconds and the initial peak size of the follow curve was reduced.

[0166] 3. ADRC Motion Control

[0167] Based on the designed Active Disturbance Rejection Controller (ADRC), different types of desired signals and total system disturbances (including but not limited to mechanical vibration, sensor noise, and voltage fluctuations) are applied to the three joints. The analysis of the first joint... n Angular displacement output of each joint y n Observer observation status z n1 Observer observation status z n2 With observer interference estimate - Mz n3 Compared to expected value r n System state variables x n1 System state variables x n2 and total system interference signal d ( t Tracking progress over time. The simulation step size is uniformly set to... T =0.01 s, improved parameters ε Set to 0.82, δ Set to 1.05, for the first n Nonlinear Extended State Observer (ESO) for ADRC of each joint δ n The values ​​are uniformly set to (2.00, 2.00, 2.00). Other parameter settings are shown in Table 3.

[0168] Table 3 Nonlinear ESO Parameter Settings

[0169]

[0170] For joint 1, the nonlinear controller (NLSEF) parameters are set as follows: K P =400、 K D = 80.6 α P =0.95、 α D =0.98、 δ P =3.5、 δ D =3.5.

[0171] Depend on Figure 8 As shown, the angular displacement of joint 1 y 1. Can follow the desired signal well r 1. Initially, spikes and fluctuations are not obvious, the error is small, and the settling time is less than 1 second. Figure 9 The observations of the state observer z 11 It can observe state variables relatively well. x 11 The value of is small, and the difference is small. (From) Figure 10 The observations of the state observer z 12 With observed state variables x 12 There is a small difference. (From...) Figure 11 Estimated system interference - Mz 13 Interference with the system d ( t The difference shows a small peak value in the initial stage, without obvious oscillation, and the absolute value of the subsequent difference is less than 0.3 rad.

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

[0173] Depend on Figure 12 As shown, the angular displacement of joint 2 y 2. Can follow the desired signal well r 2. In the initial period, there was no obvious jitter, the difference was small, and the adjustment time was less than 1 second. Figure 13 As shown, the observations of the state observer z 21 It can observe state variables relatively well. x 21 The value of . (By ) Figure 14 As shown, the observations of the state observer z 22 With observed state variables x 22 The difference is small, and there is fluctuation in the initial stage. Figure 15 As shown, the estimated system interference - Mz 23 Interference with the system d ( t The difference between the two values ​​shows little oscillation in the initial stage, and the error is small in the subsequent stage.

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

[0175] Depend on Figure 16 As shown, the angular displacement of joint 3 y 3. In the expected signal r 3. In cases including step phases, the follow curve shows no significant jitter, and the steady-state difference is small in each step change phase, with an average settling time of approximately 0.5 s. Figure 17 The observations of the state observer z 31It can observe state variables relatively well. x 31 The value has a small error. Figure 18 The observations of the state observer z 32 With observed state variables x 32 The difference is large and oscillates significantly in the initial stage, but its duration is short. Figure 19 Estimated system interference - Mz 33 Interference with the system d ( t The difference in the initial stage does not oscillate violently and has no obvious peaks, and the difference is small in the subsequent stage.

[0176] The improved active disturbance rejection controller reduces the spikes and jitter in the angular displacement output curve, especially at joints 1 and 3 where the desired signal changes at high frequencies and the system interference signal at high frequencies. At the same time, the interference estimation error of the nonlinear state observer is smaller.

[0177] The above embodiments are merely illustrative of the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to the preferred embodiments above, those skilled in the art should understand that modifications or equivalent substitutions to the technical solutions of the present invention should not depart from the spirit and scope of the present invention. Those skilled in the art can also make other changes within the spirit of the present invention and use them in the design of the present invention, as long as they do not deviate from the technical effects of the present invention. These changes made according to the spirit of the present invention should all be included within the scope of protection claimed by the present invention.

Claims

1. A method of anti-jamming motion control of a ship rust removal robotic arm introducing fluidic reaction forces, characterized by, The method comprises the following steps: S1, constructing 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 size of water jet reaction force, and using the principle of virtual work to establish a mechanical arm dynamics model; S2, designing a tracking differentiator module: tracking the input signal to avoid the phenomenon of unsmooth or discontinuous system input; S3, building a nonlinear extended state observer: observing and compensating the unknown disturbance of the mechanical arm system in real time, and reducing the influence of the disturbance on the performance of the mechanical arm system; S4, constructing a nonlinear state error feedback control module: making the output of the mechanical arm system quickly track the set value, without accumulating error to eliminate steady-state error through an integral element, avoiding slow dynamic response and integral saturation phenomenon; S5, integrating the modules of S1-S4 to construct an ADRC control algorithm: mapping the water jet reaction force to the joint torque compensation item through the Jacobian matrix and integrating it into the ADRC control law to realize precise control of the active joint angular displacement of the mechanical arm.

2. The anti-disturbance motion control method for the ship rust removal mechanical arm introducing fluidic reaction force according to claim 1, characterized in that, The water jet reaction force size calculation method in the step S1 is: a water jet reaction force model is established through the momentum conservation law, the jet thrust at the nozzle is related to the square term of the flow velocity according to the Bernoulli equation, and the jet reaction force is obtained in combination with the Newton's third law F jet is: ; wherein A is the nozzle cross-sectional area, The step S1, the method for establishing the mechanical arm dynamics model is: is the jet fluid density, u 0 is the flow rate at the nozzle exit.

3. The anti-disturbance motion control method for ship rust removal mechanical arm with introduced fluidic reaction force according to claim 1, characterized in that, The state equation and the mechanical arm dynamics model of the mechanical arm at this time can be represented as: Definitions d ( t ) For all interference terms, the manipulator dynamics model can be expressed as: ； Defining state variables x 1= q , , x 3= D ( t ), D ( t )= M -1 d ( t ), resulting in the following state equations:​​​ ； Reaction force of the fluidic jet F jet The external force acting on the end of the robot arm can be expressed according to the principle of virtual work as: ； wherein, The basic mathematical model of the tracking differentiator in the step S2 is: is the virtual displacement of the end-effector, The parameter relationship is: is the virtual displacement of the joint space, The model of the nonlinear extended state observer for the mechanical arm is: j T is the generalized force or moment of the joint space; The size and direction of the reaction force of the jet always coincide with the three axes of the connecting rod, F jet may be expressed as ； Discretization is performed to obtain: ； 。 4. The anti-disturbance motion control method for ship rust removal mechanical arm with introduced fluidic reaction force according to claim 1, characterized in that, The algorithm design for constructing the nonlinear state error feedback in the step S4 is as follows: ； where: v 1 tracking input signal r , v 2 is v 1 derivative, k v characteristic parameter of the tracking signal, Discretization is performed on the second-order nonlinear state error feedback model to obtain: (·) is a linear saturation function whose expression is: ； wherein: The control rate of the mechanical arm system at this time is represented as: is a normal number, In the formula, r3 is the differential of r2. (·) is a sign function; The Euler method is used to describe the function change of the next step by the slope of the current point, and the continuous system is converted into a discrete form T The transition of the input signal and its differential signal can be expressed as: ； wherein 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 calculation cost, the nonlinear dynamic function The construction method of the ADRC control algorithm in the step S5 is as follows: 1 ,v 2 ,r,k v ,h) is: ； wherein the threshold value a is: ； The discretization iterative calculation algorithm of the mechanical arm system can be represented as: ； wherein k v with h is a normal number.

5. The anti-disturbance motion control method for ship rust removal mechanical arm with introduced fluidic reaction force according to claim 1, characterized in that, The step S3 is performed for one n a nonlinear uncertain controlled object: The nonlinear extended state observer is established as follows: ； wherein: d 0( t ) denotes an external disturbance, b is a control amplification factor, u t is a system control variable, g i (·) denotes a nonlinear function, which takes x t as an input of a nonlinear extended state observation, whose each state variable z i t will track the extended state variable x (i-1) t respectively.​​​​ 6. The anti-disturbance motion control method of the ship rust removal mechanical arm introducing fluidic reaction force according to claim 5, characterized in that, The input of the parameter variable into the calculation algorithm can realize the control of the active joint angular displacement of the mechanical arm through programming; ； The parameter variable input calculation algorithm can realize the control of the active joint angular displacement of the mechanical arm through programming; ； z 1( k ), z 2( k )and z 3( k ) represent the outputs of the system estimated by the nonlinear extended state observation model, respectively. q The derivative of the output And the perturbation spread state parameters, observation error Represents the observed value in a certain iteration. z 1( k ) and the actual output of the system q ( k The difference between ) β 1. β 2 and β 3 represents the observer gain parameter. T For time step.

7. A method of anti-windup motion control for a ship rust removal robotic arm introducing fluidic reaction forces according to claim 6, characterized in that, The robot arm comprises three joints, the three joint errors constituting a non-linear function d(t) i ( · ) is designed in the form of: ; wherein, i = 1, 2, 3; α i with ​ i is a positive constant, the nonlinear extended state observer model of the manipulator after introducing the jet reaction force compensation is expressed as: 。 8. The anti-disturbance motion control method of ship rust removal mechanical arm introducing fluidic reaction force according to claim 1, characterized in that, ​ The mechanical arm system is designed as a second order system, the input signal tracking differentiator is a second order, the nonlinear extended state observation model is a third order, and the closed loop feedback error signal of the system obtained from the observation value is: The control amount of the system u 0 and the control rate ​ is represented as: ; wherein: K P 、K D is a constant, α P , α D , ​ D and ​ P is an adjustable parameter, ​ P (·), ​ D (·) is a non-linear function defined as: ； wherein: K P 、K D is a constant, α P , α D , ​ D and ​ P is an adjustable parameter, ​ P (·), ​ D (·) is a non-linear function defined as: ； ​ ； Wherein, the adjustment outputted by the nonlinear state error feedback model controller u 0( k ) is obtained by the error of the system With the actual adjustment outputted by the nonlinear function ​ ( k ) is obtained by the error of the system internal nonlinear With disturbance compensation.

9. A method of anti-windup motion control for a ship rust removal robotic arm introducing fluidic reaction forces according to claim 8, characterized in that, The expression of the observation error after introducing the jet reaction force compensation is: ; The interference term is: , ​ ； ​ 10. A method of anti-windup motion control for a ship rust removal robotic arm introducing fluidic reaction forces according to claim 9, characterized in that, ​ ​ ； ​ wherein T , time step; β1, β2, β3, ESO gain parameters; ai, δi, i = 1, 2, 3, ESO normal numbers; K P , K D , α P , α D , ​ P , ​ D , NLSEF parameters; r(t) , input signal; ​ , total system interference.

Images