Underactuated AUV trajectory tracking control method based on extended perturbation observer

Abstract

The present application relates to the underactuated AUV trajectory tracking control method based on the extended disturbance observer, including the following steps: step 1: the kinematics and dynamics model of underactuated AUV horizontal plane is established;Step 2: through the collection of AUV real-time position and speed information, while setting the desired trajectory, the speed tracking error model is derived from the position tracking error model;Step 3: the extended disturbance observer is constructed to estimate disturbance;Step 4: according to the speed tracking error, the sliding mode function design of nonsingular fast terminal sliding mode is carried out;Step 5: according to the sliding mode function, the nonsingular fast terminal sliding mode controller NFTSMC is constructed, and the estimated value of disturbance observer is compensated to the controller output, and the trajectory tracking control of underactuated AUV horizontal plane under unknown time-varying disturbance is realized.The method has good adaptability to complex external interference, and the convergence speed is fast.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of trajectory tracking of an autonomous underwater vehicle (AUV), and particularly relates to an underactuated AUV trajectory tracking control method based on an extended disturbance observer. BACKGROUND

[0002] With the deepening of ocean exploration, humans have put forward higher and higher requirements for the performance of underwater vehicles. As a kind of underwater vehicle with small volume, good controllability, long endurance and strong carrying capacity, its trajectory tracking and control capability is an important technical guarantee for completing underwater resource exploration, environmental monitoring and sea survey tasks. However, the marine environment is very complex, and unpredictable situations such as strong winds and unknown ocean currents often occur, which also makes the related control method face challenges. Compared with the full drive system, the underactuated system can reduce the manufacturing cost and energy consumption, and at the same time improve the propulsion efficiency, but also makes the design of the controller more challenging.

[0003] The traditional nonlinear disturbance observer needs to meet some assumptions such as that the disturbance is slowly changing or the first derivative of the disturbance is bounded or zero when designing, and the application range is relatively narrow, and the application range is relatively narrow. The present application makes improvements, which not only do not need complex and rigorous assumption conditions, but also can estimate the first derivative of the disturbance.

[0004] Due to the strong robustness of the sliding mode control, for the horizontal plane trajectory tracking of the underactuated AUV, corresponding control methods have been proposed at home and abroad. Such as using sliding mode control to design the controller, using non-singular terminal sliding mode control method, etc. On the one hand, it does not have a fast convergence speed, and on the other hand, it does not consider the influence of external disturbance on the stability of the system. SUMMARY

[0005] The purpose of the present application is to provide an underactuated AUV trajectory tracking control method based on an extended disturbance observer, which has good adaptability to complex external disturbances and fast convergence speed.

[0006] To achieve the above purpose, the technical scheme adopted by the present application is as follows: an underactuated AUV trajectory tracking control method based on an extended disturbance observer, comprising the following steps:

[0007] Step 1: establishing the kinematics and dynamics model of the underactuated AUV horizontal plane;

[0008] Step 2: through the collection of real-time position and speed information of the AUV itself, while setting the expected trajectory, the speed tracking error model is derived from the position tracking error model;

[0009] Step 3: constructing an extended disturbance observer to estimate the disturbance;

[0010] Step 4: According to the speed tracking error, the sliding mode function design of non-singular fast terminal sliding mode is carried out;

[0011] Step 5: According to the sliding mode function, the non-singular fast terminal sliding mode controller NFTSMC is constructed, and the estimated value of the disturbance observer is compensated into the controller output, so as to realize the trajectory tracking control of the underactuated AUV in the horizontal plane under unknown time-varying disturbance.

[0012] Further, in step 1, the kinematics and dynamics model of the underactuated AUV in the horizontal plane is established as follows:

[0013]

[0014] In the formula, x and y represent the horizontal and vertical coordinates of the position of the AUV in the fixed frame on the ground, and ψ is the yaw angle, are the first derivatives of x, y and ψ respectively; u, v and r are the forward, lateral and turning speeds of the AUV, are the first derivatives of u, v and r respectively; m is the vehicle mass, I z is the moment of inertia of the vehicle around the z axis, X u , Y v and N r are the linear hydrodynamic damping coefficients in the forward, lateral and turning directions, are the added mass and added inertia moment in the forward, lateral and yaw directions respectively, τ u is the propulsion force in the forward direction, τ r is the yawing moment in the turning direction; let d = [d1, d2, d3] T be the unknown environmental disturbance, where d1, d2 and d3 are the disturbances in the forward, lateral and yaw directions respectively, and it is assumed that the second derivative of the disturbance d is bounded, that is, μ is an unknown normal number.

[0015] Further, in step 2, by collecting the real-time position and speed information of the AUV itself, and setting the expected trajectory as (x d , y d ), the speed tracking error model is derived from the position tracking error model:

[0016] The position tracking error is:

[0017]

[0018] where e x is the position tracking error in the x direction, e y is the position tracking error in the y direction, and the first derivative is:

[0019]

[0020] in x d ,y d The first derivative;

[0021] The speed tracking error is:

[0022]

[0023] Where h x ,g x ,h y ,g y e is a positive constant. u e represents the speed tracking error in the forward direction. v This represents the velocity tracking error in the lateral direction; according to the Lyapunov function, when e u and e v When it converges to zero, e x and e y It also converges to zero.

[0024] Furthermore, in step 3, the extended perturbation observer is constructed as follows:

[0025]

[0026]

[0027]

[0028] Among them, let It is an estimate of the disturbance d(t), where These are the estimated values ​​of the disturbances d1, d2, and d3, respectively. Let d(t) represent the first derivative of the disturbance. The estimated value, of which These are the first derivatives of the perturbations d1, d2, and d3, respectively. The estimated value, z ab (a = 1, 2; b = 1, 2, 3) are the auxiliary state variables of the disturbance observer, and the constant L is the positive constant. ab (a = 1, 2; b = 1, 2, 3) are the observer gains; according to the Lyapunov function, the estimation error of the perturbation observer for unknown time-varying perturbations is globally consistent and eventually bounded.

[0029] Furthermore, in step 4, based on the velocity tracking error in step 2, the sliding mode function for non-singular fast terminal sliding mode is designed:

[0030] The sliding mode function is:

[0031]

[0032] Where, α i ,β i All are positive real numbers, and they have α. i >0, i=1,2,3,4, 1<β2, β4<2, β1>β2, β3>β4.

[0033] Furthermore, in step 5, a non-singular fast terminal sliding mode controller (NFTSMC) is constructed based on the sliding mode function in step 4, and the estimated value of the disturbance observer is compensated to the controller output to achieve trajectory tracking control of the underdriven AUV on the horizontal plane under unknown time-varying disturbances.

[0034] The control rate is:

[0035]

[0036] Where w1 and w2 are positive constants, and τ u ,τ r These are the thrust of the AUV's forward thruster and the turning torque required for yaw; According to the Lyapunov function, under the control law, the velocity tracking error converges to zero in a finite time, and consequently the position tracking error also converges to zero.

[0037] Furthermore, in step 5, to reduce chattering, the sign function sign(S) is replaced by the saturation function sat(S), i.e.

[0038]

[0039] Where Δ represents the boundary layer.

[0040] Compared with the prior art, the present invention has the following beneficial effects: it provides an underactuated AUV trajectory tracking control method based on an extended disturbance observer to cope with unknown external complex disturbances and the drawbacks of common disturbance observers and the slow convergence speed of general sliding mode control methods. The extended disturbance observer constructed by this method can better cope with complex and varied disturbances in reality. At the same time, the controller is designed using a non-singular fast terminal sliding mode method to accelerate the convergence of tracking errors, thereby ensuring that the control system has good performance. Attached Figure Description

[0041] Figure 1 This is a schematic diagram of the horizontal plane motion of an underdriven AUV in an embodiment of the present invention;

[0042] Figure 2 This is a principle block diagram of the underactuated AUV trajectory tracking control method according to an embodiment of the present invention;

[0043] Figure 3(a) is a diagram showing the trajectory tracking effect of an underdriven AUV under the first type of external interference in this embodiment of the invention.

[0044] Figure 3(b) is a comparison chart of the estimated value and the actual value of the disturbance observer for the first type of unknown external disturbance in the embodiment of the present invention;

[0045] Figure 3(c) is a diagram showing the convergence result of the horizontal plane position tracking error of the underdriven AUV under the first interference situation in the embodiment of the present invention.

[0046] Figure 3(d) is a diagram showing the convergence result of the horizontal velocity tracking error of an underdriven AUV under the first interference scenario in this embodiment of the invention.

[0047] Figure 3(e) is a schematic diagram of the forward thrust and yaw moment of an underdriven AUV under the first interference scenario in this embodiment of the invention.

[0048] Figure 4(a) shows the trajectory tracking effect of an underdriven AUV under the second type of external interference in this embodiment of the invention.

[0049] Figure 4(b) is a comparison chart of the estimated value and the actual value of the disturbance observer for the second type of unknown external disturbance in the embodiment of the present invention;

[0050] Figure 4(c) is a diagram showing the convergence result of the horizontal plane position tracking error of the underdriven AUV under the second interference scenario in this embodiment of the invention.

[0051] Figure 4(d) is a diagram showing the convergence result of the horizontal plane velocity tracking error of an underdriven AUV under the second interference scenario in this embodiment of the invention.

[0052] Figure 4(e) is a schematic diagram of the forward thrust and yaw moment of an underdriven AUV under the second interference scenario in this embodiment of the invention.

[0053] Figure 5(a) is a comparison of the trajectory tracking performance of an underactuated AUV under several methods in the first interference scenario of the present invention.

[0054] Figure 5(b) is a comparison of the convergence results of the horizontal plane position tracking error of the underactuated AUV under several methods in the first interference situation of the present invention.

[0055] Figure 6(a) is a comparison of the trajectory tracking performance of an underdriven AUV under several methods in the second interference scenario of the present invention.

[0056] Figure 6(b) is a comparison of the convergence results of the horizontal plane position tracking error of the underactuated AUV under several methods in the second interference scenario of the present invention. Detailed Implementation

[0057] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0058] It should be noted that the following detailed descriptions are exemplary and intended to provide further explanation of this application. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application pertains.

[0059] It should be noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the exemplary embodiments according to this application. As used herein, the singular form is intended to include the plural form as well, unless the context clearly indicates otherwise. Furthermore, it should be understood that when the terms "comprising" and / or "including" are used in this specification, they indicate the presence of features, steps, operations, devices, components, and / or combinations thereof.

[0060] This embodiment provides an underactuated AUV trajectory tracking control method based on an extended disturbance observer, the implementation principle of which is as follows: Figure 2 As shown. This method proposes an extended disturbance observer to estimate unknown external disturbances. Compared with the complexity of traditional disturbance observer design requirements, the extended disturbance observer constructed in this invention is simpler to design and can estimate the first derivative of the disturbance. The estimated value is then compensated in the proposed non-singular fast terminal sliding mode controller to solve the problem, thereby improving the accuracy of underactuated AUV horizontal plane trajectory tracking control.

[0061] In this embodiment, the horizontal plane motion of the underactuated AUV is as follows: Figure 1 As shown. The underactuated AUV only has independent actuators in the surge and yaw directions, while there is no control input in the lateral direction. Therefore, the kinematic and dynamic model of the underactuated AUV on the horizontal plane is established as follows:

[0062]

[0063] In the formula, x and y represent the horizontal and vertical coordinates of the AUV's position within the fixed ground frame, and ψ is the yaw angle. ψ represents the first derivative of x, y, and ψ, respectively; u, v, and r represent the forward, backward, lateral, and rotational velocities of the AUV, respectively. These are the first derivatives of u, v, and r, respectively; m is the vehicle mass, and I... z It is the moment of inertia of the vehicle about the z-axis, X u Y v N r These are the linear hydrodynamic damping coefficients for the forward, backward, lateral, and rotation directions, respectively. These are the additional mass and additional moment of inertia τ in the forward, backward, lateral, and yaw directions, respectively. u τ is the propulsive force in the direction of forward movement. r Let d be the yaw moment in the direction of turning; let d = [d1, d2, d3]. T The disturbances are unknown environmental disturbances, where d1, d2, and d3 are disturbances in the forward, backward, lateral, and yaw directions, respectively, and it is assumed that the second derivative of the disturbance d is... It is bounded, that is μ is an unknown positive constant.

[0064] By collecting its own real-time position and velocity information, and setting the desired trajectory as (x d ,y d The velocity tracking error model is derived from the position tracking error model as follows:

[0065] The position tracking error is:

[0066]

[0067] Among them, e x e represents the position tracking error in the x-direction. y Let be the position tracking error in the y-direction, and its first derivative is:

[0068]

[0069] Therefore, the desired speed can be designed as follows:

[0070]

[0071] In the formula h x ,g x ,h y ,g y It is a positive number.

[0072] The speed tracking error is:

[0073]

[0074] Among them, e u e represents the speed tracking error in the forward direction. v This indicates the velocity tracking error in the lateral direction.

[0075] Because of the matrix It is non-singular, therefore when e u and e v When it converges to 0, we have

[0076]

[0077] Choose the Lyapunov function as:

[0078]

[0079] According to equation (6), taking the derivative with respect to V1, we can obtain:

[0080]

[0081] Obviously It is negative definite, from which it can be proven that when e u and e v When it converges to zero, e x and e y It will also converge to zero.

[0082] Due to the complexity of the external environment, disturbance observers are designed to estimate disturbances in order to ensure that the aircraft can better cope with changes in the external environment during operation. However, common nonlinear disturbance observers make overly stringent assumptions, generally assuming that the external disturbances are steady or slowly changing. However, in reality, external disturbances are time-varying and complex, making the applicable range of such disturbance observers too small. Therefore, an extended disturbance observer is designed to improve this, which not only does not require stringent assumptions but can also estimate the first derivative of the disturbance.

[0083] The extended perturbation observer is constructed as follows:

[0084]

[0085]

[0086]

[0087] Among them, let It is an estimate of the disturbance d(t), where These are the estimated values ​​of the disturbances d1, d2, and d3, respectively. Let d(t) represent the first derivative of the disturbance. The estimated value, of which These are the first derivatives of the perturbations d1, d2, and d3, respectively. The estimated value, z ab (a = 1, 2; b = 1, 2, 3) are the auxiliary state variables of the disturbance observer, and the constant L is the positive constant. ab (a = 1, 2; b = 1, 2, 3) are the observer gains. According to the Lyapunov function, the estimation error of the perturbation observer for unknown time-varying perturbations is globally uniformly and eventually bounded.

[0088] Proof of stability of extended perturbation observer:

[0089] The extended perturbation observer constructed in this invention can be written as:

[0090]

[0091] In the formula, D d =[d1,d2,d3] T V = [u, v, r] T , Z1 = [z 11 ,z 12 ,z 13 ] T Z2 = [z 21 ,z 22 ,z 23 ] T According to the above formula:

[0092]

[0093] in According to equation (13), we can conclude that:

[0094]

[0095] in Differentiating equation (14) yields:

[0096]

[0097] make Therefore:

[0098]

[0099] in Therefore:

[0100] λ 2 +L1λ+L2=0 (17)

[0101] Therefore, matrix L can be optimized by choosing L1 and L2. d If all the eigenvalues ​​of the matrix lie in the LHP (left half-plane), then a positive definite matrix P can always be found. d Make:

[0102]

[0103] Q d It is a positive definite matrix, and its smallest eigenvalue is λ. m Choose the Lyapunov function:

[0104]

[0105] So V dThe first derivative is:

[0106]

[0107] So when When, satisfy therefore It will converge to compact set:

[0108]

[0109] Design the NFTSMC sliding mode function:

[0110]

[0111] Where, α i ,β i All are positive real numbers, and they have α. i >0 (i=1,2,3,4), 1<β2, β4<2, β1>β2, β3>β4.

[0112] Therefore, the control rate of NFTSMC can be obtained as follows:

[0113]

[0114] Where w1 and w2 are positive constants, and τ u ,τ r These are the thrust of the AUV's forward thruster and the yaw torque required for yaw, respectively.

[0115] The Lyapunov function is selected as follows:

[0116]

[0117] Differentiating V2 with respect to it, and from equations (22) and (23), we can obtain:

[0118]

[0119] Obviously, from equation (21), it can be seen that λ can be adjusted. m , making Since the values ​​are negative definite, it can be guaranteed that the two sliding surfaces S1 and S2 converge to zero in a finite time. In other words, if the velocity tracking error converges to zero in a finite time, then the convergence of the position tracking error to zero can be guaranteed.

[0120] Because the sliding mode control law contains a sign function `sign(s)`, significant chatter will occur. To reduce chatter, we replace the sign function with a saturation function `sat(s)`.

[0121]

[0122] Where Δ represents the boundary layer.

[0123] In this embodiment, the first type of disturbance is a slightly varying continuous time-varying disturbance. Figures 3(a) to 3(e) show the simulation results of trajectory tracking control under the first type of disturbance. As can be seen from Figure 3(a), the control method adopted in this invention has good robustness to this slightly varying continuous time-varying disturbance. Figure 3(b) shows that the designed extended disturbance observer has good performance. Figures 3(c) and 3(d) show that under this disturbance, the position tracking error and velocity tracking error can converge well to zero under the control method adopted in this invention. Figure 3(e) shows the control input under this disturbance, and it can be seen that flutter is well suppressed under the action of the saturation function.

[0124] In this embodiment, the second type of disturbance is a disturbance that occurs with a large abrupt change at a certain moment, taking into account the sudden occurrence of a large ocean current or the entanglement of underwater plants in reality. Figures 4(a) to 4(e) are simulation results of trajectory tracking control under the second type of disturbance. Figure 4(a) shows that the control method adopted in this invention still has good tracking performance under this disturbance with a large abrupt change at a certain moment; Figure 4(b) shows that the extended disturbance observer designed in this invention also has good performance for this abrupt disturbance; Figures 4(c) and 4(d) show the position tracking error and velocity tracking error under this disturbance, respectively; Figure 4(e) shows the control input under this disturbance.

[0125] Figures 5(a) and 5(b) show simulation comparisons of trajectory tracking performance and tracking error under several different control methods in the case of a first type of slightly varying continuous time-varying disturbance, respectively. They represent the non-singular fast terminal sliding mode control method with an extended disturbance observer, the non-singular terminal sliding mode control method with an extended disturbance observer, and the non-singular fast terminal sliding mode control method without an extended disturbance observer, as described in this invention. It can be seen that the non-singular fast terminal sliding mode control method in this invention still exhibits excellent tracking performance and robustness even without an extended disturbance observer when facing this slightly varying continuous time-varying disturbance.

[0126] Figures 6(a) and 6(b) show simulation comparisons of trajectory tracking performance and tracking error under different control methods in the second type of disturbance with a large sudden change at a certain moment, respectively. They represent the non-singular fast terminal sliding mode control method with an extended disturbance observer, the non-singular fast terminal sliding mode control method with an extended disturbance observer, and the non-singular fast terminal sliding mode control method without an extended disturbance observer, as proposed in this invention. It can be seen that the non-singular fast terminal sliding mode control method based on an extended disturbance observer proposed in this invention still has good tracking performance when facing this disturbance with a large sudden change at a certain moment. However, without the extended disturbance observer, the system's position tracking error will not converge to zero.

[0127] As can be seen from Figures 5(b) and 6(b), the non-singular fast terminal sliding mode control method based on extended perturbation observer proposed in this invention has a faster convergence speed compared with the general non-singular terminal sliding mode control method.

[0128] Simulation results show that the designed controller can effectively achieve trajectory tracking control of an underactuated AUV on a horizontal plane.

[0129] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention in any other way. Any person skilled in the art may make changes or modifications to the above-disclosed technical content to create equivalent embodiments. However, any simple modifications, equivalent changes, and modifications made to the above embodiments based on the technical essence of the present invention without departing from the scope of the present invention shall still fall within the protection scope of the present invention.

Claims

1. A method for underactuated AUV trajectory tracking control based on an extended disturbance observer, characterized in that, Includes the following steps: Step 1: Establish the kinematic and dynamic model of the horizontal plane of the underactuated AUV; Step 2: By collecting the real-time position and velocity information of the AUV itself, and setting the desired trajectory, the velocity tracking error model is derived from the position tracking error model; Step 3: Construct an extended perturbation observer to estimate the perturbation; Step 4: Based on the velocity tracking error, design the sliding mode function for the non-singular fast terminal sliding mode; Step 5: Construct a non-singular fast terminal sliding mode controller (NFTSMC) based on the sliding mode function, and compensate the estimated value of the disturbance observer into the controller output to realize trajectory tracking control of the underdriven AUV on the horizontal plane under unknown time-varying disturbances; In step 1, the kinematic and dynamic model of the underactuated AUV on the horizontal plane is established as follows: In the formula, x and y represent the horizontal and vertical coordinates of the AUV's position within the fixed ground frame, and ψ is the yaw angle. ψ represents the first derivative of x, y, and ψ, respectively; u, v, and r represent the forward, backward, lateral, and rotational velocities of the AUV, respectively. These are the first derivatives of u, v, and r, respectively; m is the vehicle mass, and I... z It is the moment of inertia of the vehicle about the z-axis, X u Y v N r These are the linear hydrodynamic damping coefficients in the forward, backward, lateral, and rotation directions, respectively. These are the additional mass and additional moment of inertia τ in the forward, backward, lateral, and yaw directions, respectively. u τ is the propulsive force in the direction of forward movement. r Let d be the yaw moment in the direction of turning; let d = [d1, d2, d3]. T The disturbances are unknown environmental disturbances, where d1, d2, and d3 are disturbances in the forward, backward, lateral, and yaw directions, respectively, and it is assumed that the second derivative of the disturbance d is... It is bounded, that is... μ is an unknown positive constant; In step 2, by collecting the AUV's real-time position and velocity information, and simultaneously setting the desired trajectory as (x... d ,y d The velocity tracking error model is derived from the position tracking error model: The position tracking error is: Among them, e x e represents the position tracking error in the x-direction. y Let be the position tracking error in the y-direction, and its first derivative is: in x d ,y d The first derivative; The speed tracking error is: Where h x ,g x ,h y ,g y e is a positive constant. u e represents the speed tracking error in the forward direction. v This represents the velocity tracking error in the lateral direction; according to the Lyapunov function, when e u and e v When it converges to zero, e x and e y It also converges to zero; In step 3, the extended perturbation observer is constructed as follows: Among them, let It is an estimate of the disturbance d(t), where These are the estimated values ​​of the disturbances d1, d2, and d3, respectively. Let d(t) represent the first derivative of the disturbance. The estimated value, of which These are the first derivatives of the perturbations d1, d2, and d3, respectively. The estimated value, z ab (a = 1, 2; b = 1, 2, 3) are the auxiliary state variables of the disturbance observer, and the constant L is the positive constant. ab (a = 1, 2; b = 1, 2, 3) are the observer gains; according to the Lyapunov function, the estimation error of the perturbation observer for unknown time-varying perturbations is globally consistent and eventually bounded; In step 4, based on the velocity tracking error in step 2, the sliding mode function for non-singular fast terminal sliding mode is designed: The sliding mode function is: where α i , β i are both positive real numbers, and there are α i > 0, i = 1, 2, 3, 4, 1 < β2, β4 < 2, β1 > β2, β3 > β4; In step 5, a non-singular fast terminal sliding mode controller (NFTSMC) is constructed based on the sliding mode function in step 4, and the estimated value of the disturbance observer is compensated into the controller output to achieve trajectory tracking control of the underdriven AUV on the horizontal plane under unknown time-varying disturbances. The control rate is: Where w1 and w2 are positive constants, and τ u ,τ r These are the thrust of the AUV's forward thruster and the turning torque required for yaw; According to the Lyapunov function, under the control law, the velocity tracking error converges to zero in a finite time, and consequently the position tracking error also converges to zero. In step 5, to reduce chattering, the sign function sign(S) is replaced by the saturation function sat(S), i.e. Where Δ represents the boundary layer.

Images