CN105786012A - UUV virtual speed control method based on bio-inspired model - Google Patents

Abstract

The invention relates to a UUV virtual speed control method based on a bio-inspired model, belongs to UUV speed control methods, and aims at solving the problem that control for the UUV speed is instable. The method comprises that a virtual speed is obtained; a present position error is input to the bio-inspired model and smooth continuous processing is carried out to obtain a new position error, the virtual speed is processed according to the new position error, and a smooth continuous virtual speed is obtained; the virtual speed and an ocean current speed are subtracted from a present practical navigational speed of the UUV to obtain a speed error, the speed error serves as input of a PID speed controller, the PID speed controller acts output on a UUV model, a practical position of the UUV in the next step is obtained, and the UUV is controlled according to the obtained practical position; and the practical position of the UUV is compared with an expected position output by UUV motion plan, and an obtained position error item serves as position error input of the bio-inspired model in the next step. The method of the invention is used to control the navigational speed of the UUV when the UUV navigates near the water surface or in a shallow sea area.

Description

The UUV pseudo-velocity control method of model is encouraged based on biology

Technical field

The present invention relates to the control method of a kind of UUV speed, particularly to a kind of UUV pseudo-velocity control method encouraging model based on biology.

Background technology

UUV is when approximately level or neritic province domain navigate by water, and ocean current is very strong for the interference of the speed of a ship or plane, when using traditional PID controller in practice, occurs in that much distinctive control problem, and as lasting delayed in speed of a ship or plane tracking, propeller response is frequent.Cause UUV noise under sail to increase, flying power is deteriorated, propeller minimizing in service life.

Existing one devises UUV Track In Track controller based on iteration sliding-mode method, but the speed of a ship or plane of UUV is not carried out necessary control, only position is controlled, and the tracking of accurate position is built upon on stable speed of a ship or plane basis in actual experiment.UUV overall performance is had material impact by whether accurate due to the speed of a ship or plane, therefore controls extremely to be necessary for the accurate stable of UUV speed over ground.

And existing employing traditional PID control system carries out speed controlling there is also instability problem.

Summary of the invention

The invention aims to solve the problem controlling instability of UUV speed, the present invention provides a kind of UUV pseudo-velocity control method encouraging model based on biology.

The UUV pseudo-velocity control method encouraging model based on biology of the present invention, described method comprises the steps:

Step one: set up speed nonlinear equation during drive lacking UUV three-dimensional space motion according to kinematics model, obtains UUV at fixed coordinate system { under E} and hull coordinate system { relation between speed under B}；

Step 2: according to the relation between the UUV speed that step one obtains, obtains pseudo-velocity；

Step 3: the current extremely biological excitation model of site error input is carried out smooth process continuously, it is thus achieved that new site error, according to described new site error, the pseudo-velocity in step 2 is processed, it is thus achieved that smooth continuous print pseudo-velocity；

Step 4: pseudo-velocity that step 3 is generated, current speed and the currently practical speed of a ship or plane of UUV subtract each other after acquisition velocity error as the input of PID speed control, output action on UUV model, is obtained next step actual speed of UUV by PID speed control；

Step 5: next step actual speed of UUV step 4 obtained { is converted to fixed coordinate system { under E}, then integration, to obtain next step the physical location of UUV, according to obtaining physical location, UUV is controlled from hull coordinate system B}；

Step 6: by the physical location of UUV compared with the desired locations that UUV motion planning exports, the position error term obtained, as the site error input of excitation model biological in next step step 3, proceeds to step 3.

In described step one, UUV fixed coordinate system under E} and hull coordinate system under B}, the relation between speed is:

Wherein, u, v and w respectively UUV is at hull coordinate system { three linear velocity components under B}；R be UUV hull coordinate system angular velocity component corresponding to angle with bow under B}, and ψ be bow to angle, ψ_dFor expectation bow to angle；ξ, η and ζ respectively UUV is at fixed coordinate system { three location components, UUV current kinetic position η=[ξ η ζ] under E}^T, η_d=[ξ_dη_dζ_d]^TFor UUV desired motion position；The site error of UUV is e=[e_ξe_ηe_ζe_ψ]^T, e_ξ、e_ηAnd e_ζIt is illustrated respectively in the error on three positions of ξ, η and ζ, e_ψFor bow to angle error, represent derivation.

In described two, according to the relation between the UUV speed that step one obtains, Backstepping is utilized to obtain pseudo-velocity:

Wherein_,k、k_ζAnd k_ψIt is the constant more than 0, U_d=[u_dv_dw_dr_d]^TFor UUV at the hull coordinate system { desired speed under B}.

In described step 3, according to described new site error, next step pseudo-velocity in step 2 is processed, it is thus achieved that the method for smooth continuous print pseudo-velocity is:

New site error is replaced the position error term in next step pseudo-velocity, thus obtaining smooth continuous print pseudo-velocity.

Described biological excitation model is:

Wherein f (e_Δ)=max (e_Δ, 0), g (e_Δ)=max (-e_Δ, 0)；The error term of the output under biological excitation model meaning input condition in office always remains in [-D, B] scope；e_iInput for biological excited modes type, it may be assumed that current site error, e_Δ=0-e_iFor the error of current site error, V_iOutput for biological excited modes type, it may be assumed that new site error, i=ζ, η, ξ, ψ:

Parameter in A, B and D respectively biological excitation model；

Smooth continuous print pseudo-velocity U '_c:

The beneficial effects of the present invention is, the UUV pseudo-velocity that the present invention obtains is based on biological excitation model, it is analyzed UUV motion model obtaining UUV site error to the UUV impact navigated by water, smoothness properties in conjunction with biological excitation model, site error combination model is smoothed by the demand controlling speed according to reality, finally gives the pseudo-velocity with smooth continuation property.The present invention is directed to the speed instability problem of traditional PI d system control generation to study, make the UUV speed of a ship or plane be controlled, and also be able to remain stable for performance under ocean current interference, reduce noise after adding biology excitation model, system will not be shaken.

Accompanying drawing explanation

Fig. 1 is UUV vertical coordinate system schematic diagram in detailed description of the invention.

Fig. 2 is UUV level coordinates system schematic diagram in detailed description of the invention.

Fig. 3 is the hull coordinate system { principle schematic of B}.

Fig. 4 is the principle schematic of the UUV pseudo-velocity control method encouraging model in detailed description of the invention based on biology.

Detailed description of the invention

Detailed description of the invention one: the UUV pseudo-velocity control method encouraging model based on biology described in present embodiment, is described in further detail present embodiment below with reference to Fig. 1 to Fig. 4.The described UUV pseudo-velocity control method based on biology excitation model comprises the steps:

Step one: set up speed nonlinear equation during drive lacking UUV three-dimensional space motion according to kinematics model, obtains UUV at fixed coordinate system { under E} and hull coordinate system { relation between speed under B}；

For ease of analyzing, UUV navigation model is made hypothesis below by present embodiment: (1) UUV speed is nonnegative value, is namely left out speed controlling during backward；(2) impact of roll motion is ignored；(3) the UUV object considered is symmetrical.In UUV motion analysis and maneuverability test, it is generally divided into horizontal plane and carries out with vertical two aspect.Wherein horizontal plane analysis is mainly used in investigating the UUV speed of a ship or plane and gyration stability, and vertical analysis is then mainly investigated UUV depthkeeping, deepened performance.Fig. 1 and Fig. 2 describes coordinate system used when UUV three-dimensional space motion is analyzed.Wherein, E_ξηζFor the earth fixed coordinate system, with the E center for the earth fixed coordinate system, ξ, η and ζ represent the coordinate that three axles under the earth fixed coordinate system are corresponding respectively.B_XYZFor UUV hull coordinate system, as it is shown on figure 3, with the B center for hull coordinate system, p, q and the r respectively UUV angular velocity vector three components under hull coordinate system；G is UUV center of gravity, and in coordinate system, { under B}, its coordinate is R_G=(x_G, 0,0)^T；The aggregate velocity U=[uvw] of UUV^T, χ and γ is UUV snorkeling angle and flight-path angle respectively；α and β is the UUV angle of attack and drift angle respectively；ψ is that bow is to angle；Due to present embodiment it is considered that approximately level UUV navigation time, so ignoring Angle of HeelWith Angle of Trim θ, namely

UUV hull coordinate system B_XYZRelevant parameter implication is as shown in table 1.

Table 1UUV kinematic variables

UUV the space motion equation utilizes speed to U=[uvw]^TThe form represented is as follows:

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\ξ}}=Ucos\mathrm{\γ}cos\mathrm{\χ}\\ \stackrel{\·}{\mathrm{\η}}=Usin\mathrm{\γ}cos\mathrm{\χ}\\ \stackrel{\·}{\mathrm{\ζ}}=Usin\mathrm{\γ}\end{array}\right.---\left(1\right)$

Wherein, u, v, w respectively UUV is at hull coordinate system { 3 velocity components under B}.

If fixing mark system of settling down E} to hull coordinate system the coordinate spin matrix between B} is S, then:

$\left[\begin{array}{c}\mathrm{\ξ}\\ \mathrm{\η}\\ \mathrm{\ζ}\end{array}\right]=S\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=\left[\begin{array}{ccc}cos\mathrm{\ψ}& -sin\mathrm{\ψ}& 0\\ sin\mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\end{array}\right]---\left(2\right)$

Wherein, { position under B}, ξ, η, ζ respectively UUV is in fixing mark system of settling down { position under E} in hull coordinate system for x, y, z respectively UUV.

It is located at hull coordinate system { under B}: the aggregate velocity U=[uvw] of UUV^T, angular velocity vector Ω=[pqr]^T, in hull coordinate system, { B}3 angular velocity component, r is that UUV is at hull coordinate system { angular velocity component corresponding to angle with bow under B} to p, q, r respectively UUV.In fixing mark system of settling down { under E}: the absolute speed of a ship or plane of UUVAngular velocity

Formula (2) is carried out derivation can obtain:

$\left[\begin{array}{c}\stackrel{\·}{\mathrm{\ξ}}\\ \stackrel{\·}{\mathrm{\η}}\\ \stackrel{\·}{\mathrm{\ζ}}\end{array}\right]=S\left[\begin{array}{c}u\\ v\\ w\end{array}\right]=\left[\begin{array}{ccc}cos\mathrm{\ψ}& -sin\mathrm{\ψ}& 0\\ sin\mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}u\\ v\\ w\end{array}\right]---\left(3\right)$

And have:

Define in fixing mark system of settling down { under E}: the site error of Near-surface motion UUV is e=η_d-η=[e_ξe_ηe_ζ]^T, its derivation can be obtained:

$\stackrel{\·}{e}={\stackrel{\·}{\mathrm{\η}}}_d-\stackrel{\·}{\mathrm{\η}}=\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\ξ}}}_d-\stackrel{\·}{\mathrm{\ξ}}\\ {\stackrel{\·}{\mathrm{\η}}}_d-\stackrel{\·}{\mathrm{\η}}\\ {\stackrel{\·}{\mathrm{\ζ}}}_d-\stackrel{\·}{\mathrm{\ζ}}\end{array}\right]=\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\ξ}}}_d-ucos\mathrm{\ψ}+vsin\mathrm{\ψ}\\ {\stackrel{\·}{\mathrm{\η}}}_d-usin\mathrm{\ψ}-vcos\mathrm{\ψ}\\ {\stackrel{\·}{\mathrm{\ζ}}}_d-w\end{array}\right]---\left(4\right)$

Wherein, UUV current kinetic position η=[ξ η ζ]^T, η_d=[ξ_dη_dζ_d]^TFor UUV desired motion position, e_ξ、e_ηAnd e_ζIt is illustrated respectively in the error on three positions of ξ, η and ζ, e_ψFor bow to angle error, represent derivation.UUV can be obtained at fixed coordinate system { under E} and hull coordinate system { relation between speed under B} by Fig. 2

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\ξ}}=ucos\mathrm{\ψ}-vsin\mathrm{\ψ}\\ \stackrel{\·}{\mathrm{\η}}=usin\mathrm{\ψ}+cos\mathrm{\ψ}\\ \stackrel{\·}{w}=w\\ \stackrel{\·}{\mathrm{\ψ}}=r\end{array}\right.---\left(5\right)$

Then have:

$\left[\begin{array}{c}{\stackrel{\·}{e}}_{\mathrm{\ξ}}\\ {\stackrel{\·}{e}}_{\mathrm{\η}}\\ {\stackrel{\·}{e}}_{\mathrm{\ζ}}\\ {\stackrel{\·}{e}}_{\mathrm{\ψ}}\end{array}\right]={\stackrel{\·}{\mathrm{\η}}}_d-\stackrel{\·}{\mathrm{\η}}=\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\ξ}}}_d-\stackrel{\·}{\mathrm{\ξ}}\\ {\stackrel{\·}{\mathrm{\η}}}_d-\stackrel{\·}{\mathrm{\η}}\\ {\stackrel{\·}{\mathrm{\ζ}}}_d-\stackrel{\·}{\mathrm{\ζ}}\\ {\stackrel{\·}{\mathrm{\ψ}}}_d-\stackrel{\·}{\mathrm{\ψ}}\end{array}\right]---\left(6\right)$

ψ be bow to angle, ψ_dFor expectation bow to angle；

Step 2: according to the relation between the UUV speed that step one obtains, obtains pseudo-velocity:

According to formula (5), according to Backstepping, it is thus achieved that pseudo-velocity:

$U_c=\left[\begin{array}{c}k(e_{\mathrm{\ξ}}cos\mathrm{\ψ}+e_{\mathrm{\η}}sin\mathrm{\ψ})+(u_d{\mathrm{cose}}_{\mathrm{\ψ}}-v_d\sin e_{\mathrm{\ψ}})\\ k(-e_{\mathrm{\ξ}}sin\mathrm{\ψ}+e_{\mathrm{\η}}cos\mathrm{\ψ})+(u_d\sin e_{\mathrm{\ψ}}+v_d\cos e_{\mathrm{\ψ}})\\ w_d+k_{\mathrm{\ζ}}{e}_{\mathrm{\ζ}}\\ r_d+k_{\mathrm{\ψ}}{e}_{\mathrm{\ψ}}\end{array}\right]---\left(7\right)$

Wherein, k, k_ζAnd k_ψIt is the constant more than 0, U_d=[u_dv_dw_dr_d]^TFor UUV at hull coordinate system { desired speed under B}, u_dcose_ψ-v_dsine_ψ, u_dsine_ψ+v_dcose_ψConversion for expectation speed coordinate to actual speed coordinate.

Step 3: the current extremely biological excitation model of site error input is carried out smooth process continuously, obtain new site error, according to described new site error, the pseudo-velocity in step 2 is processed, obtain smooth continuous print pseudo-velocity: in present embodiment, pseudo-velocity designs the biological excitation model of module application, is most important feature in present embodiment.Biological excitation model has smooth response, it is suppressed that controls the effect of the output upper limit and lower limit, and has the characteristic of globally consistent asymptotically stability.

The present invention applies the biology excitation model of following simplification:

$\stackrel{\·}{V}=-AV+(B-V)S{\left(t\right)}^{+}-(D+V)S{\left(t\right)}^{-}---\left(8\right)$

Wherein, V is neuronic excitement levels, A, B and D be the attenuation rate of neuronal excitation degree, the upper limit and lower limit respectively.S⁺It is neuronal excitability and inhibition input with S-.

Further formula (8) is expressed as the neuronic form of multichannel:

${\stackrel{\·}{V}}_i=-{\mathrm{AV}}_i+(B-V_i)f\left(e_{\mathrm{\Δ}}\right)-(D+V_i)g\left(e_{\mathrm{\Δ}}\right)---\left(9\right)$

Wherein f (e_Δ)=max (e_Δ, 0), g (e_Δ)=max (-e_Δ, 0)；The error term of the output under biological excitation model meaning input condition in office always remains in [-D, B] scope；e_iInput for biological excited modes type, it may be assumed that current site error, e_Δ=0-e_iFor the error of current site error, V_iOutput for biological excited modes type, it may be assumed that new site error, i=ζ, η, ξ, ψ:

Adjust parameter A, B and D combined effect and determine the speed of UUV error convergence, be i.e. the speed of UUV pursuit path convergence.A value is more little, and the time of the progressive desired trajectory of UUV is more short, and the scope of velocity variations is relatively more big；B and D and A has similar effect, but the value of value B and D is more little, and the time of the progressive desired trajectory of UUV is more long, and the scope of velocity variations is relatively more big；

Position error term in the pseudo-velocity that UUV pseudo-velocity control strategy generates obtains smooth process continuously:.According to described new site error, next step pseudo-velocity in step 2 is processed, the method obtaining smooth continuous print pseudo-velocity is: new site error replaces the position error term in next step pseudo-velocity, thus obtaining smooth continuous print pseudo-velocity.Smooth continuous print pseudo-velocity is:

$U_c^{\′}=\left[\begin{array}{c}k(V_{\mathrm{\ξ}}cos\mathrm{\ψ}+V_{\mathrm{\η}}sin\mathrm{\ψ})+(u_d{\mathrm{cosV}}_{\mathrm{\ψ}}-v_d\sin V_{\mathrm{\ψ}})\\ k(-V_{\mathrm{\ξ}}sin\mathrm{\ψ}+V_{\mathrm{\η}}cos\mathrm{\ψ})+(u_d\sin V_{\mathrm{\ψ}}+v_d\cos V_{\mathrm{\ψ}})\\ w_d+k_z{V}_{\mathrm{\ζ}}\\ r_d+k_{\mathrm{\ψ}}{V}_{\mathrm{\ψ}}\end{array}\right]---\left(10\right)$

UUV is according to motion planning and deviations of actual position e, strategy of speed control basis is designed UUV pseudo-velocity, owing to pseudo-velocity is subject to error impact, under ocean current disturbs, this pseudo-velocity can fluctuate within the specific limits, PID speed control is caused frequently to respond, thus causing that UUV speed over ground occurs that altofrequency fluctuates, adding UUV self-noise, decreasing angle of rake service life.For solving this problem, in pseudo-velocity designs, introduce biology excitation model, utilize the continuously smooth of this model and the output characteristics of bounded to design smooth continuous print pseudo-velocity U '_c；

Step 4: pseudo-velocity that step 3 is generated, current speed and the currently practical speed of a ship or plane of UUV subtract each other after acquisition velocity error as the input of PID speed control, output action on UUV model, is obtained next step actual speed of UUV by PID speed control；

Step 5: next step actual speed of UUV step 4 obtained { is converted to fixed coordinate system { under E}, then integration, to obtain next step the physical location of UUV, according to obtaining physical location, UUV is controlled from hull coordinate system B}；

Step 6: by the physical location of UUV compared with the desired locations that UUV motion planning exports, the position error term obtained, as the site error input of excitation model biological in next step step 3, proceeds to step 3.

Claims (5)

1. the UUV pseudo-velocity control method encouraging model based on biology, it is characterised in that described method comprises the steps:

Step one: set up speed nonlinear equation during drive lacking UUV three-dimensional space motion according to kinematics model, obtains UUV at fixed coordinate system { under E} and hull coordinate system { relation between speed under B}；

Step 2: according to the relation between the UUV speed that step one obtains, obtains pseudo-velocity；

Step 3: the current extremely biological excitation model of site error input is carried out smooth process continuously, it is thus achieved that new site error, according to described new site error, the pseudo-velocity in step 2 is processed, it is thus achieved that smooth continuous print pseudo-velocity；

Step 4: pseudo-velocity that step 3 is generated, current speed and the currently practical speed of a ship or plane of UUV subtract each other after acquisition velocity error as the input of PID speed control, output action on UUV model, is obtained next step actual speed of UUV by PID speed control；

Step 5: next step actual speed of UUV step 4 obtained { is converted to fixed coordinate system { under E}, then integration, to obtain next step the physical location of UUV, according to obtaining physical location, UUV is controlled from hull coordinate system B}；

Step 6: by the physical location of UUV compared with the desired locations that UUV motion planning exports, the position error term obtained, as the site error input of excitation model biological in next step step 3, proceeds to step 3.

2. according to claim 1 based on biology encourage model UUV pseudo-velocity control method, it is characterised in that in described step one, UUV fixed coordinate system under E} and hull coordinate system under B}, the relation between speed is:

Wherein, u, v and w respectively UUV is at hull coordinate system { three linear velocity components under B}；R be UUV hull coordinate system angular velocity component corresponding to angle with bow under B}, and ψ be bow to angle, ψ_dFor expectation bow to angle；ξ, η and ζ respectively UUV is at fixed coordinate system { three location components, UUV current kinetic position η=[ξ η ζ] under E}^T, η_d=[ξ_dη_dζ_d]^TFor UUV desired motion position；The site error of UUV is e=[e_ξe_ηe_ζe_ψ]^T, e_ξ、e_ηAnd e_ζIt is illustrated respectively in the error on three positions of ξ, η and ζ, e_ψFor bow to angle error, represent derivation.

3. the UUV pseudo-velocity control method encouraging model based on biology according to claim 2, it is characterised in that in described two, according to the relation between the UUV speed that step one obtains, utilizes Backstepping to obtain pseudo-velocity:

Wherein, k, k_ζAnd k_ψIt is the constant more than 0, U_d=[u_dv_dw_dr_d]^TFor UUV at the hull coordinate system { desired speed under B}.

4. the UUV pseudo-velocity control method encouraging model based on biology according to claim 1, it is characterized in that, in described step 3, according to described new site error, next step pseudo-velocity in step 2 is processed, it is thus achieved that the method for smooth continuous print pseudo-velocity is:

New site error is replaced the position error term in next step pseudo-velocity, thus obtaining smooth continuous print pseudo-velocity.

5. the UUV pseudo-velocity control method encouraging model based on biology according to claim 1 or 4, it is characterised in that described biological excitation model is:

Wherein f (e_Δ)=max (e_Δ,0),g(e_Δ)=max (-e_Δ,0)；The error term of the output under biological excitation model meaning input condition in office always remains in [-D, B] scope；e_iInput for biological excited modes type, it may be assumed that current site error, e_Δ=0-e_iFor the error of current site error, V_iOutput for biological excited modes type, it may be assumed that new site error, i=ζ, η, ξ, ψ:

Parameter in A, B and D respectively biological excitation model；

Smooth continuous print pseudo-velocity U '_c: