CN102768539B - 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 d=sqrt((x-xk)<2>+(y-yk)<2>+(z-zk)<2>) 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 Underwater Vehicle 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 Underwater Vehicle.

Background technology

The exploration of submarine topography and mapping have great significance to the exploitation of deep-sea resources, owe to drive Autonomous Underwater Vehicle AUV(Autonomous Underwater Vehicle) owing to thering is good maneuverability and flying power, in marine exploration and development, playing the part of important role, along with AUV deepening continuously in the application of oceanographic engineering field, make to AUV under water three-dimensional movement control technology researched and proposed new challenge, consider and be subject to navigating by water economy or load capacity restriction, conventionally topworks is configured to longitudinal afterbody thruster, horizontal direction rudder and VTOL (vertical take off and landing) rudder, AUV is not equipped with horizontal and vertical auxiliary propeller mostly, make the dimension of control inputs much smaller than the freedom of motion number of degrees, for owing drive system, cannot design invariance control rule when smooth and realize FEEDBACK CONTROL, simultaneously owing to being subject to marine environment effect complicated and changeable, AUV kinetic model is had higher non-linear, the coupling uncertain and model self exists, this also becomes the difficult point of owing to drive AUV three dimensions Tracking Control Design.

At present, the research of controlling for the three-dimensional space motion of owing to drive AUV is both at home and abroad less, the most horizontal plane motion subsystem for decoupling zero of research and vertical plane severity control subsystem be CONTROLLER DESIGN respectively, and then realize owing to drive Autonomous Underwater Vehicle three-dimensional motion control under water, owing to having ignored the coupling of model, the controller of design cannot be realized and owe to drive AUV to control the tracking of any smooth curve in space.Tracking control problem discussed here is specially the path trace control problem in underwater 3 D space, the description of underwater 3 D space path is described by parametrization equation, the equation of locus being different from three-dimensional track tracking control problem is usingd the time as parameter, overcome in traditional track tracking control problem " the virtual A UV " that due to introducing, there is isomorphism kinetic model be subject to environmental interference effect cause closed loop tracking system have unstable dynamically, in the present invention, pass through the translational speed of " virtual guide " on design expected path as the extra control inputs of tracker, the concrete kinetic model of nothing because " virtual guide " only has kinematics characteristic, therefore state is not subject to the impact of external disturbance, can guarantee 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 being singular value point, initial position Constrained to AUV, cannot realize the global convergence that AUV follows the tracks of, and there is not singular value problem in 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 drive AUV Three-dimensional Track follow the tracks of to control > > (to control and decision-making, 2012, the 38th the 2nd phase of volume) according to line of sight method (line-of-sight, LOS) calculation expectation is followed the tracks of the angle of sight, based on self-adaptation Backstepping design tracking control unit, tracking for discrete track points is controlled, do not provide the error equation that Three-dimensional Track is followed the tracks of, cannot realize the tracking to three dimensions smooth curve, and what track homing strategy was that line of sight method (Line of Sight, LOS) and this patent adopt is virtual guide strategy (Virtual Guidance), by following the tracks of " virtual guide " point on expected path, realizes AUV and converge on expected path, document < < based on discrete sliding mode prediction owe to drive AUV Three-dimensional Track to follow the tracks of to control > > (control and decision-making, 2011, the 26th the 10th phase of volume) in, provided the form of the AUV three-dimensional path tracking error equation under virtual guide coordinate system on expected path, AUV is assumed to fictitious point mass, suppose that direction of motion is consistent with resultant velocity direction vector, the three-dimensional path tracking error equation obtaining needs side drift angle and the angle of attack of AUV motion accurately can measure, this has difficulties in actual applications, be merely able to by the measurement to horizontal and catenary motion speed, and then calculate side drift angle and the angle of attack, and due to more difficult along the measurement of three axial velocities, cause final controller to resolve and have potential interruption possibility.< < based on nonlinear iterative sliding mode owe drive UUV Three-dimensional Track follow the tracks of to control > > (robotization journal, 2012, the 38th the 2nd phase of volume) design of the thought based on the decoupling zero of Engineering Control device nonlinear iterative sliding mode Track In Track controller, because plant model is six degree of freedom coupled motions models, therefore for longitudinal velocity, bow can only be by the coupling in robust item inhibition to the decoupling controller of controlling and trim control designs respectively, when the coupling between each degree of freedom of model is more obvious, controller can only be that cost is eliminated coupling by the higher controller gain of output, cause controller output saturation signal, the controller of decoupling zero is merely able to guarantee the asymptotic stability of three independent control subsystem, and cannot guarantee the asymptotic stability of whole control system, and the Three-dimensional Track tracking control unit that this patent proposes can guarantee whole system global asymptotic stability, the neural network H that < < Autonomous Underwater Vehicle three-dimensional path is followed the tracks of _∞robust Adaptive Control method > > (control theory and application, 2012, the 29 the 3rd phases of volume), based on rectangular projection Serret-Frenet establishment of coordinate system AUV three-dimensional path tracking error equation, uses H _∞robust control thought CONTROLLER DESIGN, introduce neural networks compensate model uncertainty simultaneously, but owing to having singular value point based on rectangular projection Serret-Frenet establishment of coordinate system AUV three-dimensional path tracking error model, make the starting condition Constrained to AUV, be that AUV initial position must be positioned at aircraft pursuit course minimum profile curvature radius, therefore cannot realize the global convergence that AUV follows the tracks of, and being based upon the three-dimensional tracking error model representing under AUV carrier coordinate system, this patent there is not singular value problem, therefore can guarantee the global convergence of AUV tracking error, in addition this patent adopts alternative manner CONTROLLER DESIGN to be different from H _∞robust Controller Design thought.

The method relating in above document is all carried out controller design for being based upon error equation under Serret-Frenet coordinate system, because the state variable in error model cannot directly measure, cause control system comparatively to rely on initial value accuracy, and this patent is set up three-dimensional path tracking error equation for AUV carrier coordinate system, while having avoided employing classical inverse footwork CONTROLLER DESIGN based on alternative manner design three-dimensional path tracking control unit, there is singular value point, guarantee the global convergence of system.

Summary of the invention

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

The object of the present invention is achieved 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 given expected path;

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

On step 3. calculation expectation path, the expectation translational speed, AUV kinematics of " virtual guide " point are followed the tracks of control law (as vertically move speed, AUV turn bow angular velocity and pitch velocity virtual controlling rule);

Step 4. is on the basis of kinematics control law of equal value, adopt Iterative Design thought, the dynamics Controlling rule that derivation owes to drive the three-dimensional path of Autonomous Underwater Vehicle AUV to follow the tracks of, calculates final instruction execution signal (as propeller thrust, trim control moment and turn bow control moment) according to the concrete hydrodynamic force mathematical model of AUV;

Step 5. is calculated current AUV position η ⁿ=(x, y, z) and the turning point WP demarcating _k=(x _k, y _k, z _k) between distance if be less than the flight path of setting, switch radius R, represented that the tracing task of current specified path stops navigation or switches next desired track, otherwise continued 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 AUV carrier coordinate system, kinetic characteristic in conjunction with AUV, while having avoided on expected path the AUV three-dimensional path tracking error equation under virtual guide coordinate system, AUV is assumed to fictitious point mass, suppose that direction of motion is consistent with resultant velocity direction vector, the three-dimensional path tracking error equation obtaining needs side drift angle and the angle of attack of AUV motion accurately can measure, this has difficulties in actual applications, be merely able to by the measurement to horizontal and catenary motion speed, and then side drift angle and the angle of attack of calculating AUV, and due to more difficult along the measurement of three axial velocities, cause final controller to resolve the deficiency that exists potential interruption possible.

2. introduce the translational speed of " virtual guide " point on expected path as extra control inputs, guarantee in practical application when thering is larger tracking error, tracker has good dynamic property, avoids higher gain signal and the thrust saturated phenomenon of controller output; Adopt Iterative Design thought, by AUV three-dimensional path tracking control system, be divided into kinematics and dynamics designing two portions controller of equal value, based on Lyapunov stability theory, guarantee the stability of three-dimensional path tracking error closed-loop system, and the controller model parameter uncertainty that effect causes to marine environment has certain robustness.

Accompanying drawing explanation

Fig. 1 is that the AUV three-dimensional path that the present invention is based on virtual guide is followed the tracks of schematic 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 gamma controller block diagram.

Simulation comparison curve is controlled for the present invention designs the path trace of AUV three-dimensional curve in Fig. 4～10.As can be seen from Figure 4 design control method of the present invention still can be realized accurate tracking control when AUV is larger with expectation tracking three-dimensional path initial distance, Fig. 5 and Fig. 6 are respectively the three-dimensional pursuit path of AUV and obtain drop shadow curve at surface level and vertical plane, can find out that the tracking error of following the tracks of in 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 inputs response curve.

Embodiment

For example the present invention is described in more detail below:

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

${\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 the slickness in tracked path, require x _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 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)$

Wherein therefore virtual guide point P can be expressed as respectively along the angular velocity of rotation of curved path

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

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

Then the initial position of given 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 completed the initialization setting in step 1.

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

Fig. 1 is for owing to drive AUV three-dimensional path to follow the tracks of schematic diagram, wherein expects that { position vector of I} is defined as at fixed coordinate system for virtual guide point P on track path Ω at fixed coordinate system, { position vector under I} is defined as η to AUV ⁿ=[x, y, z] ^t, ε=[x _e, y _e, z _e] ^tfor with respect to AUV carrier coordinate system, { under B}, tracking error vector, can be expressed as so obtain tracking error

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

Wherein for AUV carrier coordinate system B} to fixed coordinate system the rotation matrix of I}, representing matrix turn order.

$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)$

To formula (6), differentiate obtains

$\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)$

Due to ${\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}{ccc}0& -r& q\\ r& 0& 0\\ -q& 0& 0\end{array}\right]---\left(9\right)$

Above formula substitution formula (8) is obtained

$\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] ^tfor the velocity vector under carrier coordinate system; ν _f=[u _p, 0,0] ^tfor expectation path coordinate system, { velocity vector of reference point under F}, substitution formula (10) becomes

$\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}_p\\ 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)$

Arrange

$\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 completed and calculated the tracking error between virtual guide P on AUV and expected path, below design process how to provide according to the tracking error ε calculating, calculating control signal

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

(1) the expectation translational speed computing formula of virtual guide point P on 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 meets u _d> 0, regulatory factor 0 < λ < 1, and c > 0, γ > 0, for the tracking error distance under 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 speed kinematics 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 controller design parameter.

The detailed process that designs AUV three-dimensional path pursuit movement Virtual Controller in step 3 is as follows:

Because kinematics controller of equal value and true control inputs amount exist deviation, therefore define deviation variables and be

$\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 lyapunov energy function

$V_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 obtained

${\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}$

The detailed process of resolving control inputs instruction by AUV mathematical model in step 4 is:

According to AUV actual measurement hydrodynamic force coefficient, ignore the impact of rolling motion on model, obtain AUV five degree of freedom mathematical 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(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}{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_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 respectively carrier coordinate system { longitudinal velocity of AUV, transverse velocity, vertical velocity, pitch velocity and yaw angle speed under B}; M and m ₍₎represent respectively AUV quality and the additional mass being produced by fluid effect, I _yfor the moment of inertia of AUV around y axle, I _zfor the moment of inertia of AUV around z axle, X ₍₎, Y ₍₎, Z ₍₎, M ₍₎and N ₍₎for viscous fluid hydrodynamic force coefficient; z _gand z _bthe coordinate position that is respectively under carrier coordinate center of gravity and centre of buoyancy on Z-axis, W and B represent respectively gravity and the buoyancy that AUV is subject to, d ₍₎for nonlinear damping hydrodynamic force item, control inputs F _u, τ _qand τ _rrepresent respectively AUV propeller thrust, trim control moment and turn bow control moment.

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

$\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; By Lyapunov stability theory, prove, AUV three-dimensional path is followed the tracks of the asymptotic stability that control law formula (24) can guarantee tracking error closed-loop system.

The detailed process that designs AUV three-dimensional path tracking dynamics controller of equal value in step 4 is

Convolution (20) structure lyapunov energy function is

$V_2=V_1+\frac{1}{2}(u_e^2+r_e^2+q_e^2)---\left(26\right)$

To the differentiate of above formula both sides, formula (21) substitution is obtained

${\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)$

Arrange

${\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 obtained, 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 in formula (29) and catenary motion speed w are the less dividing value that has, and by the known existence of AUV kinetic characteristic | v| < u _max, | w| < u _max, u _maxfor 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 by LaSalle invariance principle, can be obtained, closed loop tracking error system Asymptotic Stability, by 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, z) and the turning point WP demarcating _k=(x _k, y _k, z _k) between distance if be less than the flight path of setting, switch radius R, represented that the tracing task of current specified path stops navigation or switches next desired track, otherwise continued step 2.

Simulating, verifying and analysis

Illustrating below, is the validity that the AUV Three-dimensional Track tracking control unit of design is invented in checking, carries out emulation experiment, and be analyzed with traditional PID control simulation result for the three-dimensional curve path of planning:

Described in step 1 in summary of the invention, the parametric description of three-dimensional curve is followed the tracks of in given first 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 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 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 Simulating Platform, according to step 2～4, resolve AUV three-dimensional path tracking control inputs and obtain final simulation curve.

Simulation analysis

Fig. 4～Figure 10 provides the path trace of AUV three-dimensional curve and controls simulation result.Fig. 4 is AUV three-dimensional spiral dive path trace track, Fig. 5 and Fig. 6 are respectively AUV three-dimensional path pursuit path and obtain drop shadow curve at surface level and vertical plane, therefrom can find out that each degree of freedom of three-dimensional motion due to AUV has coupling, adopt traditional PID controller Time Controller parameter to be difficult for regulating, control effect is poor, cannot 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 be followed the tracks of control, improved path trace precision.Fig. 7 is that AUV three-dimensional path is followed the tracks of tracking error curve in control, compare with traditional PID controller, can find out that the three-dimensional path controller of design has improved the precision of path trace herein, shortened the redundancy voyage of AUV, there is 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 tracking accuracy and the response speed of controller.Fig. 8 and Fig. 9 are respectively AUV three-dimensional path and follow the tracks of the change curve that each state variable in control procedure comprises linear velocity and attitude angle, can find out that AUV is less than longitudinal velocity along transverse velocity and vertical velocity in helix dive process, and for there being dividing value, when designing, controller can ignore.Figure 10 is that AUV three-dimensional path is followed the tracks of control inputs response.

Claims (1)

1. the Autonomous Underwater Vehicle 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 main submarine navigation device initial position and attitude information, the initial value of given expectation track path parameter, " virtual guide " initial position and initial movable velocity information on given expectation track path;

Step 2. is calculated the main submarine navigation device current location of initial time and " virtual guide " relative tracking error under main submarine navigation device carrier coordinate system on expectation track path;

The expectation translational speed of " virtual guide " on step 3. calculation expectation track path, main submarine navigation device kinematics are followed the tracks of control law, and what comprise the speed of vertically moving, main submarine navigation device turns bow angular velocity and pitch velocity virtual controlling rule;

Step 4. is on the basis of kinematics control law of equal value, adopt iteration, the dynamics Controlling rule that the three-dimensional path of derivation Autonomous Underwater Vehicle is followed the tracks of, according to main underwater navigation implement body hydrodynamic force mathematical model, calculate final instruction execution signal, comprise propeller thrust, trim control moment and turn bow control moment;

Step 5. is calculated current main submarine navigation device position η ⁿ=[x, y, z] ^twith the turning point WP demarcating _k=[x _k, y _k, z _k] ^tbetween distance if be less than the flight path of setting, switch radius R, represented that the tracing task of current specified path stops navigation or switches next desired track, otherwise continued step 2;

The function that virtual guide on given expectation track path is shown a certain scalar parameter s ∈ R at the coordinates table of fixed coordinate system is

${\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 expect the slickness of track path, require x _d(s), y _d(s), z _d(s) second-order partial differential coefficient exists;

The speed u of defining virtual guide _pdirection is along the expectation tangential direction of track path and the angle ψ of fixed coordinate system transverse axis _dfor

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

Velocity vector u _pangle theta with 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)$

Wherein therefore virtual guide is expressed as along the angular velocity of rotation of expectation track path

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

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

Then the initial position of given main submarine navigation device under fixed coordinate system is η ⁿ=[x, y, z] ^t, the initial bow of main submarine navigation device is respectively ψ and θ to angle and trim angle, main submarine navigation device longitudinal velocity u, transverse velocity v and vertical velocity w, yaw angle speed r and pitch velocity q;

According to following formula, calculate the main submarine navigation device current location of initial time and " virtual guide " relative tracking error under main submarine navigation device carrier coordinate system on expectation track path:

$\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=-{\mathrm{rx}}_e+u_p\sin {\mathrm{\&psi;}}_e\cos {\mathrm{\&theta;}}_e+v\\ {\stackrel{\&CenterDot;}{z}}_e={\mathrm{qz}}_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 virtual guide P on AUV current location and expectation track path is with respect to the main submarine navigation device carrier coordinate system { projection components in lower three coordinate axis of B}, main submarine navigation device longitudinal velocity u, transverse velocity v and vertical velocity w, yaw angle speed r and pitch velocity q, u _pspeed for virtual guide;

According to following formula, calculate respectively main submarine navigation device kinematics and follow the tracks of control law

(1) on expectation track path, the expectation translational speed of virtual guide is calculated:

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

Wherein parameter meets u _d>0, regulatory factor 0< λ <1, c>0, γ >0, it is the tracking error distance under main submarine navigation device carrier coordinate system;

(2) main submarine navigation device longitudinal velocity kinematics control law of equal value is:

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

(3) main underwater vehicle trim angular velocity kinematics control law of equal value is:

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

(4) main submarine navigation device yaw angle speed kinematics 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 controller design parameter;

The detailed process that calculates final instruction execution signal according to main underwater navigation implement body hydrodynamic force mathematical model is:

According to main submarine navigation device actual measurement hydrodynamic force coefficient, ignore the impact of rolling motion on model, obtain main submarine navigation device five degree of freedom mathematical 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_2}\mathrm{uq}+\frac{d_3}{m_2}w+g_1$

$\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}{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θ

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 respectively carrier coordinate system { longitudinal velocity of main submarine navigation device, transverse velocity, vertical velocity, pitch velocity and yaw angle speed under B}; M and m ₍₎represent respectively main submarine navigation device quality and the additional mass being produced by fluid effect, I _ybe that main submarine navigation device is around the moment of inertia of y axle, I _zbe that main submarine navigation device is around the moment of inertia of z axle, X ₍₎, Y ₍₎, Z ₍₎, M ₍₎and N ₍₎for viscous fluid hydrodynamic force coefficient; z _gand z _bthe coordinate position that is respectively under carrier coordinate center of gravity and centre of buoyancy on Z-axis, W and B represent respectively gravity and the buoyancy that main submarine navigation device is subject to, d ₍₎for nonlinear damping hydrodynamic force item, control inputs F _u, τ _qand τ _rrepresent respectively main submarine navigation device propeller thrust, trim control moment and turn bow control moment.