Method and system for hierarchical disturbance rejection depth tracking control of underactuated underwater vehicle

Abstract

The disclosure provides a method and system for hierarchical disturbance rejection depth tracking control of an underactuated underwater vehicle, and the depth tracking of the underactuated underwater vehicle is divided into kinematic layer guidance and dynamic layer pitch tracking. Adaptive line of sight guidance is used in the kinematic layer to convert a depth error into a desired pitch angle and to estimate and compensate an angle of attack to reject disturbance introduced by an unmeasurable true angle of attack. Based on the above, in the dynamic layer, the active disturbance rejection sliding mode pitch tracking method is used to observe a composite disturbance including an unknown dynamic model and an environmental disturbance by using the active disturbance rejection framework. The model is compensated as a unified integral series type, a sliding mode control law is finally designed to resist an observation error, and a control elevator angle is calculated.

Claims

1. A method for hierarchical disturbance rejection depth tracking control of an underactuated underwater vehicle, comprising: determining navigation information of the underwater vehicle, wherein the navigation information comprises a desired depth, a current depth, a current navigation speed, and a current pitch angle; determining a depth tracking error according to the desired depth and the current depth and converting the depth tracking error into a desired pitch angle based on an adaptive line-of-sight guidance law, specifically comprises: subtracting a current depth z from a desired depth z.sub.d to obtain the depth tracking error e.sub.z; converting the depth tracking error e.sub.z into a desired pitch angle θ.sub.d based on the adaptive line-of-sight guidance law specifically through the following formula: ${\theta }_d={\tan }^{-1}\left(\frac{e_z}{\Delta }+\hat{\alpha }\right),$ where Δ is the forward-looking distance, and {circumflex over (α)} is the adaptive parameter, wherein the adaptive line-of-sight guidance law comprises an adaptive parameter, so as to estimate a real angle of attack of the underwater vehicle and reduce a motion disturbance introduced by ignoring the angle of attack, and the adaptive parameter is determined according to the current navigation speed and the depth tracking error; determining an active disturbance rejection sliding mode pitch tracking control law comprising an extended state observer and a sliding mode feedback control law; wherein the extended state observer is configured to observe a composite disturbance of the underwater vehicle, specifically comprises: a pitch motion model of the underwater vehicle is:
{umlaut over (θ)}=f(θ,{dot over (θ)},U,I.sub.yy . . . )+b.sub.0δ+d.sub.θ, where {umlaut over (θ)} represents the pitch angular acceleration rate, f(θ, {dot over (θ)}, U, I.sub.yy . . . ) represents the acceleration rate corresponding to the moment caused by the internal structure of the vehicle, θ represents the vehicle pitch angle, {dot over (θ)} represents the vehicle pitch angular velocity, U represents the navigation speed, I.sub.yy represents the moment of inertia of the vehicle, d.sub.θ is the external environment disturbance, b.sub.0 is the elevator effect coefficient, and δ is the final elevator angle, considering f(θ, {dot over (θ)}, U, I.sub.yy . . . )+d.sub.θ as a composite disturbance and using the following extended state observer to iteratively calculate the composite disturbance: $\{\begin{array}{c}e=z_1\left[t\right]-\theta \\ z_1\left[t+1\right]=z_1\left[t\right]+h\left(z_2\left[t+1\right]-l_{1}e\right)\\ z_2\left[t+1\right]=z_2\left[t\right]+h\left(z_3\left[t+1\right]-l_{2}e+b_0\delta \right)\\ z_3\left[t+1\right]=z_3\left[t\right]+h\left(-l_{3}e\right)\end{array},$ where z.sub.1, z.sub.2, and z.sub.3 respectively are observed amounts of the vehicle pitch angle θ, the pitch angular velocity {dot over (θ)}, and the compound disturbance f(θ, {dot over (θ)}, U, I.sub.yy . . . )+d.sub.θ, [t] represents the iterative calculation of step t, h is the integration step size, and l.sub.1, l.sub.2 and l.sub.3 are the observation gains of the extended state observer, the composite disturbance is a sum of dynamic disturbances introduced by an internal structure of the vehicle and an external environment, the active disturbance rejection sliding mode pitch tracking control law further comprises: a tracking differentiator, configured to determine a numerical differential v.sub.2 of the desired pitch angle and a tracking signal v.sub.1 of the desired pitch angle, and an observation error of the extended state observer is considered in a design process of the sliding mode feedback control law and is combined with the desired pitch angle to calculate a elevator angle of the underwater vehicle, so that the observation error is overcome, specifically is: defining a sliding mode surface s=ce.sub.1+e.sub.2, where c is the weight coefficient, e.sub.1 is the pitch angle tracking error, e.sub.1=v.sub.1−z.sub.1, e.sub.2 is the pitch angular velocity tracking error, and e.sub.2=v.sub.2−z.sub.2; obtaining the sliding mode feedback control law according to an exponential reaching law design method: ${\delta }_0=\frac{1}{b_0}\left(c{e}_2+ks+\eta \mathrm{sign}\left(s\right)\right),$ where δ.sub.0 is the elevator angle calculated through the sliding mode feedback control law, the k parameter controls the rate at which the error converges to zero exponentially, and η is the coefficient of the robust term to overcome the observation error; and determining an observed amount of the composite disturbance and the elevator angle calculated through the sliding mode feedback control law in combination with the active disturbance rejection sliding mode pitch tracking control law, calculating a final elevator angle δ, specifically to be $\delta ={\delta }_0-\frac{z_3}{b_0}$ of the elevator based on the observed amount of the composite disturbance and the elevator angle calculated through the sliding mode feedback control law, so that the elevator navigates according to the final elevator angle and that the underwater vehicle navigates stably according to the desired depth, and calculating the final elevator angle according to the adaptive line of sight guidance law and the active disturbance rejection sliding mode pitch tracking control law, so that the underwater vehicle is capable of providing improved rejection to motion disturbance and dynamic disturbance and accuracy of depth tracking of the underwater vehicle is improved.

2. The method according to claim 1, Wherein {circumflex over (α)} changes adaptively according to the following formula: $\hat{\alpha }=\frac{\gamma U\Delta e_z}{\sqrt{{\Delta }^2+{\left(e_z+\Delta \hat{\alpha }\right)}^2}},$ where U is the navigation speed of the vehicle, and γ is the adaptive gain.

3. The method according to claim 1, further comprising: calculating v.sub.1 and v.sub.2 through the following formula: $\{\begin{array}{c}{v}_1\left[t+1\right]=v_1\left[t\right]+h{v}_2\left[t+1\right]\\ v_2\left[t+1\right]=v_2\left[t\right]+h.Math.\mathrm{fhan}\left(x_1,x_2,r,h\right)\end{array},$ where r is the tracking acceleration coefficient, h is the calculation step size, [t] represents the iterative calculation of step t, fhan(x.sub.1, x.sub.2, r, h) is the fastest control comprehensive function, x.sub.1 and x.sub.2 are intermediate variables expressed as x.sub.1=v.sub.1[t]−θ.sub.d and x.sub.2=v.sub.2 [t] respectively, and θ.sub.d is the desired pitch angle, wherein fhan(x.sub.1, x.sub.2, r, h) is calculated through the following formula: $\{\begin{array}{c}d=rh\\ d_0=hd\\ y=x_1+h{x}_2\\ a_0=\sqrt{d^2+8r.Math.y.Math.}\\ a=\{\begin{array}{c}{x}_2+\frac{a_0-d}{2}\mathrm{sign}\left(y\right),.Math.y.Math.>d_0\\ x_2+\frac{y}{h},.Math.y.Math.\le d_0\end{array}\\ \mathrm{fhan}\left(x_1,x_2,r,h\right)=-\{\begin{array}{c}r\mathrm{sign}\left(a\right),.Math.a.Math.>d\\ r\frac{a}{d},.Math.a.Math.\le d\end{array}\end{array},$ where d, d.sub.0, γ, α.sub.0, and α are all intermediate variables.

4. A system for hierarchical disturbance rejection depth tracking control of an underactuated underwater vehicle, comprising: a navigation information determining unit configured to determine navigation information of the underwater vehicle, wherein the navigation information comprises a desired depth, a current depth, a current navigation speed, and a current pitch angle; a desired pitch angle determining unit configured to determine a depth tracking error according to the desired depth and the current depth and convert the depth tracking error into a desired pitch angle based on an adaptive line of sight guidance law, specifically comprises: subtracting a current depth z from a desired depth z.sub.d to obtain the depth tracking error e.sub.z; converting the depth tracking error e.sub.z into a desired pitch angle θ.sub.d based on the adaptive line of sight guidance law specifically through the following formula: ${\theta }_d={\tan }^{-1}\left(\frac{e_z}{\Delta }+\hat{\alpha }\right),$ where Δ is the forward-looking distance, and {circumflex over (α)} is the adaptive parameter; wherein the adaptive line of sight guidance law comprises an adaptive parameter, so as to estimate a real angle of attack of the underwater vehicle and reduce a motion disturbance introduced by ignoring the angle of attack, and the adaptive parameter is determined according to the current navigation speed and the depth tracking error; a pitch tracking control law determining unit configured to determine an active disturbance rejection sliding mode pitch tracking control law comprising an extended state observer and a sliding mode feedback control law, wherein the extended state observer is configured to observe a composite disturbance of the underwater vehicle, specifically comprises: a pitch motion model of the underwater vehicle is:
{umlaut over (θ)}=f(θ,{dot over (θ)},U,I.sub.yy . . . )+b.sub.0δ+d.sub.θ, where {umlaut over (θ)} represents the pitch angular acceleration rate, f(θ, {dot over (θ)}, U, I.sub.yy . . . ) represents the acceleration rate corresponding to the moment caused by the internal structure of the vehicle, θ represents the vehicle pitch angle, {dot over (θ)} represents the vehicle pitch angular velocity, U represents the navigation speed, I.sub.yy represents the moment of inertia of the vehicle, d.sub.θ is the external environment disturbance, b.sub.0 is the elevator effect coefficient, and δ is the final elevator angle, considering f(θ, {dot over (θ)}, U, I.sub.yy . . . )+d.sub.θ as a composite disturbance and using the following extended state observer to iteratively calculate the composite disturbance: $\{\begin{array}{c}e=z_1\left[t\right]-\theta \\ z_1\left[t+1\right]=z_1\left[t\right]+h\left(z_2\left[t+1\right]-l_{1}e\right)\\ z_2\left[t+1\right]=z_2\left[t\right]+h\left(z_3\left[t+1\right]-l_{2}e+b_0\delta \right)\\ z_3\left[t+1\right]=z_3\left[t\right]+h\left(-l_{3}e\right)\end{array},$ where z.sub.1, z.sub.2, and z.sub.3 respectively are observed amounts of the vehicle pitch angle θ, the pitch angular velocity θ, and the compound disturbance f(θ,{dot over (θ)},U,I.sub.yy . . . )+dθ, [t] represents the iterative calculation of step t, h is the integration step size, and l.sub.1, l.sub.2 and l.sub.3 are the observation gains of the extended state observer, the composite disturbance is a sum of dynamic disturbances introduced by an internal structure of the vehicle and an external environment, the active disturbance rejection sliding mode pitch tracking control law further comprises: a tracking differentiator, configured to determine a numerical differential v.sub.2 of the desired pitch angle and a tracking signal v.sub.1 of the desired pitch angle, and an observation error of the extended state observer is considered in a design process of the sliding mode feedback control law and is combined with the desired pitch angle to calculate a elevator angle of the underwater vehicle, so that the observation error is overcome, specifically comprises: defining a sliding mode surface s=ce.sub.1+e.sub.2, where c is the weight coefficient, e.sub.1 is the pitch angle tracking error, e.sub.1=v.sub.1−z.sub.1, e.sub.2 is the pitch angular velocity tracking error, and e.sub.2=v.sub.2−z.sub.2; obtaining the sliding mode feedback control law according to an exponential reaching law design method: ${\delta }_0=\frac{1}{b_0}\left(c{e}_2+ks+\eta \mathrm{sign}\left(s\right)\right),$ where δ.sub.0 is the elevator angle calculated through the sliding mode feedback control law, the k parameter controls the rate at which the error converges to zero exponentially, and η is the coefficient of the robust term to overcome the observation error; and an elevator angle determining unit configured to determine an observed amount of the composite disturbance and the elevator angle calculated through the sliding mode feedback control law in combination with the active disturbance rejection sliding mode pitch tracking control law, calculate a final elevator angle δ, specifically to be $\delta ={\delta }_0-\frac{z_3}{b_0}$ of the elevator based on the observed amount of the composite disturbance and the elevator angle calculated through the sliding mode feedback control law, so that the elevator navigates according to the final elevator angle and that the underwater vehicle navigates stably according to the desired depth, and calculate the final elevator angle according to the adaptive line of sight guidance law and the active disturbance rejection sliding mode pitch tracking control law, so that the underwater vehicle is capable of providing improved rejection to motion disturbance and dynamic disturbance and accuracy of depth tracking of the underwater vehicle is improved.

5. The system according to claim 4, wherein the desired pitch angle determining unit includes parameter {circumflex over (α)} changes adaptively according to the following formula: $\hat{\alpha }=\frac{\gamma U\Delta e_z}{\sqrt{{\Delta }^2+{\left(e_z+\Delta \hat{\alpha }\right)}^2}},$ where U is the navigation speed of the vehicle, and γ is the adaptive gain.

6. The system according to claim 4, wherein the pitch tracking control law determining unit calculating v.sub.1 and v.sub.2 through the following formula: $\{\begin{array}{c}{v}_1\left[t+1\right]=v_1\left[t\right]+h{v}_2\left[t+1\right]\\ v_2\left[t+1\right]=v_2\left[t\right]+h.Math.\mathrm{fhan}\left(x_1,x_2,r,h\right)\end{array},$ where r is the tracking acceleration coefficient, h is the calculation step size, [t] represents the iterative calculation of step t, fhan(x.sub.1, x.sub.2, r, h) is the fastest control comprehensive function, x.sub.1 and x.sub.2 are intermediate variables expressed as x.sub.1=v.sub.1[t]−θ.sub.d and x.sub.2=v.sub.2 [t] respectively, and θ.sub.d is the desired pitch angle, wherein fhan(x.sub.1, x.sub.2, r, h) is calculated through the following formula: $\{\begin{array}{c}d=rh\\ d_0=hd\\ y=x_1+h{x}_2\\ a_0=\sqrt{d^2+8r.Math.y.Math.}\\ a=\{\begin{array}{c}{x}_2+\frac{a_0-d}{2}\mathrm{sign}\left(y\right),.Math.y.Math.>d_0\\ x_2+\frac{y}{h},.Math.y.Math.\le d_0\end{array}\\ \mathrm{fhan}\left(x_1,x_2,r,h\right)=-\{\begin{array}{c}r\mathrm{sign}\left(a\right),.Math.a.Math.>d\\ r\frac{a}{d},.Math.a.Math.\le d\end{array}\end{array},$ where d, d.sub.0, γ, α.sub.0, and α are all intermediate variables.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) FIG. 1 is a flow chart of a method for hierarchical disturbance rejection depth tracking control of an underactuated underwater vehicle according to an embodiment of the disclosure.

(2) FIG. 2 is a block diagram of the method for hierarchical anti-disturbance depth tracking control of the underwater vehicle according to an embodiment of the disclosure.

(3) FIG. 3 is a schematic view of a typical underactuated autonomous underwater vehicle according to an embodiment of the disclosure.

(4) FIG. 4a is curve graph of depth tracking in a real-prototype test according to an embodiment of the disclosure.

(5) FIG. 4b is curve graph of pitch tracking and observed pitch in the real-prototype test according to an embodiment of the disclosure.

(6) FIG. 4c is curve graph of pitch angular velocity tracking and observed angular velocity in the real-prototype test according to an embodiment of the disclosure.

(7) FIG. 4d is curve graph of composite disturbance observed values in the real-prototype test according to an embodiment of the disclosure.

(8) FIG. 4e is curve graph of estimated values of the angle of attack in the real-prototype test according to an embodiment of the disclosure.

(9) FIG. 5 is a structural diagram of a system for hierarchical disturbance rejection depth tracking control of an underactuated underwater vehicle according to an embodiment of the disclosure.

DESCRIPTION OF THE EMBODIMENTS

(10) In order to make the objectives, technical solutions, and advantages of the disclosure clearer and more comprehensible, the disclosure is further described in detail with reference to the drawings and embodiments. It should be understood that the specific embodiments described herein serve to explain the disclosure merely and are not used to limit the disclosure.

(11) The disclosure provides a method and system for hierarchical disturbance rejection depth tracking control of an underactuated underwater vehicle, and the disclosure belongs to the field of automatic control. In the disclosure, the depth tracking of the underactuated underwater vehicle is divided into kinematic layer guidance and dynamic layer pitch tracking. Adaptive line of sight guidance is used in the kinematic layer to convert a depth error into a desired pitch angle and to estimate and compensate the angle of attack to reject the disturbance introduced by the unmeasurable true angle of attack. Based on the above, in the kinematic layer, the active disturbance rejection sliding mode pitch tracking method is used to observe the composite disturbance including an unknown dynamic model and an environmental disturbance by using the active disturbance rejection framework. The model is compensated as a unified integral series type, the sliding mode control law is finally designed to resist an observation error, and the control elevator angle is calculated. The abovementioned control method is deployed on the on-board computer of the vehicle, so that the data fed back by the sensor is read, the elevator actuator is controlled, the system for hierarchical disturbance rejection depth tracking control is formed, and the robust depth tracking navigation of the vehicle is implemented.

(12) The disclosure designs a method for hierarchical disturbance rejection depth tracking control of an underactuated autonomous underwater vehicle. In the method, the depth tracking control of the vehicle is divided into the kinematic layer adaptive line of sight angle guidance law and the dynamic layer active disturbance rejection sliding mode pitch tracking control law, and the two resist disturbances from both the kinematic layer and the dynamic layer, so that the accuracy of depth tracking under the disturbances is improved.

(13) It can be understood that, starting from the actual technical means provided by the disclosure, it is obvious that the method for hierarchical anti-disturbance depth tracking control of the underwater vehicle provided by the disclosure is not only applicable to an underactuated autonomous underwater vehicle, but also applicable to the control of other types of underwater vehicles. The purpose of the technical means provided by the disclosure is to improve the accuracy of depth tracking of an underwater vehicle. The specific type of underwater vehicle cannot substantially limit the protection scope of the technical solutions of the disclosure.

(14) FIG. 1 is a flow chart of a method for hierarchical disturbance rejection depth tracking control of an underactuated underwater vehicle according to an embodiment of the disclosure. As shown in FIG. 1, the method includes the following steps.

(15) In S101, navigation information of the underwater vehicle is determined. The navigation information includes a desired depth, a current depth, a current navigation speed, and a current pitch angle.

(16) In S102, a depth tracking error is determined according to the desired depth and the current depth, and the depth tracking error is converted into a desired pitch angle based on an adaptive line of sight guidance law. The adaptive line of sight guidance law includes an adaptive parameter, so as to estimate a real angle of attack of the underwater vehicle and reduce a motion disturbance introduced by ignoring the angle of attack. The adaptive parameter is determined according to the current navigation speed and the depth tracking error.

(17) In S103, an active disturbance rejection sliding mode pitch tracking control law including an extended state observer and a sliding mode feedback control law is determined. The extended state observer is configured to observe a composite disturbance of the underwater vehicle. The composite disturbance is a sum of dynamic disturbances introduced by an internal structure of the vehicle and an external environment. An observation error of the extended state observer is considered in a design process of the sliding mode feedback control law and is combined with the desired pitch angle to calculate an elevator angle of the underwater vehicle, so that the observation error is overcome.

(18) In S104, an observed amount of the composite disturbance and the elevator angle calculated through the sliding mode feedback control law in combination with the active disturbance rejection-sliding mode pitch tracking control law are determined. A final elevator angle is calculated based on the observed amount of the composite disturbance and the elevator angle calculated through the sliding mode feedback control law, so that the elevator navigates according to the final elevator angle, and that the underwater vehicle navigates stably according to the desired depth. The final elevator angle is calculated according to the adaptive line of sight guidance law and the active disturbance rejection sliding mode pitch tracking control law, so that the underwater vehicle is capable of providing improved rejection to motion disturbance and dynamic disturbance, and accuracy of depth tracking of the underwater vehicle is improved.

(19) FIG. 2 is a block diagram of the method for hierarchical disturbance rejection depth tracking control according to an embodiment of the disclosure.

(20) As shown in FIG. 2, in the kinematic layer, in the adaptive line of sight guidance law, the depth tracking error e.sub.z is converted into the desired pitch θ.sub.d according to formula (1) and outputted into the pitch tracking control law.

(21) $\begin{array}{cc}{\theta }_d={\tan }^{-1}\left(\frac{e_z}{\Delta }+\hat{\alpha }\right).& \left(1\right)\end{array}$

(22) In the formula, the depth tracking error is defined as the difference between the desired depth z.sub.d and the actual depth z, i.e., e.sub.z=z.sub.d−z. Δ is the forward-looking distance, is a manually adjustable parameter, and is generally 3 to 5 times the length of the vehicle. The guidance law introduces an adaptive parameter {circumflex over (α)} to estimate the true angle of attack of the vehicle. Since the real and accurate angle of attack cannot be obtained easily, the angle of attack is generally ignored in the currently-available control methods of vehicles. However, ignoring the angle of attack may cause a kinematic layer disturbance, and it is precisely because of the introduction of {circumflex over (α)} that the guidance law is capable of resisting the kinematic layer disturbance. {circumflex over (α)} changes adaptively according to formula (2).

(23) $\begin{array}{cc}\hat{\alpha }=\frac{\gamma U\Delta e_z}{\sqrt{{\Delta }^2+{\left(e_z+\Delta \hat{\alpha }\right)}^2}},& \left(2\right)\end{array}$
where U is the navigation speed of the vehicle, γ is the manually adjustable adaptive gain, and the adaptive parameter {circumflex over (α)} corresponds to the estimated angle of attack.

(24) In the dynamic layer, the active disturbance rejection-sliding mode control method is used to construct the pitch tracking control law of formula (3), and the elevator angle S is calculated. The active disturbance rejection sliding mode pitch tracking control law is formed by the extended state observer, the sliding mode feedback control law, and a tracking differentiator.

(25) The composite disturbance including the internal unknown model and external environmental disturbance are observed by the extended state observer. The observed value z.sub.3 of the composite disturbance is then substituted into formula (3), the complex pitch motion model is compensated as an integral series type, and that strong pitch disturbance rejection tracking is implemented.

(26) $\begin{array}{cc}\delta ={\delta }_0-\frac{z_3}{b_0},& \left(3\right)\end{array}$
where δ.sub.0 is the elevator angle calculated through the sliding mode feedback control law, so the observation error of the active disturbance rejection can be further resisted, and b.sub.0 is the elevator effect coefficient, which is related to the vehicle motion model.

(27) A pitch motion model of the underwater vehicle can be expressed as formula (4):
{umlaut over (θ)}=f(θ,{dot over (θ)},U,I.sub.yy . . . )+b.sub.0δ+d.sub.θ  (4),
where {umlaut over (θ)} represents the pitch angular acceleration rate, f(θ, {dot over (θ)}, U, I.sub.yy . . . ) represents the acceleration rate corresponding to the torque caused by the internal properties of the vehicle and is a time variable that changes with complex unknown factors such as the vehicle pitch θ, pitch angular velocity {dot over (θ)}, navigation speed U, and moment of inertia I.sub.yy, and d.sub.θ is the external unknown environmental disturbance.

(28) The extended state observer (5) is used, and the complex and unknown total internal and external unknown amount f(θ, {dot over (θ)}, U, I.sub.yy . . . )+d.sub.θ in formula (4) can be considered as a composite disturbance and calculated iteratively, represented by z.sub.3.

(29) $\begin{array}{cc}\{\begin{array}{c}e=z_1\left[t\right]-\theta \\ z_1\left[t+1\right]=z_1\left[t\right]+h\left(z_2\left[t+1\right]-l_{1}e\right)\\ z_2\left[t+1\right]=z_2\left[t\right]+h\left(z_3\left[t+1\right]-l_{2}e+b_0\delta \right)\\ z_3\left[t+1\right]=z_3\left[t\right]+h\left(-l_{3}e\right)\end{array},& \left(5\right)\end{array}$
where z.sub.1, z.sub.2, and z.sub.3 respectively are the estimates of the vehicle pitch angle θ, the pitch angular velocity {dot over (θ)}, and the internal and external unknown compound disturbance f(θ, {dot over (θ)}, U, I.sub.yy . . . )+d.sub.θ, [t] represents the iterative calculation of step t, h is the integration step size, and l.sub.1, l.sub.2 and l.sub.3 are the observation gains of the observer.

(30) The estimated value z.sub.3 of the composite disturbance is substituted into the pitch control law (3), (3) is substituted into the complex pitch motion model (4), and the original complex controlled object is to be transformed into a simple integral series type {umlaut over (θ)}=b.sub.0δ.sub.0. Therefore, regardless of changes in internal dynamics and external environmental disturbances, by using the extended state observer (5) and the pitch control law (3), the originally-controlled object is transformed into a unified integral series type. Therefore, the pitch tracking controller is capable of resisting the dynamic layer disturbance.

(31) Since the extended state observer cannot guarantee that the observed value z.sub.3 can perfectly converge to the true value f(θ, {dot over (θ)}, U, I.sub.yy . . . )+d.sub.θ, the presence of the observation error is considered in the process of feedback control law design. The feedback control law adopts the design idea of sliding mode control to further overcome the observation error.

(32) First, a sliding mode surface s=ce.sub.1+e.sub.2 is defined, where c is the weight coefficient, weighing the contributions of e.sub.1 and e.sub.2, e.sub.1 is the pitch angle tracking error, and e.sub.2 is the pitch angular velocity tracking error. The sliding mode feedback control law is obtained according to an exponential reaching law design method {dot over (s)}=−ks−ηsign(s):

(33) $\begin{array}{cc}{\delta }_0=\frac{1}{b_0}\left(c{e}_2+ks+\eta \mathrm{sign}\left(s\right)\right),& \left(6\right)\end{array}$
where the k parameter controls the rate at which the error converges to zero exponentially, and η is the coefficient of the robust term to overcome the observation error.

(34) A tracking differentiator (7) configured to obtain a numerical differential v.sub.2 of the desired pitch angle is used, and a tracking signal v.sub.1 of the desired pitch angle is obtained. Finally, the formulas for calculating the error in formula (6) are e.sub.1=v.sub.1−z.sub.1 and e.sub.2=v.sub.2−z.sub.2.

(35) 0$\begin{array}{cc}\{\begin{array}{c}{v}_1\left[t+1\right]=v_1\left[t\right]+h{v}_2\left[t+1\right]\\ v_2\left[t+1\right]=v_2\left[t\right]+h.Math.\mathrm{fhan}\left(x_1,x_2,r,h\right)\end{array},& \left(7\right)\end{array}$
where r is the tracking acceleration coefficient, h is the calculation step size, and [t] represents the iterative calculation of step t. fhan(x.sub.1, x.sub.2, r, h) is the fastest control comprehensive function, and x.sub.1 and x.sub.2 are intermediate variables respectively represented as x.sub.1=v.sub.1[t]−θ.sub.d and x.sub.2=v.sub.2 [t].

(36) $\begin{array}{cc}\{\begin{array}{c}d=\mathrm{rh}\\ d_0=hd\\ y=x_1+h{x}_2\\ a_0=\sqrt{d^2+8r.Math.y.Math.}\\ a=\{\begin{array}{c}{x}_2+\frac{a_0-d}{2}\mathrm{sign}\left(y\right),.Math.y.Math.>d_0\\ x_2+\frac{y}{h},.Math.y.Math.\le d_0\end{array}\\ fhan\left(x_1,x_2,r,h\right)=-\{\begin{array}{c}r\mathrm{sign}\left(a\right),.Math.a.Math.>d\\ r\frac{a}{d},.Math.a.Math.\le d\end{array}\end{array}.& \left(8\right)\end{array}$

(37) In order to achieve the above, the disclosure designs a system for hierarchical disturbance rejection depth tracking control of an underactuated underwater vehicle. A conversion program of formulas (1) to (3) and (5) to (8) is deployed on an on-board computer of the vehicle. The computer calls the program, automatically reads depth data z of a depth sensor and pitch angle data θ fed back by a pitch sensor, and runs a depth tracking control algorithm program. The calculated elevator command δ is used to control an executive steering engine to form a hierarchical disturbance rejection depth tracking control system for the vehicle, so that high-accuracy robust depth-tracking navigation of an underwater autonomous vehicle under disturbance is implemented.

(38) In order to verify the depth tracking effect and the disturbance rejection capability of the method provided by the disclosure, a typical underactuated autonomous underwater vehicle as shown in FIG. 3 is used as a test platform. The underwater autonomous vehicle moves in the depth plane only with the thrust provided by a propeller and the pitch control moment provided by an elevator. The number of control forces is less than the number of degrees of freedom, so an underactuated characteristic is provided. The platform is capable of providing depth and pitch angle feedback, and an elevator is provided at the stern that can be used to control the pitch angle, so that a hierarchical disturbance rejection depth tracking control system to verify the hierarchical disturbance rejection depth tracking control method is suitable to be formed. A closed-loop control code is written in the on-board computer of the autonomous underwater vehicle to implement the functions of autonomous collection of sensor data and control of the elevator. The code specifically implements the hierarchical disturbance rejection depth tracking control method of formulas (1) to (3) and (5) to (8).

(39) The desired depth first ramps to 2 meters and then remains at 2 meters for 60 seconds. The vehicle runs this method to track the desired depth to implement the submergence action and then conduct depth-fixed navigation. After the mission, the vehicle tracks the desired depth and floats autonomously. The real-prototype test results are shown in FIG. 4a to FIG. 4e.

(40) FIG. 4a shows the curve graph of depth of the vehicle, and the actual depth of the vehicle eventually converges to the desired depth of 2 meters without deviation. High-accuracy depth tracking performance is guaranteed by good pitch tracking and estimation and compensation for angle of attack by guidance law. The pitch tracking performance of the vehicle is shown in FIG. 4b, and it can be seen that the pitch angle of the vehicle can perfectly track the desired pitch angle. The performance is guaranteed by the observation and compensation of the composite disturbance by the extended state observer as well as the observation error compensation of the sliding mode feedback control law. After 50 seconds, the vehicle enters into a stabilized navigation stage, and the pitch angle is stabilized at −1.4°.

(41) FIG. 4b and FIG. 4c respectively show that the observed pitch angle z.sub.1 converges to the actual pitch angle θ and the observed angular velocity z.sub.2 converges to the actual angular velocity {dot over (θ)}. Therefore, it can be deduced that the observed composite disturbance z.sub.3 shown in FIG. 4d also converges to the true composite disturbance f(θ, {dot over (θ)}, U, I.sub.yy . . . )+d.sub.θ. It is precisely because the extended state observer can effectively estimate the composite disturbance and compensate the complex pitch motion model into an integral series type, that the actual pitch angle can unbiasedly track the desired pitch angle. Since an underwater autonomous vehicle is generally configured with slightly positive buoyancy, the vehicle needs a certain angle of attack to balance the remaining buoyancy after the depth is stable. If the line of sight guidance is used and the influence of the angle of attack is ignored, disturbances may be introduced in the kinematic layer, resulting in a steady-state error in depth. The adaptive line of sight guidance can estimate and compensate the angle of attack in real time, and the estimated angle of attack in real time is shown in FIG. 4e. With reference to FIG. 4e and FIG. 4a together, after 50 seconds, the vehicle enters the depth-stabilized navigating stage. In the depth-stabilized navigating stage, the depth of the vehicle remains constant, and the elevation angle is 0° at this time. The estimated angle of attack after 65 seconds is −1.4°, which is approximately equal to the pitch angle shown in FIG. 4b and is in line with the motion characteristic of the vehicle. It is thus verified that the adaptive line of sight guidance can effectively estimate the angle of attack.

(42) FIG. 5 is a structural diagram of a system for hierarchical disturbance rejection depth tracking control of an underactuated underwater vehicle according to an embodiment of the disclosure. As shown in FIG. 5, the system includes:

(43) The navigation information determining unit 510 is configured to determine navigation information of the underwater vehicle. The navigation information includes a desired depth, a current depth, a current navigation speed, and a current pitch angle.

(44) The desired pitch angle determining unit 520 is configured to determine a depth tracking error according to the desired depth and the current depth and convert the depth tracking error into a desired pitch angle based on an adaptive line of sight guidance law. The adaptive line of sight guidance law includes an adaptive parameter, so as to estimate a real angle of attack of the underwater vehicle and reduce a motion disturbance introduced by ignoring the angle of attack. The adaptive parameter is determined according to the current navigation speed and the depth tracking error.

(45) The pitch tracking control law determining unit 530 is configured to determine an active disturbance rejection sliding mode pitch tracking control law including an extended state observer and a sliding mode feedback control law. The extended state observer is configured to observe a composite disturbance of the underwater vehicle. The composite disturbance is a sum of dynamic disturbances introduced by an internal structure of the vehicle and an external environment. An observation error of the extended state observer is considered in a design process of the sliding mode feedback control law and is combined with the desired pitch angle to calculate a elevator angle of the underwater vehicle, so that the observation error is overcome.

(46) The elevator angle determining unit 540 is configured to determine an observed amount of the composite disturbance and the elevator angle calculated through the sliding mode feedback control law in combination with the active disturbance rejection sliding mode pitch tracking control law. Further, the elevator angle determining unit 540 calculates a final elevator angle based on the observed amount of the composite disturbance and the elevator angle calculated through the sliding mode feedback control law, so that the elevator navigates according to the final elevator angle, and that the underwater vehicle navigates stably according to the desired depth. The elevator angle determining unit 540 calculates the final elevator angle according to the adaptive line of sight guidance law and the active disturbance rejection sliding mode pitch tracking control law, so that the underwater vehicle is capable of providing improved rejection to motion disturbance and dynamic disturbance, and accuracy of depth tracking of the underwater vehicle is improved.

(47) It can be understood that the detailed functional implementation of the foregoing units in FIG. 5 may be found with reference to the introduction in the foregoing method embodiments, and description thereof is not repeated herein.

(48) To sum up, the above technical solutions provided by the disclosure have the following beneficial effects compared with the related art.

(49) The disclosure provides a method and system for hierarchical disturbance rejection depth tracking control of an underactuated underwater vehicle, and the depth tracking of the underactuated underwater vehicle is divided into kinematic layer guidance and dynamic layer pitch tracking. The adaptive line of sight guidance is used in the kinematic layer to convert the depth error into the desired pitch angle and to estimate and compensate the angle of attack to reject the disturbance introduced by the unmeasurable true angle of attack. Based on the above, in the dynamic layer, the active disturbance rejection sliding mode pitch tracking method is used to observe the composite disturbance including an unknown dynamic model and the environmental disturbance by using the active disturbance rejection framework. The model is compensated as a unified integral series type, the sliding mode control law is finally designed to resist the observation error, and the control elevator angle is calculated. The abovementioned control method is deployed on the on-board computer of the vehicle, so that the depth and pitch angle data fed back by the sensor is read, the elevator actuator is controlled, the system for hierarchical disturbance rejection depth tracking control is formed, and the robust depth tracking navigation of the vehicle is implemented.

(50) A person having ordinary skill in the art should be able to easily understand that the above description is only preferred embodiments of the disclosure and is not intended to limit the disclosure. Any modifications, equivalent replacements, and modifications made without departing from the spirit and principles of the disclosure should fall within the protection scope of the disclosure.