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

Abstract

The invention discloses an AUV second-order sliding mode three-dimensional trajectory tracking control method based on a preset time disturbance observer, and belongs to the field of underwater vehicle control, and the method comprises the steps: building an AUV mathematical model containing external disturbance; designing a preset time interference observer by using a preset time stability theory; according to the dynamic model and the tracking error of the AUV control system, fusing preset time stability and a second-order sliding mode technology, and constructing a preset time second-order sliding mode trajectory tracking controller; and connecting the constructed interference observer and the trajectory tracking controller to the dynamic mathematical model of the AUV, and verifying the dynamic mathematical model of the AUV control system. According to the method, the preset time method is introduced, so that the AUV can quickly track the given expected trajectory, the accurate control of the three-dimensional trajectory of the AUV is realized, and the stability of the control performance of the system under the interference of external ocean current is ensured.

Description

Technical Field

[0001] The 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. It can assist humans in completing various underwater operations, such as underwater target tracking and seabed exploration. As one of the many uses of AUVs, the three-dimensional trajectory tracking control of AUVs is usually used to complete various operational tasks with real-time requirements. However, in a complex marine environment with strong nonlinearity, ocean currents and other highly coupled uncertainties, it is not easy to complete the trajectory tracking control task of AUVs in a three-dimensional environment, and it has been facing various challenges for a long time. Therefore, the study of the three-dimensional trajectory tracking control of AUVs has important practical value.

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

[0004] Achieving rapid robot stability is a key control performance indicator for AUV trajectory tracking control. Traditional AUV control methods can only achieve asymptotic stability, which will result in an infinite convergence time and the error state is difficult to converge to zero or a neighborhood close to zero within a finite time. Although finite-time control can ensure that the time and convergence time of the system state to the equilibrium point are bounded, the upper bound of the system convergence time depends on the initial conditions of the system, and the convergence time of the system error increases exponentially with the increase of the initial conditions. In addition, the finite-time controller can only improve the control accuracy and convergence speed of the system when the system state error converges to the vicinity of the equilibrium point. In order to obtain an estimable fixed maximum setting time, fixed-time stability is proposed, which is an advanced improvement of finite-time control. The fixed-time stability method solves the shortcomings of finite-time control well and ensures that the upper bound of the system convergence time only depends on the design parameters. At the same time, this method effectively avoids the finite-time control problem that the initial state is far away from the equilibrium point, resulting in a long convergence time. However, the initial state of the AUV has a great influence on the convergence time of finite-time stability, and the fixed-time stability control parameters are relatively complex, and only the maximum convergence time can be obtained. 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 accurate three-dimensional trajectory tracking of the AUV in an underwater environment under unknown complex ocean disturbances.

[0006] In order 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 of an AUV based on a preset time disturbance observer, comprising the following steps:

[0007] Step 1: According to 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 with 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 situation;

[0009] Step 3: According to the dynamic model of the AUV control system and the tracking error, 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;

[0010] 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.

[0011] The 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 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 sway, swing and heave velocities, q and r are the AUV's pitch and pitch 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 shown below:

[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 -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 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 satisfy the Lipschitz condition || M -1 τ w (t)||≤δ2(t-t0), their first-order derivatives are ||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)) maintains 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 of 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 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 scheme, 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, speed, 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, and 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, the reaching law is designed to express the controller, and 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 jitter of the AUV and improving the robustness of the system; 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 drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative work.

[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 further described in detail below in conjunction with embodiments:

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

[0055] Step 1: According to 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 with external disturbances is constructed;

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

[0057] Establish the dynamic mathematical model of 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 sway, swing and heave velocities, q and r are the AUV's pitch and pitch 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 shown below:

[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 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 satisfy the Lipschitz condition || M -1 τ w (t)||≤δ2(t-t0), their first-order derivatives are ||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)) maintains 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: According to the dynamic model of the AUV control system and the tracking error, the preset time stability 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;

[0076] The specific steps to build 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, 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 structural block diagram of the AUV three-dimensional trajectory tracking control system based on the preset time disturbance observer. ; Figure 3 The figure shows the simulation curve images of the expected trajectory and the actual trajectory of the AUV. The ideal trajectory represents the expected trajectory of the AUV, and the practical trajectory represents the actual trajectory of the AUV. It can also be seen from the figure that the actual trajectory quickly tracks the expected trajectory within the preset time and remains stable in the subsequent trajectory tracking. Figure 4 The disturbance and disturbance observer simulation images in the 20s simulation time are shown in the figure. It can be seen from the figure that the observer can well approximate the external disturbance and make real-time estimates of it. Therefore, it is obvious that the AUV second-order sliding mode three-dimensional trajectory tracking control method based on the preset time disturbance observer proposed in this paper can achieve accurate tracking of the given desired trajectory in three dimensions.

[0093] The embodiments described above are merely descriptions of preferred implementation modes of the present invention, and are not intended to limit the scope of the present invention. Without departing from the design 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 all fall within the protection scope determined by the claims of the present invention.

Claims

1. A second-order sliding mode three-dimensional trajectory tracking control method for AUV based on a preset time disturbance observer, characterized in that: The steps include: Step 1: According to 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 with external disturbances is constructed; 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 situation; Step 3: According to the dynamic model of the AUV control system and the tracking error, 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; 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.

2. According to claim 1, a method for AUV second-order sliding mode three-dimensional trajectory tracking control based on a preset time disturbance observer is characterized in that: The mathematical model in step 1 is as follows: Establish the dynamic mathematical model of 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 sway, swing and heave velocities, q and r are the AUV's pitch and pitch 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=(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; in J(η) is the rotation matrix as shown below:

3. 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 in that: The disturbance observer in step 2 is expressed as follows: First, we 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 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 satisfy the Lipschitz condition || M -1 τ w (t)||≤δ2(t-t0), their first-order derivatives are ||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)) maintains 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:

4. 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 in that: The specific steps to build 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.

5. 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 in that: In step 4, combined with the constructed model, the control performance of the AUV control system is verified and tested 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.

6. The AUV second-order sliding mode three-dimensional trajectory tracking control method based on a preset time disturbance observer according to claim 5 is characterized in that: 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