Underwater vehicle trajectory tracking control method of dynamic integral disturbance observer

Abstract

The invention relates to an underwater vehicle trajectory tracking control method based on a dynamic integral disturbance observer, and aims to solve the problems of low convergence speed and high energy consumption caused by the influence of lumped disturbance and ocean current change on an underwater vehicle in a conventional trajectory tracking method. Firstly, ocean current change and lumped disturbance are introduced during three-degree-of-freedom modeling, then a disturbance estimation error is adopted as a sliding mode surface to design a dynamic integral disturbance observer, a disturbance estimation value is updated in real time and adapts to disturbance change, a low-overshoot self-adaptive ocean current observer is designed, regulatory factor estimation is added, and initial overshoot is reduced. And system errors are prevented from being increased. And finally, designing a smooth rapid double-power sliding mode controller, constructing a control law by combining disturbance and ocean current estimation values, constructing an adaptive law by tracking errors and controlling energy consumption, and dynamically adjusting parameters of the control law to reduce energy consumption and improve the convergence speed. According to the method, the tracking energy consumption is reduced by about 25.93%, and the position error convergence speed and the speed error convergence speed are improved by 16.22% and 16.09% respectively.

Description

Technical Field

[0001] The present invention relates to the field of marine engineering equipment, and belongs to underwater vehicle motion control, and in particular to an underwater vehicle trajectory tracking control method using a dynamic integral disturbance observer. Background Art

[0002] As an ocean water quality and ecological element, acoustic, optical measurement and detection equipment, underwater submersibles have become an important tool for various ocean observation and exploration tasks such as marine environmental services, mineral geological surveys and underwater salvage. Especially in the process of underwater operations, underwater submersibles are used as underwater emergency disaster reduction and fire fighting equipment, underwater facility emergency equipment, breathing apparatus protection devices and other underwater salvage equipment manufacturing, providing an indispensable guarantee for marine operations. The successful execution of these tasks requires underwater submersibles to have precise trajectory tracking control performance. However, since the control system of underwater submersibles is affected by coupling nonlinearity, hydrodynamic parameter uncertainty, mathematical model parameter uncertainty, unknown environmental interference and time-varying ocean currents, traditional linear control methods are hindered from achieving the required tracking control performance. Therefore, precise control is a difficult and challenging task for underwater submersibles.

[0003] Patent CN11831011A "A method for planar trajectory tracking control of an underwater robot" states: When designing a disturbance observer to perform a lumped estimate of the disturbance, only the external time-varying interference is considered, while the model uncertainty of the object in dynamics and the impact of underwater current changes are not considered. This design does not quite meet actual needs, especially in complex and highly uncertain scenarios, where the accuracy and stability of trajectory tracking are difficult to meet requirements.

[0004] Patent CN114442640A "A surface unmanned boat trajectory tracking control method": The designed ocean current adaptive estimation law shows a large overshoot in the initial stage, which affects the accuracy of the initial estimation and increases the error of the system.

[0005] In the paper "Fast Finite-Time Super-Twisting Sliding Mode Control with an Extended State Higher-Order Sliding Mode Observer for UUV Trajectory Tracking", the extended state high-order sliding mode observer designed can provide good dynamic performance, but the fast dynamic response will cause excessive overshoot of the system, causing system instability, and the calculation is complex. The designed controller parameters are not adaptively adjusted, which will affect the control performance when facing environmental changes.

[0006] The paper "Trajectory Tracking Control of Remotely Operated Vehicles via a Fast-Sliding Mode Controller with a Fixed-Time Disturbance Observer" has the following problems:

[0007] (1) The model in the paper does not include the influence of ocean currents. The effect of ocean currents on underwater submersibles often presents nonlinear and time-varying characteristics, which will affect the performance of the controller. In practical applications, it is difficult to achieve trajectory tracking effects.

[0008] (2) The fixed-time disturbance observer in the paper uses the sign function and directly estimates the disturbance. Although it is simple and easy to implement, it has problems such as jitter, slow response, and low estimation accuracy in practical applications.

[0009] (3) The paper adopts the fast reaching law to increase the control force through the high gain term. Although it achieves fast response and high-precision control, it also brings a larger control input amplitude, resulting in increased energy consumption and reduced endurance of the underwater submersible. Summary of the invention

[0010] The purpose of the present invention is to propose an underwater vehicle trajectory tracking control method based on a dynamic integral disturbance observer to solve the problems of slow tracking speed, low accuracy and high energy consumption of the underwater vehicle in the trajectory tracking task.

[0011] In order to solve the above problems, the present invention adopts the following technical scheme: a method for underwater vehicle trajectory tracking control of a dynamic integral disturbance observer is designed, taking into account that the underwater submersible will be affected by external disturbances, model uncertainty and ocean current changes when sailing underwater, and ocean current changes and lumped disturbances are added when three-degree-of-freedom modeling of the underwater submersible is performed. At the same time, a dynamic integral disturbance observer is designed to perform lumped observation of the disturbance and a low overshoot adaptive ocean current observer is designed to estimate the ocean current. Then, the lumped disturbance and the ocean current estimation values ​​are combined with a smooth and fast double-power sliding mode controller to design a trajectory tracking control law, so as to achieve precise tracking of the underwater submersible.

[0012] To achieve the above purpose, the present invention adopts the following design scheme. The specific steps are as follows:

[0013] S1: Construct the three-degree-of-freedom kinematic model and dynamic model of the underwater vehicle;

[0014] S2: Based on the constructed underwater vehicle model, a dynamic integral disturbance observer is designed to estimate unknown disturbances and a low overshoot adaptive ocean current observer is designed to estimate ocean currents;

[0015] S3: According to the tracking error, design the second-order sliding mode function and the smooth and fast double power reaching law;

[0016] S4: Based on S3, a smooth and fast bi-power sliding mode controller is constructed, and the estimated value of the S2 disturbance observer and the ocean current estimate are compensated to the controller output to achieve trajectory tracking control of the underwater submersible under unknown time-varying disturbances and ocean current changes.

[0017] In S1, a three-degree-of-freedom kinematic model and a dynamic model of the underwater vehicle are constructed:

[0018] The three-degree-of-freedom kinematic model of the underwater vehicle is established as:

[0019]

[0020] Where η=[x,y,ψ] T represents the vector composed of the position information [x, y] and the heading angle information ψ of the underwater submersible in the geodetic coordinate system, where x and y represent the horizontal and vertical coordinates of the underwater submersible in the geodetic coordinate system, respectively, and v r =[u,v,r] T represents the velocity vector of the underwater vehicle relative to the ocean current in the object coordinates, which is a vector composed of the longitudinal velocity u, the transverse velocity v and the yaw angular velocity r. J(η) represents the conversion matrix between the inertial coordinate system and the object coordinate system. v f =[u f ,v f ,0] is the ocean current velocity vector, u f is the lateral velocity of the ocean current, v f is the longitudinal velocity of the ocean current, v c is the ocean current velocity in the object coordinate system.

[0021] Establish the dynamic model of underwater submersible:

[0022]

[0023] In the formula, τ E represents the unknown time-varying external disturbance force vector, τ T represents the control force and torque vector, M=M0+ΔM is the inertia matrix, C(ν r )=C0(ν r )+ΔC(ν r ) is the Coriolis and centripetal matrix, D(ν r )=D0(ν r )+ΔD(ν r ) is the damping matrix, M0, C0(ν) and D0(ν) are nominal matrices, ΔM, ΔC(ν) and ΔD(ν) represent model uncertainties, and the model uncertainty vector in underwater vehicle dynamics is expressed as:

[0024] The kinetic model of formula (2) can be rewritten as

[0025]

[0026] In the formula, τ D =τ E +τ M represents the dynamic lumped disturbance vector, including the model uncertainty term τ M and the external disturbance term τ E .

[0027] In S2, based on the constructed underwater vehicle model, a low overshoot adaptive ocean current observer and a dynamic integral disturbance observer are designed to estimate the ocean current velocity and unknown disturbances:

[0028] The low overshoot adaptive ocean current observer is designed as follows:

[0029] Ocean Current f The lateral speed is:

[0030]

[0031] In the formula, is the estimated value of the lateral position coordinate x of the underwater vehicle The first derivative of , l1, l x1 , l x2 , l x3 , l x is a normal number, is the lateral velocity u of the ocean current f The estimated value of for Adjustment factor estimation, yes The first derivative of .

[0032] Ocean Currents f The longitudinal speed is:

[0033]

[0034] In the formula, is the estimated value of the longitudinal position coordinate y of the underwater vehicle The first derivative of, l2, l y1 , l y2 , l y3 , l y is a normal number, is the longitudinal velocity v of the ocean current f The estimated value of for Adjustment factor estimation, yes The first derivative of .

[0035] According to the dynamic model (3), the disturbance error of the underwater submersible is designed to construct the state vector of the auxiliary power system: Its form is:

[0036]

[0037] In the formula, is τ D estimates;

[0038] The designed sliding mode variables are:

[0039]

[0040] In the formula, s1=[s 11 s 12 s 13 ] T represents the estimation error;

[0041] Then, the following integral sliding mode variables are adopted:

[0042]

[0043] Where 0<α<1, β>1, k1>0, k2>0, k3>0 and k4>0 are design parameters, s2=[s 21 s 22 s 23 ] T ;

[0044] The derivative of the integral sliding mode variable in formula (8) is:

[0045]

[0046] According to formula (9), design The estimation law is:

[0047]

[0048] In order to reduce unnecessary chattering and affect the estimation performance of the disturbance observer, the smooth_sign function is used instead of the sgn function, and the smooth_sign function is designed as:

[0049]

[0050] Where μ>0,

[0051] Formula (10) can be rewritten as:

[0052]

[0053] Wherein, 0<γ<1, λ>1, k5>0, k6>0, and k7>0 are design parameters.

[0054] In S3, according to the position tracking error of the underwater vehicle, a second-order sliding surface and a smooth and fast bi-power reaching law are designed:

[0055] Based on disturbance and ocean current estimation and model uncertainty observation, a smooth and fast double-power sliding mode control method is designed to ensure that the underwater submersible can track the target trajectory quickly, with high precision and low energy consumption. The details are as follows:

[0056] The underwater vehicle tracking error vector is defined as:

[0057] e η =η-η d (13)

[0058] Where η d is the target trajectory vector, where η d , and is known, continuous and bounded, the derivative estimate of the underwater vehicle's position tracking error vector is defined as:

[0059]

[0060] Design the sliding surface related parameters σ1, σ2 to satisfy the following equations:

[0061] σ1=e η +K1∫e η dt (15)

[0062]

[0063] The second-order sliding surface is defined as:

[0064]

[0065] Where, K1>0, K2>0, S=[S1 S2 S3];

[0066] Taking the derivative of the above formula, we get the following relationship:

[0067]

[0068] Substituting formula (3) into formula (18) yields:

[0069]

[0070] In the formula, f(v r,η)=C0(ν r ) r +D0(ν r ) r ;

[0071] In order to enable the control system of the underwater submersible to quickly converge to the sliding surface when the deviation from the trajectory is large, improve the response speed of the system, ensure the stability of the underwater submersible during underwater navigation, avoid excessive vibration and increase energy consumption, a smooth and fast double power reaching law is designed:

[0072]

[0073] In the formula, is the adjustment function, 0<ι<1, ρ>0, θ1>0, θ2>0 are design parameters, and the nonlinear function fal(S,θ1,ξ) is defined as follows:

[0074]

[0075] In the formula, ξ is a small positive number;

[0076] In S4, a smooth and fast bi-power sliding mode controller is constructed based on S3, and the estimation of the S2 disturbance observer and the ocean current estimation are compensated to the controller output to realize the trajectory tracking control of the underwater submersible under unknown time-varying disturbances and time-varying ocean currents.

[0077] Based on the dynamic integral disturbance observer, low overshoot adaptive ocean current observer, second-order sliding surface and smooth and fast double-power reaching law designed in the above steps, the smooth and fast double-power sliding mode control law is designed as follows:

[0078]

[0079] In the formula, is the estimated value of ocean current, is the estimated first-order derivative of the ocean current, is τ MD The observed estimate of is the estimated value of φ1, is the estimated value of φ2, and the designed parameter adaptation law is as follows:

[0080]

[0081] In the formula, represents the tracking energy consumption of the underwater vehicle, γ1, γ2, ω1, ω2 are positive adaptive design parameters.

[0082] The present invention has the following beneficial effects:

[0083] 1. The present invention provides an underwater submersible trajectory tracking control method with a dynamic integral disturbance observer, which takes into account the influence of external disturbances, model uncertainty and ocean current changes on the underwater submersible, and designs a smooth and fast dual-power sliding mode controller, which can effectively suppress the system's chattering, improve the system's stability, and reduce the energy consumption in tracking. Compared with the conventional second-order sliding mode control method of the submersible, the tracking energy consumption is reduced by about 25.93%.

[0084] 2. Compared with the conventional second-order sliding mode control method of the submersible, it has a faster convergence speed. Among them, in the position tracking error and speed tracking error curves, compared with the conventional second-order sliding mode of the submersible, the position tracking error convergence speed increased by 16.22%, and the speed tracking error convergence speed increased by 16.09%.

[0085] 3. The dynamic integrated disturbance observer and low overshoot adaptive ocean current observer designed by the method described in the present invention can better observe the aggregate disturbance and estimate the ocean current. The aggregate disturbance estimation accuracy reaches 91.36%, and the ocean current estimation accuracy reaches 95.12%. BRIEF DESCRIPTION OF THE DRAWINGS

[0086] Figure 1 This is the block diagram of the trajectory tracking control system;

[0087] Figure 2 The desired trajectory tracking curve for the horizontal plane;

[0088] Figure 3 is a position tracking graph;

[0089] Figure 4 It is a speed tracking curve graph;

[0090] Figure 5 is the position tracking error curve;

[0091] Figure 6 It is the speed error tracking curve diagram;

[0092] Figure 7 It is a comparison diagram of the control inputs τ1, τ2, τ3 of the present invention and the conventional second-order sliding mode control input of the submersible;

[0093] Figure 8 is the estimated curve of the lumped disturbance observation;

[0094] Fig. 9 Estimated map for ocean current observations;

[0095] Fig.10 To track energy consumption graph. DETAILED DESCRIPTION

[0096] like Figure 1As shown, the present invention provides a method for underwater vehicle trajectory tracking control using a dynamic integrated disturbance observer, comprising the following steps:

[0097] S1: Construct the three-degree-of-freedom kinematic model and dynamic model of the underwater vehicle;

[0098] S2: Based on the constructed underwater vehicle model, a dynamic integral disturbance observer is designed to estimate unknown disturbances and a low overshoot adaptive ocean current observer is designed to estimate ocean currents;

[0099] S3: According to the tracking error, design the second-order sliding mode function and the smooth and fast double power reaching law;

[0100] S4: Based on S3, a smooth and fast bi-power sliding mode controller is constructed, and the estimated value of the S2 disturbance observer and the ocean current estimate are compensated to the controller output to achieve trajectory tracking control of the underwater submersible under unknown time-varying disturbances and ocean current changes.

[0101] In S1, a three-degree-of-freedom kinematic model and a dynamic model of the underwater vehicle are constructed:

[0102] The three-degree-of-freedom kinematic model of the underwater vehicle is established as:

[0103]

[0104] Where η=[x,y,ψ] T represents the vector composed of the position information [x, y] and the heading angle information ψ of the underwater submersible in the geodetic coordinate system, where x and y represent the horizontal and vertical coordinates of the underwater submersible in the geodetic coordinate system, respectively, and v r =[u,v,r] T represents the velocity vector of the underwater vehicle relative to the ocean current in the object coordinates, which is a vector composed of the longitudinal velocity u, the transverse velocity v, and the yaw angular velocity r. J(η) represents the conversion matrix between the inertial coordinate system and the object coordinate system. v f =[u f ,v f ,0] is the ocean current velocity vector, u f is the lateral velocity of the ocean current, v f is the longitudinal velocity of the ocean current, v c is the ocean current velocity in the object coordinate system, v is the ocean current velocity v f The relative speed v with the underwater vehicle r The generalized speed between .

[0105] Establish the dynamic model of underwater submersible:

[0106]

[0107] In the formula, τ Erepresents the unknown time-varying external disturbance force vector, τ T represents the control force and torque vector, M=M0+ΔM is the inertia matrix, C(ν r )=C0(ν r )+ΔC(ν r ) is the Coriolis and centripetal matrix, D(ν r )=D0(ν r )+ΔD(ν r ) is the damping matrix, M0, C0(ν) and D0(ν) are nominal matrices, ΔM, ΔC(ν) and ΔD(ν) represent model uncertainties, and the model uncertainty vector in underwater vehicle dynamics is expressed as:

[0108] The kinetic model of formula (2) can be rewritten as

[0109]

[0110] In the formula, τ D =τ E +τ M represents the dynamic lumped disturbance vector, including the model uncertainty term τ M and the external disturbance term τ E .

[0111] In S2, based on the constructed underwater vehicle model, a low overshoot adaptive ocean current observer and a dynamic integral disturbance observer are designed to estimate the ocean current velocity and unknown disturbances:

[0112] The low overshoot adaptive ocean current observer is designed as follows:

[0113] Ocean Current f The lateral speed is:

[0114]

[0115] In the formula, is the estimated value of the lateral position coordinate x of the underwater vehicle The first derivative of , l1, l x1 , l x2 , l x3 , l x is a normal number, is the lateral velocity u of the ocean current f The estimated value of for Adjustment factor estimation, yes The first derivative of .

[0116] Ocean Currents f The longitudinal speed is:

[0117]

[0118] In the formula, is the estimated value of the longitudinal position coordinate y of the underwater vehicle The first derivative of, l2, l y1 , l y2 , l y3 , l y is a positive constant, is the longitudinal velocity v of the ocean current f The estimated value of for Adjustment factor estimation, yes The first derivative of .

[0119] In order to further improve the robust performance of the system and avoid the high gain control problem, a dynamic integral convergence disturbance observer is designed as follows:

[0120] According to the dynamic model (4), the state vector of the auxiliary power system is constructed by designing the disturbance error of the underwater submersible: Its form is:

[0121]

[0122] In the formula, is τ D estimates;

[0123] The designed sliding mode variables are:

[0124]

[0125] In the formula, s1=[s 11 s 12 s 13 ] T represents the estimation error;

[0126] Then, the following integral sliding mode variables are adopted:

[0127]

[0128] Where 0<α<1, β>1, k1>0, k2>0, k3>0 and k4>0 are design parameters, s2=[s 21 s 22 s 23 ] T ;

[0129] The derivative of the integral sliding mode variable in formula (9) is:

[0130]

[0131] According to formula (10), the design The estimation law is:

[0132]

[0133] In order to reduce unnecessary chattering and affect the estimation performance of the disturbance observer, the smooth_sign function is used instead of the sgn function, and the smooth_sign function is designed as:

[0134]

[0135] Where μ>0,

[0136] Formula (11) can be rewritten as:

[0137]

[0138] Wherein, 0<γ<1, λ>1, k5>0, k6>0 and k7>0 are design parameters.

[0139] In S3, according to the position tracking error of the underwater vehicle, a second-order sliding surface and a smooth and fast bi-power reaching law are designed:

[0140] Based on disturbance and ocean current estimation and model uncertainty observation, a smooth and fast double-power sliding mode control method is designed to ensure that the underwater submersible can track the target trajectory quickly, with high precision and low energy consumption. The details are as follows:

[0141] The underwater vehicle tracking error vector is defined as:

[0142] e η =η-η d (14)

[0143] Where η d is the target trajectory vector, where η d , and is known, continuous and bounded, the derivative estimate of the underwater vehicle's position tracking error vector is defined as:

[0144]

[0145] Design the sliding surface related parameters σ1, σ2 to satisfy the following equations:

[0146] σ1=e η +K1∫e η dt (16)

[0147]

[0148] The second-order sliding surface is defined as:

[0149]

[0150] Where, K1>0, K2>0, S=[S1 S2 S3];

[0151] Taking the derivative of the above formula, we get the following relationship:

[0152]

[0153] Substituting formula (4) into formula (19) yields:

[0154]

[0155] In the formula, f(v r ,η)=C0(ν r ) r +D0(ν r ) r ;

[0156] In order to enable the control system of the underwater submersible to quickly converge to the sliding surface when the deviation from the trajectory is large, improve the response speed of the system, ensure the stability of the underwater submersible during underwater navigation, avoid excessive vibration and increase energy consumption, a smooth and fast double power reaching law is designed:

[0157]

[0158] In the formula, is a regulating function used to dynamically adjust the approach rate of the system. When the underwater submersible deviates far from the target trajectory, g(S) decreases, increasing the approach force to quickly return the underwater submersible to the target trajectory; when the underwater submersible approaches the target trajectory, the control force is reduced to reduce vibration and unnecessary energy consumption. 0<ι<1, ρ>0, θ1>0, θ2>0 are design parameters, and the nonlinear function fal(S,θ1,ξ) is defined as follows:

[0159]

[0160] In the formula, when |S|≤ζ, use As the control quantity, the smoothness of the control is increased, thereby suppressing chattering when approaching the sliding surface; when |S|>ζ, the In the form of, when S is large, it provides greater control force and improves the convergence rate of the system, where ξ is a small positive number;

[0161] In S4, a smooth and fast bi-power sliding mode controller is constructed based on S3, and the estimation of the S2 disturbance observer and the ocean current estimation are compensated to the controller output to realize the trajectory tracking control of the underwater submersible under unknown time-varying disturbances and time-varying ocean currents.

[0162] Based on the dynamic integral disturbance observer, low overshoot adaptive ocean current observer, second-order sliding surface and smooth and fast double-power reaching law designed in the above steps, the smooth and fast double-power sliding mode control law is designed as follows:

[0163]

[0164] In the formula, is the estimated value of ocean current, is the estimated first-order derivative of the ocean current, is τ MD The observed estimate of is the estimated value of φ1, is the estimated value of φ2, and the designed parameter adaptation law is as follows:

[0165]

[0166] In the formula, represents the tracking energy consumption of the underwater vehicle, γ1, γ2, ω1, ω2 are positive adaptive design parameters.

[0167] Simulation of the method of the present invention

[0168] The inertia matrix M0, scalar Coriolis and centripetal matrix C0(v), and scalar water damping coefficient matrix D0(v) of the underwater vehicle under simulation are respectively:

[0169]

[0170] The expected trajectory is η d =[2sin(0.1t)2cos(0.1t)0.1t] T , the initial position and velocity are and v0 =

[000] T , the system uncertainty is selected as ΔM=0.1M0, ΔC(v r )=0.1C0(v r ) and ΔD(v r )=0.1D0(v r ), the ocean current is set to v f =[0.30.350], the external disturbance is selected as τ E =[3.6sin(0.3t)2.4cos(0.6t)0.4sin(0.2t)] T .

[0171] Table 1 Parameters of low overshoot adaptive ocean current observer

[0172]

[0173] Table 2 Dynamic integral disturbance observer parameter values

[0174]

[0175] Table 3 Controller parameter values

[0176]

[0177] Figure 2 is the trajectory tracking control curve. It can be seen from the figure that the method proposed in the present invention and the conventional second-order sliding mode control method of the submersible can both achieve accurate trajectory tracking. Figure 3 and Figure 4 Respectively represent the time history of position and velocity tracking, from Figures 2 to 4 It can be seen that under the influence of lumped disturbances and ocean currents, the method proposed in the present invention can converge to the desired signal faster and more accurately in terms of actual position and speed than the conventional second-order sliding mode control method of the submersible.

[0178] Figure 5 and Figure 6 are the position and velocity tracking errors respectively, Figure 5 and Figure 6 It can be seen that in the presence of external disturbances, model uncertainty and the influence of ocean current changes, the position and velocity errors of the method of the present invention and the conventional second-order sliding mode method of the submersible can converge, and the method of the present invention has a faster convergence speed. The position tracking error convergence speed is improved by 16.22% compared with the conventional second-order sliding mode control method of the submersible, and the velocity tracking error convergence speed is improved by 16.09%.

[0179] Figure 7 For control input Comparison diagram with conventional second-order sliding mode control input of submersible, control input Compared with the conventional second-order sliding mode control method of the submersible, it can effectively suppress the system's chattering and improve the system's stability.

[0180] Figure 8 This is the lumped disturbance observation estimation diagram. For the unknown lumped disturbance estimation, the estimation accuracy reaches 91.36%.

[0181] Fig. 9 This is the ocean current observation estimation diagram. It can be seen that the ocean current is effectively estimated, and the estimation accuracy reaches 95.12%.

[0182] Fig.10Tracking the energy consumption shows that the method of the present invention can effectively reduce the impact of disturbances and ocean currents on the system, and compared with the conventional second-order sliding mode of the submersible, the energy consumption is reduced by about 25.93%.

[0183] Finally, it should be noted that the specific implementation scheme described above further illustrates the purpose, technical scheme and beneficial effects of the present invention. The above embodiments are only used to illustrate the technical scheme of the present invention, rather than to limit the scope of protection of the present invention. Those skilled in the art should understand that any modification or equivalent substitution of the technical scheme of the present invention is included in the scope of protection of the present invention.

Claims

1. A method for tracking underwater vehicle trajectory using a dynamic integrated disturbance observer, characterized in that: The following steps are involved: S1: Construct the three-degree-of-freedom kinematic model and dynamic model of the underwater vehicle; S2: Based on the constructed underwater vehicle model, a dynamic integral disturbance observer is designed to estimate unknown disturbances and a low overshoot adaptive ocean current observer is designed to estimate ocean currents; S3: According to the tracking error, design the second-order sliding mode function and the smooth and fast double power reaching law; S4: Based on S3, a smooth and fast bi-power sliding mode controller is constructed, and the estimated value of the S2 disturbance observer and the ocean current estimate are compensated to the controller output to achieve the trajectory tracking control of the underwater submersible under unknown time-varying disturbances and ocean current changes; In S1, a three-degree-of-freedom kinematic model and a dynamic model of the underwater vehicle are constructed: The three-degree-of-freedom kinematic model of the underwater vehicle is established as: Where η = [x, y, ψ] T Represents the position information [x, y] and heading angle information of the underwater vehicle in the geodetic coordinate system ψ The vectors x and y represent the horizontal and vertical coordinates of the underwater vehicle in the geodetic coordinate system, respectively, and v r =[u,v,r] T represents the velocity vector of the underwater vehicle relative to the ocean current in the object coordinates, which is a vector composed of the longitudinal velocity u, the transverse velocity v and the yaw angular velocity r. J(η) represents the conversion matrix between the inertial coordinate system and the object coordinate system. v f =[u f ,v f ,0] is the ocean current velocity vector, u f is the lateral velocity of the ocean current, v f is the longitudinal velocity of the ocean current, v c is the ocean current velocity in the object coordinate system; Establish the dynamic model of underwater submersible: In the formula, τ E represents the unknown time-varying external disturbance force vector, τ T represents the control force and torque vector, M0, C0(ν) and D0(ν) are nominal matrices, ΔM, ΔC(ν) and ΔD(ν) represent model uncertainties, and the model uncertainty vector in underwater vehicle dynamics is expressed as: The kinetic model of formula (2) can be rewritten as: In the formula, τ D =τ E +τ M represents the dynamic lumped disturbance vector, including the model uncertainty term τ M and the external disturbance term τ E ; In S2, based on the constructed underwater vehicle model, a low overshoot adaptive ocean current observer and a dynamic integral disturbance observer are designed to estimate the ocean current velocity and unknown disturbances: The low overshoot adaptive ocean current observer is designed as follows: Ocean Current f The lateral speed is: In the formula, is the estimated value of the lateral position coordinate x of the underwater vehicle The first derivative of , l1, l x1 , l x2 , l x3 , l x is a positive constant, is the lateral velocity u of the ocean current f The estimated value of for Adjustment factor estimation, yes The first derivative of ; Ocean Currents f The longitudinal speed is: In the formula, is the estimated value of the longitudinal position coordinate y of the underwater vehicle The first derivative of, l2, l y1 , l y2 , l y3 , l y is a positive constant, is the longitudinal velocity v of the ocean current f The estimated value of for Adjustment factor estimation, yes The first derivative of ; According to the dynamic model (3), the disturbance error of the underwater submersible is designed to construct the state vector of the auxiliary power system: Its form is: In the formula, is τ D estimates; The designed sliding mode variables are: In the formula, s1=[s 11 s 12 s 13 ] T represents the estimation error; Then, the following integral sliding mode variables are adopted: Where 0<α<1, β>1, k1>0, k2>0, k3>0, and k4>0 are design parameters, s2=[s 21 s 22 s 23 ] T ; The derivative of the integral sliding mode variable in formula (8) is: According to formula (9), design The estimation law is: In order to reduce unnecessary chattering and affect the estimation performance of the disturbance observer, the smooth_sign function is used instead of the sgn function, where 0<γ<1, λ>1, k5>0, k6>0 and k7>0 are design parameters; In S3, according to the position tracking error of the underwater vehicle, a second-order sliding surface and a smooth and fast bi-power reaching law are designed: The underwater vehicle tracking error vector is defined as: e η =th-th d (11) Where η d is the target trajectory vector, η d , and is known, continuous and bounded, the derivative estimate of the underwater vehicle's position tracking error vector is defined as: Design the sliding surface related parameters σ1, σ2 to satisfy the following equations: σ1=e η +K1∫e η dt (13) The second-order sliding surface is defined as: Where, K1>0, K2>0, S=[S1 S2 S3]; Taking the derivative of the above equation, we get the following relationship: Substituting formula (3) into formula (16), we can obtain: where f(v r , η) = C0(ν r )ν r + D0(ν r )ν r ; A smooth and fast bi-power reaching law is designed as: In the formula, is the adjustment function, 0<ι<1, ρ>0, θ1>0, θ2>0 are design parameters, fal(S,θ1,ξ) is a nonlinear function, ξ is a small positive number; In S4, a smooth and fast bi-power sliding mode controller is constructed according to S3, and the estimation of the disturbance observer of S2 and the ocean current estimation are compensated to the controller output to realize the trajectory tracking control of the underwater submersible under unknown time-varying disturbances and time-varying ocean currents; Based on the dynamic integral disturbance observer, low overshoot adaptive ocean current observer, second-order sliding surface and smooth and fast double-power reaching law designed in the above steps, the smooth and fast double-power sliding mode control law is designed as follows: In the formula, is the estimated value of ocean current, is the estimated first-order derivative of the ocean current, is τ MD The observed estimate of is the estimated value of φ1, is the estimated value of φ2, and the designed parameter adaptation law is as follows: In the formula, represents the tracking energy consumption of the underwater vehicle, γ1, γ2, ω1, ω2 are positive adaptive design parameters.

Images