CN102768539A - AUV (autonomous underwater vehicle) three-dimension curve path tracking control method based on iteration - Google Patents

Abstract

The invention provides an AUV (autonomous underwater vehicle) three-dimension curve path tracking control method based on iteration. The method comprises the following steps that 1, initialization is carried out; 2, the relative tracking error of the current AUV position and the virtual guide point on an expected path in an AUV carrier coordinate system at the initial moment is calculated; 3, the expected moving speed of the virtual guide point on the expected path and the AUV kinematics tracking control law are calculated; 4, the iteration is adopted on the basis of the kinematics equivalent control law, and the kinematics equivalent control law of the underactuated AUV three-dimension path tracking is deduced; and 5, the distance between the current AUV position Eta<n>=(x, y, z) and the demarcated steering point WPk=(xk, yk, zk) is calculated, when the distance is smaller than the set track switching radius R, the result shows that the current specified path tracking task is completed, the navigation is stopped, or the track is switched to a next expected track, and otherwise, the second step is continuously carried out. The AUV three-dimension curve path tracking control method has the advantage that the AUV path tracking precision can be improved.

Description

Autonomous submarine navigation device three-dimensional curve path tracking control method based on iteration

Technical field

The present invention relates to owe to drive the three-dimensional space motion control technology field of autonomous submarine navigation device.

Background technology

The exploration of submarine topography and mapping have great significance to the exploitation of deep-sea resources; Owe to drive autonomous submarine navigation device AUV (Autonomous Underwater Vehicle) owing to have good maneuverability and flying power; In the seafari exploitation, playing the part of important role; Deepen continuously along with what AUV used in the oceanographic engineering field, make the three-dimensional under water Research on Motion Control of AUV has been proposed new challenge, consider to receive navigation economy or load capacity restriction; Usually topworks is configured to vertical afterbody thruster, horizontal direction rudder and VTOL (vertical take off and landing) rudder; AUV is not equipped with horizontal and vertical auxiliary propeller mostly, makes the dimension of controlling input much smaller than the freedom of motion number of degrees, for owing drive system; Can't design when smooth constant control law and realize FEEDBACK CONTROL; Owing to receive marine environment effect complicated and changeable, make the AUV kinetic model have the coupling that higher non-linearity, uncertainty and model self exists simultaneously, this also becomes the difficult point of owing to drive AUV three dimensions Tracking Control Design.

At present; Less both at home and abroad to the research of the three-dimensional space motion control of owing to drive AUV; Research is directed against the horizontal plane motion subsystem and the vertical plane degree of depth RACS difference CONTROLLER DESIGN of decoupling zero mostly; And then realize that owing to ignored the coupling of model, the controller of design can't realize owing to drive the tracking Control of AUV to any smooth curve in space to owing to drive the three-dimensional under water motion control of autonomous submarine navigation device.Here the tracking Control problem of discussing is specially the path trace control problem in underwater 3 D space; The description of underwater 3 D space path is described through the parametrization equation; Be different from the three-dimensional track tracking Control problem equation of locus with the time as parameter; Overcome in traditional Trajectory Tracking Control problem owing to introduce " virtual A UV " with isomorphism kinetic model and receive the environmental disturbances effect to cause that closed loop tracking system has dynamically unstable; The translational speed of passing through " virtual guide " on the design expected path among the present invention is as the extra control input of tracker; Do not have concrete kinetic model owing to " virtual guide " only has kinematics characteristic, so state do not receive the influence of external disturbance, can guarantee the stability and the dynamic property of tracker.

P.Encarnacao etc. are at paper " 3D Path Following for Autonomous Underwater Vehicle " (Proceedings of the 39th IEEE Conference on Decision and Control; IEEE Press; 2000; Sydney.) utilize the thought of rectangular projection to set up the three-dimensional path tracking error model of AUV under expected path coordinate system (Serret-Frenet); Owing to there is a singular value point, the initial position of AUV there is constraint, 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 under the AUV carrier coordinate system that this patent is set up, therefore can guarantee the global convergence of AUV tracking error; " 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) according to line of sight method (line-of-sight, LOS) the calculation expectation tracking angle of sight; Based on self-adaptation contragradience method design tracking control unit; To the tracking Control of discrete track points, do not provide the error equation of three-dimensional Track In Track, can't realize tracking to the three dimensions smooth curve; 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; Document " 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) provided the form of the AUV three-dimensional path tracking error equation under virtual guide coordinate system on the expected path in, AUV has been assumed to virtual particle, supposed that direction of motion is consistent with the resultant velocity direction vector; The three-dimensional path tracking error equation that obtains needs the side drift angle and the angle of attack of AUV motion accurately can measure; This is to have difficulties in practical application, is merely able to through the measurement to horizontal and catenary motion speed, and then calculates the side drift angle and the angle of attack; And since difficult along the measurement of three axial velocities, cause final controller to resolve and have potential interruption possibility." 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, and can't guarantee the asymptotic stability of The whole control system, and the three-dimensional Track In Track controller that this patent proposes can guarantee the total system global asymptotic stability; " 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 this patent is based upon the three-dimensional tracking error model of representing under the AUV carrier coordinate system and does not have singular value problem, therefore can guarantee the global convergence of AUV tracking error, and this patent adopts the alternative manner CONTROLLER DESIGN to be different from H in addition _∞Robust Controller Design thought.

All error equation carries out design of Controller to the method that relates in the above document under the Serret-Frenet coordinate system to being based upon; Because the state variable in the error model can't directly measure; Cause control system that the initial value accuracy is comparatively relied on; And this patent is set up three-dimensional path tracking error equation to the AUV carrier coordinate system, has the singular value point when having avoided employing classical inverse footwork CONTROLLER DESIGN based on alternative manner design three-dimensional path tracking control unit, guarantees the global convergence of system.

Summary of the invention

The object of the present invention is to provide a kind of autonomous submarine navigation device three-dimensional path tracking and controlling method based on iteration that can improve the path trace precision.

The objective of the invention is to realize like this:

Step 1. initialization; Given three dimensions expectation track path parametrization equation is described; Given AUV initial position and attitude information, the initial value of given expectation track path parameter, " virtual guide " initial position and initial movable velocity information on the given expected path;

Step 2. is calculated the relative tracking error of " virtual guide " point under the AUV carrier coordinate system on initial time AUV current location and the expected path;

The expectation translational speed of " virtual guide " point, AUV kinematics tracking Control rule (as the commentaries on classics bow angular velocity and the pitch velocity virtual controlling that vertically move speed, AUV are restrained) on the step 3. calculation expectation path;

Step 4. is on the basis of kinematics control law of equal value; Adopt Iterative Design thought; Derivation owes to drive the dynamics Controlling rule that the three-dimensional path of autonomous submarine navigation device AUV is followed the tracks of, and promptly calculates final instruction execution signal (like propeller thrust, trim control moment and change the bow control moment) based on the concrete hydrodynamic force Mathematical Modeling of AUV;

Step 5. 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 specified path of expression completion stops navigation or switches next desired track, otherwise continues step 2.

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

1. based on setting up three-dimensional path tracking error equation under the AUV carrier coordinate system; Kinetic characteristic in conjunction with AUV; When having avoided on expected path the AUV three-dimensional path tracking error equation under the virtual guide coordinate system, AUV is assumed to virtual particle, supposes that direction of motion is consistent with the resultant velocity direction vector; The three-dimensional path tracking error equation that obtains needs the side drift angle and the angle of attack of AUV motion accurately can measure; This is to have difficulties in practical application, is merely able to through the measurement to horizontal and catenary motion speed, and then calculates the side drift angle and the angle of attack of AUV; And since difficult along the measurement of three axial velocities, cause final controller to resolve the deficiency that exists potential interruption possible.

2. the translational speed of " virtual guide " point is imported as extra control on the introducing expected path; Guarantee in the practical application when having big tracking error; Tracker has good dynamic characteristics, avoids higher gain signal and the thrust saturated phenomenon of controller output; Adopt Iterative Design thought; With AUV three-dimensional path tracking control system; Be divided into kinematics and dynamics designing two portions controller of equal value; Based on the stability of Lyapunov stability theory assurance three-dimensional path tracking error closed-loop system, and the controller model parameter uncertainty that effect causes to marine environment has certain robustness.

Description of drawings

Fig. 1 is that the AUV three-dimensional path that the present invention is based on virtual guide is followed the tracks of synoptic diagram.

Fig. 2 is that AUV three-dimensional path tracking control unit of the present invention resolves process flow diagram.

Fig. 3 is that AUV three-dimensional path of the present invention is followed the tracks of the gamma controller block diagram.

AUV three-dimensional curve path trace control emulation correlation curve is designed for the present invention in Fig. 4～10.Design control method as can be seen from Figure 4 of the present invention still can be realized accurate tracking control when AUV is big with expectation tracking three-dimensional path initial distance; Fig. 5 and Fig. 6 are respectively the three-dimensional pursuit path of AUV and get drop shadow curve at surface level and vertical plane; Can find out that the tracking error of following the tracks of on three directions reduces gradually; The three-dimensional tracking error curve of AUV in Fig. 7 finally converges to zero, has verified the validity of design control method of the present invention; Fig. 8～9 are the change curve of AUV state variable; Figure 10 is AUV control input response curve.

Embodiment

For example the present invention is done more detailed description below:

For the virtual guide P on the given expectation track path Ω in the step 1 do at the function that the coordinate of fixed coordinate system can be expressed as a certain scalar parameter s ∈ R

${\mathrm{\&eta;}}_d^n\left(s\right)={[x_d\left(s\right),y_d\left(s\right),z_d\left(s\right)]}^T---\left(1\right)$

In order to guarantee to be required x by the slickness of track path _d(s), y _d(s), z _d(s) second-order partial differential coefficient exists.

The speed u of defining virtual guide point P _pDirection is the angle ψ along the tangential direction of curved path and fixed coordinate system transverse axis _dFor

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

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

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

Where

Wizard and the virtual point P along a curved path, respectively, the rotational angular velocity can be expressed as

$q_d={\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_d---\left(4\right)$

$r_d={\stackrel{\&CenterDot;}{\mathrm{\&psi;}}}_d---\left(5\right)$

The initial position of given then AUV under fixed coordinate system is η ⁿ=[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.

The detailed process of calculating the three-dimensional path tracking error in the step 2 is following:

Fig. 1 follows the tracks of synoptic diagram for owing to drive the AUV three-dimensional path, and { position vector of I} is defined as the virtual guide point P on its desired track path Ω at fixed coordinate system

{ position vector under the I} is defined as η to AUV at fixed coordinate system ⁿ=[x, y, z] ^T, ε=[x _e, y _e, z _e] ^TFor { the tracking error vector can be expressed as so obtain tracking error under the B} with respect to the AUV carrier coordinate system

$\mathrm{\&epsiv;}=R_b^{\mathrm{nT}}{\mathrm{\&eta;}}_e^n---\left(6\right)$

Wherein

is that { B} is to fixed coordinate system { rotation matrix of I}, the commentaries on classics order of

representing matrix

for the AUV carrier coordinate system.

$R_b^n=R_{y,\mathrm{\&theta;}}{R}_{z,\mathrm{\&psi;}}$

$=\left[\begin{array}{ccc}\cos \mathrm{\&theta;}& 0& -\sin \mathrm{\&theta;}\\ 0& 1& 0\\ \sin \mathrm{\&theta;}& 0& \cos \mathrm{\&theta;}\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{\&psi;}& \sin \mathrm{\&zeta;}& 0\\ -\sin \mathrm{\&psi;}& \cos \mathrm{\&psi;}& 0\\ 0& 0& 1\end{array}\right]---\left(7\right)$

Differentiate gets to formula (6)

$\stackrel{\&CenterDot;}{\mathrm{\&epsiv;}}={\stackrel{\&CenterDot;}{R}}_b^{\mathrm{nT}}{\mathrm{\&eta;}}_e^n+R_b^{\mathrm{nT}}{\stackrel{\&CenterDot;}{\mathrm{\&eta;}}}_e^n---\left(8\right)$

Because ${\stackrel{\&CenterDot;}{R}}_b^n=R_b^{n}S\left({\mathrm{\&omega;}}_{\mathrm{Nb}}^b\right),$ Wherein

$S\left({\mathrm{\&omega;}}_{\mathrm{nb}}^b\right)=\left[\begin{array}{cc}0& -\mathrm{<figure-callout\; id="0"\; label="r\; q\; r"\; filenames="DEST\_PATH\_HDA00002071157900041.png,DEST\_PATH\_HDA00002071157900042.png"\; state="\{\{state\}\}">r</figure-callout>}& q\\ r& 0& 0\\ -\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="DEST\_PATH\_HDA00002071157900041.png,DEST\_PATH\_HDA00002071157900042.png"\; state="\{\{state\}\}">q</figure-callout>}& 0& 0\end{array}\right]---\left(9\right)$

Following formula substitution formula (8) is got

$\stackrel{\&CenterDot;}{\mathrm{\&epsiv;}}=S^T\left({\mathrm{\&omega;}}_{\mathrm{nb}}^b\right)R_b^{\mathrm{NT}}{\mathrm{\&eta;}}_e^n+R_b^{\mathrm{nT}}{\stackrel{\&CenterDot;}{\mathrm{\&eta;}}}_e^n---\left(10\right)$

Consider

Wherein ν _b=[u, v, w] ^TBe the velocity vector under the carrier coordinate system;

ν _F=[u _p, 0,0] ^T{ velocity vector of RP under the F}, substitution formula (10) becomes for the expectation path coordinate system

$\stackrel{\&CenterDot;}{\mathrm{\&epsiv;}}=S^T\left({\mathrm{\&omega;}}_{\mathrm{nb}}^b\right)\mathrm{\&epsiv;}+R_b^{\mathrm{nT}}({\stackrel{\&CenterDot;}{\mathrm{\&eta;}}}^n-{\stackrel{\&CenterDot;}{\mathrm{\&eta;}}}_d^n)$

$=S^T\left({\mathrm{\&omega;}}_{\mathrm{nb}}^b\right)\mathrm{\&epsiv;}+R_b^{\mathrm{nT}}{R}_b^n{v}_b-R_b^{\mathrm{nT}}{\stackrel{\&CenterDot;}{\mathrm{\&eta;}}}_d^n---\left(11\right)$

$=S^T\left({\mathrm{\&omega;}}_{\mathrm{nb}}^b\right)\mathrm{\&epsiv;}+v_b-R_b^{\mathrm{nT}}{R}_F^n{v}_F$

Launch

$\left[\begin{array}{c}{\stackrel{\&CenterDot;}{x}}_e\\ {\stackrel{\&CenterDot;}{y}}_e\\ {\stackrel{\&CenterDot;}{z}}_e\end{array}\right]=\left[\begin{array}{c}{\mathrm{ry}}_e-{\mathrm{qz}}_e\\ -r{x}_e\\ q{x}_e\end{array}\right]+\left[\begin{array}{c}u\\ v\\ w\end{array}\right]-R_F^{\mathrm{bT}}\left[\begin{array}{c}{u}_{\mathrm{<figure-callout\; id="0"\; label="p"\; filenames="DEST\_PATH\_HDA00002071157900041.png,DEST\_PATH\_HDA00002071157900042.png"\; state="\{\{state\}\}">p</figure-callout>}}\\ 0\\ 0\end{array}\right]---\left(12\right)$

Wherein

$R_F^b=\left[\begin{array}{ccc}\cos {\mathrm{\&theta;}}_e\cos {\mathrm{\&psi;}}_e& \cos {\mathrm{\&theta;}}_e\sin {\mathrm{\&psi;}}_e& -{\sin \mathrm{\&theta;}}_e\\ -\sin {\mathrm{\&psi;}}_e& \cos {\mathrm{\&psi;}}_e& 0\\ \sin {\mathrm{\&theta;}}_e\cos {\mathrm{\&psi;}}_e& \sin {\mathrm{\&theta;}}_e\sin {\mathrm{\&psi;}}_e& \cos {\mathrm{\&theta;}}_e\end{array}\right]---\left(13\right)$

Put in order

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{x}}_e={\mathrm{ry}}_e-{\mathrm{qz}}_e+u-u_p\cos {\mathrm{\&psi;}}_e\cos {\mathrm{\&theta;}}_e\\ {\stackrel{\&CenterDot;}{y}}_e=-r{x}_e+u_p\sin {\mathrm{\&psi;}}_e\cos {\mathrm{\&theta;}}_e+v\\ {\stackrel{\&CenterDot;}{z}}_e=q{z}_e-u_p\sin {\mathrm{\&theta;}}_e+w\end{array}\right.---\left(14\right)$

ψ wherein _e=ψ-ψ _d, θ _e=θ-θ _d

So far accomplished and calculated the tracking error between the virtual guide P on AUV and the expected path, below design process provide the tracking error ε how basis calculates, calculation control signal

AUV three-dimensional path tracking error variable for providing in the step 2 calculates the kinematics Virtual Controller respectively according to following formula

(1) the expectation translational speed computing formula of virtual guide point P on the expected path:

$u_p(t,\mathrm{\&epsiv;})=u_d(1-{\mathrm{\&lambda;e}}^{-c(t-t_0)})e^{-\mathrm{\&gamma;}{d}_e}---\left(15\right)$

Wherein parameter satisfies u _d＞0, regulatory factor 0＜λ＜1, c＞0, γ＞0,

Be the tracking error distance under the AUV carrier coordinate system.

(2) AUV longitudinal velocity kinematics control law of equal value is:

a _u＝u _pcosψ _ecosθ _e-k ₁x _e (16)

(3) AUV pitch velocity kinematics control law of equal value is:

α _q＝q _d+(k ₄z _eu _p-k ₅sinθ _e) (17)

(4) AUV yaw angle speeds control law of equal value is:

α _r＝cosθ[r _d-(k ₂cosθ _ey _eu _p?+k ₃sinψ _e)] (18)

K wherein ₁＞0, k ₂＞0, k ₃＞0, k ₄＞0, k ₅＞0 is the design of Controller parameter.

The detailed process of design AUV three-dimensional path pursuit movement Virtual Controller is following in the step 3:

Because there is deviation in kinematics controller of equal value with true control input quantity, therefore defines deviation variables and do

$\left\{\begin{array}{c}{u}_e=u-{\mathrm{\&alpha;}}_u\\ r_e=r-{\mathrm{\&alpha;}}_r\\ q_e=q-{\mathrm{\&alpha;}}_q\end{array}\right.---\left(19\right)$

For given Track In Track error equation (14), structure Liapunov energy function

${\mathrm{<figure-callout\; id="1"\; label="V"\; filenames="DEST\_PATH\_HDA00002071157900052.png,DEST\_PATH\_HDA00002071157900061.png"\; state="\{\{state\}\}">V</figure-callout>}}_1=\frac{1}{2}(x_e^2+y_e^2+z_e^2)+\frac{1-\cos {\mathrm{\&psi;}}_e}{k_2}+\frac{1-{\cos \mathrm{\&theta;}}_e}{k_4}---\left(20\right)$

To formula (20) differentiate, formula (16)～(18) and formula (19) substitution are got

${\stackrel{\&CenterDot;}{V}}_1=-k_1{x}_e^2-k_3{k}_2^{-1}{\sin }^2{\mathrm{\&psi;}}_e-k_5{k}_4^{-1}{\sin }^2{\mathrm{\&theta;}}_e$

$+y_{e}v+z_{e}w+x_e(r_e{y}_e+u_e-q_e{z}_e)-y_e{r}_e{x}_e$

$+z_e{q}_e{x}_e+\frac{r_e\sin {\mathrm{\&psi;}}_e}{k_2\cos \mathrm{\&theta;}}+\frac{q_e\sin {\mathrm{\&theta;}}_e}{k_4}---\left(21\right)$

$=-k_1{x}_e^2-k_3{k}_2^{-1}{\sin }^2{\mathrm{\&psi;}}_e-k_5{k}_4^{-1}{\sin }^2{\mathrm{\&theta;}}_e$

$+y_{e}v+z_{e}w+x_e{u}_e+\frac{r_e\sin {\mathrm{\&psi;}}_e}{k_2\cos \mathrm{\&theta;}}+\frac{q_e\sin {\mathrm{\&theta;}}_e}{k_4}$

Resolving the detailed process of controlling input instruction through the AUV mathematical model in the step 4 is:

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="DEST\_PATH\_HDA00002071157900052.png,DEST\_PATH\_HDA00002071157900061.png"\; state="\{\{state\}\}">m</figure-callout>}}_1}\mathrm{vr}-\frac{m_3}{{\mathrm{<figure-callout\; id="1"\; label="m"\; filenames="DEST\_PATH\_HDA00002071157900052.png,DEST\_PATH\_HDA00002071157900061.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}{m_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(22\right)$

$\stackrel{\&CenterDot;}{q}=\frac{m_1-m_3}{m_4}\mathrm{uw}+\frac{d_4}{m_4}q-g_2+\frac{1}{m_4}{\mathrm{\&tau;}}_q$

$\stackrel{\&CenterDot;}{r}=\frac{m_1-m_2}{{\mathrm{<figure-callout\; id="5"\; label="m"\; filenames="DEST\_PATH\_HDA00002071157900041.png,DEST\_PATH\_HDA00002071157900042.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="DEST\_PATH\_HDA00002071157900041.png,DEST\_PATH\_HDA00002071157900042.png"\; state="\{\{state\}\}">m</figure-callout>}}_5}{\mathrm{\&tau;}}_r$

Wherein

${\mathrm{<figure-callout\; id="1"\; label="m"\; filenames="DEST\_PATH\_HDA00002071157900052.png,DEST\_PATH\_HDA00002071157900061.png"\; state="\{\{state\}\}">m</figure-callout>}}_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}},$ ${\mathrm{<figure-callout\; id="5"\; label="m"\; filenames="DEST\_PATH\_HDA00002071157900041.png,DEST\_PATH\_HDA00002071157900042.png"\; state="\{\{state\}\}">m</figure-callout>}}_5=I_z-N_r$

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

(23)

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, 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.

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

$\left\{\begin{array}{c}{F}_u=m_1({\stackrel{\&CenterDot;}{a}}_u-x_e-k_u{u}_e)-f_u\\ {\mathrm{\&tau;}}_q=m_4({\stackrel{\&CenterDot;}{a}}_q-k_4^{-1}\sin {\mathrm{\&theta;}}_e-k_q{q}_e)-f_q\\ {\mathrm{\&tau;}}_r=m_5({\stackrel{\&CenterDot;}{a}}_r-k_2^{-1}{\cos }^{-1}\mathrm{\&theta;}\sin {\mathrm{\&psi;}}_e)-f_r\end{array}\right.---\left(24\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.---\left(25\right)$

Here variable u _e=u-α _u, r _e=r-α _r, q _e=q-α _qBe defined as the actual value of kinematics controller output and the deviation of expectation value, gain coefficient k _u＞0, k _q＞0, k _r＞0; Prove that through Lyapunov stability theory AUV three-dimensional path tracking Control rule formula (24) can guarantee the asymptotic stability of tracking error closed-loop system.

The detailed process of design AUV three-dimensional path tracking dynamics controller of equal value does in the step 4

Convolution (20) structure Liapunov energy function does

$V_2={\mathrm{<figure-callout\; id="1"\; label="V"\; filenames="DEST\_PATH\_HDA00002071157900052.png,DEST\_PATH\_HDA00002071157900061.png"\; state="\{\{state\}\}">V</figure-callout>}}_1+\frac{1}{2}(u_e^2+r_e^2+q_e^2)---\left(26\right)$

To the differentiate of following formula both sides, formula (21) substitution is got

${\stackrel{\&CenterDot;}{V}}_2=-k_1{x}_e^2-k_3{k}_2^{-1}{\sin }^2{\mathrm{\&psi;}}_e-k_5{k}_4^{-1}{\sin }^2{\mathrm{\&theta;}}_e$

$+y_{e}v+z_{e}w+x_e{u}_e+\frac{r_e\sin {\mathrm{\&psi;}}_e}{k_2\cos \mathrm{\&theta;}}+\frac{q_e\sin {\mathrm{\&theta;}}_e}{k_4}---\left(27\right)$

$+u_e(\stackrel{\&CenterDot;}{u}-{\stackrel{\&CenterDot;}{\mathrm{\&alpha;}}}_u)+r_e(\stackrel{\&CenterDot;}{r}-{\stackrel{\&CenterDot;}{\mathrm{\&alpha;}}}_r)+q_e(\stackrel{\&CenterDot;}{q}-{\stackrel{\&CenterDot;}{\mathrm{\&alpha;}}}_q)$

Put in order

${\stackrel{\&CenterDot;}{V}}_2=-k_1{x}_e^2-k_3{k}_2^{-1}{\sin }^2{\mathrm{\&psi;}}_e-k_5{k}_4^{-1}{\sin }^2{\mathrm{\&theta;}}_e$

$+y_{e}v+z_{e}w+r_e(\stackrel{\&CenterDot;}{r}-{\stackrel{\&CenterDot;}{\mathrm{\&alpha;}}}_r+k_2^{-1}{\cos }^{-1}\mathrm{\&theta;}\sin {\mathrm{\&psi;}}_e)---\left(28\right)$

$+u_e(\stackrel{\&CenterDot;}{u}-{\stackrel{\&CenterDot;}{\mathrm{\&alpha;}}}_u+x_e)+q_e(\stackrel{\&CenterDot;}{q}-{\stackrel{\&CenterDot;}{\mathrm{\&alpha;}}}_q+k_4^{-1}\sin {\mathrm{\&theta;}}_e)$

Formula (22) and formula (24) substitution are got, and formula (28) becomes

${\stackrel{\&CenterDot;}{V}}_2=-k_1{x}_e^2-k_3{k}_2^{-1}{\sin }^2{\mathrm{\&psi;}}_e-k_5{k}_4^{-1}{\sin }^2{\mathrm{\&theta;}}_e$ (29)

$-k_u{u}_e^2-k_q{q}_e^2-k_r{r}_e^2+y_{e}v+z_{e}w\&le;0$

Because AUV lateral movement velocity v and catenary motion speed w are the less dividing value that has in the formula (29), and can know existence by the AUV kinetic characteristic | v|＜u _Max, | w|＜u _Max, u _MaxBe the AUV longitudinal velocity upper bound, so and if only if (x _e, y _e, z _e, ψ _e, θ _e, u _e, r _e, q _e)=0 o'clock

Can be got by the LaSalle invariance principle, closed loop tracking error system is asymptotic stable, through adjustment control gain coefficient k ₁, k ₂, k ₃, k ₄, k ₅And k _u, k _q, k _rThe dynamic property of assurance system.

The detailed process of step 5 is:

Calculate 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 specified path of expression completion stops navigation or switches next desired track, otherwise continues step 2.

Simulating, verifying and analysis

Illustrate below, be the validity that the three-dimensional Track In Track controller of AUV of design is invented in checking, carry out emulation experiment to the three-dimensional curve path of 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 parametric description that three-dimensional curve is followed the tracks of in expectation

$\left\{\begin{array}{c}{x}_d\left(s\right)=A\cos \left(\mathrm{\&omega;s}\right)\\ y_d\left(s\right)=A\sin \left(\mathrm{\&omega;s}\right)\\ z_d\left(s\right)=\mathrm{\&omega;s}\end{array}\right.---\left(30\right)$

Parameter A=20 wherein, ω=0.02 π

" virtual guide " initial position message on the three-dimensional curve is followed the tracks of in given expectation

$\left\{\begin{array}{c}{x}_d\left(0\right)=20\\ y_d\left(0\right)=0\\ z_d\left(0\right)=0\end{array}\right.---\left(31\right)$

The initial velocity variable parameter u of " virtual guide " on the three-dimensional curve is followed the tracks of in given expectation _d=1 (m/s), gain parameter λ=0.5, c=1, γ=1, controller parameter k ₁=50, k ₂=10, k ₃=100, k ₄=20, k ₅=100; k _u=1, k _r=5, k _q=10; Adopt MATLAB numerical simulation platform, resolve the input of AUV three-dimensional path tracking Control according to step 2～4 and obtain final simulation curve.

Simulation analysis

Fig. 4～Figure 10 provides AUV three-dimensional curve path trace control simulation result.Fig. 4 is an AUV three-dimensional spiral dive path trace track; Fig. 5 and Fig. 6 are respectively AUV three-dimensional path pursuit path and get drop shadow curve at surface level and vertical plane; Therefrom can find out because each degree of freedom of three-dimensional motion of AUV has coupling; Controller parameter is difficult for regulating when adopting the traditional PID controller controller, and the control effect is relatively poor, can't realize the accurate tracking to three-dimensional path; And the gamma controller that the present invention is based on accurate model design can fine realization tracking Control, has improved the path trace precision.Fig. 7 is a tracking error curve in the AUV three-dimensional path tracking Control; Compare with the traditional PID controller controller; Can find out that the three-dimensional path controller that this paper designs has improved the precision of path trace, shorten the redundant voyage of AUV, have more stable control ability and guarantee that AUV follows the tracks of and converge to expected path faster; Make tracking error finally converge to zero, shown the tracking accuracy and the response speed of controller.Fig. 8 and Fig. 9 are respectively the change curve that each state variable in the AUV three-dimensional path tracking Control process comprises linear velocity and attitude angle; Can find out that AUV is less than longitudinal velocity along transverse velocity and vertical velocity in the helix dive process; And, when design of Controller, can ignore for dividing value is arranged.Figure 10 is AUV three-dimensional path tracking Control input response.

Claims (5)

1. autonomous submarine navigation device three-dimensional curve path tracking control method based on iteration is characterized in that:

Step 1. initialization; Given three dimensions expectation track path parametrization equation is described; Given AUV initial position and attitude information, the initial value of given expectation track path parameter, " virtual guide " initial position and initial movable velocity information on the given expected path;

Step 2. is calculated the relative tracking error of " virtual guide " point under the AUV carrier coordinate system on initial time AUV current location and the expected path;

The expectation translational speed of " virtual guide " point, AUV kinematics tracking Control are restrained on the step 3. calculation expectation path, comprise commentaries on classics bow angular velocity and the pitch velocity virtual controlling rule of the speed of vertically moving, AUV;

Step 4. is on the basis of kinematics control law of equal value; Adopt iteration; Derivation owes to drive the dynamics Controlling rule that the three-dimensional path of autonomous submarine navigation device AUV is followed the tracks of; Promptly calculate final instruction execution signal, comprise propeller thrust, trim control moment and change the bow control moment based on the concrete hydrodynamic force Mathematical Modeling of AUV;

Step 5. 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 specified path of expression completion stops navigation or switches next desired track, otherwise continues step 2.

2. the autonomous submarine navigation device three-dimensional curve path tracking control method based on iteration according to claim 1 is characterized in that: the virtual guide P on the given expectation track path Ω is that the function of a certain scalar parameter s ∈ R does in the coordinate representation of fixed coordinate system

${\mathrm{\&eta;}}_d^n\left(s\right)={[x_d\left(s\right),y_d\left(s\right),z_d\left(s\right)]}^T$

In order to guarantee to be required x by the slickness of track path _d(s), y _d(s), z _d(s) second-order partial differential coefficient exists;

The speed u of defining virtual guide point P _pDirection is the angle ψ along the tangential direction of curved path and fixed coordinate system transverse axis _dFor

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

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

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

Where

Virtual Wizard therefore point P along a curved path of angular velocity are expressed as

$q_d={\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_d$

$r_d={\stackrel{\&CenterDot;}{\mathrm{\&psi;}}}_d$

The initial position of given then AUV under fixed coordinate system is η ⁿ=[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 three-dimensional curve path tracking control method based on iteration according to claim 1 is characterized in that: according to computes three-dimensional path tracking error:

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{x}}_e={\mathrm{ry}}_e-{\mathrm{qz}}_e+u-u_p\cos {\mathrm{\&psi;}}_e\cos {\mathrm{\&theta;}}_e\\ {\stackrel{\&CenterDot;}{y}}_e=-r{x}_e+u_p\sin {\mathrm{\&psi;}}_e\cos {\mathrm{\&theta;}}_e+v\\ {\stackrel{\&CenterDot;}{z}}_e=q{z}_e-u_p\sin {\mathrm{\&theta;}}_e+w\end{array}\right.$

ψ wherein _e=ψ-ψ _d, θ _e=θ-θ _d, ε=[x _e, y _e, z _e] ^TFor the tracking error between the virtual guide P on AUV current location and the expected path with respect to AUV carrier coordinate system { projection components on following three coordinate axis of B}, AUV longitudinal velocity u, transverse velocity v and vertical velocity w, yaw angle speed r and pitch velocity q, u _pExpectation translational speed for " virtual guide " point on the expected path to be designed.

4. the autonomous submarine navigation device three-dimensional curve path tracking control method based on iteration according to claim 1 is characterized in that: calculate the kinematics Virtual Controller respectively according to following formula

(1) the expectation translational speed of virtual guide point P is calculated on the expected path:

$u_p(t,\mathrm{\&epsiv;})=u_d(1-{\mathrm{\&lambda;e}}^{-c(t-t_0)})e^{-\mathrm{\&gamma;}{d}_e};$

Wherein parameter satisfies u _d＞0, regulatory factor 0＜λ＜1, c＞0, γ＞0,

Be the tracking error distance under the AUV carrier coordinate system;

(2) AUV longitudinal velocity kinematics control law of equal value is:

a _u＝u _pcosψ _ecosθ _e-k ₁x _e；；

(3) AUV pitch velocity kinematics control law of equal value is:

α _q＝q _d+(k ₄z _eu _p-k ₅sinθ _e)；；

(4) AUV yaw angle speeds control law of equal value is:

α _r＝cosθ[r _d-(k ₂cosθ _ey _eu _p+k ₃sinψ _e)]；；

K wherein ₁＞0, k ₂＞0, k ₃＞0, k ₄＞0, k ₅＞0 is the design of Controller parameter.

5. the autonomous submarine navigation device three-dimensional curve path tracking control method based on iteration according to claim 1, it is characterized in that: the detailed process that calculates final instruction execution signal according to the concrete hydrodynamic force mathematical model of AUV is:

According to AUV actual measurement hydrodynamic force coefficient, ignore the influence of rolling motion to model, obtain AUV degree of freedom mathematics model

As follows

$\stackrel{\&CenterDot;}{u}=\frac{m_2}{m_1}\mathrm{vr}-\frac{m_3}{m_1}\mathrm{wq}+\frac{d_1}{m_1}u+\frac{1}{m_1}{F}_u$

$\stackrel{\&CenterDot;}{v}=-\frac{m_1}{m_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(1\right)$

$\stackrel{\&CenterDot;}{q}=\frac{m_1-m_3}{m_4}\mathrm{uw}+\frac{d_4}{m_4}q-g_2+\frac{1}{m_4}{\text{\&tau;}}_q$

$\stackrel{\&CenterDot;}{r}=\frac{m_1-m_2}{m_5}\mathrm{uv}+\frac{d_5}{m_5}r+\frac{1}{m_5}{\mathrm{\&tau;}}_r$

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 _gW-z _bB)sinθ

(2)

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, 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.

Images