A method for underwater AUV position and attitude control based on PFAL function auto-disturbance rejection

Abstract

The present invention discloses a method for controlling the position and attitude of an underwater AUV based on PFAL function auto-disturbance rejection, belonging to the technical field of underwater AUV motion control. The present invention replaces the fal function used in a traditional auto-disturbance rejection controller with a continuous and smooth PFAL function. Compared with the traditional fal function, the continuous and smooth PFAL function proposed by the present invention better meets the requirements of "large error with small gain, small error with large gain", and the smoothness of the PFAL function can effectively reduce the high-frequency flutter phenomenon of nonlinear functions. Furthermore, through practical verification, the present invention uses the auto-disturbance rejection controller based on the PFAL function for the position and attitude control of an underwater AUV, ultimately achieving better control effects.

Description

Technical Field

[0001] The present invention belongs to the technical field of underwater AUV motion control, and in particular relates to a method for controlling the position and attitude of an underwater AUV based on PFAL function auto-disturbance rejection. Background Art

[0002] Underwater robots are a crucial technological tool for the sustainable development and utilization of marine resources. Autonomous underwater vehicles (AUVs) are standardized, modular, and autonomous underwater platforms that integrate energy and propulsion technologies, sensor and signal processing technologies, communication and navigation technologies, combat and human intervention technologies, and automatic control technologies. AUVs have six degrees of freedom in their underwater motion. Each control action affects each degree of freedom to varying degrees, resulting in strong coupling and severe nonlinearity in AUV motion. Therefore, ensuring that the vehicle moves within the required performance parameters is of practical significance. Numerous control methods from classical control theory, modern control theory, and intelligent control theory have been applied to AUV motion control. PID control is one of the primary control methods for AUVs. It is independent of the controlled object model and is based on feedback control based on error signals. However, for complex AUV nonlinear systems, conventional PID control alone is insufficient, as it suffers from poor robustness. In addition, due to the complex underwater environment, the time-varying nature of water flow velocity and other factors have a significant impact on the motion of AUV. Conventional PID control performs poorly in the AUV motion control performance with water flow interference.

[0003] In 2008, the Active Disturbance Rejection Control (ADCC) algorithm, proposed by researcher Han Jingqing, treats the controlled object as an integral series type and utilizes an extended state observer (ESO) to perform real-time estimation and compensation for portions of the dynamic object that differ from the standard type, thereby suppressing the impact of disturbances on the system output. This achieves better control than PIC control under complex disturbance conditions. ESO is a crucial component influencing ADRC motion control, and the fal function it uses directly impacts ESO performance. However, the nonlinear fal function used in traditional ADRC suffers from problems such as uneven inflection points, prone to small-amplitude chattering, and high system gain when errors are large, severely impacting ADRC's control performance. Summary of the Invention

[0004] Aiming at the problem that the existing AUV control technology has poor control effect under complex interference conditions, the present invention proposes an underwater AUV position and attitude control method based on PFAL function auto-disturbance rejection to solve the above problem.

[0005] In order to achieve the above-mentioned purpose, the present invention adopts the following technical solutions:

[0006] A method for controlling the position and attitude of an underwater AUV based on PFAL function auto-disturbance rejection includes the following steps:

[0007] S1: Kinematic modeling of AUV;

[0008] S2: Dynamic modeling of AUV;

[0009] S3: Constructing a PFAL function ADRC: The PFAL function ADRC consists of three parts: a tracking differentiator (TD), a nonlinear state error feedback (NLSEF) based on the PFAL function, and an extended state observer (ESO) based on the PFAL function.

[0010] S4: AUV position and attitude control: By adjusting the desired position and attitude angle (x d ,y d ,z d ,φ d ,θ d ,ψ d ) and the actual position and attitude angle (x, y, z, φ, θ, ψ) of the AUV are input into the pfal function ADRC, and the AUV controls the control output (h x ,h y ,h z ,h φ ,h θ ,h ψ ) adjust their position and posture, where (h x ,h y ,h z ,h φ ,h θ ,h ψ ) is the control output value representing the position and attitude angle in vector h.

[0011] Furthermore, the S1 specifically includes:

[0012] S1-1: Define the six-degree-of-freedom position / attitude vector of the AUV as:

[0013] ( ) (1);

[0014] The six-degree-of-freedom velocity / angular velocity vector is:

[0015] ( ) (2);

[0016] The velocity conversion equation for the AUV's linear motion along three axes using the Euler angle description method is:

[0017] (3);

[0018] in, is the velocity coordinate conversion matrix. The angular velocity conversion equation describing the AUV's rotation around the three axes is:

[0019] (4);

[0020] in, is the angular velocity coordinate transformation matrix;

[0021] S1-2: Using the concept of rotation, Euler's theorem and the properties of skew-symmetric matrices: get the transformation matrix:

[0022] (5);

[0023] (6);

[0024] Where, s·=sin(·), c·=cos(·), t·=tan(·), φ represents the roll angle, θ represents the pitch angle, and ψ represents the yaw angle;

[0025] The total coordinate transformation matrix is ​​defined as:

[0026] (7);

[0027] S1-3: Based on formulas (3) and (4), the AUV kinematic equation is:

[0028] (8).

[0029] Furthermore, the S2 includes:

[0030] S2-1: The AUV dynamics model including unknown disturbances is:

[0031] (9);

[0032] Where M is the inertia matrix including the additional mass, C(v) is the Coriolis force and centripetal force matrix including the additional mass, and D(v) is the hydrodynamic damping matrix. is the force / torque vector due to gravity and buoyancy, is the control force / torque vector generated by the propulsion system, It is an unknown disturbance force / torque vector, usually generated by ocean currents, surges, time-varying dynamic model parameters, etc.

[0033] S2-2: For ease of analysis, it is generally assumed that the AUV is a rigid body with constant mass, the center of buoyancy is located directly above the center of gravity, and has three approximate symmetry planes. Changes in water temperature and pressure are not considered. Based on the above assumptions, the simplified AUV dynamic model parameter matrix can be obtained as follows:

[0034] (10);

[0035] (11);

[0036] (12);

[0037] (13);

[0038] Where W=mg represents the gravity of AUV, m represents the mass of AUV, and g is the gravity coefficient. Indicates buoyancy, is the density of water, represents the volume of water displaced, Indicates the center of buoyancy.

[0039] S2-3: Define the thrust control coefficient matrix A(k) at the kth moment as:

[0040] (14);

[0041] Then the control force / torque vector generated by the propulsion system at the kth moment is It can be expressed as:

[0042] (15);

[0043] in, is the pseudo-inverse matrix of A, and h is the control output vector of six degrees of freedom.

[0044] Furthermore, in S3, a continuous and smooth pfal function is designed to replace the fal function, and its expression is as follows:

[0045] (twenty three);

[0046] To make the function pfal continuous and differentiable, Equation (23) must satisfy the following conditions:

[0047] (twenty four);

[0048] Where γ is defined as the constraint factor and is set to 0.01. The expressions of parameters p1, p2, b, and c are:

[0049] (25);

[0050] In formula (23), when When , the slope of the function is increased by introducing a logarithmic function, thereby increasing the gain when the error is small. By setting an appropriate constraint factor γ, the slope of the PFAL function is reduced when the error is large, thereby reducing the gain when the error is large. In addition, the present invention provides the conditions for the continuous differentiability of the PFAL function through deduction.

[0051] Furthermore, in S3, taking the AUV yaw angle ψ control as an example, it specifically includes:

[0052] S3-1: TD is used as a filter and differential function. Its function is to filter the input signal and reasonably extract the differential signal of the input signal. TD has the following advantages in the transition process: (1) It solves the contradiction between the system's fast response and overshoot; (2) It reduces the system's initial error, expands the selection range of the error feedback gain and the error differential feedback gain, and facilitates the system's tuning. Figure 3 For example, T can be expressed as:

[0053] (16);

[0054] Where h is the sampling period, fhan is the fastest control synthesis function, and its expression is as follows:

[0055] (17);

[0056] Where r is the parameter that determines the tracking speed. The larger r is, the shorter the transition process is and the faster the response speed is. Based on the design experience of control systems, r = 10 and h = 0.01 are generally used. sign is the sign function, which is expressed as follows:

[0057] (18);

[0058] S3-2: The ESO is the core component of the active disturbance rejection controller. It treats all types of disturbances from within and outside the AUV system as a total disturbance and estimates it in real time. Simultaneously, the ESO compensates for this total disturbance and transforms the nonlinear and uncertain system with unknown disturbances into an integral series linear system. The ESO is based solely on the input and output information of the control system and does not rely on the specific mathematical model that generates the disturbance. Therefore, it is highly robust. The ESO expression is as follows:

[0059] (19);

[0060] Among them, β1, β2, and β3 are the state error feedback gains to be adjusted. β1 affects the estimation of the state variable, β2 affects the estimation of the disturbance of the entire system, and β3 can speed up the response of the system. α1, α2, and α3 are variable parameters between 0 and 1, and are generally set to 0.5. δ=0.1 is the interval length of the linear segment of the pfal function, b0=1 is the control gain, and h ψ (k) is the total control amount, which is obtained from formula (22). is a nonlinear function, and its expression is as follows:

[0061] (20);

[0062] S3-3: NLSEF uses the tracking error signal in the form of nonlinear combination to obtain the control quantity h ψ0 , in order to achieve better control effect, its expression is as follows:

[0063] (twenty one);

[0064] Among them, k1=500, k2=5 are controller gains, α4=α5=0.5; the control quantity h ψ0 Together with the compensation of the extended observer for the total disturbance z3, it constitutes the total control quantity h of the system. ψ :

[0065] (twenty two);

[0066] S3-4: The fal function used in traditional ADRC controllers has excellent nonlinear characteristics and can significantly improve control system performance. However, the fal function is not smooth at the inflection point, and the system gain is large when the error is large, which affects the system's control performance. Therefore, a continuous and smooth pfal function is designed to replace the fal function. Its expression is as follows:

[0067] (twenty three);

[0068] To make the function pfal continuous and differentiable, Equation (23) must satisfy the following conditions:

[0069] (twenty four);

[0070] Where γ is defined as the constraint factor and is set to 0.01. The expressions of parameters p1, p2, b, and c are:

[0071] (25);

[0072] In formula (23), when When , the present invention increases the slope of the function by introducing a logarithmic function, thereby improving the gain when the error is small. When the error is large, the present invention reduces the slope of PFAL by setting an appropriate constraint factor γ, thereby reducing the gain when the error is large. In addition, the present invention provides the conditions for the continuous differentiability of the PFAL function by deduction.

[0073] Compared with the prior art, the advantages and positive effects of the present invention are:

[0074] This paper proposes a method for underwater AUV position and attitude control based on PFAL function-based active disturbance rejection (ADRC). This method replaces the FAL function used in traditional ADRC with a continuous and smooth PFAL function. Compared with the traditional FAL function, the proposed continuous and smooth PFAL function better meets the requirements of "large error, small gain, small error, large gain." Furthermore, the smoothness of the PFAL function effectively mitigates the high-frequency chattering phenomenon of nonlinear functions. Practical verification has demonstrated that the proposed method, using a PFAL-based ADRC controller for underwater AUV position and attitude control, ultimately achieves superior control results. BRIEF DESCRIPTION OF THE DRAWINGS

[0075] Figure 1 It is a schematic diagram of the coordinate system for the AUV position and attitude control proposed in the present invention.

[0076] Figure 2 This is a system structure diagram of the AUV position and attitude control proposed in the present invention.

[0077] Figure 3 This is the structure diagram of the pfal function active disturbance rejection controller for AUV position and attitude control proposed in the present invention.

[0078] Figure 4 This is a comparison chart of the results of the AUV position and attitude control method proposed in the present invention and other control methods. DETAILED DESCRIPTION

[0079] In order to make the objectives, technical solutions and advantages of the present invention more clearly understood, the present invention will be further described in detail below with reference to the accompanying drawings and embodiments.

[0080] Example 1

[0081] In the field of underwater AUV motion control, controlling the AUV's position and attitude is a crucial aspect of AUV motion control. The core of six-degree-of-freedom position and attitude control lies in using efficient controllers for each dimension, enabling the AUV to achieve the desired position or attitude more stably and quickly. Therefore, the technical problem addressed by this present invention is how to design a more efficient controller suitable for AUV position and attitude control.

[0082] Example 1:

[0083] This embodiment proposes a method for controlling the position and attitude of an underwater AUV based on PFAL function auto-disturbance rejection, including the following steps:

[0084] The kinematic and dynamic modeling of AUV includes the following steps:

[0085] (1) Kinematic modeling of AUV: The motion of AUV is a typical six-degree-of-freedom motion. In order to analyze the six-degree-of-freedom motion of AUV, it is necessary to define the coordinate system first. The commonly used coordinate system in AUV motion is the fixed coordinate system. and mobile coordinate system ,like Figure 1 shown.

[0086] For ease of analysis, it is generally assumed that the origin O of the moving coordinate system coincides with the center of gravity of the AUV, the x-axis points toward the bow, the y-axis is perpendicular to the x-axis and points to the starboard side, and the z-axis is perpendicular to O-xy and points downward. The symbols used to describe the motion and forces of the AUV based on the Society of Naval Architects and Marine Engineers (SNAME) are shown in Table 1.

[0087] Table 1 AUV motion and force description based on SNAME

[0088] .

[0089] The six-degree-of-freedom position / attitude vector of the AUV is defined as:

[0090] ( ) (1);

[0091] The six-degree-of-freedom velocity / angular velocity vector is:

[0092] ( ) (2);

[0093] The velocity conversion equation for the AUV's linear motion along three axes using the Euler angle description method is:

[0094] (3);

[0095] in, is the velocity coordinate conversion matrix. The angular velocity conversion equation describing the AUV's rotation around the three axes is:

[0096] (4);

[0097] in, is the angular velocity coordinate transformation matrix.

[0098] Using the concept of rotation, Euler's theorem and the properties of skew-symmetric matrices, we can get the transformation matrix:

[0099] (5);

[0100] (6);

[0101] Where s·=sin(·), c·=cos(·), t·=tan(·), φ represents the roll angle, θ represents the pitch angle, and ψ represents the yaw angle.

[0102] The total coordinate transformation matrix is ​​defined as:

[0103] (7);

[0104] Then, the AUV kinematic equation can be obtained from formulas (3) and (4):

[0105] (8);

[0106] (2) AUV dynamic modeling: The AUV dynamic model including unknown interference is:

[0107] (9);

[0108] Where M is the inertia matrix including the additional mass, C(v) is the Coriolis force and centripetal force matrix including the additional mass, and D(v) is the hydrodynamic damping matrix. is the force / torque vector due to gravity and buoyancy, is the control force / torque vector generated by the propulsion system, It is an unknown disturbance force / torque vector, usually generated by ocean currents, surges, time-varying dynamic model parameters, etc.

[0109] For ease of analysis, it is generally assumed that the AUV is a rigid body with constant mass, the center of buoyancy is located directly above the center of gravity, and has three approximate symmetry planes, and changes in water temperature, pressure, etc. are not considered. Based on the above assumptions, the simplified AUV dynamic model parameter matrix can be obtained as follows:

[0110] (10);

[0111] (11);

[0112] (12);

[0113] (13);

[0114] Where W=mg represents the gravity of AUV, m represents the mass of AUV, and g is the gravity coefficient. Indicates buoyancy, is the density of water, represents the volume of water displaced, Indicates the center of buoyancy.

[0115] The thrust control coefficient matrix A(k) at the kth moment is defined as:

[0116] (14);

[0117] Then the control force / torque vector generated by the propulsion system at the kth moment is Expressed as:

[0118] (15);

[0119] in, is the pseudo-inverse matrix of A, and h is the control output vector of six degrees of freedom.

[0120] (3) AUV position and attitude control system model: The structural block diagram of the AUV position and attitude control system is as follows: Figure 2 shown.

[0121] like Figure 2 As shown in the figure, by d ,y d ,z d ,φ d ,θ d ,ψ d ) and the actual position and attitude angle (x, y, z, φ, θ, ψ) of the AUV are input to the controller, and the AUV responds to the received control output (h x ,h y ,h z ,h φ,h θ ,h ψ ) adjust their position and posture, where (h x ,h y ,h z ,h φ ,h θ ,h ψ ) is the control output value representing the position and attitude angle in vector h.

[0122] (4) Active Disturbance Rejection Controller Based on PFAL Function: The active disturbance rejection controller based on PFAL function consists of three parts: Tracking Differentiator (TD), Nonlinear State Error Feedback (NLSEF) based on PFAL function, and Extended State Observer (ESO) based on PFAL function. Taking the AUV yaw angle ψ control as an example, its structure is as follows: Figure 3 shown.

[0123] TD is used as a filter and differential function. Its function is to filter the input signal and reasonably extract the differential signal of the input signal. TD has the following advantages in the transition process: (1) It solves the contradiction between the system's fast response and overshoot; (2) It reduces the system's initial error, expands the selection range of the error feedback gain and the error differential feedback gain, and facilitates the tuning of the system. Figure 3 For example, TD can be expressed as:

[0124] (16);

[0125] Where h is the sampling period, fhan is the fastest control synthesis function, and its expression is as follows:

[0126] (17);

[0127] Where r is the parameter that determines the tracking speed. The larger r is, the shorter the transition process is and the faster the response speed is. Based on the design experience of control systems, r = 10 and h = 0.01 are generally used. sign is the sign function, which is expressed as follows:

[0128] (18);

[0129] The ESO is the core component of the active disturbance rejection controller. It treats all disturbances from within and outside the AUV system as a total disturbance and estimates it in real time. Simultaneously, the ESO compensates for this total disturbance and transforms the nonlinear and uncertain system with unknown disturbances into an integral series linear system. The ESO is based solely on the input and output information of the controlled system, not on the specific mathematical model that generates the disturbance. Therefore, it is highly robust. The ESO expression is as follows:

[0130] (19);

[0131] Among them, β1, β2, and β3 are the state error feedback gains to be adjusted. β1 affects the estimation of the state variable, β2 affects the estimation of the disturbance of the entire system, and β3 can speed up the response of the system. α1, α2, and α3 are variable parameters between 0 and 1, and are generally set to 0.5. δ=0.1 is the interval length of the linear segment of the pfal function, b0=1 is the control gain, and h ψ (k) is the total control amount, which is obtained from formula (22). is a nonlinear function, and its expression is as follows:

[0132] (20);

[0133] NLSEF uses the tracking error signal in the form of nonlinear combination to obtain the control quantity hψ0 to achieve better control effect. Its expression is as follows:

[0134] (twenty one);

[0135] Among them, k1=500, k2=5 is the controller gain, α4=α5=0.5. ψ0 Together with the compensation of the extended observer for the total disturbance z3, it constitutes the total control quantity h of the system. ψ :

[0136] (twenty two).

[0137] The fal function used in traditional ADRC controllers has excellent nonlinear characteristics and can significantly improve the performance of the control system. However, the fal function is not smooth at the inflection point, and when the error is large, the system gain is large, which affects the control performance of the system. Therefore, the present invention designs a continuous and smooth pfal function to replace the fal function. Its expression is as follows:

[0138] (twenty three);

[0139] To make the function pfal continuous and differentiable, Equation (23) must satisfy the following conditions:

[0140] (twenty four);

[0141] Where γ is defined as the constraint factor and is set to 0.01. The expressions of parameters p1, p2, b, and c are:

[0142] (25);

[0143] In formula (23), when When , the present invention increases the slope of the function by introducing a logarithmic function, thereby improving the gain when the error is small. When the error is large, the present invention reduces the slope of PFAL by setting an appropriate constraint factor γ, thereby reducing the gain when the error is large. In addition, the present invention provides the conditions for the continuous differentiability of the PFAL function by deduction.

[0144] Example 2:

[0145] In order to verify this method, the kinematic and dynamic models of the AUV were established using the Simulink module of MATLAB software in this embodiment. The model parameters are shown in Table 2.

[0146] Table 2 AUV dynamics and kinematic model parameters

[0147] .

[0148] Assume that the initial position and attitude angle of the AUV are all 0, and the desired position is x d =2,y d =3,z d =2, the desired attitude angle is φ d =π / 3,θ d =π / 3,ψ d =π / 3. Then, the present invention numerically simulates the tracking effect of the AUV's desired position and attitude based on the pfal function ADRC controller, and compares it with the classic PID controller, the traditional ADRC controller and the existing ADRC controller based on the fal' function. The results are as follows Figure 4 As shown in Table 3. Among them, the parameters of the PID position controller are (K p ,K i ,K d )=(5,1,1), the parameters of the attitude controller are (10,2,2), and the parameters of the three ADRC controllers are β=[5,5,5,5,5,5,5,5,5,15,15,15,5,5,5,15,15,15].

[0149] Table 3 PFAL function ADRC controller and other controllers

[0150] .

[0151] Comparison of overshoot and steady-state time results for AUV position and attitude control

[0152] Depend on Figure 4 As shown in Table 3, in terms of position control, the pfal-ADRC achieves optimal overshoot in all other aspects, except for a slightly larger overshoot in the y-axis than the traditional ADRC. The PID controller takes a long time to reach steady state, resulting in poor performance. The three ADRC controllers reach steady state in similar times, all less than 4 seconds, indicating good control performance. In terms of attitude control, although the PID has zero overshoot, its steady-state time for the φ and ψ attitude angles is relatively long. The steady-state times of the three ADRC controllers are similar, with the pfal-ADRC achieving a better steady-state time for the ψ attitude angle than the ADRC and fal'-ADRC. Overall, while the PID controller has a better control effect on θ, it has a poorer control effect on the other five degrees of freedom. The steady-state times of the three ADRC controllers are significantly better than those of the PID, and the pfal-ADRC control proposed in this invention achieves significantly better overshoot than the ADRC and fal'-ADRC.

[0153] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, it is still possible for a person skilled in the art to modify the technical solutions described in the aforementioned embodiments, or to replace some of the technical features therein with equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions claimed to be protected by the present invention.

Claims

1. A method for underwater AUV position and attitude control based on PFAL function auto-disturbance rejection, characterized in that: The method comprises the following steps: S1: Kinematic modeling of AUV; S2: Dynamic modeling of AUV; S3: Constructing a PFAL function active disturbance rejection controller: The PFAL function active disturbance rejection controller includes three parts: tracking differentiator TD, nonlinear error feedback NLSEF based on PFAL function, and extended state observer ESO based on PFAL function. In S3, the PFAL function expression is as follows: To make the function pfal continuous and differentiable, Equation (23) must satisfy the following conditions: Among them, γ is defined as the constraint factor with a value of 0.01, and δ is the interval length of the linear segment of the PFAL function. The expressions of the parameters p1, p2, b and c are: In formula (23), when |e|≤δ, the slope of the function is increased by introducing a logarithmic function, thereby improving the gain when the error is small. When |e|≥1, the slope of pfal is reduced when the error is large by setting an appropriate constraint factor γ, thereby reducing the gain when the error is large. S4: AUV position and attitude control: By controlling the desired position and attitude angle and the actual position and attitude angle of the AUV Input the pfal function ADRC, and the AUV outputs the received control Adjust your position and posture, including: is the control output value representing the position and attitude angle in vector h.

2. The underwater AUV position and attitude control method according to claim 1, characterized in that: Said S1 specifically includes: S1-1: Define the six-degree-of-freedom position / attitude vector of the AUV as: The six-degree-of-freedom velocity / angular velocity vector is: The velocity conversion equation describing the AUV's linear motion along three axes is: Among them, J1(η2) is the velocity coordinate transformation matrix; The angular velocity conversion equation describing the AUV's rotation around three axes is: Where, J2(η2) is the angular velocity coordinate transformation matrix; S1-2: Using the concept of rotation, Euler's theorem and the properties of skew-symmetric matrices, we can get the transformation matrix: Among them, s·=sin(·), c·=cos(·), t·=tan(·), represents the roll angle, θ represents the pitch angle, and ψ represents the yaw angle; The total coordinate transformation matrix is ​​defined as: S1-3: Based on formulas (3) and (4), the AUV kinematic equation is:

3. The underwater AUV position and attitude control method according to claim 2, characterized in that: The S2 includes: S2-1: The AUV dynamics model including unknown disturbances is: Where M is the inertia matrix including the added mass, C(v) is the Coriolis force and centripetal force matrix including the added mass, D(v) is the hydrodynamic damping matrix, g(η2) is the force / torque vector due to gravity and buoyancy, and τ c is the control force / torque vector generated by the propulsion system, Δf is the unknown disturbance force / torque vector; S2-2: The AUV dynamics model parameter matrix is: Where W=mg represents the gravity of AUV, m represents the mass of AUV, and g is the gravity coefficient. represents the buoyancy, ρ is the density of water, represents the volume of water displaced, (x B ,y B ,z B ) represents the position of the center of buoyancy; S2-3: Define the thrust control coefficient matrix A(k) at the kth moment as: A(k)=J(η2)M -1 (14) Then the control force / torque vector τ generated by the propulsion system at the kth moment is c (k) is expressed as: t c (k)=A * (k-1)h (15) Among them, A * is the pseudo-inverse matrix of A, and h is the control output vector of six degrees of freedom.

4. The underwater AUV position and attitude control method according to claim 1, characterized in that: In S3, when the control variable is the AUV yaw angle ψ, it specifically includes: S3-1: TD is represented by: Where i is the sampling period, fhan is the fastest control synthesis function, and its expression is as follows: Among them, r is the parameter that determines the tracking speed, sign is the sign function, and the expression is as follows: S3-2: The ESO expression is as follows: Among them, β1, β2, β3 are the state error feedback gains to be adjusted, α2, α3 are variable parameters between 0 and 1, δ = 0.1 is the interval length of the linear segment of the pfal function, b0 = 1 is the control gain, h ψ (k) is the total control amount, which is obtained from formula (22); S3-3: The expression of NLSEF is as follows: Among them, k1=500, k2=5 are controller gains, α4=α5=0.5; the control quantity h ψ0 Together with the compensation of the extended observer for the total disturbance z3, it constitutes the total control quantity h of the system. ψ : When the control quantity is x,y,z, When θ is θ, the processing steps are the same as those for the yaw angle ψ.

Images