Underwater manipulator trajectory tracking control method and system based on disturbance observer

Abstract

The application discloses a kind of underwater manipulator trajectory tracking control method and system based on interference observer, comprising: constructing non-singular fast terminal sliding mode surface, guide manipulator joint position tracking error converges in limited time quickly, and avoid singularity.Introduce super-twisting algorithm to reach control law in the sliding mode reaching stage, weaken sliding mode chattering to optimize reaching process.Nonlinear interference observer is designed to estimate superimposed disturbance in real time, and the estimated value is fed forward to the global control law.The application realizes high-precision, high-robustness trajectory tracking of joint under superimposed disturbance without changing the standard dynamics model of manipulator through the triple coordination of non-singular fast terminal sliding mode surface, super-twisting algorithm and nonlinear interference observer.At the same time, the control performance of the system under complex ocean disturbance is significantly enhanced, and it is suitable for deep sea exploration, submarine pipeline maintenance, underwater salvage and other ocean engineering operations.

Description

Technical Field

[0001] This invention relates to the field of marine robot and underwater operation control technology, and in particular to an underwater robotic arm trajectory tracking control method and system based on an interference observer. Background Technology

[0002] With the increasing demand for marine resource development and deep-sea exploration, underwater robotic arms play an irreplaceable role in tasks such as subsea oil and gas pipeline inspection, deep-sea mining, shipwreck salvage, and scientific sampling. However, the working environment of underwater robotic arms is harsh. Deep-sea currents exert time-varying and unevenly distributed resistance and torque. Underwater pressure varies with depth and fluctuates, and the robotic arm itself may encounter sudden obstacle impacts while operating on the seabed. These factors lead to significant uncertainties in the model parameters of the robotic arm (such as buoyancy, added mass, and damping coefficient), making it difficult for traditional control methods to achieve high-precision trajectory tracking. Existing robotic arm control methods, such as PID control, adaptive control, and standard sliding mode control, are mostly designed for terrestrial or laboratory environments and do not fully consider the effects of ocean current resistance, underwater added mass, and random impacts. In particular, although traditional sliding mode control is robust, it suffers from long convergence times and large chattering problems. While terminal sliding mode can converge in a finite time, it is prone to singularity issues. In addition, disturbance observers are mostly used to estimate constant values ​​or slowly varying disturbances, and have limited ability to estimate high-frequency random components in deep-sea currents. Summary of the Invention

[0003] This invention provides an underwater robotic arm trajectory tracking control method and system based on a disturbance observer, to overcome the technical problems of poor tracking accuracy and slow convergence speed of existing underwater robotic arm control methods under conditions of random ocean current disturbance and water pressure fluctuation.

[0004] To achieve the above objectives, the technical solution of the present invention is as follows: The underwater robotic arm trajectory tracking control method based on interference observers includes: S1: Establish a dynamic model of the robotic arm that considers superimposed disturbances, including external environmental disturbances and uncertainties in the hydrodynamic model parameters; S2: Calculate the robot arm joint position tracking error, and design a non-singular fast end-effector sliding surface to guide the robot arm joint position tracking error to converge rapidly within a finite time, including: The desired trajectory of the robotic arm is set as follows: ; in, These are the expected trajectories of the first and second joints in the robotic arm, respectively. The tracking error is calculated based on the desired trajectory. , represented as: ,in, For the desired joint position of the robotic arm, This refers to the actual position of the robotic arm joints; Based on the tracking error, the non-singular fast terminal sliding surface is designed as follows: ; in, For design parameters, , The ratio is a positive odd number and satisfies and ; S3: The arrival control law is designed using the super-twisting algorithm to reduce sliding mode chattering during the sliding mode arrival stage; The arrival control law is expressed as follows: ; in, For the time-order first derivative of the sliding mode of a nonsingular fast terminal, denoted as . For the super-twisted gain parameter, Represents a symbolic function. This is the integral compensation term built into the super-twisting algorithm. The integral compensation term built into the super-twisting algorithm The derivative with respect to time; S4: Design a nonlinear disturbance observer for real-time estimation of the superimposed disturbance, denoted as: ; in, For nonlinear disturbance observer auxiliary variables, For the derivative of the auxiliary variable of the nonlinear disturbance observer with respect to time, The observer gain matrix is... , This is the superimposed disturbance estimate output by the nonlinear disturbance observer; For control input; S5: Combining the non-singular fast terminal sliding surface, arrival control law, and nonlinear disturbance observer, design a global control law to obtain the final control command, and realize trajectory tracking of the robotic arm joints under ocean disturbance based on the final control command. The global control law is expressed as follows: ; in, For the robotic arm's inertia matrix, The matrix of Coriolis force and centrifugal force. This is the gravitational torque.

[0005] Furthermore, the established dynamic model of the robotic arm considering superimposed disturbances is expressed as follows: ; in, These are the actual velocity and actual acceleration vectors of the robotic arm joints, respectively. External superimposed disturbance; The parameters of the robotic arm's dynamics model are defined as follows: ; in: , , , These are the inertia matrices. Element; , 、 、 The matrices of Coriolis force and centrifugal force are respectively. Element; and These are the gravity vectors. Element; ; in: … These are the model constants derived based on the mass and length of the robotic arm links; h Solve for intermediate variables for the Coriolis force and centrifugal force matrices; and These are the first joint angle and the second joint angle of the robotic arm, respectively. and These are the angular velocities of the first and second joints, respectively.

[0006] An underwater robotic arm trajectory tracking control system, used to implement an underwater robotic arm trajectory tracking control method, includes: The robotic arm dynamics model building module is used to build a robotic arm dynamics model that considers superimposed disturbances; Error calculation module, used to calculate the tracking error of the robotic arm joint position; A sliding surface construction module is used to design a non-singular fast terminal sliding surface that guides the joint position tracking error of the robotic arm to converge rapidly within a finite time. The arrival control law design module is used to design arrival control laws using the super-twisting algorithm. The nonlinear disturbance observer design module is used to design nonlinear disturbance observers. The global control law design module is used to combine the non-singular fast terminal sliding surface, arrival control law and nonlinear disturbance observer to design the global control law, and then obtain the final control command. The actuator module is used to convert the final control command into joint driving torque to achieve trajectory tracking of the robotic arm joints under ocean disturbance.

[0007] Beneficial Effects: This invention addresses the problem of superimposed disturbances encountered by subsea robotic arms during deep-sea operations by proposing a composite control scheme integrating a nonlinear disturbance observer and a nonsingular fast terminal sliding mode. First, a nonsingular fast terminal sliding mode surface is constructed to guide the robotic arm's joint position tracking error to converge rapidly within a finite time, avoiding singularity. During the sliding mode arrival phase, a super-twisting algorithm is introduced to design the arrival control law, reducing sliding mode chattering and thus optimizing the arrival process. Simultaneously, a nonlinear disturbance observer is designed to estimate the superimposed disturbances in real time, and the estimated values ​​are fed forward to compensate the global control law. Without altering the standard dynamic model of the robotic arm, this invention achieves high-precision and robust trajectory tracking of joints under superimposed disturbances (including ocean current impact, water pressure changes, and uncertainties in hydrodynamic parameters) through the triple synergy of the nonsingular fast terminal sliding mode surface, the super-twisting algorithm, and the nonlinear disturbance observer. Furthermore, it significantly enhances the system's control performance under complex ocean disturbances, making it suitable for marine engineering operations such as deep-sea exploration, subsea pipeline repair, and underwater salvage. Attached Figure Description

[0008] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0009] Figure 1 This is a flowchart of an underwater robotic arm trajectory tracking control method based on an interference observer according to the present invention; Figure 2 This is a schematic diagram of the dynamics of a two-degree-of-freedom planar robotic arm in an embodiment of the present invention; Figure 3 and Figure 4 A comparison curve of joint position tracking under equivalent ocean current disturbance underwater between the method proposed in this embodiment of the invention and existing methods; Figure 5 and Figure 6 This is a comparison curve of joint velocity tracking between the method proposed in this embodiment of the invention and existing methods; Figure 7 and Figure 8 This is a control input comparison curve diagram in an embodiment of the present invention; Figure 9 and Figure 10 This is a comparison chart of the disturbance estimation effects in the embodiments of the present invention; Figure 11 This is a comparison diagram of the end effector trajectory tracking of the robotic arm in an embodiment of the present invention. Detailed Implementation

[0010] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0011] This embodiment provides a method for tracking and controlling the trajectory of an underwater robotic arm based on an interference observer, such as... Figure 1 As shown, it includes: S1: Establish a dynamic model of the robotic arm that considers superimposed disturbances, including external environmental disturbances and uncertainties in the hydrodynamic model parameters; Specifically, the external environmental disturbances include the impact force of ocean currents and the additional loads caused by changes in water pressure; the uncertainty disturbances of the hydrodynamic model parameters refer to the deviation disturbances between the actual values ​​and the theoretical model values ​​of key hydrodynamic parameters such as additional mass and fluid damping, which are difficult to calibrate accurately.

[0012] S2: Calculate the tracking error of the robotic arm joint position and design a non-singular fast terminal sliding surface to guide the tracking error of the robotic arm joint position to converge rapidly within a finite time. Specifically, the non-singular fast terminal sliding surface can ensure that the tracking error of the robotic arm joint position converges to the equilibrium point within a finite time and avoids singularity problems, making it suitable for the rapid suppression of sudden disturbances in deep-sea operations.

[0013] S3: The arrival control law is designed using the super-twisting algorithm to reduce sliding mode chattering during the sliding mode arrival stage, thereby optimizing the sliding mode arrival process; Specifically, the super-twisted algorithm, as a second-order sliding mode control method, effectively suppresses the chattering phenomenon of traditional sliding mode control by introducing an integral term, while ensuring that the sliding mode variables and their derivatives converge to zero in a finite time, thereby enhancing the control performance in the arrival stage and adapting to the random fluctuation characteristics of ocean currents.

[0014] S4: Design a nonlinear disturbance observer for real-time estimation of the superimposed disturbance; Specifically, the nonlinear disturbance observer introduces auxiliary variables and nonlinear functions to achieve rapid and accurate estimation of the disturbances caused by the uncertainty of ocean current resistance, water pressure fluctuations and hydrodynamic model parameters, and performs feedforward compensation on the control law based on the estimated values, effectively reducing the impact of disturbances on the system.

[0015] S5: Combining the non-singular fast terminal sliding surface, arrival control law, and nonlinear disturbance observer, design a global control law to obtain the final control command, and realize high-precision and high-robust trajectory tracking of the robotic arm joints under complex ocean disturbances based on the final control command.

[0016] Specifically, this embodiment accelerates the convergence speed of system errors by using a non-singular fast terminal sliding surface, suppresses and controls chattering by employing a super-twisting algorithm, and compensates for ocean current disturbances by using a nonlinear disturbance observer; the three work together to effectively improve the trajectory tracking performance of the underwater robotic arm under complex deep-sea conditions.

[0017] In a specific embodiment, the established robotic arm dynamics model considering superimposed disturbances is expressed as follows: (1) in, These represent the actual position, actual velocity, and actual acceleration vector of the robotic arm joints, respectively. For the robotic arm's inertia matrix, The matrix of Coriolis force and centrifugal force. For gravitational torque, To control the input, This is due to external superimposed disturbances; The parameters of the robotic arm's dynamics model are defined as follows: (2) in: , , , These are the inertia matrices. Element; , 、 、 The matrices of Coriolis force and centrifugal force are respectively. Element; and These are the gravity vectors. The amount; ; in: … These are the model constants derived based on the mass and length of the robotic arm links; h Solve for intermediate variables for the Coriolis force and centrifugal force matrices; and These are the rotation angles of the first and second joints of the robotic arm, respectively. and These are the angular velocities of the first and second joints, respectively.

[0018] Specifically, such as Figure 2 As shown, O-xy It is a plane rectangular coordinate system. and The length of the link. and This is the equivalent mass of the connecting rod.

[0019] Specifically, in this embodiment, the parameters of the robotic arm dynamics model are set to: The internal uncertainty is set to ±20% of the nominal value to account for random fluctuations.

[0020] The external superposition perturbation is defined as follows: (3) Specifically, the superimposed disturbance signal corresponds to the equivalent torque of the periodic ocean currents on the seabed (frequency approximately 0.3~0.5Hz).

[0021] In a specific embodiment, the robot arm joint position tracking error is calculated, and a non-singular fast end-of-state sliding surface is designed to guide the robot arm joint position tracking error to converge rapidly within a finite time, including: The desired trajectory of the robotic arm is set as follows: ; in, These are the expected trajectories of the first and second joints in the robotic arm, respectively. Specifically, in this embodiment, the following is set: and They are respectively: (4) In this embodiment, the initial positions of the first and second joints of the robotic arm are set as follows: The initial velocities of the first and second joints of the robotic arm are set as follows: .

[0022] The tracking error is calculated based on the desired trajectory. , represented as: ,in, For the desired trajectory; Based on the tracking error, the non-singular fast terminal sliding surface is designed as follows: (5) in, For design parameters, , The ratio is a positive odd number and satisfies and .

[0023] In this embodiment, the parameter values ​​are: .

[0024] In a specific embodiment, the arrival control law designed using the super-twisting algorithm is expressed as follows: (6) in, For the time-order first derivative of the sliding mode of a nonsingular fast terminal, denoted as . For the super-twisted gain parameter, Represents a symbolic function. This is the integral compensation term built into the super-twisting algorithm. The integral compensation term built into the super-twisting algorithm The derivative with respect to time.

[0025] Specifically, the super-twisting algorithm can ensure sliding mode variables Its derivative converges to zero within a finite time, effectively suppressing chattering caused by sensor noise during seabed operations.

[0026] In this embodiment, the parameters of the super-twist algorithm are set as follows: .

[0027] In a specific embodiment, the designed nonlinear disturbance observer is represented as follows: (7) in, For nonlinear disturbance observer auxiliary variables, For the derivative of the auxiliary variable of the nonlinear disturbance observer with respect to time, The observer gain matrix is... , This is the superimposed disturbance estimate output by the nonlinear disturbance observer.

[0028] Specifically, in this embodiment, the following is set: By using a nonlinear disturbance observer, it can be ensured that the observer error dynamically satisfies the requirements. , This represents the observer error; the observer error can converge exponentially to zero, enabling rapid tracking of ocean current disturbances.

[0029] In a specific embodiment, combining the nonsingular fast terminal sliding surface, the arrival control law, and the nonlinear disturbance observer, the designed global control law is expressed as follows: (8) Specifically, this embodiment proves the stability of the closed-loop system using Lyapunov theory, ensuring that the tracking error asymptotically converges to zero, including: constructing the Lyapunov function. Taking its derivative, we get According to Lyapunov stability theory, the system state asymptotically converges to the equilibrium point.

[0030] Specifically, to verify the effectiveness of the method (NDOB-NFTSMC) proposed in this embodiment, the following simulation experiments were conducted in the MATLAB / Simulink environment: The simulation time was set to 10 seconds with a step size of 0.001 seconds. The comparison methods selected were traditional sliding mode control based on disturbance observer (NDOB-SMC) and non-singular terminal sliding mode control based on disturbance observer (NDOB-NTSMC). Figure 3 and Figure 4 The position tracking curves of the first and second joints under the action of equivalent ocean current disturbance (simulated ocean current periodic resistance) are shown. As can be seen from the figure, the method proposed in this embodiment can complete the tracking within 0.5s, and the convergence speed is better than NDOB-NTSMC (1.2s) and NDOB-SMC (2.5s), which verifies that the method has the ability to quickly suppress ocean current disturbance. Figure 5 and Figure 6 The figure shows a comparison curve of joint velocity tracking. As can be seen from the figure, the method proposed in this embodiment has the smallest overshoot and the lowest fluctuation amplitude. Figure 7 and Figure 8 To control the input comparison curve, the results show that, compared with NDOB-NTSMC, the method proposed in this embodiment reduces the jitter amplitude by about 60%, which can effectively suppress jitter caused by underwater sensor noise. Figure 9 and Figure 10 The figure shows a comparison of the disturbance estimation results. The blue curve represents the equivalent disturbance of the ocean current-impact composite, and the red curve represents the observer's estimated value. As can be seen from the figure, the estimation error of the nonlinear disturbance observer for the equivalent ocean current disturbance in this embodiment can converge to within ±0.02 within 0.3s. Figure 11The figure shows a comparison of the trajectory tracking of the robotic arm's end effector (3D spatial trajectory). As can be seen from the figure, the trajectory of the method proposed in this embodiment almost completely overlaps with the desired trajectory. Further quantitative analysis results show that the root mean square error (RMSE) of the tracking of the first joint in the method proposed in this embodiment is 0.012 rad, a 57% reduction compared to NDOB-NTSMC (0.028 rad) and a 73% reduction compared to NDOB-SMC (0.045 rad); the RMS error of the tracking of the second joint is 0.015 rad, a 53% reduction compared to NDOB-NTSMC (0.032 rad) and a 69% reduction compared to NDOB-SMC (0.048 rad). The above simulation results fully verify that the method proposed in this embodiment possesses excellent control performance in complex deep-sea flow environments.

[0031] This embodiment also proposes an underwater robotic arm trajectory tracking control system for implementing an underwater robotic arm trajectory tracking control method, including: The robotic arm dynamics model building module is used to build a robotic arm dynamics model that considers superimposed disturbances; Error calculation module, used to calculate the tracking error of the robotic arm joint position; A sliding surface construction module is used to design a non-singular fast terminal sliding surface that guides the joint position tracking error of the robotic arm to converge rapidly within a finite time. The arrival control law design module is used to design arrival control laws using the super-twisting algorithm. The nonlinear disturbance observer design module is used to design a nonlinear disturbance observer capable of estimating superimposed disturbances in real time. The global control law design module is used to combine the non-singular fast terminal sliding surface, arrival control law and nonlinear disturbance observer to design the global control law, and then obtain the final control command. The actuator module is used to convert the final control command into joint driving torque (hydraulic or electric drive, adapted to underwater sealed environment) to achieve high-precision and robust trajectory tracking of the robotic arm joints under complex marine disturbances.

[0032] In a specific embodiment, the nonlinear disturbance observer design module has exponentially convergent observer dynamic characteristics, which can ensure that the disturbance estimation error is bounded and achieves rapid convergence, thereby ensuring the real-time performance of disturbance compensation in deep-sea operation scenarios.

[0033] Specifically, the system proposed in this embodiment can be deployed inside the controller of a deep-sea robotic arm, and is suitable for marine engineering scenarios with high requirements for trajectory tracking accuracy, such as inspection of seabed oil and gas pipelines, deep-sea mining equipment, shipwreck salvage robotic arms, and underwater scientific research. It can also be extended to the robotic arm control system mounted on ROV / AUV.

[0034] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for tracking and controlling the trajectory of an underwater robotic arm based on an interference observer, characterized in that, include: S1: Establish a dynamic model of the robotic arm that considers superimposed disturbances, including external environmental disturbances and uncertainties in the hydrodynamic model parameters; S2: Calculate the robot arm joint position tracking error, and design a non-singular fast end-effector sliding surface to guide the robot arm joint position tracking error to converge rapidly within a finite time, including: The desired trajectory of the robotic arm is set as follows: ； in, These are the expected trajectories of the first and second joints in the robotic arm, respectively. The tracking error is calculated based on the desired trajectory. , represented as: ,in, For the desired joint position of the robotic arm, This refers to the actual position of the robotic arm joints; Based on the tracking error, the non-singular fast terminal sliding surface is designed as follows: ； in, For design parameters, , The ratio is a positive odd number and satisfies and ; S3: The arrival control law is designed using the super-twisting algorithm to reduce sliding mode chattering during the sliding mode arrival stage; The arrival control law is expressed as follows: ； in, For the time-order first derivative of the sliding mode of a nonsingular fast terminal, denoted as . For the super-twisted gain parameter, Represents a symbolic function. This is the integral compensation term built into the super-twisting algorithm. The integral compensation term built into the super-twisting algorithm The derivative with respect to time; S4: Design a nonlinear disturbance observer for real-time estimation of the superimposed disturbance, denoted as: ； in, For nonlinear disturbance observer auxiliary variables, For the derivative of the auxiliary variable of the nonlinear disturbance observer with respect to time, The observer gain matrix is... , This is the superimposed disturbance estimate output by the nonlinear disturbance observer; For control input; S5: Combining the non-singular fast terminal sliding surface, arrival control law, and nonlinear disturbance observer, design a global control law to obtain the final control command, and realize trajectory tracking of the robotic arm joints under ocean disturbance based on the final control command. The global control law is expressed as follows: ； in, For the robotic arm's inertia matrix, The matrix of Coriolis force and centrifugal force. This is the gravitational torque.

2. The underwater robotic arm trajectory tracking control method based on an interference observer according to claim 1, characterized in that, The established dynamic model of the robotic arm considering superimposed disturbances is expressed as follows: ； in, These are the actual velocity and actual acceleration vectors of the robotic arm joints, respectively. External superimposed disturbance; The parameters of the robotic arm's dynamics model are defined as follows: ； in: , , , These are the inertia matrices. Element; , 、 、 The matrices of Coriolis force and centrifugal force are respectively. Element; and These are the gravity vectors. Element; ； in: … These are the model constants derived based on the mass and length of the robotic arm links; h Solve for intermediate variables for the Coriolis force and centrifugal force matrices; and These are the first joint angle and the second joint angle of the robotic arm, respectively. and These are the angular velocities of the first and second joints, respectively.

3. An underwater robotic arm trajectory tracking control system, used to implement the underwater robotic arm trajectory tracking control method of claim 1, characterized in that, include: The robotic arm dynamics model building module is used to build a robotic arm dynamics model that considers superimposed disturbances; Error calculation module, used to calculate the tracking error of the robotic arm joint position; A sliding surface construction module is used to design a non-singular fast terminal sliding surface that guides the joint position tracking error of the robotic arm to converge rapidly within a finite time. The arrival control law design module is used to design arrival control laws using the super-twisting algorithm. The nonlinear disturbance observer design module is used to design nonlinear disturbance observers. The global control law design module is used to combine the non-singular fast terminal sliding surface, arrival control law and nonlinear disturbance observer to design the global control law, and then obtain the final control command. The actuator module is used to convert the final control command into joint driving torque to achieve trajectory tracking of the robotic arm joints under ocean disturbance.

Images