Finite time trajectory tracking control method for autonomous underwater robot

Abstract

The invention discloses an autonomous underwater robot finite time trajectory tracking control method, which belongs to the field of underwater robot three-dimensional trajectory tracking control, and comprises the following steps: establishing a mathematical model and a kinetic model of an autonomous underwater robot; designing a high-order sliding-mode observer of the autonomous underwater robot according to the mathematical model and the kinetic model; according to the mathematical model of the autonomous underwater robot and the high-order sliding-mode observer, designing a nonsingular terminal sliding-mode controller to perform finite time trajectory tracking control; determining a tracking position error and a speed error of the autonomous underwater robot according to the mathematical model of the autonomous underwater robot and the high-order sliding mode observer; a nonsingular terminal sliding mode controller is designed, so that the tracking error of the system is converged to zero within limited time. According to the method, the problems of rapidness and accuracy of speed estimation of the underwater robot are solved, the observation error can be converged to zero in finite time, and the convergence speed is remarkably improved.

Description

Technical Field

[0001] The present invention relates to the field of three-dimensional trajectory tracking control of underwater robots, and in particular to a finite-time trajectory tracking control method for autonomous underwater robots. Background Art

[0002] With the rapid development of the ocean exploration field, a series of UUVs (Unmanned Underwater Vehicles) have been applied to underwater exploration, such as ocean resource exploration, observation, and ocean science research tasks. However, in practical applications, the control of UUVs still faces many technical challenges, such as complex ocean environments, multi-axis nonlinear motions, hydrodynamic coefficient disturbances, and unmeasurable external disturbances. These problems make the design of robust controllers a challenging task. In the motion control of underwater robots, it is generally divided into path planning and trajectory tracking. Trajectory tracking control enables a UUV to track a parameterized reference curve. Achieving precise trajectory tracking control and higher autonomy of underwater robots is a challenging problem.

[0003] Currently, the design of UUV trajectory tracking controllers is mainly based on full-state feedback. An adaptive nonlinear fuzzy proportional integral derivative (FPID) controller is used for UUV trajectory tracking of a multi-input multi-output (MIMO) fully actuated six-degree-of-freedom autonomous underwater vehicle (AUV). Using a Doppler velocimeter to measure position and velocity, a PID second-order sliding mode controller is designed to stabilize the underwater robot, and its closed-loop system has obvious stability. A non-singular fast fuzzy terminal sliding mode controller (NFFTSMC) is used for finite-time error convergence and robust control task implementation of the six-degree-of-freedom dynamics of an AUV, and it can compensate for disturbances in advance through a disturbance observer. However, for these above-mentioned controllers, the velocity state information of the UUV needs to be obtained. In fact, it is difficult to obtain the velocity state of an underwater robot. The unmeasurability of velocity information limits the application of some controllers that require velocity information for controller design.

[0004] As a classic control method, sliding mode control (SMC) has strong robustness and is suitable for dealing with problems such as large delays, strong couplings, nonlinearities, and disturbance uncertainties. Sliding mode control does not depend on the mathematical model of the controlled object and does not require knowledge of the exact information of the controlled system. In addition, the sliding mode control system can automatically adjust the parameters of the controller according to the change of the error. However, sliding mode control is essentially a discontinuous control, and its chattering problem is also the main bottleneck in the application of sliding mode control. Summary of the Invention

[0005] The technical problem to be solved by the present invention is to provide a finite-time trajectory tracking control method for an autonomous underwater vehicle, which solves the problems of rapidity and accuracy of underwater vehicle speed estimation, can converge the observation error to zero within a finite time, and significantly improves the convergence speed.

[0006] To solve the above technical problems, the technical solution adopted by the present invention is as follows:

[0007] A finite-time trajectory tracking control method for an autonomous underwater vehicle, comprising the following steps:

[0008] S1. Establish the mathematical model and dynamic model of the autonomous underwater vehicle;

[0009] S2. Design a high-order sliding mode observer for the autonomous underwater vehicle according to the mathematical model and dynamic model;

[0010] S3. Design a non-singular terminal sliding mode controller for finite-time trajectory tracking control according to the mathematical model and high-order sliding mode observer of the autonomous underwater vehicle;

[0011] S31. Determine the tracking position error and speed error of the autonomous underwater vehicle according to the mathematical model and high-order sliding mode observer of the autonomous underwater vehicle;

[0012] S32. Design a non-singular terminal sliding mode controller to make the tracking error of the system converge to zero within a finite time.

[0013] A further improvement of the technical solution of the present invention lies in that: S1 includes the following steps:

[0014] S11. Establish the mathematical model of the autonomous underwater vehicle;

[0015] The mathematical model of the autonomous underwater vehicle is expressed as:

[0016]

[0017] where η is the position and angle vector of the autonomous underwater vehicle in the navigation coordinate system; J(η) is the generalized rotation matrix of the body coordinate system in the navigation coordinate system; v is the speed vector of the autonomous underwater vehicle in the body coordinate system; M is the mass matrix; C(v) is the Coriolis matrix; D(v) is the damping matrix; G(η) is the restoring force and moment vector; τ η +τ d is the resultant force acting on the autonomous underwater vehicle, τ η is the control input, τ d is the external disturbance;

[0018] S12. Establish the mathematical model of the autonomous underwater vehicle according to the mathematical model of the autonomous underwater vehicle;

[0019] Introduce the velocity of the mathematical model in the inertial coordinate system into the dynamic model of the autonomous underwater vehicle to obtain the dynamic model system of the autonomous underwater vehicle in the inertial coordinate system;

[0020]

[0021] Among them, the model parameter system of the autonomous underwater vehicle in the inertial coordinate is as follows:

[0022]

[0023] However, due to the influence of the movement of the autonomous underwater vehicle, the model parameters in the actual system navigation coordinate system are not fixed; therefore, the system model parameters are divided into two parts, namely, the nominal value part of the model parameters and the actual value deviation part:

[0024]

[0025] In practice, the system dynamics is divided into estimated dynamics and unknown dynamics:

[0026]

[0027] Among them, is the estimated dynamics, calculated based on known model parameters; is the unknown dynamics, including unknown factors such as model errors, environmental disturbances, and external forces;

[0028] Considering the uncertainty of the model parameters, the autonomous underwater vehicle model is written as an estimated part and an unknown part:

[0029]

[0030] The total disturbance τ of the system dtotal mainly comes from:

[0031] Environmental disturbance τ d :

[0032] The movement state of the autonomous underwater vehicle is affected by water flow, wind and waves, temperature changes, etc.;

[0033] Unmodeled dynamics

[0034] The non-linear characteristics of the autonomous underwater vehicle propulsion system, sensor errors, control execution errors;

[0035] The total disturbance of the system is expressed as:

[0036]

[0037] Among them, d x is the disturbance torque in the x-axis direction, d yis the disturbing torque in the y-axis direction, d z is the disturbing torque in the z-axis direction, d p is the disturbing force moment of the rotating shaft;

[0038] Regarding the motion state of the robot, the translational and rotational motions under the thrust in the body coordinate system are expressed as:

[0039]

[0040] The above equation is the rigid body motion equation, involving translational motion and rotational motion;

[0041] The dynamic model of the autonomous underwater vehicle is as follows:

[0042]

[0043] A further improvement of the technical solution of the present invention lies in: in S2, a high-order sliding mode observer is established based on the dynamic model of the autonomous underwater vehicle to estimate the system state;

[0044] The position and velocity states can be reconstructed within a finite time. The observation method of the high-order sliding mode observer for the autonomous underwater vehicle system is as follows:

[0045]

[0046] Among them,

[0047] Define the observation error variable: e3 = τ dtotal - z3;

[0048] Among them,

[0049] When z1, z2, z3 → 0, the velocity of the robot is observed only based on the position information; it is obtained that e1, e2, e3 can converge to zero within a finite time; is the observed position information output by the high-order sliding mode observer, is the observed velocity information, and x3 = ρ1 is the lumped disturbance of the autonomous underwater vehicle system.

[0050] A further improvement of the technical solution of the present invention lies in: in S31, it specifically includes:

[0051] The conserved velocity of the autonomous underwater vehicle in the inertial coordinate system is The lumped interference in the system is The high-order sliding mode observer is stable in a finite time. Therefore, we can conclude that e can converge within a finite time; by selecting appropriate gains k1, k2, and k3; we can find that and occurs within a finite time;

[0052] According to the expected speed and the observed speed in the high-order sliding mode observer, the speed error is rewritten as:

[0053]

[0054] Select the singular terminal sliding surface, i.e., the sliding surface, for the function s(t) of the 4-DOF autonomous underwater vehicle tracking system, and obtain its derivative:

[0055]

[0056] The entire controller is combined with the high-order sliding mode observer, expressed as:

[0057]

[0058] The tracking position error of the autonomous underwater vehicle is:

[0059] The tracking speed error of the autonomous underwater vehicle is:

[0060] A further improvement of the technical solution of the present invention lies in: in S32, it specifically includes:

[0061] According to the Lyapunov stability criterion, the system based on the adaptive super-twisting nonsingular terminal sliding mode control can reach a stable state within a finite time, and it is necessary to make the sliding surface approach zero; the convergence time of the corresponding control tracking error is as follows:

[0062]

[0063] The eigenvalues of the matrix are:

[0064] The function of the 4-DOF autonomous underwater vehicle tracking system is: s(t) = [s1 s2 s3 s4] T ;

[0065] Parameter control of finite-time convergence and control smoothness: α = q / p, q > p, 1 < α < 2; since there is a discontinuous bounded sign function in the switching term of the traditional sliding mode controller, jitter and system performance and accuracy restrict each other; this defect uses the super-twisting algorithm to generate a continuous switching signal instead of reducing the switching function to reduce chattering:

[0066]

[0067] Design a nonsingular terminal sliding mode controller according to the estimated state information of the system. For this purpose, apply the original sliding surface function s(t) and take its time derivative The following conclusions are obtained:

[0068]

[0069] An adaptive adjustment variable for controlling gain, usually used to dynamically adjust the sliding mode gain to enhance the adaptability to system uncertainties;

[0070] w1: A coefficient related to the system dynamics, possibly related to the error signal on the sliding surface;

[0071] γ1: A usually positive adjustment parameter used to affect the convergence rate of the super-twisting algorithm;

[0072] s: The sliding surface or sliding mode variable;

[0073] μ: A threshold parameter that determines when to adjust the update rule of β, possibly used to prevent over-adjustment;

[0074] When β is less than or equal to β m The fixed gain value used, which may be used to ensure that the system does not over-adjust;

[0075] β m : A threshold value that determines the critical point at which β adopts different adjustment strategies;

[0076] The time derivative of the variable ρ, usually related to the adaptive parameter adjustment in sliding mode control;

[0077] ε1: A small positive parameter, usually used to adjust the adaptability of the controller to ensure smooth convergence;

[0078] At this time, the error

[0079] The equivalent control is based on a continuous control model. The equivalent control law can achieve the control of an autonomous underwater vehicle without external interference, while the sliding surface of the adaptive super-twisting algorithm eliminates the jitter caused by external interference through discontinuous switching control; among them, it includes the equivalent control law and the approximation control law;

[0080] The total disturbance is expressed as:

[0081] τ = τ0 + τ1

[0082]

[0083] That is

[0084]

[0085] When w1, γ1, ε1, and μs satisfy a reasonable range of positive constants, enabling the tracking error of the system to converge to zero within a finite time, the non-singular terminal sliding mode controller is stable. When the sliding surface approaches 0, the adaptive gain stops growing; meanwhile, η will converge to zero within a finite time.

[0086] Due to the adoption of the above technical solution, the technical progress achieved by the present invention is as follows:

[0087] 1. The present invention combines an underwater positioning method with a high-order observer technical means to solve the problems of rapidity and accuracy of underwater robot speed estimation, enabling the observation error to converge to zero within a finite time and significantly improving the convergence speed.

[0088] 2. The present invention adopts an adaptive super-twisting controller technical means to solve the trajectory tracking problem of an underwater robot reaching a target point in a complex environment, avoiding system jitter caused by excessive control gain, and simultaneously realizing speed observation and tracking control within a finite time, improving the convergence speed and anti-interference ability of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0089] Figure 1 shows the situation of this UUV in the object-fixed and inertial reference frames;

[0090] Figure 2 is a schematic diagram of trajectory tracking;

[0091] Figure 3 is a comparison curve of the trajectory tracking results of HOSMO-ASTNTSMC (hyper-twisting sliding mode control for the end of a two-degree-of-freedom lower limb system based on a high-order sliding mode observer) and NTSMC (non-singular terminal sliding mode control);

[0092] Figure 4 is a comparison of the tracking errors of HOSMO-ASTNTSMC and NTSMC;

[0093] Figure 5 is the change of the sliding mode surface in four degrees of freedom;

[0094] Figure 6 is a comparison of the speeds of the two algorithms in four degrees of freedom;

[0095] Figure 7 is the speed error in four degrees of freedom;

[0096] Figure 8 is the change of the STW (sliding mode control and terminal sliding mode control) adaptive gain. DETAILED DESCRIPTION OF THE INVENTION

[0097] The present invention will be further described in detail below with reference to the drawings and embodiments:

[0098] A finite-time trajectory tracking control method for an autonomous underwater vehicle, comprising the following steps:

[0099] S1. Establish the mathematical model and dynamic model of the autonomous underwater vehicle;

[0100] S11. Establish the mathematical model of the UUV;

[0101] The mathematical model of the UUV is expressed as:

[0102]

[0103] where η is the position and angle vector of the UUV in the navigation coordinate system; J(η) is the generalized rotation matrix of the body coordinate system in the navigation coordinate system; v is the velocity vector of the UUV in the body coordinate system; M is the mass matrix; C(v) is the Coriolis matrix; D(v) is the damping matrix; G(η) is the restoring force and moment vector; τ η +τ d is the resultant force acting on the UUV, τ η is the control input (thruster thrust), and τ d is the external disturbance (such as water flow disturbance);

[0104] As Figure 1 shown, the coordinates of the UUV include the fixed coordinate (Op) and the inertial coordinate (O); in particular, 1n = [x, y, z, Ψ] is the Figure 1 vector of the position and direction in; the velocity vector of the UUV in the fixed coordinate is expressed as v = [u, v, w, r];

[0105] S12. Establish the mathematical model of the UUV according to the mathematical model of the UUV;

[0106] In actual control, the four-degree-of-freedom motion is sufficient to satisfy the complex three-dimensional motion in the underwater environment; introducing the velocity in the inertial coordinate system of the mathematical model into the UUV dynamic model to obtain the UUV dynamic model system in the inertial coordinate system;

[0107]

[0108] where the parameter system of the UUV model (the above formula) in the inertial coordinate is as follows:

[0109]

[0110] However, due to the influence of the UUV motion, the model parameters in the actual system navigation coordinate system are not fixed; thus, the system model parameters are divided into two parts, namely, the nominal value part and the actual value deviation part of the model parameters:

[0111]

[0112] In practice, system dynamics is divided into estimated dynamics and unknown dynamics:

[0113]

[0114] Among them, is the estimated dynamics, calculated based on known model parameters; is the unknown dynamics, including unknown factors such as model error, environmental disturbance, and external force;

[0115] Considering the uncertainty of model parameters, the UUV model is written as an estimated part and an unknown part:

[0116]

[0117]

[0118] The total disturbance τ of the system dtotal mainly comes from:

[0119] Environmental disturbance τ d :

[0120] Water flow, wind and waves, temperature changes, etc. affect the motion state of the UUV;

[0121] Unmodeled dynamics (Internal uncertainty of the system):

[0122] Nonlinear characteristics of the UUV propulsion system, sensor error, control execution error;

[0123] The total disturbance of the system is expressed as:

[0124]

[0125] Among them, d x is the disturbance torque in the x-axis direction, d y is the disturbance torque in the y-axis direction, d z is the disturbance torque in the z-axis direction, d p is the disturbing force torque of the rotating shaft.

[0126] Regarding the motion state of the robot, the translational and rotational motions under the thrust in the body coordinate system are expressed as:

[0127]

[0128] The above equation is the rigid body motion equation, involving translational motion (Translation) and rotational motion (Rotation);

[0129] Finally, the dynamic model of the UUV is as follows:

[0130]

[0131] S2. Design a high-order sliding mode observer (HOSMO) for the autonomous underwater vehicle according to the mathematical model and the dynamic model;

[0132] Since it is difficult to measure the speed of the underwater vehicle, the observer is an effective method for reconstructing the external state information, with advantages such as low computational complexity and strong robustness. Based on the design method of Moreno (J.A. Moreno Pérez), a speed observer based on a high-order sliding mode observer is proposed based on the dynamic model of the UUV in the navigation coordinate system;

[0133] S21. Establish a HOSMO based on the dynamic model of the UUV to estimate the system state;

[0134] The position and speed states can be reconstructed within a finite time. The observation method of the HOSMO for the UUV system is as follows:

[0135]

[0136] Among them,

[0137] Define the observation error variable: e3 = τ dtotal - z3;

[0138] Among them, z3 = k3sign(e1)

[0139] When z1, z2, z3 → 0, the speed of the robot is observed only based on the position information; it is obtained that e1, e2, e3 can converge to zero within a finite time; is the observed position information output by the HOSMO, is the observed speed information, and x3 = ρ1 is the lumped disturbance of the UUV system;

[0140] S22. Prove the stability of the finite-time HOSMO observer:

[0141] (1) The error between the feasible trajectory and the optimal trajectory satisfies the derivative along the trajectory of the undisturbed system. Assume there is a continuously differentiable function V(e):

[0142]

[0143] (2) Since the initial value e = 0 can converge to zero within a finite time T, that is, it satisfies the finite stable time:

[0144]

[0145] (3) Define the estimated tracking error as e = [e1 e2 e3]T Here, the following function \(V(e)\) is defined as:

[0146] \(V(\xi)=\xi\) T \(P\xi\)

[0147]

[0148] (4) Let \(A\) be the control gain matrix:

[0149]

[0150] where \(A\) is Hurwitz; the stability criterion, the Hurwitz criterion, is used to determine whether all the roots of a polynomial have negative real parts, that is, to determine the stability of a linear system;

[0151] For a linear system, if there exists a symmetric positive definite matrix \(P\) such that the Lyapunov equation: \(PA + A\) T \(P=-Q\) holds for a certain positive definite matrix \(Q\), then the system is asymptotically stable; where \(P\) is a symmetric positive definite matrix, representing the Lyapunov function; \(Q\) is an arbitrarily given symmetric positive definite matrix;

[0152] If the equation holds, all the eigenvalues of the system matrix \(A\) are in the left half plane, that is, the system is stable;

[0153]

[0154] Considering So the above system is asymptotically stable;

[0155] From the Rayleigh inequality and the above equation, we can get where

[0156] The Rayleigh inequality is mainly used for eigenvalue analysis, and its form is:

[0157] where \(\lambda\) min (A), \(\lambda\) max (A) are the minimum and maximum eigenvalues of matrix \(A\) respectively; \(x\) is an arbitrary non - zero vector;

[0158] From this, it can be concluded that compared with the traditional ESO (Extended State Observer), the HOMSO has a faster convergence speed, and the system can remain stable within a finite time \(t\) o inside.

[0159] S3. Design an ASNTSMC (Non - Singular Terminal Sliding Mode Control) controller according to the mathematical model of the UUV and the high - order sliding mode observer;

[0160] HOSMO is an effective and practical research method that does not rely on the system's mathematical model and can effectively reconstruct external state information. It has the advantages of low computational complexity and strong robustness. In the actual underwater environment, relatively accurate robot speed information can be obtained from the robot's attitude information.

[0161] S31 determines the tracking position error and speed error of the UUV according to the mathematical model of the UUV and the high-order sliding mode observer.

[0162] The conserved velocity of the UUV in the inertial coordinate system is The lumped interference in the system is HOSMO is stable in finite time. Therefore, we can conclude that e can converge in finite time. By choosing appropriate gains k1, k2, and k3, we can find that and occurs in finite time;

[0163] According to the expected velocity and observed velocity in HOSMO, the velocity error is rewritten as:

[0164]

[0165] Select the singular terminal sliding surface (NTSS) for the 4-DOF UUV tracking system function s(t), that is, the sliding surface, and obtain its derivative:

[0166]

[0167] The entire controller is combined with the high-order sliding mode observer and is expressed as:

[0168]

[0169] The tracking position error of the UUV is:

[0170] The tracking speed error of the UUV is:

[0171] S32 designs an ASNTSMC (nonsingular terminal sliding mode control) controller to make the tracking error of the system converge to zero in finite time.

[0172] According to the Lyapunov stability criterion, the system based on ASTNTSMC (adaptive super-twisting nonsingular terminal sliding mode control) can reach a stable state in finite time, and the sliding surface needs to approach zero. The convergence time of the corresponding control tracking error is as follows:

[0173]

[0174] The eigenvalues of the matrix are:

[0175] The function of the 4 - degree - of - freedom UUV tracking system is: s(t) = [s1 s2 s3 s4] T ;

[0176] Parameter control of finite - time convergence and control smoothness: α = q / p, q > p, 1 < α < 2; Since there is a discontinuous bounded sign function in the switching term of the traditional sliding - mode controller, chattering and system performance and accuracy restrict each other; This defect uses the super - twisting algorithm (STA) to generate a continuous switching signal instead of reducing the switching function to reduce chattering:

[0177]

[0178] Design an ASTNTSMC according to the estimated state information of the system. For this purpose, apply the original sliding - surface function s(t) and take its time derivative The following conclusions are obtained:

[0179]

[0180] The adaptive adjustment variable of the control gain is usually used to dynamically adjust the sliding - mode gain to enhance the adaptability to system uncertainties;

[0181] w1: A coefficient related to the system dynamics, which may be related to the error signal on the sliding surface;

[0182] γ1: Usually a positive adjustment parameter used to affect the convergence rate of the super - twisting algorithm;

[0183] s: The sliding surface or sliding - mode variable;

[0184] μ: A threshold parameter that determines when to adjust the update rule of β and may be used to prevent over - adjustment;

[0185] The fixed - gain value used when β is less than or equal to β m may be used to ensure that the system does not over - adjust;

[0186] β m : A threshold value that determines the critical point at which β adopts different adjustment strategies;

[0187] The time derivative of the variable ρ, which is usually related to the adaptive parameter adjustment in sliding - mode control;

[0188] ε1: A small positive parameter usually used to adjust the adaptability of the controller to ensure smooth convergence;

[0189] Among them, what needs to be designed is often closely related to the selection of control parameters in many practical situations. However, in many practical situations, it is not easy to estimate the boundary of the disturbance gradient. Using too high a control coefficient may also reduce the control accuracy of the system.

[0190] Generally speaking, the selection of the control gain of STW (Sliding Mode Control and Terminal Sliding Mode Control) is related to the upper limit of the disturbance; improving the performance of the super-twisting sliding mode controller; the parameters of the controller can be adaptively estimated by designing an adaptation law; the characteristics of adaptive control are to handle the lumped disturbance and external disturbance of uncertain objects; it can adapt to additive and multiplicative disturbances with unknown boundaries, avoid overestimation of the control, and ensure fast convergence of suppression and finite-time stability; the adaptive super-twisting terminal sliding mode control can not only overcome the problem of finite-time convergence in traditional sliding mode control, but also use adaptive control to avoid overestimation of control parameters; the idea of designing ASTA (Adaptive Sliding Mode Control and Terminal Sliding Mode Control) is to dynamically increase the control gain until the sliding surface of the system reaches near the equilibrium point;

[0191] At this time, the error

[0192] The control method based on sliding mode control usually directly designs the control input, and the control input generally includes an equivalent control law and a switching term part. Generally speaking, the equivalent control is based on a continuous control model. The equivalent control law can achieve UUV control without external interference, while the ASTA (Adaptive Super-Twisting Algorithm) sliding surface eliminates the jitter caused by external interference through discontinuous switching control. Among them, there is an equivalent control law and an approximation control law;

[0193] The total disturbance is expressed as:

[0194] τ = τ0 + τ1

[0195]

[0196] That is

[0197]

[0198] When w1, γ1, ε1, μs satisfy a reasonable range of positive constants, the tracking error of the system converges to zero within a finite time. In short, it can be proved that the ASTNTSMC controller has stability. When the sliding surface approaches 0, the adaptive gain stops growing. At the same time, η will converge to zero within a finite time.

[0199] Embodiment

[0200] The experimental simulation object will be introduced based on the UUV mathematical model;

[0201] The BlueROV2 has 4 horizontal thrusters and 4 vertical thrusters, with a weight of 12.5 inches kilograms;

[0202] Table 1 shows the dynamic parameters of the BlueROV2;

[0203] Table 1

[0204]

[0205] The main purpose of the simulation is to demonstrate the stability and flutter suppression ability of the proposed control strategy in tracking guidance commands:

[0206]

[0207] For the controller design proposed in S3, simulate the 3D reference trajectories of the two controllers, and set the initial state of the UUV as:

[0208] [x0 y0 z0 ψ0] T = [0.9 1.2 -0.3 0] T ;

[0209] [u0 v0 w0 r0] T = [0.5 0.2 0.1 0.2] T ;

[0210] Assume that the initial value of the observer is the same as the actual state of the UUV;

[0211] In addition, the uncertainty parameters and external disturbances are:

[0212]

[0213] d x d y d z are the changes in the position and orientation of the UUV.

[0214] Figure 3 and Figure 4 are the trajectory and position errors of the Bluerov2 under the action of the controller respectively, where the total simulation time is 50s.

[0215] Figure 3 is the three-dimensional trajectory tracking result under the action of and NTSMC. The green dashed line represents the desired trajectory, the red curve is the trajectory tracking curve using the HOSMO-ASTNTSMC method, and the blue curve is the trajectory tracking curve using the NTSMC method. It can be seen that the HOSMO-ASTNTSMC can still achieve good control effects when relying entirely on position information.

[0216] Figure 4 For the comparison of the position error of the three-dimensional trajectory tracking curve of the four degrees of freedom of the ROV, it can be seen that the position error of the ROV system can converge to zero in finite time by using the ASTNTSMC controller. And the designed adaptive super-twisting sliding mode control in this section can track the reference trajectory in a faster time compared with the traditional terminal sliding mode control method. In the heading angle part, although the convergence speed of the ASTNTSMC controller is slightly lower than that of the NTSMC, the chattering of the system is less than that of the NTSMC control, and the steady-state error accuracy is also higher than that of the NTSMC control under the same control parameters.

[0217] Figure 5 Shows the response of the sliding mode variable designed for the control scheme. It can be observed from the figure that the HOSMO-ASTNTSMC scheme converges to zero at t = 7s. Figure 7 Are the UUV speed and the observed speed. We can see that the constructed HOSMO can accurately recover the speed state of the UUV. Figure 8 Is the disturbance estimation in the experiment, which illustrates the output of the lumped disturbance estimation in the experiment. By introducing the estimated lumped disturbance into the controller for compensation, disturbance rejection can be achieved.

[0218] Figures 3 - 6 Is the speed error curve of the four degrees of freedom of the ROV, indicating that in the presence of ocean current disturbances, the speed error of the ROV using the ASTNTSMC system can converge to zero in finite time, and the designed ASTNTSMC in this section can track the reference trajectory in a faster time compared with the traditional NTSMC controller. And the steady-state error of the ASTNTSMC controller is smaller than that of the NTSMC controller. The speed chattering of the ASTNTSMC controller is also less than that of the NTSMC controller. Thus, it is proved that the combination of the ASTW super-twisting non-singular terminal sliding mode algorithm can significantly reduce the chattering phenomenon.

[0219] Meanwhile Figure 7 From the simulation results, it can be seen that under the action of the adaptive control law, the speed error of the ASTNTSMC controller will eventually tend to be stable. Due to the adoption of the adaptive control law, the system can make the system more adaptable to external disturbances faster. Because the coefficient design of the traditional super-twisting algorithm needs to consider the upper bound of the disturbance. In the face of unexpected situations such as greater disturbances generated in extreme environments, too high control gain often makes the ROV lose stability. And the introduction of the adaptive control law well solves this problem.

[0220] From Figure 8 It can be seen that the gain k i, i ∈ (1, 2, 3, 4) can be increased according to the adaptive law of the design until a reasonable value is reached, thus ensuring the finite-time stability of the system. When the adaptive parameter of the system reaches an appropriate size, the adaptive parameter k i stops increasing.

[0221] Table 2 shows the control parameters used in the experiment. This is to compare the efficiency of ASTNISMC and NTSMC.

[0222] Table 2

[0223]

[0224] In summary, the ASTNTSMC in the present invention can track the reference trajectory in a shorter time compared with the traditional NTSMC controller. And the steady-state error of the ASTNTSMC controller is smaller than that of the NTSMC controller. The speed chattering of the ASTNTSMC controller is also smaller than that of the NTSMC controller. Thus, it is proved that the combination of the ASTW super-twisting nonsingular terminal sliding mode algorithm can significantly reduce the chattering phenomenon.

Claims

1. A finite-time trajectory tracking control method for an autonomous underwater robot, characterized in that: The following steps are involved: S1. Establish mathematical model and dynamic model of autonomous underwater robot; S2. Design a high-order sliding mode observer for an autonomous underwater robot based on mathematical and dynamic models; S3. Design a non-singular terminal sliding mode controller for finite-time trajectory tracking control based on the mathematical model of the autonomous underwater robot and a high-order sliding mode observer; S31 determines the tracking position error and speed error of the autonomous underwater robot according to the mathematical model of the autonomous underwater robot and the high-order sliding mode observer; S32 designs a non-singular terminal sliding mode controller to make the tracking error of the system converge to zero in a finite time.

2. The finite-time trajectory tracking control method of an autonomous underwater robot according to claim 1, characterized in that: S1 includes the following steps: S11 Establish a mathematical model of an autonomous underwater robot; The mathematical model of the autonomous underwater robot is expressed as: Wherein, η is the position and angle vector of the autonomous underwater robot in the navigation coordinate system; J(η) is the generalized rotation matrix of the carrier coordinate system in the navigation coordinate system; v is the velocity vector of the autonomous underwater robot in the carrier coordinate system; M is the mass matrix; C(v) is the Coriolis matrix; D(v) is the damping matrix; G(η) is the restoring force and torque vector; τ η +τ d is the total force acting on the autonomous underwater robot, τ η is the control input, τ d For external interference; S12 establishes a mathematical model of the autonomous underwater robot according to the mathematical model of the autonomous underwater robot; The velocity of the mathematical model in the inertial coordinate system is introduced into the dynamic model of the autonomous underwater robot to obtain the dynamic model system of the autonomous underwater robot in the inertial coordinate system; Among them, the autonomous underwater robot model parameter system in inertial coordinates is as follows: M η =M(v)J -1 (η) D η =D(v)J -1 (η) G η =J -1 (n)G(n) t η =J -1 (h)t However, due to the influence of the movement of the autonomous underwater robot, the model parameters in the actual system navigation coordinate system are not fixed; therefore, the system model parameters are divided into two parts, namely the nominal value part of the model parameters and the actual value deviation part: In practice, the system dynamics are divided into estimated dynamics and unknown dynamics: in, To estimate the dynamics, calculations were performed based on known model parameters; It is an unknown dynamics, including unknown factors such as model errors, environmental disturbances, and external forces; Taking into account the uncertainty of model parameters, the autonomous underwater robot model is divided into an estimated part and an unknown part: The total disturbance of the system τ dtotal Mainly from: Environmental disturbance τ d : Water currents, wind and waves, temperature changes, etc. affect the motion state of autonomous underwater robots; Unmodeled dynamics Nonlinear characteristics, sensor errors, and control execution errors of autonomous underwater robot propulsion systems; The total disturbance of the system is expressed as: τ dtotal =[d x the y the z the p ] Among them, d x is the disturbance torque in the x-axis direction, d y is the disturbance torque in the y-axis direction, d z is the disturbance torque in the z-axis direction, d p is the perturbation torque about the axis of rotation; Regarding the robot motion state, the translation and rotation motion under the thrust in the object coordinate system is expressed as: The above equation is the rigid body motion equation, involving translational motion and rotational motion; The dynamic model of the autonomous underwater robot is as follows:

3. The finite-time trajectory tracking control method of an autonomous underwater robot according to claim 1, characterized in that: In S2, a high-order sliding mode observer is established based on the dynamic model of the autonomous underwater vehicle to estimate the system state; The position and velocity states can be reconstructed in a limited time. The observation method of the high-order sliding mode observer for the autonomous underwater robot system is as follows: in, z1, z2, z3; Define the observation error variable: e3=τ dtotal -z3; in, z3=k3sign(e1); When z1, z2, z3→0, the robot's speed is observed only based on the position information; it is found that e1, e2, e3 can converge to zero in a limited time; is the observation position information output by the high-order sliding mode observer, is the observed velocity information, and x3=ρ1 is the lumped disturbance of the autonomous underwater robot system.

4. The finite-time trajectory tracking control method of an autonomous underwater robot according to claim 1, characterized in that: S31 specifically includes: The conservative velocity of the autonomous underwater robot in the inertial coordinate system is The lumped interference in the system is The high-order sliding mode observer is stable in finite time, so we can conclude that e can converge in finite time; by choosing appropriate gains k1, k2 and k3; we can find and occurs within a limited time; According to the expected speed and observed speed in the high-order sliding mode observer, the speed error is rewritten as: Select the singular terminal sliding surface, i.e., the sliding surface, for the 4-DOF autonomous underwater robot tracking system function s(t) and obtain its derivative: The entire controller is combined with a high-order sliding mode observer and is expressed as: The tracking position error of the autonomous underwater robot is: The tracking velocity error of the autonomous underwater robot is:

5. The finite-time trajectory tracking control method of an autonomous underwater robot according to claim 1, characterized in that: S32 specifically includes: According to the Lyapunov stability criterion, the system based on adaptive super-twist non-singular terminal sliding mode control can reach a stable state in a finite time, which requires the sliding surface to approach zero; the corresponding convergence time of the control tracking error is as follows: The eigenvalues ​​of the matrix are: The tracking system function of the 4-DOF autonomous underwater robot is: s(t) = [s1 s2 s3 s4] T ; Parameter control of finite time convergence and control smoothness: α = q / p, q>p, 1<α<2; Since the switching term of the traditional sliding mode controller has a discontinuous bounded sign function, the jitter and system performance and accuracy are mutually constrained; This defect uses the super-twist algorithm to generate a continuous switching signal instead of reducing the switching function to reduce the chatter: A non-singular terminal sliding mode controller is designed based on the estimated state information of the system. To this end, the original sliding surface function s(t) is applied and its time derivative is taken The following conclusions were drawn: The adaptive adjustment variable of the control gain is usually used to dynamically adjust the sliding mode gain to enhance the adaptability to system uncertainty; w1: a coefficient related to the system dynamics, which may be related to the error signal on the sliding surface; γ1: A usually positive tuning parameter that affects the convergence rate of the superwarp algorithm; s: sliding surface or sliding variable; μ: a threshold parameter that determines when to adjust the update rule for β, possibly to prevent over-adjustment; θ: When β is less than or equal to β m A fixed gain value used when , possibly to ensure that the system does not over-adjust; β m : A threshold value that determines the critical point at which β adopts different adjustment strategies; The time derivative of the variable ρ, which is usually related to adaptive parameter adjustment in sliding mode control; ε1: a small positive parameter, usually used to tune the adaptability of the controller to ensure smooth convergence; The error at this time Equivalent control is a model based on continuous control. The equivalent control law can realize the control of autonomous underwater robots without external interference, while the sliding surface of the adaptive super-helical algorithm eliminates the beating caused by external interference through discontinuous switching control; including equivalent control law and approximate control law; The total disturbance is expressed as: τ=τ0+τ1 Right now When w1,γ1,ε1,μs satisfy a reasonable positive constant range, the tracking error of the system converges to zero in a finite time, the non-singular terminal sliding mode controller is stable, and when the sliding surface approaches 0, the adaptive gain stops growing; at the same time, η will converge to zero in a finite time.

Images