CN102722177A - Autonomous underwater vehicle (AUV) three-dimensional straight path tracking control method with PID (Piping and Instruments Diagram) feedback gain - Google Patents

Abstract

The invention provides an autonomous underwater vehicle (AUV) three-dimensional straight path tracking control method with PID (Piping and Instruments Diagram) feedback gain. The method comprises the following steps of: step 1, initializing; step 2, calculating a relative position deviation between an AUV and a virtual guide point on a desired track under a Serret-Frenet coordinate system through an AUV three-dimensional track tracking error equation; step 3, calculating a virtual controlling quantity of movement speed, a trim angle and a yawing angle of the virtual guide point on the desired track through adopting a backstepping method based on the feedback gain; step 4, calculating a virtual controlling quantity of trim angle speed and yawing angle speed of the AUV; and step 5, deducing an under-actuated AUV three-dimensional path tracking dynamics control law comprising control signals of propeller thrust, a trim control moment and a yawing control moment, and realizing under-actuated AUV three-dimensional straight path tracking control. The method can be used for realizing tracking control on a track point and a straight path track of an AUV three-dimensional space.

Description

Autonomous submarine navigation device 3 d-line path tracking control method with PID feedback gain

Technical field

What the present invention relates to is a kind of control method of autonomous submarine navigation device, specifically a kind of three-dimensional track points and straight line path tracking and controlling method of owing to drive autonomous submarine navigation device.

Background technology

Autonomous submarine navigation device (AUV, Autonomous Underwater Vehicle) three-dimensional path tracking Control has significant advantage with respect to the surface level of realizing decoupling zero respectively and degree of depth control.Owing to considered the coupling of model self; Can satisfy more seabed three dimensions landform/landforms mapping, submarine pipeline detection and follow the tracks of job requirements such as planning flight path; Because the topworks of autonomous submarine navigation device AUV is configured to afterbody axial advance device, aft rudders and afterbody elevating rudder usually; No direct driving mechanism (like thruster) on horizontal and vertical direction makes AUV satisfy second order nonholonomic constraint condition, for owing drive system; And owing to receive the uncertainty influence of extraneous ocean current interference effect and self model parameter, this all becomes the difficult point of the three-dimensional Track In Track design of Controller of AUV.

Three aspects 1 of AUV underwater 3 D space tracking Control research) track points is followed the tracks of, and requires AUV to follow the tracks of and converge to spatial discrete points; 2) track following, requiring AUV to follow the tracks of in the space one is the track of parameter with time, has the time conditions constraint, promptly requires AUV to move to assigned address constantly specifying; 3) path trace requires AUV to follow the tracks of the curve that has nothing to do with the time in space, makes AUV converge to given curve, and does not where require when arriving.Track points is also referred to as the discrete point of path point for representing through the underwater 3 D volume coordinate; The path then can be linked to each other by the point of series of discrete and constitute; The 3 d-line path is exactly to be linked to each other successively by the sequence of a series of three dimensions discrete points to constitute, and the space arbitrarily curved path also can carry out match through the multistage straight line path.At present; Flight path (comprising track points, track and path) tracking Control research to AUV both at home and abroad focuses mostly in the tracking Control of surface level; Or be that two independent subsystem of surface level and vertical plane are carried out design of Controller respectively with the decoupling zero of three-dimensional tracking Control problem; Because not whole coupled motions model and the three-dimensional tracking error equation of considering the six degree of freedom of AUV makes the design of Controller thinking of decoupling zero can't realize the accurate tracking control for given space flight path." based on self-adaptation Backstepping owe to drive the three-dimensional Track In Track control of AUV " (control and decision-making, 2012, the 38 the 2nd phases of volume) are three-dimensional track points tracking Control; According to line of sight method (Line ofSight;, LOS) calculation expectation is followed the tracks of the angle of sight, based on self-adaptation contragradience method design tracking control unit; Do not relate to the track points tracking is converted into parametrization path trace control problem; And the track homing strategy is that (Line of Sight LOS) and this patent adopts be virtual guide strategy (Virtual Guidance), converges on expected path through " virtual guide " some realization AUV that follows the tracks of on the expected path to line of sight method; " the neural network H that the Autonomous Underwater Vehicle three-dimensional path is followed the tracks of _∞The Robust Adaptive Control method " (control theory and application, 2012, the 29 volume the 3rd phases) set up AUV three-dimensional path tracking error equation based on rectangular projection Serret-Frenet coordinate system, utilization H _∞Robust control thought CONTROLLER DESIGN; Introduce the neural networks compensate model uncertainty simultaneously; But have the singular value point owing to set up AUV three-dimensional path tracking error model based on rectangular projection Serret-Frenet coordinate system, feasible starting condition to AUV has constraint, and promptly the AUV initial position must be positioned at the aircraft pursuit course minimum profile curvature radius; Therefore can't realize the global convergence that AUV follows the tracks of; And there is not singular value problem in the three-dimensional tracking error model that this patent is set up, therefore can guarantee the global convergence of AUV tracking error, and this patent design of Controller is different from H based on contragradience method (Backstepping) in addition _∞Robust Controller Design thought; " disturbing the three-dimensional Track In Track control of the underwater robot that suppresses " (control theory and application based on L2; 2011; The 28th the 5th phase of volume) proposes to disturb the three-dimensional Track In Track controller of the Robust Neural Network that suppresses based on L2; Its research contents is to be the Trajectory Tracking Control problem of parameter with time, and this patent is to parametrization path (not comprising time parameter) design tracking control unit; " based on discrete sliding mode prediction owe to drive the three-dimensional Track In Track control of AUV " (control and decision-making; 2011; The 26th the 10th phase of volume) adopts recurrence sliding formwork thought discrete sliding mode predictive control device to discrete system; Utilize rolling optimization and feedback compensation method to compensate the influence of indeterminate, and this patent design the three-dimensional path tracking control unit to continuous system based on the contragradience method to the sliding mode predictive model; " based on the nonlinear iteration sliding formwork owe to drive the three-dimensional Track In Track control of UUV " (robotization journal; 2012; The 38th the 2nd phase of volume) based on the thought design nonlinear iteration sliding formwork Track In Track controller of Engineering Control device decoupling zero; Because plant model is six degree of freedom coupled motions models, therefore to longitudinal velocity, bow to control and trim control respectively the decoupling controller of design can only suppress the coupling in the model through the robust item, when the coupling between each degree of freedom of model obviously the time; Controller can only be that cost is eliminated coupling through exporting higher controller gain; Cause controller output saturation signal, the controller of decoupling zero is merely able to guarantee the asymptotic stability of three independent RACSs, thereby the system that guarantees is stable; And can't obtain the asymptotic stability of The whole control system; And the three-dimensional Track In Track controller that this patent proposes adopts contragradience method iterative construction Lyapunov function, can guarantee the total system global asymptotic stability, and be different from the thought of the independent design subsystem tracking control unit of iteration sliding formwork; " 3D Path Following for Autonomous Underwater Vehicle " (Proceedings ofthe39th Conference on Decision and Control, 2000, Sydney; Australia) utilize the thought of rectangular projection to set up AUV three-dimensional path tracking error model, owing to there is a singular value point, the starting condition of AUV is had constraint; And there is not singular value problem in the three-dimensional tracking error model that this patent is set up; Therefore can guarantee the global convergence of AUV tracking error, based on traditional feedback linearization method, the controller architecture complex forms that obtains; Be unfavorable for practical applications; And this patent is based on feedback gain contragradience method, through the CONTROLLER DESIGN parameter predigesting form of virtual controlling amount, the final controller form that has obtained when having guaranteed tracker stability has the gain form of PID controller; And the parameter regulation rule can help practical applications with reference to PID controller parameter setting method.

Summary of the invention

The object of the present invention is to provide a kind of autonomous submarine navigation device 3 d-line path tracking control method that can realize the tracking Control of AUV three dimensions track points and rectilinear path with PID feedback gain.

The objective of the invention is to realize like this:

Step 1. initialization; Survey sensor (comprising ultra-short baseline fixed sonar, attitude sensor) image data through the AUV lift-launch; Can obtain AUV current position coordinates, position angle and along the linear velocity and the angular velocity data information of three of carrier coordinate system; The parametric equation that given desired track point is converted into the three-dimensional flight path of the continuous straight line of segmentation is described, and the position coordinates of initialization " virtual guide " point;

(it is coordinate origin that tracing deviation is illustrated in " virtual guide " point to step 2., rotates ψ around ζ axle and η respectively by fixed coordinate system through the relative position deviation of " virtual guide " point under the Serret-Frenet coordinate system on three-dimensional Track In Track error equation calculating AUV of AUV and the desired track _FAnd θ _FAngle, translation makes on fixed coordinate system initial point O and the path P point overlap to obtain, be called Fu Leinie-Sai Lei (Serret-Frenet) coordinate system then);

Step 3. adopts the contragradience method design philosophy based on feedback gain based on the tracking error that calculates in the step 2, the virtual controlling amount of translational speed, trim angle and the yaw angle of " virtual guide " point on the calculation expectation path;

Step 4. is calculated the virtual controlling amount of AUV pitch velocity and yaw angle speed on the basis of step 3;

Step 5. is based on given AUV six degree of freedom Mathematical Modeling; Derivation is owed to drive the dynamics Controlling rule that the three-dimensional path of autonomous submarine navigation device AUV follows the tracks of and is comprised propeller thrust, trim control moment and change bow control moment control signal, adopts this controller to realize owing to drive the 3 d-line Track In Track control of AUV then;

Step 6. is calculated current AUV position η ⁿ=(x, y is z) with the turning point WP that demarcates _k=(x _k, y _k, z _k) between distance

If switch radius R less than the flight path of setting, then current appointment is accomplished in expression

The tracing task of flight path stops navigation or switches next section desired track, otherwise continues step 2.

The relative prior art of the present invention has following advantage and effect:

1. the present invention is converted into the tracking Control to the 3 d-line flight path with the tracking of three-dimensional track points; And provided 3 d-line Track In Track error equation; Through design flight path switching law; Realization is to the tracking of the continuous flight path of segmentation, and the assurance controller can be realized the tracking Control to AUV three dimensions track points and rectilinear path.

2. based on the tracking control unit of the contragradience method of feedback gain design; Through the CONTROLLER DESIGN parameter; Simplified the form of virtual controlling amount; The complex form that causes the virtual controlling amount when having avoided adopting classical inverse footwork thought design tracking control unit, when having avoided based on line of sight method design virtual controlling amount simultaneously, control law is at ψ _eThere is the singular value point in=± pi/2, so the design of controller must guarantee that prerequisite is ψ _e∈ (pi/2, pi/2) makes the initial bow of AUV suffer restraints to error, and controller can't be realized the deficiency of the global convergence of tracking error system.

3. the controller of design has the gain form of similar PID controller, and linear segment is the linear combination of state variable and error variance, and parameter regulation meets the pid parameter rule of adjusting, and non-linear partial dynamically compensates model is known.

Description of drawings

The three-dimensional Track In Track synoptic diagram of the autonomous submarine navigation device of Fig. 1;

The three-dimensional Track In Track control system of Fig. 2 AUV structural drawing;

The three-dimensional Track In Track controller of Fig. 3 AUV resolves process flow diagram;

The three-dimensional Track In Track curve map of Fig. 4 AUV;

Fig. 5 AUV surface level Track In Track perspective view;

Fig. 6 AUV surface level Track In Track projection partial enlarged drawing;

Fig. 7 AUV vertical plane Track In Track perspective view;

Fig. 8 AUV vertical plane Track In Track projection partial enlarged drawing;

The three-dimensional Track In Track error curve diagram of Fig. 9 AUV;

Figure 10 AUV linear velocity response diagram;

Figure 11 AUV trim angle and bow are to the angle change curve;

Figure 12 AUV control input response curve.

Embodiment

For example the present invention is further described below:

In the step 1 according to given desired track point calculate planning rectilinear path parameter x ' _d, y ' _dAnd z ' _dDetailed process do

According to the adjacent expected path point WP that sets _i=(x _i, y _i, z _i) and WP _I+1=(x _I+1, y _I+1, z _I+1) coordinate information calculate the parameter x of this section expectation rectilinear path ' _d, ' _dAnd z ' _dConcrete form

$x_d^{\′}=\frac{{\mathrm{dx}}_d}{\mathrm{ds}}=x_{i+1}-x_i$

$y_d^{\′}=\frac{{\mathrm{dy}}_d}{\mathrm{ds}}=y_{i+1}-y_i---\left(1\right)$

$z_d^{\′}=\frac{{\mathrm{dz}}_d}{\mathrm{ds}}=z_{i+1}-z_i$

Therefore by a WP _i=(x _i, y _i, z _i) and WP _I+1=(x _I+1, y _I+1, z _I+1) the line parametrization equation that constitutes the path of 3 d-line can be expressed as

x _d(s)＝x′ _ds+x _i

y _d(s)＝y′ _ds+y _i (2)

z _d(s)＝z′ _ds+z _i

S is a path parameter, the speed u of defining virtual guide point P _pDirection is the angle ψ along the tangential direction of straight line path and fixed coordinate system transverse axis _FFor

ψ _F＝arctan(y′ _d/x′ _d) (3)

Velocity vector u _pAngle theta with the fixed coordinate system Z-axis _FBe defined as

${\mathrm{\θ}}_F={\tan }^{-1}\left(\frac{{-z}_d^{\′}}{\sqrt{{\left(x_d^{\′}\right)}^2+{\left(y_d^{\′}\right)}^2}}\right)---\left(4\right)$

Where

for the three-dimensional linear track in terms of the presence

{ initial position under the I} is η to given then AUV at fixed coordinate system ⁿ=[x, y, z] ^T, the initial bow of AUV is respectively ψ and θ to angle and trim angle, AUV longitudinal velocity u, transverse velocity v and vertical velocity w, yaw angle speed r and pitch velocity q.

So far accomplished the initialization setting in the step 1.

Calculate the three-dimensional Track In Track error of AUV x in the step 2 _e, y _eAnd z _eDetailed process following:

Definition desired track l _k{ position vector under the I} does at fixed coordinate system to go up virtual guide P

{ position vector under the I} is η to AUV current point Q at fixed coordinate system ⁿ=[x, y, z] ^T, ε=[x _e, y _e, z _e] ^TFor { tracking error under the F} coordinate system is so tracking error ε can be expressed as

$\mathrm{\ϵ}=R_F^T{\mathrm{\η}}_e^n---\left(5\right)$

Wherein

is that { I} is to the coordinate system { rotation matrix of F} for fixed coordinate system; To formula (5) differentiate, get the Track In Track error equation

$\stackrel{\·}{\mathrm{\ϵ}}={\stackrel{\·}{R}}_F^T{\mathrm{\η}}_e^n+R_F^T{\stackrel{\·}{\mathrm{\η}}}_e^n---\left(6\right)$

Because

wherein

$S\left({\mathrm{\&omega;}}_F\right)=\left[\begin{array}{ccc}0& -{\stackrel{\&CenterDot;}{\mathrm{\&psi;}}}_F& {\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_F\\ {\stackrel{\&CenterDot;}{\mathrm{\&psi;}}}_{\mathrm{<figure-callout\; id="0"\; label="F"\; filenames="2012102146615100003DEST\_PATH\_IMAGE002.png,2012102146615100003DEST\_PATH\_IMAGE003.png"\; state="\{\{state\}\}">F</figure-callout>}}& 0& 0\\ -{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_{\mathrm{<figure-callout\; id="0"\; label="F"\; filenames="2012102146615100003DEST\_PATH\_IMAGE002.png,2012102146615100003DEST\_PATH\_IMAGE003.png"\; state="\{\{state\}\}">F</figure-callout>}}& 0& 0\end{array}\right]---\left(7\right)$

For rectilinear path, there be

and

to set up, formula (7) substitution formula (6) is put in order

$\stackrel{\&CenterDot;}{\mathrm{\&epsiv;}}=R_F^T({\stackrel{\&CenterDot;}{\mathrm{\&eta;}}}^n-{\stackrel{\&CenterDot;}{\mathrm{\&eta;}}}_d^n)---\left(8\right)$

Wherein

v _b=[v _t, 0,0] ^TFor AUV resultant velocity vector, satisfy

{ B} is to the fixed coordinate system { rotation matrix of I} for the satellite coordinate system;

The relation that can be known desired track scalar parameter and " virtual guide " translational speed by differential geometric theory does

v _F=[u _p, 0,0] ^TFor the velocity vector of virtual guide under the F} coordinate system, substitution formula (8)

$\stackrel{\&CenterDot;}{\mathrm{\&epsiv;}}=R_F^T(R_b^n{v}_b-R_F{v}_F)$ (9)

$=R({\mathrm{\&psi;}}_e,{\mathrm{\&theta;}}_e)v_b-v_F$

Wherein

$R({\mathrm{\&psi;}}_e,{\mathrm{\&theta;}}_e)=\left[\begin{array}{ccc}\mathrm{Cos}{\mathrm{\&theta;}}_e\mathrm{Cos}{\mathrm{\&psi;}}_e& -\mathrm{Sin}{\mathrm{\&psi;}}_e& \mathrm{Sin}{\mathrm{\&theta;}}_e\mathrm{Cos}{\mathrm{\&psi;}}_e\\ \mathrm{Sin}{\mathrm{\&psi;}}_e\mathrm{Cos}{\mathrm{\&theta;}}_e& \mathrm{Cos}{\mathrm{\&psi;}}_e& \mathrm{Sin}{\mathrm{\&theta;}}_e\mathrm{Sin}{\mathrm{\&psi;}}_e\\ -\mathrm{Sin}{\mathrm{\&theta;}}_{\mathrm{<figure-callout\; id="0"\; label="e"\; filenames="2012102146615100003DEST\_PATH\_IMAGE002.png,2012102146615100003DEST\_PATH\_IMAGE003.png"\; state="\{\{state\}\}">e</figure-callout>}}& 0& \mathrm{Cos}{\mathrm{\&theta;}}_e\end{array}\right]$ Put in order to such an extent that AUV pursuit movement error model does

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{x}}_e=v_t\cos {\mathrm{\&psi;}}_e\cos {\mathrm{\&theta;}}_e-u_p\\ {\stackrel{\&CenterDot;}{y}}_e=v_t\sin {\mathrm{\&psi;}}_e\cos {\mathrm{\&theta;}}_e\\ {\stackrel{\&CenterDot;}{z}}_e=-v_t\sin {\mathrm{\&theta;}}_e\end{array}\right.---\left(10\right)$

Here

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{\mathrm{\&psi;}}}_e=\frac{r}{\cos \mathrm{\&theta;}}-{\stackrel{\&CenterDot;}{\mathrm{\&psi;}}}_F\\ {\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_e=q-{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_F\end{array}\right.---\left(11\right)$

ψ wherein _e=ψ-ψ _F, θ _e=θ-θ _F

Calculate translational speed, AUV trim angle tracing deviation and the bow of " virtual guide " virtual controlling amount respectively by following formula in the step 3 to the angle tracking deviation:

(1) design " virtual guide " translational speed

u _p＝v _t?cosψ _e?cosθ _e+k ₁x _e (12)

K wherein ₁＞0 is the design of Controller parameter.

(2) AUV trim angle tracing deviation virtual controlling amount of equal value

α _θ＝c ₂z _e (13)

C wherein ₂＞0 is the design of Controller parameter.

(3) AUV yaw angle tracing deviation virtual controlling amount of equal value

α _ψ＝-c ₁y _e (14)

C wherein ₁＞0 is the design of Controller parameter.

Design virtual controlling amount u based on feedback gain contragradience method in the step 3 _p, α _θAnd α _ψConcrete steps for choosing the Lyapunov energy function do

${\mathrm{<figure-callout\; id="1"\; label="V"\; filenames="2012102146615100003DEST\_PATH\_IMAGE008.png,HDA00001815847500012.png"\; state="\{\{state\}\}">V</figure-callout>}}_1=\frac{1}{2}{e}^2---\left(15\right)$

Wherein

formula (14) both sides differentiate is got by formula

${\stackrel{\&CenterDot;}{V}}_1={\stackrel{\&CenterDot;}{x}}_e{x}_e+{\stackrel{\&CenterDot;}{y}}_e{y}_e+{\stackrel{\&CenterDot;}{z}}_e{z}_e$ (16)

$=x_e(v_t\cos {\mathrm{\&psi;}}_e\cos {\mathrm{\&theta;}}_e-u_p)+y_e{v}_t\sin {\mathrm{\&psi;}}_e\cos {\mathrm{\&theta;}}_e-z_e{v}_t\sin {\mathrm{\&theta;}}_e$

The translational speed of design " virtual guide " point does

u _p＝k ₁x _e+v _t?cos _e?cosθ _e,k ₁＞0 (17)

Formula (16) becomes

${\stackrel{\&CenterDot;}{V}}_1=-k_1{\mathrm{<figure-callout\; id="2"\; label="x\; e"\; filenames="2012102146615100003DEST\_PATH\_IMAGE003.png,2012102146615100003DEST\_PATH\_IMAGE007.png"\; state="\{\{state\}\}">x</figure-callout>}}_e^{}+y_e{v}_t\sin {\mathrm{\&psi;}}_e\cos {\mathrm{\&theta;}}_e-z_e{v}_t\sin {\mathrm{\&theta;}}_e---\left(18\right)$

Formula (18) is rewritten as following form

${\stackrel{\&CenterDot;}{V}}_1={-k}_1{\mathrm{<figure-callout\; id="2"\; label="x\; e"\; filenames="2012102146615100003DEST\_PATH\_IMAGE003.png,2012102146615100003DEST\_PATH\_IMAGE007.png"\; state="\{\{state\}\}">x</figure-callout>}}_e^{}+y_e{v}_t[\frac{\sin {\mathrm{\&psi;}}_e}{{\mathrm{\&psi;}}_e}({\mathrm{\&psi;}}_e-{\mathrm{\&alpha;}}_{\mathrm{\&psi;}})+\frac{\sin {\mathrm{\&psi;}}_e}{{\mathrm{\&psi;}}_e}{\mathrm{\&alpha;}}_{\mathrm{\&psi;}}]\cos {\mathrm{\&theta;}}_e$ (19)

$-z_e{v}_t[\frac{\sin {\mathrm{\&theta;}}_e}{{\mathrm{\&theta;}}_e}({\mathrm{\&theta;}}_e-{\mathrm{\&alpha;}}_{\mathrm{\&theta;}})+\frac{\sin {\mathrm{\&theta;}}_e}{{\mathrm{\&theta;}}_e}{\mathrm{\&alpha;}}_{\mathrm{\&theta;}}]$

Designing the virtual controlling amount respectively does

α _ψ＝-c ₁y _e,c ₁＞0 (20)

α _θ＝c ₂z _e,c ₂＞0 (21)

Formula (20) and (21) substitution formula (19) are got

${\stackrel{\&CenterDot;}{V}}_1=-k_1{x}_e^2-c_1{v}_t\frac{\sin {\mathrm{\&psi;}}_e}{{\mathrm{\&psi;}}_e}\cos {\mathrm{\&theta;}}_e{y}_e^2-c_2{v}_t\frac{\sin {\mathrm{\&theta;}}_e}{{\mathrm{\&theta;}}_e}{z}_e^2$ (22)

$+v_t\frac{\sin {\mathrm{\&psi;}}_e}{{\mathrm{\&psi;}}_e}\cos {\mathrm{\&theta;}}_e{z}_1{y}_e-v_t\frac{\sin {\mathrm{\&theta;}}_e}{{\mathrm{\&theta;}}_e}{z}_2{z}_e$

Z wherein ₁=ψ _e-α _ψ, z ₂=θ _e-α _θBecause

The limit exists, and for

Condition is set up, so satisfy

Condition is set up; Because

The limit exists, simultaneously

And

So satisfy Condition is set up.

Calculate AUV pitch velocity and yaw angle speed virtual controlling amount respectively according to following formula in the step 4:

(1) AUV yaw angle speed virtual controlling amount

α _r＝-c ₃z ₁ (23)

C wherein ₃＞0 is design of Controller parameter z ₁=ψ _e-α _ψ

(2) AUV pitch velocity virtual controlling amount

α _q＝-c ₄z ₂ (24)

C wherein ₄＞0 is the design of Controller parameter, z ₂=θ _e-α _θ

Design AUV pitch velocity and yaw angle speed virtual controlling amount α in the step 4 _qAnd α _rThe following convolution of step (15) structure Lyapunov function do

${\mathrm{<figure-callout\; id="2"\; label="V"\; filenames="2012102146615100003DEST\_PATH\_IMAGE003.png,2012102146615100003DEST\_PATH\_IMAGE007.png"\; state="\{\{state\}\}">V</figure-callout>}}_2={\mathrm{<figure-callout\; id="1"\; label="V"\; filenames="2012102146615100003DEST\_PATH\_IMAGE008.png,HDA00001815847500012.png"\; state="\{\{state\}\}">V</figure-callout>}}_1+\frac{1}{2}{p}_1{\mathrm{<figure-callout\; id="1"\; label="z"\; filenames="2012102146615100003DEST\_PATH\_IMAGE008.png,HDA00001815847500012.png"\; state="\{\{state\}\}">z</figure-callout>}}_1^2+\frac{1}{2}{p}_2{\mathrm{<figure-callout\; id="2"\; label="z"\; filenames="2012102146615100003DEST\_PATH\_IMAGE003.png,2012102146615100003DEST\_PATH\_IMAGE007.png"\; state="\{\{state\}\}">z</figure-callout>}}_2^2---\left(25\right)$

P wherein ₁And p ₂For the design of Controller parameter, satisfy p ₁＞0, p ₂＞0; To formula (25) both sides differentiate, formula (22) substitution arrangement is obtained

${\stackrel{\&CenterDot;}{V}}_2={\stackrel{\&CenterDot;}{V}}_1+p_1{z}_1{\stackrel{\&CenterDot;}{z}}_1+p_2{z}_2{\stackrel{\&CenterDot;}{z}}_2$

$=-k_1{x}_e^2-k_2{y}_e^2-k_3{\mathrm{<figure-callout\; id="2"\; label="z\; e"\; filenames="2012102146615100003DEST\_PATH\_IMAGE003.png,2012102146615100003DEST\_PATH\_IMAGE007.png"\; state="\{\{state\}\}">z</figure-callout>}}_e^{}+v_t\frac{\sin {\mathrm{\&psi;}}_e}{{\mathrm{\&psi;}}_e}\cos {\mathrm{\&theta;}}_e{z}_1{y}_e-v_t\frac{\sin {\mathrm{\&theta;}}_e}{{\mathrm{\&theta;}}_e}{z}_2{z}_e+p_1{z}_1{\stackrel{\&CenterDot;}{z}}_1+p_2{z}_2{\stackrel{\&CenterDot;}{z}}_2---\left(26\right)$

Defining variable wherein

Satisfy k ₂＞0, k ₃＞0 condition is set up.

Formula (26) is put in order

${\stackrel{\&CenterDot;}{V}}_2=-k_1{x}_e^2-k_2{y}_e^2-k_3{\mathrm{<figure-callout\; id="2"\; label="z\; e"\; filenames="2012102146615100003DEST\_PATH\_IMAGE003.png,2012102146615100003DEST\_PATH\_IMAGE007.png"\; state="\{\{state\}\}">z</figure-callout>}}_e^{}+p_1{z}_1({\stackrel{\&CenterDot;}{z}}_1+\frac{{\mathrm{<figure-callout\; id="1"\; label="v\; t\; p"\; filenames="2012102146615100003DEST\_PATH\_IMAGE008.png,HDA00001815847500012.png"\; state="\{\{state\}\}">v</figure-callout>}}_t}{p_{}}\frac{\sin {\mathrm{\&psi;}}_e}{{\mathrm{\&psi;}}_e}\cos {\mathrm{\&theta;}}_e{y}_e)+p_2{z}_2({\stackrel{\&CenterDot;}{z}}_2-\frac{{\mathrm{<figure-callout\; id="2"\; label="v\; t\; p"\; filenames="2012102146615100003DEST\_PATH\_IMAGE003.png,2012102146615100003DEST\_PATH\_IMAGE007.png"\; state="\{\{state\}\}">v</figure-callout>}}_t}{p_{}}\frac{\sin {\mathrm{\&theta;}}_e}{{\mathrm{\&theta;}}_e}{z}_e)---\left(27\right)$

Wherein

${\stackrel{\&CenterDot;}{z}}_1={\stackrel{\&CenterDot;}{\mathrm{\&psi;}}}_e-{\stackrel{\&CenterDot;}{\mathrm{\&alpha;}}}_{\mathrm{\&psi;}}={\stackrel{\&CenterDot;}{\mathrm{\&psi;}}}_e+c_1{\stackrel{\&CenterDot;}{y}}_e---\left(28\right)$

${\stackrel{\&CenterDot;}{z}}_2={\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_e-{\stackrel{\&CenterDot;}{\mathrm{\&alpha;}}}_{\mathrm{\&theta;}}={\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_e-c_2{\stackrel{\&CenterDot;}{z}}_e---\left(29\right)$

Obtained by formula (10)～(11) and (28)～(29), formula (27) becomes

${\stackrel{\&CenterDot;}{V}}_2=-k_1{x}_e^2-k_2{y}_e^2-k_3{\mathrm{<figure-callout\; id="2"\; label="z\; e"\; filenames="2012102146615100003DEST\_PATH\_IMAGE003.png,2012102146615100003DEST\_PATH\_IMAGE007.png"\; state="\{\{state\}\}">z</figure-callout>}}_e^{}+$

$p_1{z}_1(r/\cos \mathrm{\&theta;}+c_1{v}_t\sin {\mathrm{\&psi;}}_e\cos {\mathrm{\&theta;}}_e+\frac{{\mathrm{<figure-callout\; id="1"\; label="v\; t\; p"\; filenames="2012102146615100003DEST\_PATH\_IMAGE008.png,HDA00001815847500012.png"\; state="\{\{state\}\}">v</figure-callout>}}_t}{p_{}}\frac{\sin {\mathrm{\&psi;}}_e}{{\mathrm{\&psi;}}_e}\cos {\mathrm{\&theta;}}_e{y}_e)---\left(30\right)$

$+p_2{z}_2(q+c_2{v}_t\sin {\mathrm{\&theta;}}_e-\frac{{\mathrm{<figure-callout\; id="2"\; label="v\; t\; p"\; filenames="2012102146615100003DEST\_PATH\_IMAGE003.png,2012102146615100003DEST\_PATH\_IMAGE007.png"\; state="\{\{state\}\}">v</figure-callout>}}_t}{p_{}}\frac{\sin {\mathrm{\&theta;}}_e}{{\mathrm{\&theta;}}_e}{z}_e)$

Following formula is rewritten as

${\stackrel{\&CenterDot;}{V}}_2=-k_1{x}_e^2-k_2{y}_e^2-k_3{\mathrm{<figure-callout\; id="2"\; label="z\; e"\; filenames="2012102146615100003DEST\_PATH\_IMAGE003.png,2012102146615100003DEST\_PATH\_IMAGE007.png"\; state="\{\{state\}\}">z</figure-callout>}}_e^{}+$

$p_1{z}_1[r/\cos \mathrm{\&theta;}+c_1{v}_t\frac{\sin {\mathrm{\&psi;}}_e}{{\mathrm{\&psi;}}_e}({\mathrm{<figure-callout\; id="1"\; label="z"\; filenames="2012102146615100003DEST\_PATH\_IMAGE008.png,HDA00001815847500012.png"\; state="\{\{state\}\}">z</figure-callout>}}_1+{\mathrm{\&alpha;}}_{\mathrm{\&psi;}})\cos {\mathrm{\&theta;}}_e+\frac{{\mathrm{<figure-callout\; id="1"\; label="v\; t\; p"\; filenames="2012102146615100003DEST\_PATH\_IMAGE008.png,HDA00001815847500012.png"\; state="\{\{state\}\}">v</figure-callout>}}_t}{p_{}}\frac{\sin {\mathrm{\&psi;}}_e}{{\mathrm{\&psi;}}_e}\cos {\mathrm{\&theta;}}_e{y}_e]---\left(31\right)$

$+p_2{z}_2[q+c_2{v}_t\frac{\sin {\mathrm{\&theta;}}_e}{{\mathrm{\&theta;}}_e}({\mathrm{<figure-callout\; id="2"\; label="z"\; filenames="2012102146615100003DEST\_PATH\_IMAGE003.png,2012102146615100003DEST\_PATH\_IMAGE007.png"\; state="\{\{state\}\}">z</figure-callout>}}_2+{\mathrm{\&alpha;}}_{\mathrm{\&theta;}})-\frac{{\mathrm{<figure-callout\; id="2"\; label="v\; t\; p"\; filenames="2012102146615100003DEST\_PATH\_IMAGE003.png,2012102146615100003DEST\_PATH\_IMAGE007.png"\; state="\{\{state\}\}">v</figure-callout>}}_t}{p_{}}\frac{\sin {\mathrm{\&theta;}}_e}{{\mathrm{\&theta;}}_e}{z}_e]$

Owing in step 2, designed virtual controlling amount α _ψAnd α _θSuc as formula (20) and (21), so formula (30) can be changed into

${\stackrel{\&CenterDot;}{V}}_2=-k_1{x}_e^2-k_2{y}_e^2-k_3{\mathrm{<figure-callout\; id="2"\; label="z\; e"\; filenames="2012102146615100003DEST\_PATH\_IMAGE003.png,2012102146615100003DEST\_PATH\_IMAGE007.png"\; state="\{\{state\}\}">z</figure-callout>}}_e^{}+$

$p_1{z}_1(r/\cos \mathrm{\&theta;}+c_1{v}_t\frac{\sin {\mathrm{\&psi;}}_e}{{\mathrm{\&psi;}}_e}\cos {\mathrm{\&theta;}}_{\mathrm{<figure-callout\; id="1"\; label="e\; z"\; filenames="2012102146615100003DEST\_PATH\_IMAGE008.png,HDA00001815847500012.png"\; state="\{\{state\}\}">e</figure-callout>}}{z}_{}{}_1+(\frac{1}{p_1}-{\mathrm{<figure-callout\; id="1"\; label="c"\; filenames="2012102146615100003DEST\_PATH\_IMAGE008.png,HDA00001815847500012.png"\; state="\{\{state\}\}">c</figure-callout>}}_1^2)v_t\frac{\sin {\mathrm{\&psi;}}_e}{{\mathrm{\&psi;}}_e}\cos {\mathrm{\&theta;}}_e{y}_e)---\left(32\right)$

$+p_2{z}_2(q+c_2{v}_t\frac{\sin {\mathrm{\&theta;}}_e}{{\mathrm{\&theta;}}_e}{z}_2-(\frac{1}{p_2}-{\mathrm{<figure-callout\; id="2"\; label="c"\; filenames="2012102146615100003DEST\_PATH\_IMAGE003.png,2012102146615100003DEST\_PATH\_IMAGE007.png"\; state="\{\{state\}\}">c</figure-callout>}}_2^2)v_t\frac{\sin {\mathrm{\&theta;}}_e}{{\mathrm{\&theta;}}_e}{z}_e)$

Design controller parameters

and

eliminate the nonlinear coupling term to get

${\stackrel{\&CenterDot;}{V}}_2={-k}_1{x}_e^2-k_2{y}_e^2-k_3{\mathrm{<figure-callout\; id="2"\; label="z\; e"\; filenames="2012102146615100003DEST\_PATH\_IMAGE003.png,2012102146615100003DEST\_PATH\_IMAGE007.png"\; state="\{\{state\}\}">z</figure-callout>}}_e^{}+p_1{z}_1(r/\cos \mathrm{\&theta;}+c_1{v}_t\frac{\sin {\mathrm{\&psi;}}_e}{{\mathrm{\&psi;}}_e}\cos {\mathrm{\&theta;}}_e{z}_1)$ (33)

$+p_2{z}_2(q+c_2{v}_t\frac{\sin {\mathrm{\&theta;}}_e}{{\mathrm{\&theta;}}_e}{z}_2)$

Here design virtual controlling amount α _qAnd α _rBe respectively:

α _r＝-c ₃z ₁ (34)

α _q＝-c ₄z ₂ (35)

C wherein ₃And c ₄Be the design of Controller parameter

Then formula (33) becomes

${\stackrel{\&CenterDot;}{V}}_2=-k_1{x}_e^2-k_2{y}_e^2-k_3{z}_e^2-c_3{p}_1(1-\frac{c_1{v}_t}{c_3}\frac{\sin {\mathrm{\&psi;}}_e}{{\mathrm{\&psi;}}_e}\cos {\mathrm{\&theta;}}_e){\mathrm{<figure-callout\; id="1"\; label="z"\; filenames="2012102146615100003DEST\_PATH\_IMAGE008.png,HDA00001815847500012.png"\; state="\{\{state\}\}">z</figure-callout>}}_1^2$ (36)

$-c_4{p}_2(1-\frac{c_2{v}_t}{c_4}\frac{\sin {\mathrm{\&theta;}}_e}{{\mathrm{\&theta;}}_e}){\mathrm{<figure-callout\; id="2"\; label="z"\; filenames="2012102146615100003DEST\_PATH\_IMAGE003.png,2012102146615100003DEST\_PATH\_IMAGE007.png"\; state="\{\{state\}\}">z</figure-callout>}}_2^2+p_1{z}_1{z}_3+p_2{z}_2{z}_4$

Z wherein ₃=r/cos θ-α _r, z ₄=q-α _q, choose parameter c ₃Satisfy c ₃＞c ₁v _tCondition is set up; Choose parameter c ₄Satisfy c ₄＞c ₂v _tCondition is set up.

In the step 5, it is following to resolve AUV actuating mechanism controls command signal form:

(1) AUV propeller thrust control input signals

$F_u=m_1({\stackrel{\&CenterDot;}{u}}_d-k_u{u}_e)-f_u---\left(37\right)$

(2) AUV Trimming Moment control input signals

τ _q＝γ ₁z _e-γ ₂θ _e-γ _３q-c ₂c ₄v _t?sinθ _e-f _q?(38)

$\left\{\begin{array}{c}{\mathrm{\&gamma;}}_1=m_4{c}_2(\frac{p_2}{p_4}+c_4{c}_6)\\ {\mathrm{\&gamma;}}_2=m_4(\frac{p_2}{p_4}+c_4{c}_6)\\ {\mathrm{\&gamma;}}_3=m_4(c_4+c_6)\end{array}\right.---\left(39\right)$

(3) AUV changes bow Torque Control input signal

τ _r＝-λ ₁y _e-λ ₂ψ _e-λ ₃r-m ₅(c ₁c ₃v _t?cosθ?sinψ _e?cosθ _e+qr?tanθ)-f _r (40)

$\left\{\begin{array}{c}{\mathrm{\&lambda;}}_1=c_1{m}_5(\frac{p_1}{p_3}+c_3{c}_5)\cos \mathrm{\&theta;}\\ {\mathrm{\&lambda;}}_2=m_5(\frac{p_1}{p_3}+c_3{c}_5)\cos \mathrm{\&theta;}\\ {\mathrm{\&lambda;}}_3=m_5(c_3+c_5)\end{array}\right.---\left(41\right)$

Wherein

$\left\{\begin{array}{c}{f}_u={\mathrm{<figure-callout\; id="2"\; label="m"\; filenames="2012102146615100003DEST\_PATH\_IMAGE003.png,2012102146615100003DEST\_PATH\_IMAGE007.png"\; state="\{\{state\}\}">m</figure-callout>}}_2\mathrm{vr}-m_3\mathrm{wq}+d_{1}u\\ f_q=(m_1-m_3)\mathrm{uw}+d_{4}q-g_2\\ f_r=(m_1-m_2)\mathrm{uv}+d_{5}r\end{array}\right.---\left(42\right)$

The concrete steps that obtain AUV three-dimensional path tracking control unit in the step 5 do

According to AUV actual measurement hydrodynamic force coefficient, ignore the influence of rolling motion to model, it is following to obtain AUV five degree of freedom mathematical model

$\stackrel{\&CenterDot;}{u}=\frac{m_2}{{\mathrm{<figure-callout\; id="1"\; label="m"\; filenames="2012102146615100003DEST\_PATH\_IMAGE008.png,HDA00001815847500012.png"\; state="\{\{state\}\}">m</figure-callout>}}_1}\mathrm{vr}-\frac{m_3}{{\mathrm{<figure-callout\; id="1"\; label="m"\; filenames="2012102146615100003DEST\_PATH\_IMAGE008.png,HDA00001815847500012.png"\; state="\{\{state\}\}">m</figure-callout>}}_1}\mathrm{wq}+\frac{d_1}{m_1}u+\frac{1}{m_1}{F}_u$

$\stackrel{\&CenterDot;}{v}=-\frac{m_1}{{\mathrm{<figure-callout\; id="2"\; label="m"\; filenames="2012102146615100003DEST\_PATH\_IMAGE003.png,2012102146615100003DEST\_PATH\_IMAGE007.png"\; state="\{\{state\}\}">m</figure-callout>}}_2}\mathrm{ur}+\frac{d_2}{m_2}v$

$\stackrel{\&CenterDot;}{w}=\frac{m_1}{m_3}\mathrm{uq}+\frac{d_3}{m_3}w+g_1---\left(43\right)$

$\stackrel{\&CenterDot;}{q}=\frac{m_1-m_3}{m_4}\mathrm{uw}+\frac{d_4}{m_4}q-{\mathrm{<figure-callout\; id="2"\; label="g"\; filenames="2012102146615100003DEST\_PATH\_IMAGE003.png,2012102146615100003DEST\_PATH\_IMAGE007.png"\; state="\{\{state\}\}">g</figure-callout>}}_2+\frac{1}{m_4}{\mathrm{\&tau;}}_q$

$\stackrel{\&CenterDot;}{r}=\frac{m_1-m_2}{{\mathrm{<figure-callout\; id="5"\; label="m"\; filenames="2012102146615100003DEST\_PATH\_IMAGE006.png"\; state="\{\{state\}\}">m</figure-callout>}}_5}\mathrm{uv}+\frac{d_5}{m_5}r+\frac{1}{{\mathrm{<figure-callout\; id="5"\; label="m"\; filenames="2012102146615100003DEST\_PATH\_IMAGE006.png"\; state="\{\{state\}\}">m</figure-callout>}}_5}{\mathrm{\&tau;}}_r$

Wherein

${\mathrm{<figure-callout\; id="1"\; label="m"\; filenames="2012102146615100003DEST\_PATH\_IMAGE008.png,HDA00001815847500012.png"\; state="\{\{state\}\}">m</figure-callout>}}_1=m-X_{\stackrel{\&CenterDot;}{u}},{\mathrm{<figure-callout\; id="2"\; label="m"\; filenames="2012102146615100003DEST\_PATH\_IMAGE003.png,2012102146615100003DEST\_PATH\_IMAGE007.png"\; state="\{\{state\}\}">m</figure-callout>}}_2=m-Y_{\stackrel{\&CenterDot;}{v}},m_3=m-Z_{\stackrel{\&CenterDot;}{w}}$

$m_4=I_y-M_{\stackrel{\&CenterDot;}{q}},{\mathrm{<figure-callout\; id="5"\; label="m"\; filenames="2012102146615100003DEST\_PATH\_IMAGE006.png"\; state="\{\{state\}\}">m</figure-callout>}}_5=I_z-N_{\stackrel{\&CenterDot;}{r}}$

g ₁＝(W-B)cosθ，g ₂＝(z _gW-z _bB)sinθ

d ₁＝X _u+X _u|u||u|，d ₂＝Y _v+Y _v|v||v｜(44)

d ₃＝Z _w+Z _w|w||w|,d ₄＝M _q+M _q|q||q｜

d ₅＝N _r+N _r|r||r|

Wherein, state variable u, v, w, q and r represent carrier coordinate system { longitudinal velocity of AUV, transverse velocity, vertical velocity, pitch velocity and yaw angle speed under the B} respectively; M and m () represent AUV quality and the additional mass that is produced by the fluid effect, I respectively _yBe the moment of inertia of AUV around the y axle, I _zBe the moment of inertia of AUV around the z axle, X ₍₎, Y ₍₎, Z ₍₎, M ₍₎And N ₍₎Be the viscous fluid hydrodynamic force coefficient; z _gAnd z _bBe respectively under the carrier coordinate coordinate position of center of gravity and centre of buoyancy on the Z-axis, W and B represent gravity and the buoyancy that AUV receives, d respectively ₍₎Be nonlinear damping hydrodynamic force item, control input F _u, τ _qAnd τ _rRepresent AUV propeller thrust, trim control moment respectively and change the bow control moment.

Convolution (25) structure Lyapunov function does

$V_3={\mathrm{<figure-callout\; id="2"\; label="V"\; filenames="2012102146615100003DEST\_PATH\_IMAGE003.png,2012102146615100003DEST\_PATH\_IMAGE007.png"\; state="\{\{state\}\}">V</figure-callout>}}_2+\frac{1}{2}{p}_3{z}_3^2+\frac{1}{2}{p}_4{z}_4^2+\frac{1}{2}{u}_e^2---\left(45\right)$

To the differentiate of following formula both sides, formula (36) substitution put in order

${\stackrel{\&CenterDot;}{V}}_3=-k_1{x}_e^2-k_2{y}_e^2-k_3{z}_e^2-k_4{\mathrm{<figure-callout\; id="1"\; label="z"\; filenames="2012102146615100003DEST\_PATH\_IMAGE008.png,HDA00001815847500012.png"\; state="\{\{state\}\}">z</figure-callout>}}_1^2-k_5{\mathrm{<figure-callout\; id="2"\; label="z"\; filenames="2012102146615100003DEST\_PATH\_IMAGE003.png,2012102146615100003DEST\_PATH\_IMAGE007.png"\; state="\{\{state\}\}">z</figure-callout>}}_2^2$

$+p_3{z}_3({\stackrel{\&CenterDot;}{z}}_3+\frac{p_1{z}_1}{p_3})+p_4{z}_4({\stackrel{\&CenterDot;}{z}}_4+\frac{p_2{z}_2}{p_4})+u_e(\stackrel{\&CenterDot;}{u}-{\stackrel{\&CenterDot;}{u}}_d)---\left(46\right)$

Variable k wherein ₄And k ₅Be defined as $k_4=c_3{p}_1(1-\frac{c_1{v}_t}{c_3}\frac{\mathrm{Sin}{\mathrm{\&psi;}}_e}{{\mathrm{\&psi;}}_e}\mathrm{Cos}{\mathrm{\&theta;}}_e),$ ${\mathrm{<figure-callout\; id="5"\; label="k"\; filenames="2012102146615100003DEST\_PATH\_IMAGE006.png"\; state="\{\{state\}\}">k</figure-callout>}}_5=c_4{p}_2(1-\frac{c_2{v}_t}{c_4}\frac{{\mathrm{Sin}\mathrm{\&theta;}}_e}{{\mathrm{\&theta;}}_e}),$ And it is full

Foot k ₄＞0 and k ₅＞0 condition is set up.

Wherein

${\stackrel{\&CenterDot;}{z}}_3=\frac{\stackrel{\&CenterDot;}{r}\cos \mathrm{\&theta;}+\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\sin \mathrm{\&theta;<figure-callout\; id="2"\; label="r\; cos"\; filenames="2012102146615100003DEST\_PATH\_IMAGE003.png,2012102146615100003DEST\_PATH\_IMAGE007.png"\; state="\{\{state\}\}">r</figure-callout>}}{{\cos }^{}}-{\stackrel{\&CenterDot;}{\mathrm{\&alpha;}}}_r---\left(47\right)$

${\stackrel{\&CenterDot;}{z}}_4=\stackrel{\&CenterDot;}{q}-{\stackrel{\&CenterDot;}{\mathrm{\&alpha;}}}_q---\left(48\right)$

Got by formula (47) and (48), formula (46) becomes

${\stackrel{\&CenterDot;}{V}}_3={-k}_e^2-k_2{y}_e^2-k_3{z}_e^2-k_4{\mathrm{<figure-callout\; id="1"\; label="z"\; filenames="2012102146615100003DEST\_PATH\_IMAGE008.png,HDA00001815847500012.png"\; state="\{\{state\}\}">z</figure-callout>}}_1^2-k_5{\mathrm{<figure-callout\; id="2"\; label="z"\; filenames="2012102146615100003DEST\_PATH\_IMAGE003.png,2012102146615100003DEST\_PATH\_IMAGE007.png"\; state="\{\{state\}\}">z</figure-callout>}}_2^2+p_3{z}_3(\frac{\stackrel{\&CenterDot;}{r}\cos \mathrm{\&theta;}+\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\sin \mathrm{\&theta;<figure-callout\; id="2"\; label="r\; cos"\; filenames="2012102146615100003DEST\_PATH\_IMAGE003.png,2012102146615100003DEST\_PATH\_IMAGE007.png"\; state="\{\{state\}\}">r</figure-callout>}}{{\cos }^{}}-{\stackrel{\&CenterDot;}{\mathrm{\&alpha;}}}_r+\frac{p_1{z}_1}{p_3})$ (49)

$+p_4{z}_4(\stackrel{\&CenterDot;}{q}-{\stackrel{\&CenterDot;}{\mathrm{\&alpha;}}}_q+\frac{p_2{z}_2}{p_4})+u_e(\stackrel{\&CenterDot;}{u}-{\stackrel{\&CenterDot;}{u}}_d)$

Here convolution (44) design AUV three-dimensional path is followed the tracks of Dynamics Controller and is done

$\left\{\begin{array}{c}\\ \stackrel{\&CenterDot;}{u}={\stackrel{\&CenterDot;}{u}}_d-k_u(u-u_d)\\ \stackrel{\&CenterDot;}{q}={\stackrel{\&CenterDot;}{\mathrm{\&alpha;}}}_q-\frac{p_2{z}_2}{p_4}-c_6{z}_4\\ \stackrel{}{r}=\cos \mathrm{\&theta;}({\stackrel{\&CenterDot;}{\mathrm{\&alpha;}}}_r-\frac{p_1{z}_1}{p_3}-c_5{z}_3-\frac{\mathrm{qr}\sin \mathrm{\&theta;}}{{\mathrm{<figure-callout\; id="2"\; label="cos"\; filenames="2012102146615100003DEST\_PATH\_IMAGE003.png,2012102146615100003DEST\_PATH\_IMAGE007.png"\; state="\{\{state\}\}">cos</figure-callout>}}^2\mathrm{\&theta;}})\end{array}\right.---\left(50\right)$

Formula (50) substitution formula (49) can be got

${\stackrel{\&CenterDot;}{V}}_3=-k_1{x}_e^2-k_2{y}_e^2-k_3{z}_e^2-k_4{\mathrm{<figure-callout\; id="1"\; label="z"\; filenames="2012102146615100003DEST\_PATH\_IMAGE008.png,HDA00001815847500012.png"\; state="\{\{state\}\}">z</figure-callout>}}_1^2-k_2{\mathrm{<figure-callout\; id="2"\; label="z"\; filenames="2012102146615100003DEST\_PATH\_IMAGE003.png,2012102146615100003DEST\_PATH\_IMAGE007.png"\; state="\{\{state\}\}">z</figure-callout>}}_2^2-c_5{p}_3{z}_3^2-c_6{p}_4{z}_4^2-k_u{u}_e^2$ (51)

$\&le;0$

To sum up can work as the design of Controller parameter and satisfy c ₁＞0, c ₂＞c ₁v _t, c ₃＞0, c ₄＞c ₃v _t, p ₃＞0, p ₄＞0, c ₅＞0, c ₆Under＞0 prerequisite, and if only if (u _e, x _e, y _e, z _e, z ₁, z ₂, z ₃, z ₄)=0 o'clock,

Can be got by the LaSalle invariance principle, closed loop tracking error system is asymptotic stable.

Emulation experiment checking and analysis:

Illustrate below; Invent the validity of the three-dimensional Track In Track controller of AUV of design for checking; Emulation experiment is carried out in three-dimensional curve path to planning; And compare analysis with the traditional PID control simulation result: said according to step 1 in the summary of the invention, at first provide the position coordinates of desired track point

WP ₁＝(80,0,0)，WP ₂＝(120,100,20)，WP ₃＝(0,180,40)，WP ₄＝(-120,100,60)，

WP ₅＝(-80,0,80)

Provide AUV initial position, attitude initial value

x ₀=90, y ₀=-10, z ₀=0, θ ₀=0,

u ₀=0, v ₀=0, w ₀=0, q ₀=0, r ₀=0 provides the design of Controller parameter

c ₁＝0.18，c ₂＝0.5，c ₃＝0.2，c ₄＝0.5，

p ₃＝1000，p ₄＝1000，c ₅＝10，c ₆＝10，k _u＝0.1，k ₁＝1

Carry out controller according to summary of the invention step 2 of the present invention～6 then and resolve, flow process is as shown in Figure 3, and obtains simulation result.

Simulation analysis

Fig. 4～Figure 12 is the three-dimensional Track In Track simulation curve of AUV.For verifying the performance of the controller that the present invention designs; Compare with the simulation curve of PID controller the three-dimensional track points tracking Control of AUV; Fig. 4 is the three-dimensional Track In Track control of an AUV curve; Fig. 5 and Fig. 7 are respectively that the three-dimensional Track In Track curve corresponding with Fig. 3 schemed in the drop shadow curve of surface level and vertical plane, when perspective view partial enlarged drawing 6 can be found out the traditional PID controller controller to changing operate-point with Fig. 8, cause control performance to reduce, and can't realize the quick tracking to three-dimensional flight path; Controller based on the design of the contragradience method of feedback gain has robustness preferably to environmental disturbances; Improved the precision of Track In Track, shortened the redundant flight path of AUV, made itself and desired track more approaching; Fig. 9 is a tracking error curve in the three-dimensional Track In Track control of AUV, and the controller of this patent design has guaranteed higher tracking accuracy and response speed; Figure 10 and Figure 11 are respectively the change curve that each state variable in the three-dimensional Track In Track control procedure of AUV comprises linear velocity and attitude angle; Trim angle under the PID control action and bow have certain overshoot vibration to the angle variation; The adjusting time is longer; The control effect is relatively poor, causes system's unstability easily, and the controller that this paper proposes has more stable control ability to attitude angle; Figure 12 is the three-dimensional Track In Track control of AUV input response.

Claims (6)

1. autonomous submarine navigation device 3 d-line path tracking control method with PID feedback gain is characterized in that:

Step 1. initialization; Survey sensor image data through the AUV lift-launch; Can obtain AUV current position coordinates, position angle and along the linear velocity and the angular velocity data information of three of carrier coordinate system; The parametric equation that given desired track point is converted into the three-dimensional flight path of the continuous straight line of segmentation is described, and the position coordinates of initialization " virtual guide " point;

Step 2. is calculated the relative position deviation of " virtual guide " point under the Serret-Frenet coordinate system on AUV and the desired track through the three-dimensional Track In Track error equation of AUV;

Step 3. adopts the contragradience method based on feedback gain based on the tracking error that calculates in the step 2, the virtual controlling amount of translational speed, trim angle and the yaw angle of " virtual guide " point on the calculation expectation path;

Step 4. is calculated the virtual controlling amount of AUV pitch velocity and yaw angle speed on the basis of step 3;

Step 5. is based on given AUV six degree of freedom Mathematical Modeling; Derivation is owed to drive the dynamics Controlling rule that the three-dimensional path of autonomous submarine navigation device AUV follows the tracks of and is comprised propeller thrust, trim control moment and change bow control moment control signal, realizes owing to drive the 3 d-line Track In Track control of AUV;

Step 6. is calculated current AUV position η ⁿ=(x, y is z) with the turning point WP that demarcates _k=(x _k, y _k, z _k) between distance

If switch radius R less than the flight path of setting, then the tracing task of the current appointment flight path of expression completion stops navigation or switches next section desired track, otherwise continues step 2.

2. the autonomous submarine navigation device 3 d-line path tracking control method with PID feedback gain according to claim 1 is characterized in that said initialized detailed process is:

According to given desired track point calculate planning rectilinear path parameter x ' _d, y ' _dAnd z ' _dDetailed process be according to the adjacent expected path point WP that sets _i=(x _i, y _i, z _i), i=1 wherein ..., n, n counts and WP for setting flight path _I+1=(x _I+1, y _I+1, z _I+1) coordinate information calculate the parameter x of this section expectation rectilinear path ' _d, y ' _dAnd z ' _dConcrete form:

$x_d^{\&prime;}=\frac{{\&PartialD;x}_d}{\&PartialD;s}=x_{i+1}-x_i$

$y_d^{\&prime;}=\frac{\&PartialD;y_d}{\&PartialD;s}=y_{i+1}-y_i$

$z_d^{\&prime;}=\frac{\&PartialD;z_d}{\&PartialD;s}=z_{i+1}-z_i$

Therefore by a WP _i=(x _i, y _i, z _i) and WP _I+1=(x _I+1, 1, y _I+1, z _I+1) the parametrization equation in the 3 d-line path that constitutes of line can be expressed as

x _d(s)＝x′ _ds+x _i

y _d(s)＝y′ _ds+y _i

z _d(s)＝z′ _ds+z _i

S is a path parameter, the speed u of defining virtual guide point P _pDirection is the angle ψ along the tangential direction of straight line path and fixed coordinate system transverse axis _FFor

ψ _F＝arctan(y′ _d/x′ _d)

Velocity vector u _pAngle theta with the fixed coordinate system Z-axis _FBe defined as

${\mathrm{\&theta;}}_F={\tan }^{-1}\left(\frac{{-z}_d^{\&prime;}}{\sqrt{{\left(x_d^{\&prime;}\right)}^2+{\left(y_d^{\&prime;}\right)}^2}}\right)$

Where

For the three-dimensional linear track in terms of the presence

{ initial position under the I} is η to given then AUV at fixed coordinate system ⁿ=[x, y, z] ^T, the initial bow of AUV is respectively ψ and θ to angle and trim angle, AUV longitudinal velocity u, transverse velocity v and vertical velocity w, yaw angle speed r and pitch velocity q.

3. the autonomous submarine navigation device 3 d-line path tracking control method with PID feedback gain according to claim 1 is characterized in that according to the three-dimensional Track In Track error of computes AUV x _e, y _eAnd z _e:

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{x}}_e=v_t\cos {\mathrm{\&psi;}}_e\cos {\mathrm{\&theta;}}_e-u_p\\ {\stackrel{\&CenterDot;}{y}}_e=v_t\sin {\mathrm{\&psi;}}_e\cos {\mathrm{\&theta;}}_e\\ {\stackrel{\&CenterDot;}{z}}_e=-v_t\sin {\mathrm{\&theta;}}_e\end{array}\right.$

Here

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{\mathrm{\&psi;}}}_e=\frac{r}{\cos \mathrm{\&theta;}}-{\stackrel{\&CenterDot;}{\mathrm{\&psi;}}}_F\\ {\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_e=q-{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_F\end{array}\right.$

ε=[x wherein _e, y _e, z _e] ^TFor tracking error under the F} coordinate system, wherein, AUV resultant velocity

u _pBe " virtual guide " translational speed, ψ _e=ψ-ψ _F, θ _e=θ-θ _F

4. the autonomous submarine navigation device 3 d-line path tracking control method with PID feedback gain according to claim 1 is characterized in that being calculated as follows translational speed, AUV trim angle tracing deviation and the bow of " virtual guide " the virtual controlling amount to the angle tracking deviation:

(1) " virtual guide " translational speed

u _p＝v _t?cosψ _e?cosθ _e+k ₁x _e

K wherein ₁＞0 is the design of Controller parameter, x _eBe { F} coordinate system ventrocephalad tracking error;

(2) AUV trim angle tracing deviation virtual controlling amount of equal value

α _θ＝c ₂z _e

C wherein ₂＞0 is the design of Controller parameter, z _eBe { vertical tracking error under the F} coordinate system;

(3) AUV yaw angle tracing deviation virtual controlling amount of equal value

α _ψ＝-c ₁y _e

C wherein ₁＞0 is the design of Controller parameter, y _eBe { horizontal tracing error under the F} coordinate system.

5. the autonomous submarine navigation device 3 d-line path tracking control method with PID feedback gain according to claim 1 is characterized in that being calculated as follows AUV pitch velocity α _qWith the yaw angle speed alpha _rThe virtual controlling amount:

(1) AUV yaw angle speed virtual controlling amount

α _r＝-c ₃z ₁

C wherein ₃＞0 is wherein z of design of Controller parameter ₁=ψ _e-α _ψ

(2) AUV pitch velocity virtual controlling amount

α _q＝-c ₄z ₂

C wherein ₄＞0 is the design of Controller parameter, z ₂=θ _e-α _θ

6. the autonomous submarine navigation device 3 d-line path tracking control method with PID feedback gain according to claim 1 is characterized in that resolving AUV actuating mechanism controls command signal by following form:

(1) AUV propeller thrust control input signals

$F_u=m_1({\stackrel{\&CenterDot;}{u}}_d-k_u{u}_e)-f_u$

U wherein _dBe expectation AUV longitudinal velocity;

(2) AUV Trimming Moment control input signals

τ _q＝γ ₁z _e-γ ₂θ _e-γ ₃q-c ₂c ₄v _t?sinθ _e-f _q

$\left\{\begin{array}{c}{\mathrm{\&gamma;}}_1=m_4{c}_2(\frac{p_2}{p_4}+c_4{c}_6)\\ {\mathrm{\&gamma;}}_2=m_4(\frac{p_2}{p_4}+c_4{c}_6)\\ {\mathrm{\&gamma;}}_3=m_4(c_4+c_6)\end{array}\right.;$

(3) AUV changes bow Torque Control input signal

τ _r＝-λ ₁y _e-λ ₂ψ _e-λ ₃r-m ₅(c ₁c ₃v _t?cos _θsinψ _e?cosθ _e+qr?tanθ)-f _r

$\left\{\begin{array}{c}{\mathrm{\&lambda;}}_1=c_1{m}_5(\frac{p_1}{p_3}+c_3{c}_5)\cos \mathrm{\&theta;}\\ {\mathrm{\&lambda;}}_2=m_5(\frac{p_1}{p_3}+c_3{c}_5)\cos \mathrm{\&theta;}\\ {\mathrm{\&lambda;}}_3=m_5(c_3+c_5)\end{array}\right.$

Wherein

$\left\{\begin{array}{c}{f}_u=m_2\mathrm{vr}-m_3\mathrm{wq}+d_{1}u\\ f_q=(m_1-m_3)\mathrm{uw}+d_{4}q-g_2\\ f_r=(m_1-m_2)\mathrm{uv}+d_{5}r\end{array}\right.$

Wherein

$m_1=m-X_{\stackrel{\&CenterDot;}{u}},m_2=m-Y_{\stackrel{\&CenterDot;}{v}},m_3=m-Z_{\stackrel{\&CenterDot;}{w}}$

$m_4=I_y-M_{\stackrel{\&CenterDot;}{q}},m_5=I_z-N_{\stackrel{\&CenterDot;}{r}}$

g ₁＝(W-B)cosθ，g ₂＝(z _g?W-z _bB)sinθ

d ₁＝X _u+X _u|u||u|,d ₂＝Y _v+Y _v|v||v|

d ₃＝Z _w+Z _w|w||w|,d ₄＝M _q+M _q|q||q｜

d ₅＝N _r+N _r|r||r|

Wherein, state variable u, v, w, q and r represent carrier coordinate system { longitudinal velocity of AUV, transverse velocity, vertical velocity, pitch velocity and yaw angle speed under the B} respectively; M and m ₍₎Represent AUV quality and the additional mass that produces by the fluid effect respectively, I _yBe the moment of inertia of AUV around the y axle, Iz is the moment of inertia of AUV around the z axle, X ₍₎, Y ₍₎, Z ₍₎, M ₍₎And N ₍₎Be the viscous fluid hydrodynamic force coefficient; z _gAnd z _bBe respectively under the carrier coordinate coordinate position of center of gravity and centre of buoyancy on the Z-axis, W and B represent gravity and the buoyancy that AUV receives, d respectively ₍₎Be nonlinear damping hydrodynamic force item, control input F _u, τ _qAnd τ _rRepresent AUV propeller thrust, trim control moment respectively and change the bow control moment.

Images