Cable underwater robot trajectory tracking control method for coping with ocean current disturbance

Abstract

The invention discloses a cable underwater robot trajectory tracking control method for coping with ocean current disturbance, and belongs to the technical field of underwater robot trajectory tracking, and the method comprises the following steps: S1, constructing a kinematics and dynamics model of a cable underwater robot, and simplifying the kinematics model of the cable underwater robot; s2, combining speed information of a Doppler velocimeter sensor, designing a super-spiral expansion state observer, and compensating a cable underwater robot system; designing a self-adaptive super-spiral expansion state observer by combining a self-adaptive law; and S3, designing a self-adaptive super-spiral integral terminal sliding mode controller based on the self-adaptive super-spiral expansion state observer. The real-time dynamic performance and stability of the underwater robot in sliding mode control are improved, the buffeting problem existing in sliding mode control is solved, it is ensured that under the condition of ocean current disturbance, the ROV can still achieve accurate trajectory tracking, and the robustness is very high.

Description

Technical Field

[0001] The present invention belongs to the technical field of underwater robot trajectory tracking, and in particular relates to a cabled underwater robot trajectory tracking control method for coping with ocean current disturbances. Background Art

[0002] With the rapid development of ocean exploration, underwater robots (ROVs), as submersibles, are essential tools for human exploration of the ocean. They can perform tasks such as detection and dangerous underwater operations in underwater environments, whether controlled by humans, autonomously, or remotely. However, in practical applications, ROVs face numerous challenges in trajectory tracking control, such as strong nonlinear characteristics, uncertainty in system and hydrodynamic parameters, and disturbances from ocean currents. These factors place higher demands on the design of ROV trajectory tracking control algorithms.

[0003] Currently, the design of ROV trajectory tracking controllers is primarily based on full-state feedback. An adaptive nonlinear fuzzy proportional-integral-differential (FPID) controller was developed for trajectory tracking of a multi-input, multi-output (MIMO) fully driven, 6-DOF autonomous underwater vehicle (AUV). Using a Doppler velocimeter to measure position and velocity, a PID second-order sliding mode controller was designed to stabilize the AUV, resulting in significant closed-loop stability. A nonsingular fast fuzzy terminal sliding mode controller (NFFTSMC) was developed for finite-time error convergence and robust control of the AUV's 6-DOF dynamics. This controller can compensate for disturbances in advance using a disturbance observer. However, these controllers fail to account for the ROV's presence in the ocean, and the algorithm's real-time dynamic performance and stability need to be improved. Furthermore, these methods require high computational resources, resulting in response delays.

[0004] Sliding Mode Control (SMC) is a robust control method based on nonlinear theory. Its core concept is to design a sliding surface, forcing the system state to reach the surface within a finite time and move along the sliding surface toward the system equilibrium point, thereby achieving strong robustness against disturbances and system parameter uncertainties. As long as the system state reaches the sliding surface, the control system can effectively suppress the effects of external disturbances and achieve stable operation. However, due to the presence of high-frequency switching terms in the control algorithm, the system is subject to chattering, which can damage hardware and cause noise. Summary of the Invention

[0005] In order to address the problems that existing controllers cannot take into account the situation where ROVs are disturbed by ocean currents in the sea, and the real-time dynamic performance and stability of the algorithm need to be improved; in order to address the problem that existing controllers have high requirements for computing resources, resulting in response delays; and in order to address the problem that the high-frequency switching items in existing control algorithms cause system chattering problems, which may damage hardware and cause noise. The present invention provides a tethered underwater robot trajectory tracking control method for responding to ocean current disturbances, improves the real-time dynamic performance and stability of the underwater robot in sliding mode control, and addresses the chattering problem existing in sliding mode control, ensuring that the ROV can still achieve accurate trajectory tracking and has strong robustness under the condition of ocean current disturbances.

[0006] The technical solution adopted by the trajectory tracking control method of a cabled underwater robot to cope with ocean current disturbances is as follows:

[0007] A trajectory tracking control method for a cabled underwater robot in response to ocean current disturbances comprises the following steps:

[0008] S1. Construct the kinematics and dynamics model of the tethered underwater robot and simplify the kinematics model of the tethered underwater robot;

[0009] S2. Combine the speed information of the Doppler velocimeter sensor to design a super-helical expansion state observer to compensate the tethered underwater robot system; combine the adaptive law to design an adaptive super-helical expansion state observer;

[0010] S3. Design an adaptive superhelical integral terminal sliding mode controller based on an adaptive superhelical extended state observer.

[0011] A further improvement of the technical solution of the present invention is that: in step S1, a kinematic model of a cable underwater robot is constructed, specifically,

[0012] The variables of the tethered underwater robot in the longitudinal, transverse and vertical directions are defined respectively; the generalized attitude coordinates and velocity vector of the tethered underwater robot are,

[0013] η=[x,y,z,φ,θ,ψ] T (1-1)

[0014] v=[u,v,w,p,q,r] T (1-2)

[0015] Among them, the three-dimensional coordinates of the tethered underwater robot in the inertial coordinate system are η1 = [x, y, z] T , the attitude angle coordinates can be described as η2=[φ,θ,ψ] T The linear velocity vector of the tethered underwater robot in the body coordinate system can be described as v1 = [u, v, w]T , the angular velocity vector can be described as v2 = [p, q, r] T ;

[0016] The following is the kinematic transformation relationship of the tethered underwater robot in the inertial coordinate system and the body coordinate system.

[0017]

[0018] in, represents the change in the position vector and attitude vector of the tethered underwater robot in the inertial coordinate system, J(η) is the rotation matrix between the inertial coordinate system and the body coordinate system, also known as the Jacobi matrix;

[0019]

[0020] η1=J1(η)v1 (1-5)

[0021] η2=J2(η)v2 (1-6)

[0022] The Jacobian matrix J(η) consists of the position change rotation matrix J1(η) and the attitude change rotation matrix J2(η);

[0023] The rotation coordinate systems around the x-axis, y-axis, and z-axis are:

[0024]

[0025]

[0026]

[0027] Using Euler angles, the position change rotation matrix can be obtained as:

[0028]

[0029] Similarly, the transformation matrix of attitude change in the inertial coordinate system is:

[0030]

[0031] During the actual operation, the tethered underwater robot has restoring force in the rolling and pitching directions. The transformation matrix J2(η) always makes sense;

[0032] Through the kinematic research and analysis of the tethered underwater robot, a three-dimensional 6-DOF kinematic model of the tethered underwater robot was established:

[0033]

[0034] A further improvement of the technical solution of the present invention is that: in step S1, a dynamic model of a cable underwater robot is constructed, specifically,

[0035] According to the Newton-Euler equation and the Lagrange equation, the dynamic equation of the tethered underwater robot can be expressed as:

[0036]

[0037] Among them, η = [x, y, z, φ, θ, ψ] T is the pose vector of the tethered underwater robot in the inertial coordinate system, v = [u, v, w, p, q, r] T is the velocity and angular velocity vector of the tethered underwater robot in the body coordinate system, M∈R 6×6 is the rigid body inertia matrix of the tethered underwater vehicle including the additional mass, C(v)∈R 6×6 It is represented by the Coriolis force and centripetal force matrix caused by the rotation of the tethered underwater vehicle itself, D(v)∈R 6×6 is the fluid damping matrix, g(η)∈R 6×1 is the restoring force and torque vector composed of buoyancy and gravity. τ∈R 6×1 represents the control input of the tethered underwater robot, i.e., the force and torque on each degree of freedom, τ d =MJ T (η)d,d∈R 6×1 The ocean current disturbance to the tethered underwater robot.

[0038] A further improvement of the technical solution of the present invention is that the kinematic model of the tethered underwater robot is simplified in step S1, specifically,

[0039] In fluid statics, a tethered underwater robot is affected by gravity W and buoyancy B. The resulting force and torque are called restoring forces. According to Newton's law:

[0040] B=ρgV b (1-14)

[0041] W=mg (1-15)

[0042] Where m is the mass of ROV, g is the acceleration due to gravity, V b is the volume of the tethered underwater robot, ρ is the density of the fluid; the coordinates of the buoyancy center of the tethered underwater robot in the body coordinate system are defined as r b =[x g ,y g ,z g ] T , then the restoring force and moment matrix g(η) can be expressed as:

[0043]

[0044] The point of buoyancy is r b =[x g ,y g ,z g ] T =[0,0,0] T , and the coordinates of its center of gravity are r g =[x g ,y g ,z g ] T =[0,0,z g ] T , then formula (1-16) can be simplified to:

[0045]

[0046] A further improvement of the technical solution of the present invention is that: in step S2, the super-helical expansion state observer compensates for the interference received by the tethered underwater robot and performs stability analysis on it;

[0047] In step S1, the kinematic and dynamic models of the tethered underwater vehicle are given, namely:

[0048]

[0049] According to the dynamic model of the tethered underwater robot and the actual environment, the following assumptions and characteristics can be made:

[0050] Assumption 1: The Jacobian matrix J(η) between the velocity and position of the cabled underwater robot is reversible, that is, it satisfies

[0051] According to the formula of Jacobian matrix J(η), only when When , the Jacobian matrix between the velocity and position of the cabled underwater robot appears singular, and because the cabled underwater robot has a restoring force in the pitch direction, it always satisfies The assumption is established;

[0052] Assumption 2: The external unknown disturbance is bounded and satisfies the Lipschitz condition, that is, |d| <L, Among them, L and l are positive numbers;

[0053] Characteristics 1. Inertia matrix M η (η) is a positive definite symmetric matrix, and there exist positive scalars m1 and m2 that satisfy

[0054] m1I≤M η (η)≤m2I

[0055] Where I represents the 6-dimensional identity matrix;

[0056] Feature 2: is a skew-symmetric matrix;

[0057] Let τ d =Md, formula (2-1) can be changed to

[0058]

[0059] Define z1 as the estimated value of the velocity v of the tethered underwater robot, z2 as the estimated value of d, and the adaptive superhelical state observer can be designed as follows:

[0060]

[0061] Among them, k1 and k2 are positive scalars, τ1=M -1 (τ-C(v)vD(v)vg(η)), e1=v-z1 is the estimated error of the tethered underwater robot speed, and e2=d-z1 is the estimated error of d; then, the compensation error of the interference is

[0062] A further improvement of the technical solution of the present invention is that the adaptive law in step S2 can be designed as follows:

[0063]

[0064] Among them, α and ε are positive parameters, and the expression of k1 contains χ, while

[0065]

[0066] When the speed estimation error exists, if |e1|>ε, the derivatives of k1 and k2 are positive, and the adaptive gain variable value will continue to increase, thereby suppressing the speed estimation error e1 from increasing and gradually reducing it; when the speed estimation error reaches the allowable error accuracy, the value of the adaptive gain variable will gradually decrease and stabilize within the required accuracy range;

[0067] When the tethered underwater robot system (2-1) is used and the observer (2-4) is selected, and its gain satisfies (2-5), the observer can compensate for the interference of the tethered underwater robot.

[0068] A further improvement of the technical solution of the present invention is that: step S3 includes the following steps:

[0069] S3.1. Design a non-singular integral terminal sliding surface to achieve fast finite-time convergence;

[0070] S3.2. Design a super-helical integral terminal sliding mode reaching law to significantly alleviate the chattering problem of traditional sliding mode control and use the adaptive law to solve the excessively high value of the super-helical gain.

[0071] A further improvement of the technical solution of the present invention is that: the step S3.1 is specifically,

[0072] In the world coordinate system, the position and posture of the tethered underwater robot can be expressed as η = [x, y, z, φ, θ, ψ] T , in the body coordinate system, its linear velocity and angular velocity can be expressed as v = [u,υ,ω,p,q,r] T ; Considering a time-varying reference trajectory η d ∈R 6×1 , then the tracking error of the pose can be defined as Combining formula (2-1), we can get:

[0073]

[0074] Taking the linear velocity and angular velocity v as virtual control inputs, the virtual reference velocity vector can be defined as:

[0075] v r =v+κ (3-2)

[0076] In the above formula,

[0077] K1=diag([k 11 ,k 12 ,k 13 ,k 14 ,k 15 ,k 16 ])

[0078] K2=diag([k 21 ,k 22 ,k 23 ,k 24 ,k 25 ,k 26 ])

[0079] The above K, K1, K2 are all positive definite diagonal matrices, and k ij >0(i=1,2;j=1,2,...,6), and satisfy

[0080] The velocity tracking error can be defined as:

[0081]

[0082] In order to design a robust system with tracking error convergence in finite time, a sliding mode surface based on velocity tracking error is defined:

[0083]

[0084] Among them, β1, β2>0 are two positive parameters that need to be designed, p>0, q>0 are both odd numbers, and satisfy 1 <p / q<2;

[0085] The derivative of the sliding surface is:

[0086]

[0087] When sliding on the sliding surface, that is, s = 0, then:

[0088]

[0089] make Then taking its derivative we get:

[0090]

[0091] Combined with Lemma 3-2, we can see that the sliding surface converges in a finite time during the sliding stage; on this sliding surface, when it is far away from the equilibrium point, that is, at this time at this time The derivative of grows exponentially in the opposite direction, so that Gradually approaches the equilibrium point, that is, the speed tracking error tends to 0.

[0092] A further improvement of the technical solution of the present invention is that: the step S3.2 is specifically,

[0093] Combining the state observer (2-3) and formula (3-5), the control law can be derived as follows:

[0094]

[0095] Among them, τ2=C(v)v+D(v)v+g(η);

[0096] The standard STW reaching law is as follows:

[0097]

[0098] Among them, k3 and k4 are the control gains of the STW algorithm, and k3, k4>0. The selection of the control gain of the STW algorithm is often related to the boundary information of the disturbance. In actual situations, the disturbance boundary is difficult to estimate. Usually, a larger control gain is selected to achieve system stability, but a larger gain will reduce the control accuracy of the system. For this reason, an adaptive STW control algorithm is selected to automatically adjust the control gain. The characteristic of adaptive control is that it can adaptively adjust the gain size for uncertain objects and external disturbances to ensure rapid convergence and stability of the system.

[0099] ASTW systems can be designed as:

[0100]

[0101] Among them, the control gains k3>0, k4>0, and the adaptive law is as follows:

[0102]

[0103] Among them, the values of ε1 and α>0 are the same as the adaptive law setting values in the state observer, and k 3min 、k 4min , are the minimum values of the control gain; when |s|≤ε1, the system is stable within the specified accuracy range, and is slowly adjusted by the set minimum gain to improve the stability of the system; when |s|>ε1, the system is not within the stable accuracy range. At this time, the control gain will continue to increase, and the adjustment force will become greater and greater, so that the system will move closer to the stable point.

[0104] Due to the adoption of the above technical solution, the technical advancements achieved by the present invention include:

[0105] Taking into account the presence of disturbances in the marine environment, this paper proposes an adaptive super-helical expansion state observer to compensate for disturbances within a finite time. Because traditional linear sliding surfaces converge slowly and cannot achieve finite-time convergence, a non-singular fast integral terminal sliding surface is designed. Combining the advantages of both terminal and integral sliding surfaces, this approach achieves finite-time convergence while eliminating steady-state errors and enhancing robustness.

[0106] Considering the chattering phenomenon of traditional sliding mode control, this invention employs a superhelical algorithm to mitigate this chattering, preventing wear and energy loss caused by severe chattering in the ROV. Furthermore, adaptive control is employed to adjust the gain in the superhelical algorithm to avoid excessive chattering caused by excessive control gain. This allows the proposed controller to achieve ROV trajectory tracking control within a limited time. BRIEF DESCRIPTION OF THE DRAWINGS

[0107] Figure 1 It is the ROV inertial coordinate system IRF and the body coordinate system BRF;

[0108] Figure 2 This is the ASTWESO-ASTWITSMC controller structure diagram;

[0109] Figure 3 There are three situations of switching surface motion points;

[0110] Figure 4 It is the ROV plane trajectory diagram;

[0111] Figure 5 is a position tracking graph;

[0112] Figure 6 is the position error curve;

[0113] Figure 7 It is a speed tracking curve graph;

[0114] Figure 8 is the speed error curve;

[0115] Figure 9 is the sliding surface s value;

[0116] Figure 10 It is a real-time control input curve diagram. DETAILED DESCRIPTION

[0117] In order to make the purpose, technical solutions and advantages of the present invention more clear, the present invention is further described in detail below in conjunction with specific embodiments and with reference to the accompanying drawings. In the following description, descriptions of well-known structures and technologies are omitted to avoid unnecessary confusion of the concept of the present invention.

[0118] The present invention provides a trajectory tracking control method for a tethered underwater robot in response to ocean current disturbances, comprising the following steps:

[0119] S1. Establish mathematical and dynamic models of autonomous underwater robots;

[0120] S11. Construct ROV kinematic and dynamic models;

[0121] In order to perform motion analysis on ROV, the position and speed of ROV in water are determined. Usually, inertial coordinate system and body coordinate system are established, such as Figure 1 As shown. The inertial coordinate system is also called the earth coordinate system, that is, the origin of the coordinate system is fixed at a certain point on the ground, which is used to describe the position and posture of the ROV; the body coordinate system is the ROV's own coordinate system, which moves with the robot and is usually defined at the robot's geometric center. It is generally used to describe the ROV's linear velocity and angular velocity. Figure 1In the figure, the body coordinate system is (RBF) and the inertial coordinate system is (IBF).

[0122] By establishing the kinematic model and dynamic model of the ROV, the velocity of the ROV in the inertial coordinate system and the velocity in the body coordinate system can be converted into each other, and the relationship between the thrust and the ROV state information such as speed and position can be established.

[0123] Table 1 ROV motion parameters

[0124]

[0125]

[0126] As shown in Table 1, the variables of ROV in the longitudinal, transverse and vertical directions are defined respectively. The generalized attitude coordinates and velocity vector of ROV are:

[0127] η=[x,y,z,φ,θ,ψ] T (1-1)

[0128] v=[u,v,w,p,q,r] T (1-2)

[0129] Among them, the three-dimensional coordinates of ROV in the inertial coordinate system are η1 = [x, y, z] T , the attitude angle coordinates can be described as η2=[φ,θ,ψ] T ; The linear velocity vector of ROV in the body coordinate system can be described as v1 = [u, v, w] T , the angular velocity vector can be described as v2 = [p, q, r] T .

[0130] The ROV is a multi-input, multi-output, strongly coupled, nonlinear, time-varying system. Multiple nonlinear relationships exist in the ROV model. Therefore, to simplify the modeling process and subsequent controller design, the present invention makes the following assumptions about the ROV kinematic and dynamic models:

[0131] (1) ROV is a rigid body with constant mass, and its mass does not change with time;

[0132] (2) The earth coordinate system is set as an inertial coordinate system. In a small area of water, the effect of the earth's rotation on the ROV motion is not considered;

[0133] (3) The position of the ROV's center of buoyancy and center of gravity relative to the body coordinate system does not change;

[0134] (4) The propeller of the ROV does not generate reverse torque on the ROV when it rotates.

[0135] In the design process of ROV controller, the mutual conversion between the two coordinate systems is essential. The following is the ROV kinematic conversion relationship between the two coordinate systems:

[0136]

[0137] Represents the change in the position vector and attitude vector of the ROV in the inertial coordinate system, and J(η) is the rotation matrix between the inertial coordinate system and the body coordinate system, also known as the Jacobi matrix.

[0138]

[0139] η1=J1(η)v1 (1-5)

[0140] η2=J2(η)v2 (1-6)

[0141] The Jacobian matrix J(η) consists of the position change rotation matrix J1(η) and the attitude change rotation matrix J2(η).

[0142] The rotation coordinate systems around the x-axis, y-axis, and z-axis are:

[0143]

[0144]

[0145]

[0146] Using Euler angles, the position change rotation matrix can be obtained as:

[0147]

[0148] Similarly, the transformation matrix of attitude change in the inertial coordinate system is:

[0149]

[0150] During actual operation, ROV has restoring force in the rolling and pitching directions, so The transformation matrix J2(η) is always meaningful.

[0151] Through the study and analysis of ROV kinematics, a three-dimensional 6-DOF ROV kinematic model was established:

[0152]

[0153] The above completes the kinematic model of the ROV. Next, the dynamic model of the ROV is established. According to the Newton-Euler equation and the Lagrange equation, the dynamic equation of the ROV can be expressed as:

[0154]

[0155] Among them, η = [x, y, z, φ, θ, ψ] T is the pose vector of ROV in the inertial coordinate system, v = [u, v, w, p, q, r] T is the velocity and angular velocity vector of ROV in the body coordinate system, M∈R 6×6 is the rigid body inertia matrix of ROV including the additional mass, C(v)∈R 6×6 It is represented by the Coriolis force and centripetal force matrix caused by the ROV's own rotation, D(v)∈R 6×6 is the fluid damping matrix, g(η)∈R 6×1 is the restoring force and torque vector composed of buoyancy and gravity. τ∈R 6×1 represents the ROV control input, i.e., the force and torque on each degree of freedom, τ d =MJ T (η)d,d∈R 6×1 The ocean current disturbance to which the ROV is subjected.

[0156] S12. According to the structural characteristics of the ROV under study, the mathematical model of ROV motion was simplified;

[0157] On the restoring force and restoring torque matrix g(η)∈R 6×1

[0158] In fluid statics, the ROV is affected by gravity W and buoyancy B. The resulting force and torque are called restoring forces. According to Newton's law:

[0159] B=ρgV b (1-14)

[0160] W=mg (1-15)

[0161] Where m is the mass of ROV, g is the acceleration due to gravity, V b is the volume of ROV, ρ is the density of the fluid. The coordinates of the ROV buoyancy point in the body coordinate system are defined as r b =[x g ,y g ,z g ] T , then the restoring force and moment matrix g(η) can be expressed as:

[0162]

[0163] In order to facilitate the study of the trajectory tracking control algorithm of the underwater robot, when selecting the mechanical structure of the underwater robot, attention is paid to the counterweight so that the buoyancy point of the underwater robot selected in this invention is at the center of the frame to simplify the complexity of the controller. That is, the buoyancy point is r b =[x g ,y g ,z g ] T =[0,0,0] T , and the coordinates of its center of gravity are r g =[x g ,y g ,z g ] T =[0,0,z g ] T , then formula (1-16) can be simplified to:

[0164]

[0165] S1 primarily established the kinematic and dynamic models of the underwater robot to facilitate subsequent research into the ROV's trajectory tracking control algorithm. It established an inertial coordinate system and a body coordinate system, and made some assumptions about the ROV to simplify the ROV modeling process and the complexity of the control algorithm.

[0166] S2. Combining the velocity information of the Doppler velocimeter (DVL) sensor, a super-helical expansion state observer (ESO) is designed. Combining the adaptive law, an adaptive super-helical expansion state observer (ASTESO) is designed.

[0167] The extended state observer (ESO) is a special type of state observer, a core component of active disturbance rejection control (ADRC) technology. While the basic function of a state observer is to estimate the state of a system based on its output, the ESO goes a step further, estimating the effects of unmeasured state variables and external disturbances. The core concept of ADRC is to treat all uncertainties in the system as unknown but estimable disturbances. The ESO is a specialized controller designed based on this concept, capable of online, real-time estimation and offsetting the effects of these disturbances.

[0168] In S2, a novel adaptive superhelical expansion state observer is proposed. Using the velocity information of the DVL as the observation object, it achieves finite-time convergence for disturbances to the ROV. Incorporating adaptive laws, the observer improves its real-time performance and adaptability. Finally, a corresponding Lyapunov function is constructed to prove that the observer is finite-time stable. The observer compensates for disturbances to the ROV, and its stability is analyzed.

[0169] In S1, the kinematic and dynamic models of ROV are given, namely:

[0170]

[0171] Characteristics 2-1, Inertia Matrix M η (η) is a positive definite symmetric matrix, and there exist positive scalars m1 and m2 that satisfy

[0172] m1I≤M η (η)≤m2I

[0173] Where I represents the 6-dimensional identity matrix.

[0174] Features 2-2 is a skew-symmetric matrix.

[0175] Let τ d =Md, formula (2-1) can be changed to

[0176]

[0177] Define z1 as the estimated value of ROV velocity v, z2 as the estimated value of d, and the adaptive superhelical state observer can be designed as

[0178]

[0179] Among them, k1 and k2 are positive scalars, τ1=M -1 (τ-C(v)vD(v)vg(η)), e1=v-z1 is the estimated error of ROV velocity, and e2=d-z1 is the estimated error of d. Then, the compensation error of the interference is

[0180] The experimental equipment in the present invention includes a DVL, which can measure the real-time speed v of the ROV. The estimated error model of the system is:

[0181]

[0182] Where δ is the derivative of the disturbance d experienced by the ROV.

[0183] The selection of k1 and k2 in the general supercoil algorithm is related to the disturbance d. Since it is difficult to obtain the true value of the ROV disturbance in practice, in order to ensure the stability of the supercoil control algorithm, a larger value is often selected when selecting the gain. This results in a larger gain and produces larger vibrations. To this end, the selected gain of the present invention is as follows, which can achieve adaptive adjustment of the gain without binding the disturbance and realize compensation for the disturbance. The adaptive law can be designed as:

[0184]

[0185] Among them, α and ε are positive parameters, and the expression of k1 contains χ, and:

[0186]

[0187] When the speed estimation error exists, if |e1|>ε, the derivatives of k1 and k2 are positive, and the value of the adaptive gain variable will continue to increase, thereby suppressing the increase of the speed estimation error e1 and making it gradually decrease; when the speed estimation error reaches the allowable error accuracy, the value of the adaptive gain variable will gradually decrease and stabilize within the required accuracy range.

[0188] When the ROV system (2-1) selects the observer (2-4) and its gain satisfies (2-5), the observer can compensate for the ROV interference.

[0189] ESO stability proof:

[0190] The state observer can compensate for disturbances to the ROV and improve the accuracy of ROV trajectory tracking. The previous section designed the observer, and this section will analyze its stability.

[0191] Construct the following Lyapunov function:

[0192]

[0193] where r>0 and E=[φ,ψ] T ,in:

[0194]

[0195] Since r and k1 are positive numbers, P is a positive definite matrix. Let λ m ,λ M are the minimum and maximum eigenvalues of the positive definite matrix, respectively. From this we can conclude that:

[0196] λ m (φ 2 +ψ 2 )≤V1-2rk2||e1||≤λ M(φ 2 +ψ 2 ) (2 - 9)

[0197] That is:

[0198] λ m (||e1|| + ψ 2 ) ≤ V1 - 2rk2||e1|| ≤ λ M (||e1|| + ψ 2 ) (2 - 10)

[0199] Taking the derivative of the Lyapunov function, we get:

[0200]

[0201] According to Assumption 2 - 2, let |δ| < l. When the speed estimation error satisfies ||e1|| > ε, equation (2 - 11) can be derived as:

[0202]

[0203] Substituting equation (2 - 5) into equation (2 - 12), we get:

[0204]

[0205] Lemma 2 - 1: If x > 0, y > 0, μ > 0, p ≠ 1, and satisfy then:

[0206]

[0207]

[0208] Combining Lemma 2 - 1, let y = ψ 2 , p = - 1, then the following inequality holds:

[0209]

[0210] Combining equation (2 - 15) and equation (2 - 16), we get:

[0211]

[0212] where:

[0213]

[0214] According to the adaptive law, when the observation estimation error is not within the allowable range, the gains k1, k2 will keep increasing, then there exists:

[0215]

[0216] When gains k1 and k2 satisfy (2-20), ω1>0, ω2>0. According to Jensen inequality, we can get:

[0217]

[0218] Among them, the parameter σ satisfies:

[0219]

[0220] Then, formula (2-21) can be derived as:

[0221]

[0222] Lemma 2-2: For a nonlinear system With x(0)=0, f(0)=0, Assume that there exists a continuous positive definite Lyapunov function V(x) that satisfies:

[0223]

[0224] Where c>0, α∈(0,1); then, the system is stable in a finite time.

[0225] From Lemma 2-2, we can see that the adaptive super-helical expansion state observer designed by the present invention is stable, and E will converge to the origin in a finite time, which means that the convergence of φ and ψ in a finite time is guaranteed. as well as Then e1 and Convergence in finite time. That is, when |e1|>ε, the adaptive superhelical state observer designed by the present invention is finite-time stable.

[0226] When |e1|≤ε, the observed error is within the required accuracy range. At this time, in order to prevent overcharging, the gain value will continue to decrease, and the control error will be within the required accuracy range.

[0227] S3. An adaptive superhelical integral terminal sliding mode controller based on adaptive superhelical expansion state observer (ASTESO-ASTASMC) is designed.

[0228] An adaptive super-helical sliding mode controller is designed to achieve finite-time convergence of trajectory tracking and alleviate chattering.

[0229] In sliding mode control, if a certain area on the switching surface is the end point, the control system can gradually converge to 0 on the sliding surface through the design of the reaching law, such as Figure 3As shown. For traditional sliding mode control, a linear error function is generally selected as the sliding surface. When the sliding surface of the system reaches the sliding mode, the tracking error of the system can converge to zero. The traditional linear sliding surface can only make the state of the system converge to zero asymptotically but cannot make the sliding surface converge to zero in a finite time. When high-frequency oscillation occurs in the control signal, it will damage the system and affect the stability and control accuracy of the system. The superhelical algorithm transforms the sliding mode of the traditional sliding mode control into a superhelical motion mode, which can more accurately control the target system. It effectively suppresses the occurrence of sliding mode chattering by introducing nonlinear transcendental functions and the superhelical shape of the sliding surface curve. Compared with the conventional sliding mode observation control algorithm, it has achieved good results in reducing sliding mode chattering and improved the reliability and service life of the system.

[0230] Taking into account the chattering phenomenon in sliding mode control, the superhelical control algorithm is adopted as the controller. Based on the superhelical controller algorithm, the non-singular terminal sliding mode is improved. While ensuring system stability and reducing chattering, it accelerates the convergence speed and reduces the steady-state error.

[0231] Based on the super-helical algorithm, the reaching law of the non-singular terminal sliding mode (NTSMC) is improved, which accelerates the convergence speed and reduces the system chattering while ensuring finite time convergence.

[0232] S31. A non-singular integral terminal sliding surface is designed to achieve fast finite-time convergence;

[0233] In the world coordinate system, the position and posture of the underwater robot can be expressed as η = [x, y, z, φ, θ, ψ] T , in the body coordinate system, its linear velocity and angular velocity can be expressed as v = [u,υ,ω,p,q,r] T Considering a time-varying reference trajectory η d ∈R 6×1 , then the tracking error of the pose can be defined as Combining formula (2-1), we can get:

[0234]

[0235] Taking the linear velocity and angular velocity v in equation (3-1) as virtual control inputs, the virtual reference velocity vector can be defined as:

[0236] v r =v+κ (3-2)

[0237] In the above formula,

[0238] K1=diag([k 11 ,k 12 ,k 13 ,k 14 ,k 15 ,k 16 ])

[0239] K2=diag([k 21 ,k 22 ,k 23 ,k 24 ,k 25 ,k 26 ])

[0240] are all positive definite diagonal matrices, and k ij >0(i=1,2;j=1,2,...,6), and satisfy

[0241] The velocity tracking error can be defined as:

[0242]

[0243] In order to design a robust system with tracking error convergence in finite time, a sliding mode surface based on velocity tracking error is defined:

[0244]

[0245] Among them, β1, β2>0 are two positive parameters that need to be designed, p>0, q>0 are both odd numbers, and satisfy 1 <p / q<2。

[0246] The derivative of the sliding surface is:

[0247]

[0248] When sliding on the sliding surface, that is, s = 0, then:

[0249]

[0250] make Then taking its derivative we get:

[0251]

[0252] Combined with Lemma 2-2, we can see that the sliding surface converges in a finite time during the sliding phase. On the sliding surface, when it is far away from the equilibrium point, that is, at this time According to formula (3-6), at this time The derivative of grows exponentially in the opposite direction, so that Gradually approaches the equilibrium point, that is, the speed tracking error tends to 0.

[0253] S32. A super-helical integral terminal sliding mode reaching law is designed to significantly alleviate the chattering problem of traditional sliding mode control;

[0254] Because the switching term of a traditional sliding mode controller has a discontinuous bounded sign function, chattering can occur, which in turn affects the system's performance and accuracy. The Super-Twisting (STW) algorithm reduces chattering by generating a continuous switching signal instead of the switching function.

[0255] Combining the state observer (2-3) and formula (3-5), the control law can be derived as follows:

[0256]

[0257] Among them, τ2=C(v)v+D(v)v+g(η).

[0258] While traditional sliding mode control offers strong robustness and robustness, it also suffers from significant drawbacks. Traditional sliding mode control relies on discontinuous sign functions, resulting in high-frequency switching of the control signal and chattering. Compared to traditional sliding mode control, the STW algorithm can effectively reduce chattering. It maintains system smoothness and robustness on the sliding surface, particularly for systems with significant uncertainty and disturbances. It significantly reduces system chattering while maintaining control stability.

[0259] The standard STW reaching law is as follows:

[0260]

[0261] Here, k3 and k4 are the control gains of the STW algorithm, satisfying k3 and k4 > 0. The selection of control gains for the STW algorithm is often related to the disturbance boundary information. However, in practice, disturbance boundaries are difficult to estimate. Typically, a larger control gain is chosen to achieve system stability, but a larger gain reduces control accuracy. Therefore, an adaptive STW control algorithm is used to automatically adjust the control gain. The characteristic of adaptive control is that it can adaptively adjust the gain to the uncertain object and external disturbances to ensure rapid system convergence and stability.

[0262] ASTW systems can be designed as:

[0263]

[0264] Among them, the control gains k3>0, k4>0, and the adaptive law is as follows:

[0265]

[0266] Among them, the values of ε1 and α>0 are the same as the adaptive law setting values in the state observer, and k 3min 、k 4min , are the minimum values of the control gain. When |s| ≤ ε1, the system is stable within the specified accuracy range, and the set minimum gain is slowly adjusted to improve system stability. When |s| > ε1, the system is outside the stable accuracy range. At this point, the control gain will continue to increase, and the adjustment force will become increasingly stronger, bringing the system closer to the stable point.

[0267] Control system stability proof

[0268] In order to analyze the stability of the system, the following Lyapunov function can be constructed:

[0269] V=Z T P1Z+2γ1k4|s| (3-12)

[0270] Where γ>0, Z=[φ1,ψ1] T ,and:

[0271]

[0272] Vector γ>0, matrix P1 is a positive definite matrix, and it can be deduced that:

[0273] ′2 2′2 2

[0274] λ m (φ1+ψ1)≤V-2γk4|s|≤λ M (φ1+ψ1)

[0275] λ′ m and λ′ M are the minimum and maximum eigenvalues of the matrix P1, respectively. The derivative of the Lyapunov function is:

[0276]

[0277] Assuming |s|>ε, the derivative of the Lyapunov function can be derived as:

[0278]

[0279] Combined with Lemma 2-1, the following inequality holds.

[0280]

[0281] Combining formula (3-14) and formula (3-15), we can get:

[0282]

[0283] in If the control gain meets the following conditions:

[0284]

[0285] Then the parameters ω3 and ω4 are strictly positive definite.

[0286] Formula (3-16) can be deduced:

[0287]

[0288] in, at this time:

[0289]

[0290] According to Theorem 2-2, the system converges in a finite time, that is, the sliding surface s can converge to the equilibrium point.

[0291] When |s|≤ε, the derivative of the Lyapunov function is:

[0292]

[0293] The system is still stable, so the sliding surface can converge to the equilibrium point.

[0294] This completes the proof.

[0295] Simulation Verification

[0296] Comparisons were made between the adaptive superhelical nonsingular integral terminal sliding mode controller without an extended state observer (NOESO-ASITSMC), the adaptive superhelical nonsingular integral terminal sliding mode controller based on a superhelical extended state observer (SESO-ASITSMC), the adaptive superhelical nonsingular integral terminal sliding mode controller based on a finite-time extended state observer (FTESO-ASITSMC), the adaptive superhelical nonsingular integral terminal sliding mode controller based on an adaptive superhelical extended state observer (ASESO-ASITSMC), and the adaptive superhelical integral terminal sliding mode controller based on an adaptive superhelical extended state observer (ASESO-ITSMC). The convergence of each algorithm for ROV trajectory tracking, as well as the position and velocity tracking performance in each degree of freedom, were studied. The velocity and position errors were also analyzed.

[0297] Figure 4The following graphs show the planar trajectory tracking curves of various algorithms when the ROV is subjected to severe disturbances. For a fair comparison, the superhelical sliding mode control parameters are the same. It can be seen that all control algorithms eventually converge to 0. Comparing NOESO-ASITSMC with other control algorithms, it is clear that the controller with an extended state observer to compensate for disturbances has better tracking than the controller without the extended state observer. It is obvious from the figure that the controller without disturbance compensation will deviate from the desired trajectory when subjected to disturbances. When combined with the adaptive law, the convergence effect is better, and the ASESO-ASITSMC algorithm is more stable during the tracking process. Comparing the ASESO-ASITSMC and ASESO-ITSMC algorithms, the improved adaptive superhelical integral terminal sliding mode control is faster than the control algorithm without the adaptive law.

[0298] Figure 5 The tracking status of ROV in each axis and direction, Figure 6 Figure 2 shows the trajectory tracking error of the ROV. As can be seen from the figure, all algorithms stably track the desired trajectory, and the trajectory tracking error remains stable within a certain range. It is clear that the error fluctuations of the NOESO-ASITSMC, SESO-ASITSMC, and ASESO-ITSMC algorithms are relatively large. When used alone, the adaptive superhelical expanded state observer and the adaptive superhelical integral terminal sliding mode controller exhibit poor position error convergence. Their combination offers superior tracking performance, with faster convergence than the FTESO-ASITSMC algorithm and reduced chattering.

[0299] Figure 7 、 Figure 8 They are respectively the speed tracking curve and the speed error curve. It shows that when the ROV is affected by ocean current disturbances, the speed error of the ASESO-ASITSMC control system can converge to zero in a finite time. Moreover, the ASESO-ASITSMC controller designed by the present invention can track the reference speed in a faster time than the ASESO-ITSMC controller. As can be seen from the figure, the speed tracking effect of the NOESO-ASITSMC algorithm without the extended state observer is poor, and the remaining algorithms can track the desired speed. Compared with the finite-time extended state observer algorithm, the speed tracking jitter of the algorithm proposed by the present invention is weak, the speed error is small, and the desired speed can be tracked perfectly. In the pitch angle part, although the speed error of the ASESO-ASITSMC controller of the algorithm of the present invention is higher than that of the FTESO-ASITSMC at the beginning, the subsequent steady-state error accuracy is higher than that of the FTESO-ASITSMC controller under the same parameters.

[0300] Figure 9The sliding surface values of each algorithm are shown in the figure. As can be seen from the figure, the ASESO-ASITSMC algorithm proposed in this paper converges quickly and has a smaller error. However, since the NOESO-ASITSMC controller lacks an observer to estimate the disturbance and compensate for it, its sliding surface value becomes unstable. However, the sliding surface value of the NOESO-ASITSMC controller with an observer to compensate for the disturbance converges to zero more effectively.

[0301] Figure 10 The real-time control input values for each algorithm are shown in Figure 2. A controller without an observer experiences significant deviations in its control input under disturbances, whereas a control algorithm with a stateful observer for disturbance compensation is relatively stable. Compared to FTESO-ASITSMC, the algorithm of this invention offers more stable control inputs and less chattering.

[0302] The above results show that the adaptive superhelical extended state observer proposed in the present invention has better convergence than the finite-time extended state observer and can converge within a finite time. The adaptive superhelical integral terminal sliding mode controller based on the observer in the present invention has better control effect and stronger tracking ability than the superhelical integral terminal sliding mode controller and can converge within a finite time.

[0303] At the same time, combined with the six-degree-of-freedom model of the ROV, the main problem of chattering in the sliding mode control is solved. Taking into account the problem of disturbances in the marine environment, an adaptive super-helical expansion state observer is proposed to compensate for the disturbance within a finite time. Since the traditional linear sliding surface converges slowly and cannot achieve convergence within a finite time, a non-singular fast integral terminal sliding surface is designed, which combines the advantages of the terminal sliding surface and the integral sliding surface. It can not only achieve convergence within a finite time, but also eliminate steady-state errors and enhance robustness. In view of the chattering phenomenon in traditional sliding mode control, the super-helical algorithm is used to reduce the degree of chattering and avoid wear and tear of the ROV mechanical structure and energy loss caused by more severe chattering.

[0304] The embodiments described above are merely descriptions of preferred embodiments of the present invention and are not intended to limit the concept and scope of the present invention. Any modifications and improvements made to the technical solution of the present invention by a person of ordinary skill in the art without departing from the design concept of the present invention shall fall within the scope of protection of the present invention. The technical content for which protection is sought in the present invention is fully set forth in the claims.

Claims

1. A trajectory tracking control method for a tethered underwater robot in response to ocean current disturbances, characterized by: The following steps are included: S1. Construct the kinematics and dynamics model of the tethered underwater robot and simplify the kinematics model of the tethered underwater robot; S2. Combine the speed information of the Doppler velocimeter sensor to design a super-helical expansion state observer to compensate the tethered underwater robot system; combine the adaptive law to design an adaptive super-helical expansion state observer; S3. Design an adaptive superhelical integral terminal sliding mode controller based on an adaptive superhelical extended state observer.

2. The method for tracking and controlling a tethered underwater robot in response to ocean current disturbances according to claim 1, characterized in that: In step S1, a kinematic model of the cabled underwater robot is constructed, specifically, The variables of the tethered underwater robot in the longitudinal, transverse and vertical directions are defined respectively; the generalized attitude coordinates and velocity vector of the tethered underwater robot are, η=[x,y,z,φ,θ,ψ] T (1-1) v=[u,v,w,p,q,r] T (1-2) Among them, the three-dimensional coordinates of the tethered underwater robot in the inertial coordinate system are η1 = [x, y, z] T , the attitude angle coordinates can be described as η2=[φ,θ,ψ] T The linear velocity vector of the tethered underwater robot in the body coordinate system can be described as v1 = [u, v, w] T , the angular velocity vector can be described as v2 = [p, q, r] T ; The following is the kinematic transformation relationship of the tethered underwater robot in the inertial coordinate system and the body coordinate system. in, represents the change in the position vector and attitude vector of the tethered underwater robot in the inertial coordinate system, J(η) is the rotation matrix between the inertial coordinate system and the body coordinate system, also known as the Jacobi matrix; η1=J1(η)v1 (1-5) η2=J2(η)v2 (1-6) The Jacobian matrix J(η) consists of the position change rotation matrix J1(η) and the attitude change rotation matrix J2(η); The rotation coordinate systems around the x-axis, y-axis, and z-axis are: Using Euler angles, the position change rotation matrix can be obtained as: Similarly, the transformation matrix of attitude change in the inertial coordinate system is: During the actual operation, the tethered underwater robot has restoring force in the rolling and pitching directions. The transformation matrix J2(η) always makes sense; Through the kinematic research and analysis of the tethered underwater robot, a three-dimensional 6-DOF kinematic model of the tethered underwater robot was established:

3. The method for tracking and controlling a tethered underwater robot in response to ocean current disturbances according to claim 2, wherein: In step S1, a dynamic model of a cable underwater robot is constructed, specifically, According to the Newton-Euler equation and the Lagrange equation, the dynamic equation of the tethered underwater robot can be expressed as: Among them, η = [x, y, z, φ, θ, ψ] T is the pose vector of the tethered underwater robot in the inertial coordinate system, v = [u, v, w, p, q, r] T is the velocity and angular velocity vector of the tethered underwater robot in the body coordinate system, M∈R 6×6 is the rigid body inertia matrix of the tethered underwater vehicle including the additional mass, C(v)∈R 6×6 It is represented by the Coriolis force and centripetal force matrix caused by the rotation of the tethered underwater vehicle itself, D(v)∈R 6×6 is the fluid damping matrix, g(η)∈R 6×1 is the restoring force and torque vector composed of buoyancy and gravity. τ∈R 6×1 represents the control input of the tethered underwater robot, i.e., the force and torque on each degree of freedom, τ d =MJ T (η)d,d∈R 6×1 The ocean current disturbance to the tethered underwater robot.

4. The method for tracking and controlling a tethered underwater robot in response to ocean current disturbances according to claim 3, characterized in that: In step S1, the kinematic model of the tethered underwater robot is simplified, specifically, In fluid statics, a tethered underwater robot is affected by gravity W and buoyancy B. The resulting force and torque are called restoring forces. According to Newton's law: B=ρgV b (1-14) W=mg (1-15) Where m is the mass of ROV, g is the acceleration due to gravity, V b is the volume of the tethered underwater robot, ρ is the density of the fluid; the coordinates of the buoyancy center of the tethered underwater robot in the body coordinate system are defined as r b =[x g ,y g ,z g ] T , then the restoring force and moment matrix g(η) can be expressed as: The point of buoyancy is r b =[x g ,y g ,z g ] T =[0,0,0] T , and the coordinates of its center of gravity are r g =[x g ,y g ,z g ] T =[0,0,z g ] T , then formula (1-16) can be simplified to:

5. The method for tracking and controlling a tethered underwater robot in response to ocean current disturbances according to claim 1, characterized in that: In step S2, the super-helical expansion state observer compensates for the interference to the tethered underwater robot and performs stability analysis on it; In step S1, the kinematic and dynamic models of the tethered underwater vehicle are given, namely: According to the dynamic model of the tethered underwater robot and the actual environment, the following assumptions and characteristics can be made: Assumption 1: The Jacobian matrix J(η) between the velocity and position of the cabled underwater robot is reversible, that is, it satisfies According to the formula of Jacobian matrix J(η), only when When , the Jacobian matrix between the velocity and position of the cabled underwater robot appears singular, and because the cabled underwater robot has a restoring force in the pitch direction, it always satisfies The assumption is established; Assumption 2: The external unknown disturbance is bounded and satisfies the Lipschitz condition, that is, |d| <L, Among them, L and l are positive numbers; Characteristics 1. Inertia matrix M η (η) is a positive definite symmetric matrix, and there exist positive scalars m1 and m2 that satisfy m1I≤M η (η)≤m2I Where I represents the 6-dimensional identity matrix; Feature 2: is a skew-symmetric matrix; Let τ d =Md, formula (2-1) can be changed to Define z1 as the estimated value of the velocity v of the tethered underwater robot, z2 as the estimated value of d, and the adaptive superhelical state observer can be designed as follows: Among them, k1 and k2 are positive scalars, τ1=M -1 (τ-C(v)vD(v)vg(η)), e1=v-z1 is the estimated error of the tethered underwater robot speed, and e2=d-z1 is the estimated error of d; then, the compensation error of the interference is 6. The method for tracking and controlling a tethered underwater robot in response to ocean current disturbances according to claim 5, characterized in that: The adaptive law in step S2 can be designed as follows: Among them, α and ε are positive parameters, and the expression of k1 contains χ, while When the speed estimation error exists, if |e1|>ε, the derivatives of k1 and k2 are positive, and the adaptive gain variable value will continue to increase, thereby suppressing the speed estimation error e1 from increasing and gradually reducing it; when the speed estimation error reaches the allowable error accuracy, the value of the adaptive gain variable will gradually decrease and stabilize within the required accuracy range; When the tethered underwater robot system (2-1) is used and the observer (2-4) is selected, and its gain satisfies (2-5), the observer can compensate for the interference of the tethered underwater robot.

7. The method for tracking and controlling a tethered underwater robot in response to ocean current disturbances according to claim 1, characterized in that: The step S3 includes the following steps: S3.

1. Design a non-singular integral terminal sliding surface to achieve fast finite-time convergence; S3.

2. Design a super-helical integral terminal sliding mode reaching law to significantly alleviate the chattering problem of traditional sliding mode control and use the adaptive law to solve the excessively high value of the super-helical gain.

8. The method for tracking and controlling a tethered underwater robot in response to ocean current disturbances according to claim 7, characterized in that: The step S3.1 is specifically as follows: In the world coordinate system, the position and posture of the tethered underwater robot can be expressed as η = [x, y, z, φ, θ, ψ] T , in the body coordinate system, its linear velocity and angular velocity can be expressed as v = [u,υ,ω,p,q,r] T ; Considering a time-varying reference trajectory η d ∈R 6×1 , then the tracking error of the pose can be defined as Combining formula (2-1), we can get: Taking the linear velocity and angular velocity v as virtual control inputs, the virtual reference velocity vector can be defined as: v r =v+κ(3-2) In the above formula, K1=diag([k 11 ,k 12 ,k 13 ,k 14 ,k 15 ,k 16 ]) K2=diag([k 21 ,k 22 ,k 23 ,k 24 ,k 25 ,k 26 ]) The above K, K1, K2 are all positive definite diagonal matrices, and k ij >0(i=1,2;j=1,2,...,6), and satisfy The velocity tracking error can be defined as: In order to design a robust system with tracking error convergence in finite time, a sliding mode surface based on velocity tracking error is defined: Among them, β1, β2>0 are two positive parameters that need to be designed, p>0, q>0 are both odd numbers, and satisfy 1 <p / q<2; The derivative of the sliding surface is: When sliding on the sliding surface, that is, s = 0, then: make Then taking its derivative we get: Combined with Lemma 3-2, we can see that the sliding surface converges in a finite time during the sliding stage; on this sliding surface, when it is far away from the equilibrium point, that is, at this time at this time The derivative of grows exponentially in the opposite direction, so that Gradually approaches the equilibrium point, that is, the speed tracking error tends to 0.

9. The method for tracking and controlling a tethered underwater robot in response to ocean current disturbances according to claim 8, characterized in that: The step S3.2 is specifically as follows: Combining the state observer (2-3) and formula (3-5), the control law can be derived as follows: Among them, τ2=C(v)v+D(v)v+g(η); The standard STW reaching law is as follows: Among them, k3 and k4 are the control gains of the STW algorithm, and k3, k4>0. The selection of the control gain of the STW algorithm is often related to the boundary information of the disturbance. In actual situations, the disturbance boundary is difficult to estimate. Usually, a larger control gain is selected to achieve system stability, but a larger gain will reduce the control accuracy of the system. For this reason, an adaptive STW control algorithm is selected to automatically adjust the control gain. The characteristic of adaptive control is that it can adaptively adjust the gain size for uncertain objects and external disturbances to ensure rapid convergence and stability of the system. ASTW systems can be designed as: Among them, the control gains k3>0, k4>0, and the adaptive law is as follows: Among them, the values of ε1 and α>0 are the same as the adaptive law setting values in the state observer, and k 3min 、k 4min , are the minimum values of the control gain; when |s|≤ε1, the system is stable within the specified accuracy range, and is slowly adjusted by the set minimum gain to improve the stability of the system; when |s|>ε1, the system is not within the stable accuracy range. At this time, the control gain will continue to increase, and the adjustment force will become greater and greater, so that the system will move closer to the stable point.

Images