A second-order sliding mode three-dimensional trajectory tracking control method for AUV based on preset time disturbance observer

Abstract

The present invention discloses a second-order sliding mode three-dimensional trajectory tracking control method for an AUV based on a preset time disturbance observer. This method belongs to the field of underwater vehicle control and includes constructing a mathematical model of the AUV containing external disturbances; designing a preset time disturbance observer using the preset time stability theory; constructing a preset time second-order sliding mode trajectory tracking controller based on the dynamic model and tracking error of the AUV control system by integrating the preset time stability and second-order sliding mode technology; and integrating the constructed disturbance observer and trajectory tracking controller into the AUV dynamic mathematical model to verify the dynamic mathematical model of the AUV control system. By introducing the preset time method, the present invention enables the AUV to quickly track a given desired trajectory, achieves precise control of the AUV's three-dimensional trajectory, and ensures the stability of the system's control performance under external ocean current interference.

Description

Technical Field

[0001] The present invention relates to the field of underwater vehicle control, and is an AUV second-order sliding mode three-dimensional trajectory tracking control method based on a preset time interference observer. Background Art

[0002] With the continuous development of science and technology, autonomous underwater vehicles (AUVs) are gradually becoming an important tool for human exploration of the deep sea. They can assist humans in completing various underwater operations, such as underwater target tracking and seabed exploration. As one of the many uses of AUVs, three-dimensional trajectory tracking control of AUVs is often used to complete various operational tasks with real-time requirements. However, in a complex marine environment with strong nonlinearities, ocean currents and other highly coupled uncertainties, completing the trajectory tracking control task of AUVs in a three-dimensional environment is not easy and has long been faced with various challenges. Therefore, studying the three-dimensional trajectory tracking control of AUVs has important practical value.

[0003] Generally speaking, trajectory tracking control methods are divided into linear and nonlinear methods. Highly nonlinear dynamics, time-varying external disturbances, uncertainty in hydrodynamic coefficients, and other unknown environmental factors make linear methods difficult to apply in practice. Therefore, nonlinear control methods are widely used in AUVs, including model predictive control (MPC), intelligent control, adaptive control, backstepping control, and sliding mode control (SMC). Among these control methods, Lyapunov-based high-order sliding mode control has been widely used in controller design for various AUVs. To improve the convergence speed of existing adaptive nonsingular integral terminal sliding mode control methods, an adaptive fast nonsingular integral terminal sliding mode control method is proposed. This method enables underwater vehicles to maintain the desired speed despite external disturbances. Based on preset performance control and sliding mode control techniques, an adaptive preset performance second-order sliding mode control method is proposed. However, with the continuous deepening of human exploration of the ocean, and the uncertainty of underwater vehicle models and unknown nonlinear external disturbances, the development of sliding mode control remains insufficient.

[0004] Achieving rapid robot stabilization is a key control performance metric for AUV trajectory tracking control. Traditional AUV control methods can only achieve asymptotic stability, resulting in infinite convergence time and difficulty in converging the error state to zero or near zero within a finite time. While finite-time control can ensure that the system state converges to an equilibrium point and the convergence time is bounded, the upper bound of the system convergence time depends on the system's initial conditions, and the convergence time of the system error increases exponentially with increasing initial conditions. Furthermore, finite-time controllers can only improve control accuracy and convergence speed when the system state error converges near an equilibrium point. To obtain an estimable fixed maximum settling time, fixed-time stability was proposed as an advanced improvement to finite-time control. This fixed-time stability method effectively addresses the shortcomings of finite-time control, ensuring that the upper bound of the system convergence time depends only on the design parameters. Furthermore, this method effectively avoids the finite-time control problem of excessively long convergence times caused by initial states far from the equilibrium point. However, the initial state of the AUV significantly affects the convergence time of finite-time stability, and the fixed-time stability control parameters are relatively complex, resulting in only a maximum convergence time. The preset time stability theory can obtain the convergence time in advance through model parameter design, and the convergence speed is fast. Summary of the Invention

[0005] The technical problem to be solved by the present invention is to provide an AUV second-order sliding mode three-dimensional trajectory tracking control method based on a preset time disturbance observer, aiming to solve the problem of completing the AUV three-dimensional trajectory accurate tracking in an underwater environment under unknown complex ocean disturbances.

[0006] To solve the above technical problems, the technical solution adopted by the present invention is: a second-order sliding mode three-dimensional trajectory tracking control method for an AUV based on a preset time disturbance observer, comprising the following steps:

[0007] Step 1: Based on the control input of the AUV control system, nonlinear model parameters and external unknown ocean current disturbances, a dynamic mathematical model of the AUV control system including external disturbances is constructed;

[0008] Step 2: Using the preset time stability theory and the error state of the dynamic mathematical model of the AUV control system, a preset time disturbance observer is designed to observe and approximate the external disturbance.

[0009] Step 3: Based on the dynamic model of the AUV control system and the tracking error, a preset time second-order sliding mode trajectory tracking controller is constructed by integrating the preset time stability theory and the second-order sliding mode technology to improve the stability and robustness of the system;

[0010] Step 4: Combine the preset time disturbance observer, second-order sliding mode controller and the dynamic model of the AUV control system to verify the control performance of the AUV control system.

[0011] A further improvement of the technical solution of the present invention is that the mathematical model in step 1 is specifically as follows:

[0012] Establish the dynamic mathematical model of the AUV control system with external disturbance:

[0013]

[0014] Among them, v=[u,v,w,q,r] T is the velocity in the carrier coordinate system, u, v, w are the AUV's surge, sway, and heave velocities, respectively, and q and r are the AUV's pitch and yaw angular velocities, respectively; η = [x, y, z, θ, ψ] T is the position coordinate in the geodetic coordinate system, τ=[τ1,τ2,τ3,τ4,τ5] T is the input force and moment, τ w =[τ w1 ,τ w2 ,τ w3 ,τ w4 ,τ w5 ] T For external disturbances;

[0015] M∈R 5×5 is the matrix containing the additional mass; D(v)∈R 5×5 is the viscous damping matrix; C(v)∈R 5×5 is the Coriolis force additional matrix; inertia g(η)∈R 5×5 are the restoring force and torque vectors of the AUV;

[0016] D(v)=diag(d 11 ,d 22 ,d 33 ,d 44 ,d 55 ); g(η)=[g1,g2,g3,g4,g5] T ;

[0017] where d 11 =-X u -X uu |u|,d 22 =-Y v -Y vv |v|,d 33 =-Z w -Z ww |w|,d 44 =-M q-M qq |q|,d 55 =-N r -N rr |r|, g1=(PB)sinθ, g2=0, g3=-(PB)cosθ, g4=-Z B Bsinθ, g5=0; P and B are the center r P and r B The gravity and buoyancy of underwater vehicles;

[0018]

[0019] in

[0020] J(η) is the rotation matrix as follows:

[0021]

[0022] A further improvement of the technical solution of the present invention is that the interference observer in step 2 is specifically expressed as follows:

[0023] First, we define v and M as -1 τ w The estimated values ​​of z1(t) and Then the estimation errors can be defined as e1(t)=v-z1 and e2(t)=M -1 τ w -z2; The velocity is defined as follows: Then the preset time disturbance observer model can be expressed as:

[0024]

[0025] Choose appropriate preset time disturbance observer parameters γ, σ, T P , so that the estimation errors e1, e2 converge to zero, set the coefficients 0<γ<1, σ>0, T P >0, the external disturbance can be estimated within the preset time T1, and e2 converges to zero within the preset time; the disturbance is set to meet the Lipschitz condition || M -1 τ w (t)||≤δ2(t-t0), their first-order derivatives are based on ||D w (t)||≤δ2 is bounded, where δ2>0;

[0026]

[0027] because and ||e2(0)||=||M -1 τ w(0)-z2(0)||=0, so e 2,i (0)=0, Thus we get e 1,i (0)>0; means in e 1,i Before changing the sign, sgn(e 1,i (t)) maintain the same 2,i (t)) has the opposite sign, we can get: Where, e1=[e 1,1 ,e 1,2 ,e 1,3 ,e 1,4 ,e 1,5 ] T and e2=[e 2,1 ,e 2,2 ,e 2,3 ,e 2,4 ,e 2,5 ] T , i=1,2...5. At the same time, the convergence time can be calculated:

[0028]

[0029] A further improvement of the technical solution of the present invention is that the specific steps of constructing the trajectory tracking controller in step 3 are as follows:

[0030] Step 3.1: Define tracking error:

[0031]

[0032] Among them, η is the actual trajectory, η d is the expected trajectory; are the first-order derivatives of the actual and desired trajectories, respectively;

[0033] Step 3.2: Define the sliding surface S1 as:

[0034] S1=E2+F0

[0035] in,

[0036] Step 3.3: Define the sliding surface S2 as:

[0037]

[0038] in, The first-order derivative of the sliding surface S1 is

[0039] The τ of the preset time second-order sliding mode trajectory tracking controller is obtained as follows:

[0040]

[0041] in, At the same time, the given parameters are 0<β0<1, 0<β1<1, 0<β2<1, T0>0, T1>0, T2>0.

[0042] A further improvement to the technical solution of the present invention is that in step 4, the control performance of the AUV control system is verified and tested in combination with the constructed model. The test process is as follows: first, the expected trajectory and external unknown disturbance of the AUV tracking are given, and then the various model parts are merged to simulate and verify the preset time convergence and stability of the entire AUV system. The entire AUV system converges within the preset time and has strong anti-vibration ability and robustness.

[0043] A further improvement of the technical solution of the present invention is that the expected trajectory is composed of the following formula:

[0044]

[0045] The external unknown disturbance is determined by the complex environment including ocean currents and waves and is composed of the following formula:

[0046]

[0047] Due to the adoption of the above technical solution, the technical progress achieved by the present invention is as follows: according to research needs, a dynamic model of a five-degree-of-freedom underwater robot with unknown ocean current disturbances is established, including a kinematic model and a dynamic model containing external disturbances, robot position, velocity, and control input; secondly, a preset time disturbance observer is designed using preset time theory and variable replacement, and a preset time disturbance observer algorithm is given based on which the convergence time of the observer is determined by design parameters. The proposed observer can effectively avoid the problem of excessive convergence time caused by the initial state of the system being far away from the equilibrium point. At the same time, the convergence time can be calculated based on the design parameters and is independent of the initial state; secondly, a preset time sliding mode control method is designed by integrating preset time stability and second-order sliding mode technology. The sliding mode control method mainly lies in the design of the sliding mode surface. Once the state reaches the sliding mode surface, it will reach a stable state in an exponential approach manner. Then, a reaching law is designed to express the controller. The Lyapunov function is used as a guarantee of stability, that is, to ensure that S=0 is reachable. At the same time, combined with the preset time stability, the control system can quickly converge and stabilize within the preset time on the basis of reducing the AUV's vibration and improving the system robustness. Finally, the design of the preset time second-order sliding mode control method for three-dimensional trajectory tracking based on the preset time disturbance observer is completed. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are only some embodiments of the present invention, and those skilled in the art can derive other drawings based on these drawings without inventive effort.

[0049] Figure 1 It is a flow chart of the control method of the present invention;

[0050] Figure 2 This is the AUV control system block diagram;

[0051] Figure 3 It is the simulation curve diagram of the expected trajectory and actual trajectory of the AUV system;

[0052] Figure 4 It is the interference observation simulation curve and the real-time simulation curve of disturbance. DETAILED DESCRIPTION

[0053] The present invention is described in further detail below in conjunction with the embodiments:

[0054] like Figure 1 FIG. 1 is a flow chart of a second-order sliding mode three-dimensional trajectory tracking control method for an AUV based on a preset time disturbance observer, which includes the following steps:

[0055] Step 1: Based on the control input of the AUV control system, nonlinear model parameters and external unknown ocean current disturbances, a dynamic mathematical model of the AUV control system including external disturbances is constructed;

[0056] The mathematical model in step 1 is as follows:

[0057] Establish the dynamic mathematical model of the AUV control system with external disturbance:

[0058]

[0059] Among them, v=[u,v,w,q,r] T is the velocity in the carrier coordinate system, u, v, w are the AUV's surge, sway, and heave velocities, respectively, and q and r are the AUV's pitch and yaw angular velocities, respectively; η = [x, y, z, θ, ψ] T is the position coordinate in the geodetic coordinate system, τ=[τ1,τ2,τ3,τ4,τ5] T is the input force and moment, τ w =[τ w1 ,τ w2 ,τ w3 ,τ w4 ,τ w5 ]T For external disturbances;

[0060] M∈R 5×5 is the matrix containing the additional mass; D(v)∈R 5×5 is the viscous damping matrix; C(v)∈R 5×5 is the Coriolis force additional matrix; inertia g(η)∈R 5×5 are the restoring force and torque vectors of the AUV;

[0061] D(v)=diag(d 11 ,d 22 ,d 33 ,d 44 ,d 55 ); g(η)=[g1,g2,g3,g4,g5] T ;

[0062] where d 11 =-X u -X uu |u|,d 22 =-Y v -Y vv |v|,d 33 =-Z w -Z ww |w|,d 44 =-M q -M qq |q|,d 55 =-N r -N rr |r|, g1=(PB)sinθ, g2=0, g3=-(PB)cosθ, g4=-Z B Bsinθ, g5=0. P and B are the center r P and r B The gravity and buoyancy of underwater vehicles;

[0063]

[0064] in

[0065] J(η) is the rotation matrix as follows:

[0066]

[0067] Step 2: Using the preset time stability theory and the error state of the dynamic mathematical model of the AUV control system, a preset time disturbance observer is designed to observe and calculate the external disturbance.

[0068] The disturbance observer in step 2 is expressed as follows:

[0069] Defined as v and M respectively -1 τ w The estimated values ​​of z1(t) and Then the estimation errors can be defined as e1(t)=v-z1 and e2(t)=M -1 τ w -z2; The velocity is defined as follows: Then the preset time disturbance observer is expressed as:

[0070]

[0071] Choose appropriate preset time disturbance observer parameters γ,σ,T P , so that the estimation errors e1, e2 converge to zero, set the coefficients 0<γ<1, σ>0, T P >0, the external disturbance can be estimated within the preset time T1, and e2 converges to zero within the preset time; the disturbance is set to meet the Lipschitz condition || M -1 τ w (t)||≤δ2(t-t0), their first-order derivatives are based on ||D w (t)||≤δ2 is bounded, where δ2>0;

[0072]

[0073] because and ||e2(0)||=||M -1 τ w (0)-z2(0)||=0, so e 2,i (0)=0, Thus we get e 1,i (0)>0; means in e 1,i Before changing the sign, sgn(e 1,i (t)) maintain the same 2,i (t)) has the opposite sign, we can get: Where, e1=[e 1,1 ,e 1,2 ,e 1,3 ,e 1,4 ,e 1,5 ] T and e2=[e 2,1 ,e 2,2 ,e 2,3 ,e 2,4 ,e 2,5 ] T , i=1,2...5. At the same time, the convergence time can be calculated in advance:

[0074]

[0075] Step 3: Based on the dynamic model of the AUV control system and the tracking error, a preset time second-order sliding mode trajectory tracking controller is constructed by integrating preset time stability and second-order sliding mode technology to improve system stability and robustness;

[0076] The specific steps for building the trajectory tracking controller in step 3 are as follows:

[0077] Step 3.1: Define tracking error:

[0078]

[0079] Among them, η is the actual trajectory, η d is the reference trajectory; are the first-order derivatives of the actual and reference trajectories, respectively;

[0080] Step 3.2: Define the sliding surface S1 as:

[0081] S1=E2+F0

[0082] in,

[0083] Step 3.3: Define the sliding surface S2 as:

[0084]

[0085] in, The first-order derivative of the sliding surface S1 is

[0086] The τ of the preset time second-order sliding mode trajectory tracking controller is obtained as follows:

[0087]

[0088] in, At the same time, the given parameters are 0<β0<1, 0<β1<1, 0<β2<1, T0>0, T1>0, T2>0. At the same time, according to Lipuyanov’s theorem, the convergence of the controller can be proved, and the controller converges quickly and reaches stability within the preset time.

[0089] Step 4: Combine the preset time disturbance observer, second-order sliding mode controller and the dynamic model of the AUV control system to verify the control performance of the AUV control system. Figure 3 、 4 As shown in the figure, the reference trajectory, disturbance and design parameters are given to complete the three-dimensional trajectory tracking control of the AUV. The complex environment including ocean currents and waves is composed of the left formula below, and the reference trajectory is composed of the right formula below:

[0090]

[0091] Finally, according to the simulation results, it can be seen that the entire AUV system converges within the preset time and has strong anti-vibration ability and robustness.

[0092] Figure 2 This is the block diagram of the AUV three-dimensional trajectory tracking control system based on a preset time disturbance observer. Figure 3 The figure shows the simulation curve images of the AUV's desired trajectory and actual trajectory. The ideal trajectory represents the AUV's desired trajectory, and the practical trajectory represents the AUV's actual trajectory. It can also be seen from the figure that the actual trajectory quickly tracks the desired trajectory within the preset time and remains stable in subsequent trajectory tracking. Figure 4 The following figure shows the disturbance and disturbance observer simulation images within a 20-second simulation timeframe. The figure shows that the observer can closely approximate external disturbances and estimate them in real time. Therefore, it is clear that the proposed second-order sliding mode 3D trajectory tracking control method for an AUV based on a preset time disturbance observer can accurately track a given desired trajectory in 3D.

[0093] The embodiments described above are merely descriptions of preferred implementations of the present invention and are not intended to limit the scope of the present invention. Without departing from the spirit of the present invention, various modifications and improvements made to the technical solutions of the present invention by ordinary technicians in this field should fall within the scope of protection determined by the claims of the present invention.

Claims

1. A second-order sliding mode three-dimensional trajectory tracking control method for an AUV based on a preset time disturbance observer, characterized by: The steps are as follows: Step 1: Based on the control input of the AUV control system, nonlinear model parameters and external unknown ocean current disturbances, a dynamic mathematical model of the AUV control system including external disturbances is constructed; the mathematical model in step 1 is as follows: Establish the dynamic mathematical model of the AUV control system with external disturbance: Among them, v=[u,v,w,q,r] T is the velocity in the carrier coordinate system, u, v, w are the AUV's surge, sway, and heave velocities, respectively, and q and r are the AUV's pitch and yaw angular velocities, respectively; η = [x, y, z, θ, ψ] T is the position coordinate in the geodetic coordinate system, τ=[τ1,τ2,τ3,τ4,τ5] T is the input force and moment, τ w =[τ w1 ,τ w2 ,τ w3 ,τ w4 ,τ w5 ] T For external disturbances; M∈R 5×5 is the matrix containing the additional mass; D(v)∈R 5×5 is the viscous damping matrix; C(v)∈R 5×5 is the Coriolis force additional matrix; inertia g(η)∈R 5×5 are the restoring force and torque vectors of the AUV; <h2 style=";text-align:left;direction:ltr">D(v)=diag(d<h2 style=";text-align:left;direction:ltr"> 11 <h2 style=";text-align:left;direction:ltr"> ,d<h2 style=";text-align:left;direction:ltr"> 22 <h2 style=";text-align:left;direction:ltr"> ,d<h2 style=";text-align:left;direction:ltr"> 33 <h2 style=";text-align:left;direction:ltr"> ,d<h2 style=";text-align:left;direction:ltr"> 44 <h2 style=";text-align:left;direction:ltr"> ,d<h2 style=";text-align:left;direction:ltr"> 55 <h2 style=";text-align:left;direction:ltr"> );g(η)=[g1,g2,g3,g4,g5]<h2 style=";text-align:left;direction:ltr"> T <h2 style=";text-align:left;direction:ltr"> ; where d 11 = -X u -X |u|u |u|, d 22 = -Y v -Y v|v |v|, d 33 = -Z w -Z |w|w |w|, d 44 = -M q -M |q|q |q|, d 55 = -N r -N |r|r |r|, g1 = (P - B)sinθ, g2 = 0, g3 = -(P - B)cosθ, g4 = -Z B Bsinθ, g5 = 0; in J(η) is the rotation matrix as follows: Step 2: Using the preset time stability theory and the error state of the dynamic mathematical model of the AUV control system, a preset time disturbance observer is designed to observe and approximate the external disturbance. The disturbance observer in step 2 is expressed as follows: First, define v and M -1 τ w The estimated values ​​of z1(t) and Then the estimation errors can be defined as e1(t)=v-z1 and e2(t)=M -1 τ w -z2; The velocity is defined as follows: Then the preset time disturbance observer model can be expressed as: Choose appropriate preset time disturbance observer parameters γ, σ, T P , so that the estimation errors e1, e2 converge to zero, set the coefficients 0<γ<1, σ>0, T P >0, the external disturbance can be estimated within the preset time T1, and e2 converges to zero within the preset time; the disturbance is set to meet the Lipschitz condition || M -1 τ w (t)||≤δ2(t-t0), their first-order derivatives are based on ||D w (t)||≤δ2 is bounded, where δ2>0; because and ||e2(0)||=||M -1 τ w (0)-z2(0)||=0, so e 2,i (0)=0, Thus we get e 1,i (0)>0; means in e 1,i Before changing the sign, sgn(e 1,i (t)) maintain the same 2,i (t)) has the opposite sign, we can get: Where, e1=[e 1,1 ,e 1,2 ,e 1,3 ,e 1,4 ,e 1,5 ] T and e2=[e 2,1 ,e 2,2 ,e 2,3 ,e 2,4 ,e 2,5 ] T , i=1,2...5; at the same time, the convergence time can be calculated in advance: Step 3: Based on the dynamic model and tracking error of the AUV control system, the preset time stability theory and the second-order sliding mode technology are integrated to construct a preset time second-order sliding mode trajectory tracking controller that can improve the stability and robustness of the system. The specific steps for constructing the trajectory tracking controller in step 3 are as follows: Step 3.1: Define tracking error: Among them, η is the actual trajectory, η d is the expected trajectory; are the first-order derivatives of the actual and desired trajectories, respectively; Step 3.2: Define the sliding surface S1 as: S1=E2+F0 in, Step 3.3: Define the sliding surface S2 as: in, The first-order derivative of the sliding surface S1 is The τ of the preset time second-order sliding mode trajectory tracking controller is obtained as follows: in, At the same time, the given parameters are 0<β0<1, 0<β1<1, 0<β2<1, T0>0, T1>0, T2>0; Step 4: Combine the preset time disturbance observer, second-order sliding mode controller and dynamic model of the AUV control system to verify the control performance of the AUV control system. The control performance verification test process of the AUV control system in step 4 is as follows: first, the expected trajectory and external unknown disturbance of the AUV tracking are given, and then the various model parts are merged to simulate and verify the preset time convergence and stability of the entire AUV system. The entire AUV system converges within the preset time and has strong anti-vibration ability and robustness.

2. The AUV second-order sliding mode three-dimensional trajectory tracking control method based on a preset time disturbance observer according to claim 1 is characterized by: The expected trajectory is composed of the following formula: The external unknown disturbance is determined by the complex environment including ocean currents and waves and is composed of the following formula:

Images