A robust and precise control method for AUV with X-shaped rudder

Abstract

This invention discloses a robust and precise control method for an X-rudder AUV. The method includes: obtaining the position, attitude, linear velocity, and angular velocity of the AUV hull, establishing a kinematic model, a dynamic model, and an X-distribution model to form a mathematical model of the X-rudder AUV; obtaining the current AUV position and route information in the horizontal and vertical planes, establishing a horizontal plane kinematic controller, and designing a vertical plane kinematic control law; designing a nonlinear disturbance observer to estimate and compensate for the unmodeled dynamics of the X-rudder AUV mathematical model and the vertical plane kinematic control law, generating a high-order sliding mode controller for the AUV; and distributing the control of the X-rudder based on the control outputs calculated by the AUV high-order sliding mode controller based on a damping matrix. This control method can effectively reduce the impact of model uncertainty and external disturbances on AUV control.

Description

Technical Field

[0001] The present invention relates to the technical field of AUV control, and in particular to a robust and precise control method for an X-shaped rudder AUV. Background Art

[0002] Autonomous underwater vehicles (AUVs) have attracted widespread attention in scientific research, civilian, and military fields due to their unmanned and autonomous operation capabilities. AUV control technology is the core technology and key capability for achieving autonomous navigation. AUVs equipped with X-shaped stern rudders outperform traditional cross-rudder AUVs in terms of safety and hydrodynamic performance. Consequently, X-shaped rudder AUV control has attracted considerable attention and is in great demand.

[0003] The existing research on AUV during navigation faces the following problems:

[0004] 1. Most AUVs are underactuated vehicles, requiring a kinematic controller to generate the desired heading and pitch angles to guide the AUV to a desired location. However, the sideslip angle caused by ocean currents can easily lead to static errors in the AUV's kinematic controller, reducing control accuracy.

[0005] 2. The AUV dynamics model is highly nonlinear and includes various hydrodynamic parameters, making it difficult to accurately obtain the model. Furthermore, the AUV is susceptible to disturbances from ocean currents during motion, which can affect control accuracy and robustness. Traditional PID control applied to highly nonlinear AUVs can easily cause problems such as overshoot and oscillation, as well as actuator saturation, resulting in certain deficiencies in robustness and accuracy. Sliding mode control is a robust control method that can, to a certain extent, address issues such as model uncertainty and external disturbances. However, traditional sliding mode control also suffers from output jitter, which can affect actuator life.

[0006] 3. The X-rudder AUV controller outputs force squares, which require control distribution to convert them into commanded rudder angles. Traditional pseudo-inverse methods offer strong real-time performance but fail to consider rudder angle output limits. Nonlinear optimization methods, such as the SQP method, consider output limits but suffer from complex algorithms, consume significant AUV computing resources, and exhibit poor real-time performance.

[0007] Therefore, based on the existing AUV navigation control, how to solve the problems of low control accuracy, poor robustness, and dependence on model parameters caused by AUV current interference, model uncertainty, etc. in traditional methods has become an urgent problem that technicians in this field need to solve. Summary of the Invention

[0008] In view of the above problems, the present invention proposes a robust and precise control method for an X-type rudder AUV that solves at least some of the above technical problems. This method can effectively reduce the impact of model uncertainty and external interference on AUV control.

[0009] An embodiment of the present invention provides a robust and precise control method for an X-shaped rudder AUV, comprising:

[0010] Obtain the position, attitude, linear velocity and angular velocity of the AUV hull, establish the kinematic model, dynamic model and X distribution model, and form the X-rudder AUV mathematical model;

[0011] Obtain the current AUV position and route information in the horizontal and vertical planes, establish a horizontal plane kinematic controller, and design a vertical plane kinematic control law;

[0012] Designing a nonlinear disturbance observer to estimate and compensate for the unmodeled dynamics of the X-rudder AUV mathematical model and the vertical plane kinematic control law, and generating a high-order sliding mode controller for the AUV;

[0013] The control output of the AUV high-order sliding mode controller is solved based on the damping matrix to perform control allocation of the X rudder.

[0014] Furthermore, the kinematic model is:

[0015]

[0016]

[0017]

[0018]

[0019]

[0020]

[0021] In the above formula, the vector [x, y, z] represents the position of the AUV hull in the earth coordinate system; the Euler angle vector [φ, θ, ψ] represents the hull posture; [u, v, w] represents the linear velocity of the hull in the hull coordinate system, and [p, q, r] represents the angular velocity of the hull in the hull coordinate system.

[0022] Furthermore, the kinetic model is:

[0023]

[0024]

[0025]

[0026]

[0027]

[0028]

[0029] In the above formula, m represents the mass of AUV; f r represents the known terms of the kinetic model; D r represents the model uncertainty.

[0030] Furthermore, the X distribution model is:

[0031]

[0032] In the above formula, k δi 、m δi 、n δi , i = 1, ..., 4 are the hydrodynamic coefficients of the four rudders in heel, pitch, and rotational freedom, respectively; δ i , i=1,…,4 are the four rudder angles of the X rudder.

[0033] Furthermore, the horizontal plane kinematic controller is established as follows:

[0034] Obtain the two-dimensional coordinates of the current AUV position on the horizontal plane and the starting waypoint, target waypoint, and next waypoint of the current segment respectively;

[0035] Calculating a track angle of a first straight track segment, wherein the first straight track segment is composed of a starting waypoint and a target waypoint of the current segment in the horizontal plane;

[0036] Draw a perpendicular line from the current position of the AUV on the horizontal plane to the first straight track segment, and obtain the track deviation of the AUV on the horizontal plane according to the track angle of the first straight track segment;

[0037] The track deviation of the AUV in the horizontal plane is eliminated, interference estimation and sideslip angle estimation are calculated, and a horizontal plane kinematic controller is generated.

[0038] Furthermore, the track angle of the first straight track segment is calculated by the following formula:

[0039] ψ k =atan2(y k+1 -y k ,x k+1 -x k )

[0040] In the above formula, (x k ,y k) represents the horizontal two-dimensional coordinates of the target waypoint of the current segment; (x k+1 ,y k+1 ) represents the horizontal plane two-dimensional coordinate of the next waypoint of the current segment.

[0041] Furthermore, the track deviation of the AUV horizontal plane is calculated by the following formula:

[0042] e=-(xx k-1 )sin(ψ k )+(yy k-1 )cos(ψ k )

[0043] In the above formula, (x k-1 ,y k-1 ) represents the horizontal two-dimensional coordinate of the starting waypoint of the current segment; (x, y) represents the current AUV position point on the horizontal plane; ψ k Indicates the track angle of the first straight track segment.

[0044] Furthermore, the eliminating of the AUV horizontal track deviation and calculating the interference estimate include:

[0045] The AUV horizontal plane track deviation is derived:

[0046]

[0047] In the above formula, V represents the horizontal navigation speed of the AUV; β represents the sideslip angle caused by the ocean current interference; ψ k represents the track angle of the first straight track segment;

[0048] Assume g = Vβcos(ψ-ψ k );

[0049] The interference estimation of g is performed by the following formula:

[0050]

[0051] In the above formula, is the estimated value of g; k g is the parameter of the observer; p g is the auxiliary variable of the observer; V represents the horizontal navigation speed of the AUV; e represents the horizontal track deviation of the AUV; ψ k Indicates the track angle of the first straight track segment.

[0052] Furthermore, the estimated side slip angle is calculated using the following formula:

[0053]

[0054] In the above formula, is the estimated value of g; V represents the horizontal navigation speed of AUV; ψ k Indicates the track angle of the first straight track segment.

[0055] Furthermore, the vertical plane kinematic control law is designed in the following manner:

[0056] Obtain the current AUV position point in the vertical plane, as well as the two-dimensional coordinates of the starting waypoint and the target waypoint of the current segment;

[0057] Calculating a submergence angle of a second straight track segment, wherein the second straight track segment is composed of a starting waypoint and a target waypoint of the current segment in the vertical plane;

[0058] Draw a perpendicular line from the current position of the AUV on the vertical plane to the second straight track segment, and obtain the track deviation of the AUV on the vertical plane according to the submergence angle of the second straight track segment;

[0059] The track deviation of the AUV in the vertical plane is eliminated, and an integral action vertical plane kinematic control law is introduced.

[0060] Furthermore, the vertical kinematic control law of the integral action is:

[0061]

[0062] In the above formula, θ d is the desired heading angle in the vertical plane, θ k is the diving angle of the second straight track segment, e h is the track deviation of the AUV in the vertical plane, Δ is the forward distance, k i is the integration parameter.

[0063] Furthermore, the AUV high-order sliding mode controller is:

[0064]

[0065]

[0066]

[0067]

[0068] In the above formula, k1 and k2 are controller gains; s u 、 s θ 、s ψ is the sliding surface; sgn is the sign function; e u 、e θ 、e ψ is the error, is g in the kinetic model u 、g p 、g q 、g r estimated value of; represents the disturbance caused by the model uncertainty of the observer estimate and the ocean current.

[0069] Furthermore, the control output of the AUV high-order sliding mode controller is solved based on the damping matrix to perform the control distribution of the X rudder, which is:

[0070] y=(W -1 B T B) -1 W -1 B T τ

[0071] In the above formula, τ=[τ p ,τ q ,τ r ] T is the control output solved by the AUV high-order sliding mode controller; W is the damping matrix, W=diag(w1,w2,w3,w4), w i ,i∈{1,2,3,4}; B is the X-rudder control allocation matrix.

[0072] Furthermore, considering the rudder angle output limit, w i satisfy:

[0073]

[0074] In the above formula, w s is a constant; u i , i∈{1,2,3,4} is the rudder angle output of the AUV high-order sliding mode controller; δ min,i is the minimum limit of the corresponding rudder angle; δ max,i is the maximum limit of the corresponding rudder angle; A constant that is preset to be greater than 0.

[0075] The beneficial effects of the above technical solutions provided by the embodiments of the present invention include at least:

[0076] An embodiment of the present invention provides a robust and precise control method for an X-rudder AUV, including: obtaining the position, attitude, linear velocity, and angular velocity of the AUV hull, establishing a kinematic model, a dynamic model, and an X-distribution model to form a mathematical model of the X-rudder AUV; obtaining the current AUV position and route information in the horizontal and vertical planes, establishing a horizontal plane kinematic controller, and designing a vertical plane kinematic control law; designing a nonlinear disturbance observer to estimate and compensate for the unmodeled dynamics of the X-rudder AUV mathematical model and the vertical plane kinematic control law, generating a high-order sliding mode controller for the AUV; and distributing the control of the X-rudder based on the control outputs calculated by the AUV high-order sliding mode controller based on a damping matrix. This control method can effectively reduce the impact of model uncertainty and external disturbances on AUV control.

[0077] Other features and advantages of the present invention will be described in the following description, and in part will become apparent from the description, or will be understood by practicing the present invention. The purposes and other advantages of the present invention can be realized and obtained by the structures particularly pointed out in the written description, claims, and drawings.

[0078] The technical solution of the present invention is further described in detail below through the accompanying drawings and embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0079] The accompanying drawings are used to provide a further understanding of the present invention and constitute a part of the specification. Together with the embodiments of the present invention, they are used to explain the present invention and do not constitute a limitation of the present invention. In the accompanying drawings:

[0080] Figure 1 Flowchart of the robust and precise control method for an X-type rudder AUV provided by an embodiment of the present invention;

[0081] Figure 2 A structural diagram of the X-rudder AUV controller provided by an embodiment of the present invention;

[0082] Figure 3 A schematic diagram of a sight line navigation method provided by an embodiment of the present invention;

[0083] Figure 4 AUV dynamics controller structure diagram provided by an embodiment of the present invention;

[0084] Figure 5 This is a simulation effect diagram of the AUV three-dimensional path tracking control provided by an embodiment of the present invention;

[0085] Figure 6 This is a simulation effect diagram of the AUV horizontal trajectory control provided by an embodiment of the present invention;

[0086] Figure 7 This is a simulation effect diagram of the AUV vertical trajectory control provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0087] Exemplary embodiments of the present disclosure will be described in more detail below with reference to the accompanying drawings. Although exemplary embodiments of the present disclosure are shown in the accompanying drawings, it should be understood that the present disclosure can be implemented in various forms and should not be limited by the embodiments set forth herein. Rather, these embodiments are provided to enable a more thorough understanding of the present disclosure and to fully convey the scope of the present disclosure to those skilled in the art.

[0088] The embodiment of the present invention provides a robust and precise control method for an X-type rudder AUV, referring to Figure 1 Shown, including:

[0089] Obtain the position, attitude, linear velocity and angular velocity of the AUV hull, establish the kinematic model, dynamic model and X distribution model, and form the X-rudder AUV mathematical model;

[0090] Obtain the current AUV position and route information in the horizontal and vertical planes, establish a horizontal plane kinematic controller, and design a vertical plane kinematic control law;

[0091] A nonlinear disturbance observer is designed to estimate and compensate the unmodeled dynamics of the X-rudder AUV mathematical model and vertical plane kinematic control law, generating a high-order sliding mode controller for the AUV.

[0092] The control output of the AUV high-order sliding mode controller is solved based on the damping matrix to distribute the control of the X-rudder.

[0093] This control method can effectively reduce the impact of model uncertainty and external interference on AUV control.

[0094] The following is a detailed description of the robust and precise control method for the X-type rudder AUV:

[0095] Step 1. Construct the X-rudder AUV mathematical model:

[0096] The mathematical model of the X-rudder AUV includes the AUV kinematic model, dynamic model, and X-distribution model. The kinematic model describes the relationship between the first-order differential of the AUV's position in the earth coordinate system and the generalized velocity in the hull coordinate system, as shown in the following formula:

[0097]

[0098]

[0099]

[0100]

[0101]

[0102]

[0103] The AUV motion state is described and constructed using the form recommended by the SNAME system, where the vector [x, y, z] is defined to represent the position of the hull in the earth coordinate system, the Euler angle vector [φ, θ, ψ] is defined to describe the hull posture, [u, v, w] represents the linear velocity of the hull in the hull coordinate system, and [p, q, r] represents the angular velocity of the hull in the hull coordinate system. The specific parameters are shown in Table 1:

[0104] Table 1 SNAME symbol definition

[0105]

[0106]

[0107] The AUV dynamics model describes the motion state of the AUV under the influence of force / torque. The ocean current velocity in the hull coordinate system can be expressed as:

[0108] u c =U c cos(θ)cos(ψ c -ψ)

[0109]

[0110]

[0111] Among them, U c , ψ c They respectively represent the magnitude and direction of ocean current speed in the geodetic coordinate system.

[0112] The relative velocity in the hull coordinate system is:

[0113] u r =uu c

[0114] v r =vv c

[0115] w r =ww c

[0116] Assuming that the AUV is subject to zero buoyancy in water, the longitudinal dynamic model of the AUV considering the influence of ocean current can be described as:

[0117]

[0118] Where m is the mass of AUV, C mnis the hydrodynamic coefficient, C∈[X,Y,Z,K,M,N] is the hydrodynamic force corresponding to the six degrees of freedom of the AUV, the subscript mn represents the corresponding motion, m,n∈[u,v,w,p,q,r....].

[0119] Similarly, the dynamic model of the remaining 5 degrees of freedom of the AUV can be written as a form of decoupling the known model from the interference and unknown models, which can be described as:

[0120]

[0121]

[0122]

[0123] Among them, f o represents the known terms of the kinetic model, D o represents the uncertainty term of the dynamic model, τ o is the control force / torque corresponding to the degree of freedom, generated by the AUV thruster and X-shaped rudder. u , f v , f w , f p , f q , D v , D w , D p , D q , τ p , τ q , τ r , g u , g p , g q , g r These parameters only need to be given in the four equations of g, and the remaining several terms can be easily derived from the AUV longitudinal dynamics model equation considering the influence of ocean currents. In addition, there are:

[0124]

[0125]

[0126] Among them, I x , I y , I z is the moment of inertia around the corresponding coordinate axis of the AUV, τ p , τ q and τ r All are generated by X-rudder.

[0127] The X-rudder distribution model of the AUV describes the relationship between the rudder angle and the output torque, as shown in the following formula:

[0128]

[0129] Where: k δi 、m δi 、n δi , i=1,…,4 are the hydrodynamic coefficients of the four rudders in heel, pitch and rotation degrees of freedom, δ i The four rudder angles of the X rudder.

[0130] Step 2. Build the X-rudder AUV controller:

[0131] The X-rudder AUV controller is composed of a kinematic controller based on sideslip angle compensation, a dynamic controller based on a high-order sliding mode, and a nonlinear disturbance observer. The X-rudder AUV controller structure is as follows: Figure 2 shown.

[0132] Specifically, the horizontal plane kinematic controller of the AUV adopts the line of sight navigation method, as shown in the following diagram: Figure 3 As shown in the figure. k-1 (x k-1 ,y k-1 ), p k (x k ,y k ) are the horizontal two-dimensional coordinates of the starting waypoint and the target waypoint of the current segment, p k+1 (x k+1 ,y k+1 ) is the horizontal plane two-dimensional coordinate of the next waypoint, (x t ,y t ) is the current position of the AUV, then the track point p k-1 (x k-1 ,y k-1 ) and p k (x k ,y k ) is the track angle ψ of the straight track segment k The calculation is as follows:

[0133] ψ k =atan2(y k+1 -y k ,x k+1 -x k ) (6)

[0134] By drawing a perpendicular line from the current position of the AUV to the target straight track segment, the track deviation e of the AUV distance track can be obtained. The calculation formula is as follows:

[0135] e=-(xx k-1 )sin(ψ k )+(yy k-1 )cos(ψ k) (7)

[0136] In order to make the track deviation of AUV track tracking converge to zero asymptotically, according to the traditional straight line LOS design algorithm, the heading command is calculated as follows:

[0137]

[0138] Where Δ is the forward-looking distance. When the AUV is disturbed by ocean currents during navigation, the sideslip angle caused by the interference will cause the traditional LOS to fail to output the correct heading command. After the straight track tracking stabilizes, static error will exist. This embodiment improves the LOS algorithm to address this problem and designs a sideslip angle observer to compensate for the LOS algorithm to achieve the goal of zero static error in track control. The sideslip angle observer design process is as follows:

[0139] First, take the derivative of the track deviation and get the following formula:

[0140]

[0141] Where: V is the horizontal navigation speed of the AUV, β is the sideslip angle caused by the ocean current interference; ψ k represents the heading angle under the sname system. Since the sideslip angle of the AUV during navigation is very small, the above formula (9) can be converted into the following formula:

[0142]

[0143] Assume g = Vβcos(ψ-ψ k ), the interference estimation algorithm of g can be designed according to the above formula (10) as shown below:

[0144]

[0145] Where: is the estimated value of g, k g is the parameter of the observer, p g is the auxiliary variable of the observer.

[0146] Then, the estimated value of the sideslip angle is calculated according to formula (10) and formula (11):

[0147]

[0148] The observed sideslip angle is used with the modified LOS algorithm. Considering the under-actuated nature of the AUV, the horizontal plane kinematic controller considering sideslip angle compensation is as follows:

[0149]

[0150] Among them, ψ d is the desired heading angle in the horizontal plane, ψk is the track angle of the straight track segment, e is the track deviation, Δ is the forward distance, is the estimated sideslip angle.

[0151] On the vertical plane, let p k (y k ,z k ), p k+1 (y k+1 ,z k+1 ) are the two-dimensional coordinates representing the eastward displacement and depth of the starting waypoint and the target waypoint of the current segment, respectively. (y, z) is the current position of the AUV. k (y k ,z k ), p k+1 (y k+1 ,z k+1 The diving angle of the straight track segment composed of ) can be calculated as shown below:

[0152] θ k =atan2(z k+1 -z k ,y k+1 -y k ) (14)

[0153] Draw a perpendicular line from the current position of the AUV to the target track segment, and the track deviation e of the AUV from the track can be obtained. h , and its calculation formula is shown as follows:

[0154] e h =-(yy k )sin(θ k )+(zz k )cos(θ k ) (15)

[0155] In order to make the AUV vertical track deviation converge to zero, the algorithm for generating the command pitch angle is designed as shown in the following formula:

[0156]

[0157] Where: Δ is the forward sight distance.

[0158] In order to eliminate the vertical plane track control static error caused by the attack angle generated by the vertical plane imbalance, an integral action is added to the above formula (16) to eliminate the vertical plane track control static error through integral accumulation. The vertical plane kinematic control law with the integral action is shown as follows:

[0159]

[0160] Where: θ dis the desired heading angle in the vertical plane, θ k is the diving angle of the straight track segment, e h is the vertical track deviation, Δ is the forward distance, k i is the integration parameter.

[0161] Afterwards, the feedback control law of the AUV is designed based on the high-order sliding mode algorithm and nonlinear disturbance observer.

[0162] like Figure 4 As shown in Figure 2, taking the AUV roll stabilization control as an example, the tracking error of the system is defined as:

[0163]

[0164] in, is the desired heel angle, set to 0. The designed linear sliding surface is:

[0165]

[0166] In the above formula, c is the control constant.

[0167] Taking the derivative of the above formula, we have:

[0168]

[0169] According to (1), we have:

[0170]

[0171]

[0172] Design the high-order sliding mode reaching law as:

[0173]

[0174] Where k1 and k2 are gains, and sgn() is the sign function. Substituting equations (4), (20), and (21) into equation (22), we obtain the high-order sliding mode control law for the roll degree of freedom:

[0175]

[0176] Considering that the AUV roll angle is small, formula (1) can be rewritten as:

[0177]

[0178]

[0179] Similar to the process of equations (18)-(23), the high-order sliding mode control law of the AUV pitch and bow degrees of freedom can be obtained as follows:

[0180]

[0181]

[0182] Among them, e θ =θ-θ d , e ψ =ψ-ψ d ,

[0183] The AUV also needs to achieve speed control, and the sliding surface is designed as follows:

[0184]

[0185] Among them, e u =uu d is the speed error. Similar to the process of equations (20)-(23), the high-order sliding mode control law for AUV speed can be obtained as follows:

[0186]

[0187] Among them, e u =uu d Because the control laws (23), (25), and (27) contain unmodeled dynamics, a nonlinear disturbance observer is designed to estimate and compensate for the unmodeled dynamics. The observer form corresponding to the four degrees of freedom is:

[0188]

[0189] In formula (28), L is the observer gain, ξ u ,ξ p and ξ q is an auxiliary variable. The AUV high-order sliding mode controller combined with the nonlinear disturbance observer is:

[0190]

[0191]

[0192]

[0193]

[0194] In the above formula, k1 and k2 are controller gains, su, s θ 、s ψ is the sliding surface, sgn is the sign function, e u 、e θ 、e ψ is the error, is g in the kinetic modelu 、g p 、g q 、g r The estimated value of can be treated as a constant; is the disturbance caused by the model uncertainty estimated by the observer and the ocean current, where the observer refers to the disturbance observer of formula (28); θd is the desired heading angle in the vertical plane; ψ d is the desired heading angle in the horizontal plane.

[0195] The control law (29) only requires the known quantities in model (1). The unknown quantities and external disturbances are compensated by the disturbance observer (28). If the dynamic model is completely unknown, the disturbance observer and high-order sliding mode controller composed of (28) and (29) can be further written as:

[0196]

[0197]

[0198]

[0199]

[0200]

[0201] Where L is the disturbance observer gain, ξ u ,ξ p ,ξ q ,ξ r is an auxiliary variable.

[0202] Thus, AUV control can be completed when the dynamic model is completely unknown.

[0203] Step 3. Build the X-rudder AUV control allocation strategy:

[0204] The control output [τ u ,τ p ,τ q ,τ r ], τ u Directly generated by the AUV main thruster, τ = [τ p ,τ q ,τ r ] T Generated by the AUV's X-rudder, the X-rudder angles need to be calculated according to the control allocation strategy.

[0205] According to formula (5), we have:

[0206] τ=Bδ (10)

[0207] in, δ=[δ1,δ2,δ3,δ4] T .

[0208] This embodiment proposes an improved pseudo-inverse algorithm to implement X-rudder control allocation based on the damping matrix. It can quickly calculate the command rudder angle while meeting the requirements of rudder angle limitation and fast response.

[0209] y=(W -1 B T B) -1 W -1 B T τ (11)

[0210] Where y is the command rudder angle output by the algorithm, τ=[τ p ,τ q ,τ r ] T is the controller output, W=diag(w1,w2,w3,w4) is the damping matrix, B is the X-rudder control distribution matrix, w i , i∈{1,2,3,4} is a constant greater than 0, and w in the damping matrix i The larger the value, the stronger the corresponding rudder angle output limitation. Considering the rudder angle output limit, w i satisfy:

[0211]

[0212] Among them, w s is a large constant, u i ,i∈{1,2,3,4} is the rudder angle output of the control allocation algorithm (referring to formula (33)), δ min,i and δ max,i are the minimum and maximum limits of the corresponding rudder angle, A constant greater than 0.

[0213] The AUV control algorithm in this embodiment is simulated according to steps 1 to 3. In the simulation environment, it is assumed that the AUV is disturbed by a sinusoidal ocean current and the dynamic model is unknown. The AUV starts from point [-8, 102, 0] and traverses 6 path points [500, 300, 15], [1000, 300, 30], [1500, 0, 45], [1000, -300, 30], [500, -300, 15], and [80, 0, 0]. The expected speed is 3 knots. Robust and accurate 3D path tracking is completed according to the proposed control algorithm. The 3D path tracking results are shown in Figure 2. Figure 5 As shown, the AUV horizontal trajectory is as follows Figure 6 As shown, the AUV vertical trajectory is as follows Figure 7 shown.

[0214] The robust and precise control method for an X-rudder AUV, provided in this embodiment, effectively addresses the low control accuracy, poor robustness, and dependence on model parameters inherent in traditional methods, often caused by current disturbances and model uncertainty. First, a kinematic controller with sideslip angle compensation is designed based on an observer, reducing the control error of the kinematic controller. Second, considering the actuator chattering problem associated with traditional sliding mode control, a dynamic controller is designed, combining a high-order sliding mode controller with a nonlinear disturbance force observer. This controller is suitable for controlling AUVs under external disturbances and model uncertainty. Finally, an improved adaptive X-rudder control allocation method is proposed, combining a damping matrix to balance X-rudder angle output constraints with real-time computational performance.

[0215] Obviously, those skilled in the art may make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if such changes and modifications fall within the scope of the claims and their equivalents, the present invention is intended to include such changes and modifications.

Claims

1. A robust and precise control method for an X-type rudder AUV, characterized by: include: Obtain the position, attitude, linear velocity and angular velocity of the AUV hull, establish the kinematic model, dynamic model and X distribution model, and form the X-rudder AUV mathematical model; The current AUV position and route information in the horizontal and vertical planes are obtained respectively, a horizontal plane kinematic controller is established, and a vertical plane kinematic control law is designed; wherein, establishing the horizontal plane kinematic controller includes: obtaining the two-dimensional coordinates of the current AUV position point in the horizontal plane and the starting waypoint, target waypoint, and next waypoint of the current segment respectively; calculating the track angle of the first straight track segment; the first straight track segment is composed of the starting waypoint and target waypoint of the current segment in the horizontal plane; drawing a perpendicular line from the current AUV position point in the horizontal plane to the first straight track segment, and obtaining the track deviation of the AUV in the horizontal plane according to the track angle of the first straight track segment; eliminating the track deviation of the AUV in the horizontal plane, calculating the interference estimation and the sideslip angle estimation value, and generating the horizontal plane kinematic controller; Eliminating the AUV horizontal track deviation and calculating the interference estimate include: Derivative of the AUV horizontal track deviation: In the above formula, V represents the horizontal navigation speed of the AUV; β represents the sideslip angle caused by the ocean current interference; ψ k represents the track angle of the first straight track segment; Assuming g=Vβcos(ψ-ψ k ); The interference g is estimated by the following formula: In the above formula, is the estimated value of g; k g is the parameter of the observer; p g is the auxiliary variable of the observer; V represents the horizontal navigation speed of the AUV; e represents the horizontal track deviation of the AUV; ψ k represents the track angle of the first straight track segment; Designing a nonlinear disturbance observer to estimate and compensate for the unmodeled dynamics of the X-rudder AUV mathematical model and the vertical plane kinematic control law, and generating a high-order sliding mode controller for the AUV; The control output of the AUV high-order sliding mode controller is solved based on the damping matrix to perform control allocation of the X rudder.

2. The robust and precise control method for an X-type rudder AUV according to claim 1, characterized in that: The track angle of the first straight track segment is calculated using the following formula: ψ k =atan2(y k+1 -y k ,x k+1 -x k ) In the above formula, (x k ,y k ) represents the horizontal two-dimensional coordinates of the target waypoint of the current segment; (x k+1 ,y k+1 ) represents the horizontal plane two-dimensional coordinate of the next waypoint of the current segment.

3. The robust and precise control method for an X-type rudder AUV according to claim 1, characterized in that: The track deviation of the AUV horizontal plane is calculated by the following formula: e=-(xx k-1 )sin(ψ k )+(yy k-1 )cos(ψ k ) In the above formula, (x k-1 ,y k-1 ) represents the horizontal two-dimensional coordinate of the starting waypoint of the current segment; (x, y) represents the current AUV position point on the horizontal plane; ψ k Indicates the track angle of the first straight track segment.

4. The robust and precise control method for an X-type rudder AUV according to claim 1, characterized in that: The estimated sideslip angle is calculated using the following formula: In the above formula, is the estimated value of g; V represents the horizontal navigation speed of AUV; ψ k Indicates the track angle of the first straight track segment.

5. The robust and precise control method for an X-type rudder AUV according to claim 1, characterized in that: The vertical plane kinematic control law is designed as follows: Obtain the current AUV position point in the vertical plane, as well as the two-dimensional coordinates of the starting waypoint and the target waypoint of the current segment; Calculating a submergence angle of a second straight track segment, wherein the second straight track segment is composed of a starting waypoint and a target waypoint of the current segment in the vertical plane; Draw a perpendicular line from the current position of the AUV on the vertical plane to the second straight track segment, and obtain the track deviation of the AUV on the vertical plane according to the submergence angle of the second straight track segment; The track deviation of the AUV in the vertical plane is eliminated, and an integral action vertical plane kinematic control law is introduced.

6. The robust and precise control method for an X-type rudder AUV according to claim 5, characterized in that: The vertical plane kinematic control law of the integral action is: In the above formula, θ d is the desired heading angle in the vertical plane, θ LOS is the commanded pitch angle, θ k is the diving angle of the second straight track segment, e h is the track deviation of the AUV in the vertical plane, Δ is the forward distance, k i is the integration parameter.

7. The robust and precise control method for an X-type rudder AUV according to claim 1, characterized in that: The AUV high-order sliding mode controller is: In the above formula, k1 and k2 are controller gains; su, s θ 、s ψ is the sliding surface; sgn is the sign function; e u 、e θ 、e ψ is the error, is g in the kinetic model u 、g p 、g q 、g r estimated value of; represents the disturbance caused by the model uncertainty of the observer estimate and the ocean current.

8. The robust and precise control method for an X-shaped rudder AUV according to claim 1, characterized in that: Based on the damping matrix, the control output of the AUV high-order sliding mode controller is solved to perform the control distribution of the X rudder, which is: y=(W -1 B T B) -1 W -1 B T τ In the above formula, τ=[τ p ,τ q ,τ r ] T is the control output solved by the AUV high-order sliding mode controller; W is the damping matrix, W=diag(w1,w2,w3,w4), w i ,i∈{1,2,3,4}; B is the X-rudder control allocation matrix.

Images