CN104678762A - UUV (Unmanned Underwater Vehicle) fault-tolerant control system based on thruster faults - Google Patents

Abstract

The invention relates to a UUV (Unmanned Underwater Vehicle) fault-tolerant control system based on thruster faults. The UUV fault-tolerant control system comprises a UUV motion model, a horizontal plane motion measuring system, a fault model, a strong tracking filer and a diagnosis module. The system is operated in the following mode: (1) establishing the UUV motion model; (2) simplifying and linearizing a horizontal plane motion equation; (3) establishing a UUV main thruster fault model; (4) arranging the strong tracking filer; (5) diagnosing faults; (6) processing fault tolerance. According to the UUV fault-tolerant control system, the fault-tolerant control over UUV main thruster faults is mainly researched, so as to prevent the reduction of control performance and hazardous conditions caused by the thruster faults, and the UUV fault-tolerant control system can be used for improving the viability and operating performance of a UUV in complicated ocean environment.

Description

A kind of UUV fault-tolerant control system based on thruster fault

Technical field

What the present invention relates to is a kind of fault-tolerant control system, is related specifically to a kind of UUV fault-tolerant control system based on thruster fault.

Background technology

UAV navigation (Unmanned Underwater Vehicle mono-UUV) plays vital effect in the development space that this block human future of seabed is valuable.Along with the development of UUV and correlation technique, UUV has been used to perform the tasks such as clearance, scouting, information gathering and hydrospace detection, also may use as the equipment such as Underwater Battery platform, logistic support platform in future naval battle.

Control system is a ring very important in UAV navigation system, and its stability is the prerequisite ensureing UUV safe operation.Faults-tolerant control can make system in case of a fault, the performance of the impact that auto-compensation fault produces with the stability of maintenance system and as far as possible before recovery system fault.Therefore, faults-tolerant control is the last line of defense of system safety operation, in UUV technology, have important realistic meaning.

The invention provides the fault tolerant control method in UUV sensor fault situation, prevent because thruster fault causes the decline of control performance and the generation of unsafe condition, can be used for improving the viability of UUV under complicated marine environment and exercise performance.

Summary of the invention

The object of the present invention is to provide a kind of UUV fault-tolerant control system based on thruster fault, effectively improve UUV reliability.

The object of the present invention is achieved like this:

Based on a UUV fault-tolerant control system for thruster fault, its composition comprises: UUV motion model, horizontal plane motion calculating system, fault model, strong tracking filfer, diagnostic module.

Described a kind of UUV fault-tolerant control system based on thruster fault, is characterized in that described fault diagnosis is by the strong tracking filfer under design three groups of UUV states: fault model is promoted mainly on a left side, fault model is promoted mainly on the right side and UUV proper motion model.In order to obtain three groups of residual values like this, compare actual condition value and the state estimation of these three groups of strong tracking filfer, then contrast residual values and just can analyze immediately and obtain fault and which occurs in promote mainly.

The described UUV fault-tolerant control system based on thruster fault, it is characterized in that described fault-tolerant processing is when a main thruster fault, in order to keep the normal operation of the correct of other operational factors of UUV and UUV, just must stop the work of fault main thruster, add the normal operation of the control realization UUV that rudder substitutes with a main thruster.

Specifically comprise following step:

1 sets up UUV motion model;

2 simplify and linearization horizontal plane motion equation;

3 set up UUV main thruster fault model;

4 strong tracking filfer;

5 fault diagnosises;

6 fault-tolerant processing.

Accompanying drawing explanation

Fig. 1 is that earth-magnetic navigation reference map builds process flow diagram;

Fig. 2 UUV fault diagnosis structure;

Fig. 3 UUV fault model speed residual error;

Fig. 4 UUV fault model angular velocity residual error;

The essence of technical scheme of the present invention is: set up strong tracking filfer respectively to the clear condition of UUV normal condition and each thruster fault, is carried out fault diagnosis by the feature residual error of the estimated value and actual value of analyzing each strong tracking filfer and is carried out fault-tolerant processing.

The advantage of this technical scheme is:

For real system parameter variations, there is stronger robustness;

Initial value sensitivity for various noise and statistical property is lower;

Extremely strong tracking power is had to catastrophe, and when steady state (SS), still keeps the tracking power to soft phase and mutation status.

Embodiment

Step 1: set up UUV motion model:

Consider the situation of UUV three degree of freedom:

1 axially-movable equation

$m[\stackrel{\·}{u}-(v-v_c)r+(w-w_c)q]=\frac{1}{2}{\mathrm{\ρL}}^4[X_{\mathrm{qq}}^{\′}{q}^2+X_{\mathrm{rr}}^{\′}{r}^2+X_{\mathrm{pr}}^{\′}\mathrm{pr}]$

$+\frac{1}{2}\mathrm{\ρ}{L}^3[X_{\stackrel{\·}{u}}^{\′}\stackrel{\·}{u}+X_{\mathrm{vr}}^{\′}(v-v_c)r+X_{\mathrm{wq}}^{\′}(w-w_c)q]$

$+\frac{1}{2}\mathrm{\ρ}{L}^2[X_{\mathrm{uu}}^{\′}{(u-u_c)}^2+X_{\mathrm{vv}}^{\′}{(v-v_c)}^2+X_{\mathrm{ww}}^{\′}{(w-w_c)}^2---\left(1\right)$

$+X_{\mathrm{uw}}^{\′}(u-u_c)(w-w_c)+X_{\mathrm{uv}}^{\′}(u-u_c)\left|(v-v_c)\right|]$

$-(W-B)\sin \mathrm{\θ}+\frac{1}{2}\mathrm{\ρ}{L}^2{(u-u_c)}^2[X_{{\mathrm{\δ}}_r{\mathrm{\δ}}_r}^{\′}{\mathrm{\δ}}_r^2+X_{{\mathrm{\δ}}_s{\mathrm{\δ}}_s}^{\′}{\mathrm{\δ}}_s^2]+X_{\mathrm{prop}}$

2 transverse movement equations

$m[\stackrel{\·}{v}+(u-u_c)r-(w-w_c)p]=\frac{1}{2}\mathrm{\ρ}{L}^4[Y_{\stackrel{\·}{r}}^{\′}\stackrel{\·}{r}+Y_{\stackrel{\·}{p}}^{\′}\stackrel{\·}{p}+Y_{p\left|p\right|}^{\′}p|p|+Y_{\mathrm{pq}}^{\′}\mathrm{pq}+Y_{\mathrm{qr}}^{\′}\mathrm{qr}+Y_{r\left|r\right|}^{\′}r|r\left|\right]$

$+\frac{1}{2}\mathrm{\ρ}{L}^3[Y_{\stackrel{\·}{v}}^{\′}\stackrel{\·}{v}+Y_{\mathrm{ur}}^{\′}(u-u_c)r+Y_{\mathrm{vq}}^{\′}(v-v_c)q+Y_{\mathrm{wp}}^{\′}(w-w_c)p+Y_{\mathrm{wr}}^{\′}(w-w_c)r$

$+Y_{v\left|r\right|}^{\′}\frac{v}{\left|v\right|}\sqrt{{(v-v_c)}^2+{(w-w_c)}^2}\left|r\right|]---\left(2\right)$

$+\frac{1}{2}\mathrm{\ρ}{L}^2[Y_{\mathrm{uu}}^{\′}{(u-u_c)}^2+Y_{\mathrm{uv}}^{\′}(u-u_c)(v-v_c)+Y_{\mathrm{vw}}^{\′}(v-v_c)(w-w_c)$

$+Y_{v\left|v\right|}^{\′}(v-v_c)\sqrt{{(v-v_c)}^2+{(w-w_c)}^2}]$

3 yawing rotation equations

$I_z\stackrel{\·}{r}(I_y-I_x)\mathrm{pq}=\frac{1}{2}\mathrm{\ρ}{L}^5[N_{\stackrel{\·}{r}}^{\′}\stackrel{\·}{r}+N_{\stackrel{\·}{p}}^{\′}\stackrel{\·}{p}+N_{\left|p\right|p}^{\′}|p|p+N_{\left|r\right|r}^{\′}|r|r+N_{\mathrm{pq}}^{\′}\mathrm{pq}+N_{\mathrm{qr}}^{\′}\mathrm{qr}]$

$+\frac{1}{2}\mathrm{\ρ}{L}^4[N_{\stackrel{\·}{v}}^{\′}\stackrel{\·}{v}+N_{\mathrm{up}}^{\′}(u-u_c)p+N_{\mathrm{ur}}^{\′}(u-u_c)r+N_{\mathrm{wr}}^{\′}(w-w_c)r$

$+N_{\mathrm{wp}}^{\′}(w-w_c)p+N_{\mathrm{vq}}^{\′}(v-v_c)q$

$+N_{\left|v\right|r}^{\′}\sqrt{{(v-v_c)}^2+{(w-w_c)}^{2}r}---\left(3\right)$

$+\frac{1}{2}\mathrm{\ρ}{L}^3[N_{\mathrm{uu}}^{\′}{(u-u_c)}^2+N_{\mathrm{uv}}^{\′}(u-u_c)(v-v_c)+N_{\mathrm{vw}}^{\′}(v-v_c)(w-w_c)r$

$+N_{\left|v\right|v}^{\′}(v-v_c)\sqrt{{(v-v_c)}^2+{(w-w_c)}^2}]$

Wherein, m is that UUV is corresponding to the quality that water discharge is corresponding entirely under water; U is longitudinal velocity; V is transverse velocity; W is vertical velocity; P is heeling angle speed; Q is pitch velocity; R is yaw rate; Ψ, θ, be respectively pitching, rolling, course attitude angle; X, Y, Z are respectively as acting on making a concerted effort of various external force on UUV, and N is the moment relative to true origin acted on UUV, and subscript c represents calculated value.

Step 2: simplify and linearization horizontal plane motion equation:

$(m-\frac{1}{2}\mathrm{\ρ}{L}^3{X}_{\stackrel{\·}{u}}^{\′})\stackrel{\·}{u}=-\frac{1}{2}\mathrm{\ρ}{L}^2{X}_{\mathrm{uu}}^{\′}{u}_0^2+\mathrm{\ρ}{L}^2{X}_{\mathrm{uu}}^{\′}{u}_{0}u+X_{\mathrm{prop}}---(4-a)$

$(m-\frac{1}{2}\mathrm{\ρ}{L}^3{Y}_{\stackrel{\·}{v}}^{\′})\stackrel{\·}{v}-\frac{1}{2}\mathrm{\ρ}{L}^4{Y}_{\stackrel{\·}{r}}^{\′}\stackrel{\·}{r}=\frac{1}{2}\mathrm{\ρ}{L}^2{Y}_{\mathrm{uv}}^{\′}{u}_{0}v+(\frac{1}{2}\mathrm{\ρ}{L}^3{Y}_{\mathrm{ur}}^{\′}{u}_0-m{u}_0)r+Y_{\mathrm{prop}}---(4-b)$

$(m-\frac{1}{2}\mathrm{\ρ}{L}^3{N}_{\stackrel{\·}{v}}^{\′})\stackrel{\·}{v}+(I_z-\frac{1}{2}\mathrm{\ρ}{L}^5{N}_{\stackrel{\·}{r}}^{\′})\stackrel{\·}{r}=\frac{1}{2}\mathrm{\ρ}{L}^3{N}_{\mathrm{uv}}^{\′}{u}_{0}v+\frac{1}{2}\mathrm{\ρ}{L}^4{N}_{\mathrm{ur}}^{\′}{u}_{0}r+N_{\mathrm{prop}}---(4-c)$

If:

$X_{\stackrel{\·}{u}}=\frac{1}{2}\mathrm{\ρ}{L}^3{X}_{\stackrel{\·}{u}}^{\′},X_{\mathrm{uu}}=\frac{1}{2}\mathrm{\ρ}{L}^2{X}_{\mathrm{uu}}^{\′},Y_{\stackrel{\·}{v}}=\frac{1}{2}\mathrm{\ρ}{L}^3{Y}_{\stackrel{\·}{v}}^{\′},Y_{\stackrel{\·}{r}}^{\′}=\frac{1}{2}\mathrm{\ρ}{L}^4{Y}_{\stackrel{\·}{r}}^{\′}$

$Y_{\mathrm{uv}}=\frac{1}{2}\mathrm{\ρ}{L}^2{Y}_{\mathrm{uv}}^{\′},Y_{\mathrm{ur}}=\frac{1}{2}\mathrm{\ρ}{L}^3{Y}_{\mathrm{ur}}^{\′},N_{\stackrel{\·}{v}}=\frac{1}{2}\mathrm{\ρ}{L}^3{N}_{\stackrel{\·}{v}}^{\′},N_{\stackrel{\·}{r}}=\frac{1}{2}\mathrm{\ρ}{L}^5{N}_{\stackrel{\·}{r}}^{\′}---\left(5\right)$

$(m-X_{\stackrel{\·}{u}})\stackrel{\·}{u}={-X}_{\mathrm{uu}}{u}_0^2+2X_{\mathrm{uu}}{u}_{0}u+X_{\mathrm{prop}}$

$(m-Y_{\stackrel{\·}{v}})\stackrel{\·}{v}-Y_{\stackrel{\·}{r}}\stackrel{\·}{r}=Y_{\mathrm{uv}}{u}_{0}v+(Y_{\mathrm{ur}}{u}_0-{\mathrm{mu}}_0)r+Y_{\mathrm{prop}}---\left(6\right)$

$(m-N_{\stackrel{\·}{v}})\stackrel{\·}{v}+(I_z-N_{\stackrel{\·}{r}})\stackrel{\·}{r}=N_{\mathrm{uv}}{u}_{0}v+n_{\mathrm{ur}}{u}_{0}r+N_{\mathrm{prop}}$

Further horizontal plane motion equation is rewritten as:

$\stackrel{\·}{u}=\frac{X_f}{A}---\left(7\right)$

$\stackrel{\·}{v}=\frac{B\·Y_f+C\·N_f}{P}---\left(8\right)$

$\stackrel{\·}{r}=\frac{D\·Y_f+E\·N_f}{P}---\left(9\right)$

Wherein:

$U=m-X_{\stackrel{\·}{u}},B=N_{\stackrel{\·}{v}},C=m-Y_{\stackrel{\·}{v}},D=Y_{\stackrel{\·}{r}},E=I_z-N_{\stackrel{\·}{r}}---\left(10\right)$

$P=(m-Y_{\stackrel{\·}{v}})(I_z-N_{\stackrel{\·}{r}})-N_{\stackrel{\·}{v}}{Y}_{\stackrel{\·}{r}}$

For formula (6)

$X_f=-X_{\mathrm{uu}}{u}_0^2+2X_{\mathrm{uu}}{u}_{0}u+X_{\mathrm{prop}}$

Y _f=Y _uvu ₀v+(Y _uru ₀-mu ₀)r+Y _prop(11)

N _f=N _uvu ₀v+N _uru ₀r+N _prop

Getting state variable is:

x=[u v r] ^T(12)

Getting output variable is:

y=[u v r] ^T(13)

System input variable is:

u＝[X _propY _propN _prop] ^T(14)

If formula (7) ~ (9) discretize obtains for then adopting Euler method by the sampling period of system:

$\left\{\begin{array}{c}{x}_1(k+1)=x_1\left(k\right)+T\frac{X_f\left(k\right)}{A}\\ x_2(k+1)=x_2\left(k\right)+T\frac{B\·Y_f\left(k\right)+C\·N_f\left(k\right)}{P}\\ x_3(k+1)=x_3\left(k\right)+T\frac{D\·Y_f\left(k\right)+E\·N_f\left(k\right)}{P}\end{array}\right.---\left(15\right)$

The discrete form of output equation is:

$y(k+1)=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]x(k+1)+e(k+1)---\left(16\right)$

$X_f\left(k\right)=-X_{\mathrm{uu}}{x}_{1o}^2+2X_{\mathrm{uu}}{x}_{1o}{x}_1\left(k\right)+u_1\left(k\right)---\left(17\right)$

Y _f(k)＝Y _uvx _1ox ₂(k)+(Y _urx _1o-mx _1o)x ₃(k)+u ₂(k) (18)

N _f(k＝N _uvx _1ox ₂(k)+N _urx _1ox ₃(k)+u ₃(k) (19)

E (k) is zero mean Gaussian white noise sequence, if its variance matrix is R (k).Formula (15) ~ (16) also can be expressed as:

$\left\{\begin{array}{c}x(k+1)=f(k,u\left(k\right),x\left(k\right))\\ y(k+1)=\mathrm{Hx}(k+1)+e(k+1)\end{array}\right.---\left(20\right)$

Wherein:

$f(k,u\left(k\right),x\left(k\right))=\left[\begin{array}{c}{x}_1\left(k\right)+T\frac{X_f\left(k\right)}{A}\\ x_2\left(k\right)+T\frac{B\·Y_f\left(k\right)+C\·N_f\left(k\right)}{P}\\ x_3\left(k\right)+T\frac{D\·Y_f\left(k\right)+C\·N_f\left(k\right)}{P}\end{array}\right]---\left(21\right)$

$H=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]---\left(22\right)$

Use in SFEKF:

$F(k,u\left(k\right),\hat{x}\left(k\right|k))=\frac{\∂f(k,u\left(k\right),x\left(k\right))}{\∂x}{|}_{x\left(k\right)=\hat{x}\left(k\right|k)}---\left(23\right)$

Order:

$F(k,u\left(k\right),\hat{x}\left(k\right|k))={\left|\left[\begin{array}{ccc}{F}_{11}& F_{12}& F_{13}\\ F_{21}& F_{22}& F_{23}\\ F_{31}& F_{32}& F_{33}\end{array}\right]\right|}_{x=\hat{x}\left(k\right|k)}---\left(24\right)$

? in each element as follows:

$F_{11}=1+2\frac{T}{A}{X}_{\mathrm{uu}}{x}_{1o};F_{12}=0;F_{13}=0;$

$F_{21}=0;F_{22}=1+\frac{T}{P}(B{Y}_{\mathrm{uv}}{x}_{1o}+C{N}_{\mathrm{uv}}{x}_{1o});F_{23}=\frac{T}{P}[B(Y_{\mathrm{ur}}{x}_{1o}-{\mathrm{mx}}_{1o})+{\mathrm{CN}}_{\mathrm{ur}}{x}_{1o}]---\left(25\right)$

$F_{31}=0;F_{32}=\frac{T}{P}(D{Y}_{\mathrm{uv}}{x}_{1o}+{\mathrm{EN}}_{\mathrm{uv}}{x}_{1o});F_{33}=1+\frac{T}{P}[D(Y_{\mathrm{ur}}{x}_{1o}-{\mathrm{mx}}_{1o})+{\mathrm{EN}}_{\mathrm{ur}}{x}_{1o}]$

Step 3: set up UUV main thruster fault model:

1) longitudinal thrust

$\left\{\begin{array}{c}{X}_{\mathrm{TL}}=T_L\\ X_{\mathrm{TR}}=T_R\end{array}\right.---\left(26\right)$

X _prop＝X _TL+X _TR

2) lateral thrust

$\left\{\begin{array}{c}{Y}_{\mathrm{TB}}=T_{\mathrm{BL}}\cos {\mathrm{\θ}}_{\mathrm{TBL}}-T_{\mathrm{BR}}{\cos \mathrm{\θ}}_{\mathrm{TBR}}\\ Y_{\mathrm{TS}}=T_{\mathrm{SL}}{\cos \mathrm{\θ}}_{\mathrm{TSL}}-T_{\mathrm{SR}}\cos {\mathrm{\θ}}_{\mathrm{\φ}}\end{array}\right.---\left(27\right)$

Y _prop＝Y _TB+Y _TS

3) bow moment of thrust is turned

$\left\{\begin{array}{c}{N}_{\mathrm{TL}}=T_L\frac{B_{\mathrm{TLR}}}{2}\\ N_{\mathrm{TR}}=-T_R\frac{B_{\mathrm{TLR}}}{2}\\ N_{\mathrm{TB}}=T_{\mathrm{BL}}{\cos \mathrm{\θ}}_{\mathrm{TBL}}{x}_{\mathrm{TBLR}}-T_{\mathrm{BR}}{\cos \mathrm{\θ}}_{\mathrm{TBR}}{x}_{\mathrm{TBLR}}\\ N_{\mathrm{TS}}=-T_{\mathrm{SL}}{\cos \mathrm{\θ}}_{\mathrm{TSL}}{x}_{\mathrm{TSLR}}+T_{\mathrm{SR}}{\cos \mathrm{\θ}}_{\mathrm{TSR}}{x}_{\mathrm{TSLR}}\end{array}\right.---\left(28\right)$

N _prop＝N _TL+N _TR+N _TB+N _TS

T in formula _l, T _rrepresent that thrust is promoted mainly in left and right respectively; T _bL, T _bR, T _sL, T _sRrepresent that bow is left, bow is right, stern is left, stern inside right forward thrust respectively; θ is the auxiliary angle pushed away between slurry axle and midship vertical plane, x _tBLR, x _tSLRrepresent that auxiliary the pushing away of bow auxiliaryly with stern pushes away the lengthwise position of starching axle respectively.

If the thrust that each degree of freedom is expected is: , when not having Actuator failure, each thruster actual thrust is equal with expecting the thrust sent, like this:

$\left\{\begin{array}{c}{X}_{\mathrm{prop}}=X_{\mathrm{prop}}^{*}\\ Y_{\mathrm{prop}}=Y_{\mathrm{prop}}^{*}\\ N_{\mathrm{prop}}=N_{\mathrm{prop}}^{*}\end{array}\right.---\left(29\right)$

If break down, position occurs in left main thruster, then T _l=0

$\left\{\begin{array}{c}{X}_{\mathrm{prop}}=0.5X_{\mathrm{prop}}^{*}\\ X_{\mathrm{prop}}=Y_{\mathrm{prop}}^{*}\\ N_{\mathrm{prop}}=N_{\mathrm{prop}}^{*}-N_{\mathrm{TL}}\end{array}\right.---\left(30\right)$

Position of breaking down occurs in right main thruster and has similar situation.

Step 4: strong tracking filfer:

Strong tracking filfer has following general structure:

$\hat{x}(k+1|k+1)=\hat{x}(k+1|k)+K(k+1)\mathrm{\γ}(k+1)---\left(31\right)$

Wherein:

$\hat{x}(k+1\}k)=f(k,u\left(k\right),\hat{x}\left(k\right|k))$

$\mathrm{\γ}(k+1)=y(k+1)-h(k+1,\hat{x}(k+1|k))---\left(32\right)$

Wherein, x ∈ R ⁿfor state vector, u ∈ R ^pfor input vector, y ∈ R ^mfor output vector, γ is residual sequence.F, h are nonlinear function, have the single order continuous offset derivative about state.

So we determine time-varying gain battle array K (k+1) online, the strong tracking characteristic of this wave filter just can be reached.Following orthogonality principle is proposed for this reason:

Orthogonality principle: make the adequate condition that wave filter (33) is strong tracking filfer be on-line selection time-varying gain battle array K (k+1), make:

$\left(1\right)E[x(k+1)-\hat{x}(k+1|k+1)]{[x(k+1)-\hat{x}(k+1|k+1)]}^T=\min ---\left(33\right)$

(2)E[γ(k+1+j)γ ^T(k+1)]=0，k=0，1，2…，j=1，2，… (34)

Its conditional (2) requires that residual sequence keeps mutually orthogonal everywhere, and condition (l) is exactly the performance index of EKF.The physical significance that this strong tracking filfer is very strong.If when it shows model uncertain, answer on-line tuning gain battle array K (k+1), cause like this and export the situation that residual error produces similar white Gaussian noise.This also shows to be extracted by all effective informations exported in residual error.When uncertainty does not exist in a model, strong tracking filfer belongs to normal condition, and (34) meet naturally, do not have regulating action.

In extended Kalman filter, introduce that the data of fading factor to the past fade is a kind of method constructing strong tracking filfer, can weaken the impact of old data on current filter value like this.In order to reach this target, we adjust the covariance matrix P (k+1|k) of state forecast error and corresponding gain battle array K (k+1).Prediction error conariance battle array in amendment EKF recursion formula:

$P(k+1|k)=F(k,u\left(k\right),\hat{x}\left(k\right|k)P\left(k\right|k)F^T(k,u\left(k\right),\hat{x}\left(k\right|k))+\mathrm{\Γ}\left(k\right)Q\left(k\right){\mathrm{\Γ}}^T\left(k\right)---\left(35\right)$

Then amended prediction error conariance battle array is:

$P(k+1|k)=\mathrm{\λ}(k+1)F(k,u\left(k\right),\hat{x}\left(k\right|k)P\left(k\right|k)F^T(k,u\left(k\right),\hat{x}\left(k\right|k))+\mathrm{\Γ}\left(k\right)Q\left(k\right){\mathrm{\Γ}}^T\left(k\right)---\left(36\right)$

Wherein, λ (k+1) >=1 for time become fading factor.Then constitute the extended Kalman filter of band fading factor, be designated as SFEKF (SuboptimUl FUding Extended KUlmUn Filter).

The fading factor λ (k+1) become when utilizing orthogonality principle to determine below, and then determine time-varying gain battle array K (k+1).For solving of λ (k+1), give two kinds of derivation algorithms, a kind of algorithm needing iteration optimizing to solve, another is a step approximate data.The fading factor λ (k+1) that iteration optimizing solves is actually a kind of optimum solution, but it can not ensure can both restrain in each sampling instant, and therefore, possible calculated amount is very large, is unfavorable in line computation.Here adopt and solve fading factor λ (k+1) approximate data (sub-optimal algorithm) ^[33].Suboptimum fading factor, gains the name thus.

Suboptimum fading factor λ (k+1) can obtain by following formula is approximate:

$\mathrm{\&lambda;}(k+1)=\left\{\begin{array}{cc}{\mathrm{\&lambda;}}_0& {\mathrm{\&lambda;}}_0\&GreaterEqual;1\\ 1& {\mathrm{\&lambda;}}_0<1\end{array}\right.---\left(37\right)$

Wherein:

${\mathrm{\&lambda;}}_0=\frac{\mathrm{tr}\left[N(k+1)\right]}{\mathrm{tr}\left[M(k+1)\right]}---\left(38\right)$

$N(k+1)=V_0(k+1)-H(k+1,\hat{x}(k+1|k))\mathrm{\&Gamma;}\left(k\right)Q\left(k\right){\mathrm{\&Gamma;}}^T\left(k\right)$

$\&CenterDot;H(k+1,\hat{x}(k+1|k))+\mathrm{\&beta;R}(k+1)---\left(39\right)$

$M(k+1)=H(k+1,\hat{x}(k+1|k))F(k,u\left(k\right),\hat{x}\left(k\right|k)P\left(k\right|k)P\left(k\right|k)$

$\&CenterDot;F^T(k,u\left(k\right),\hat{x}\left(k\right|k))H^T(k+1,\hat{x}(k+1|k))$

In formula, β >=1 is a selected reduction factor.Introducing this object weakening the factor is make state estimation more level and smooth.This numerical value can be selected by rule of thumb, also can be obtained by emulation, and it is chosen and can be determined by following formula:

$\mathrm{\&beta;}:\underset{\mathrm{\&beta;}}{\min }\left(\underset{k=0}{\overset{L}{\mathrm{\&Sigma;}}}\underset{t=1}{\overset{n}{\mathrm{\&Sigma;}}}\right|x_i\left(k\right)-x_i\left(k\widehat{|}k\right)\left|\right)---\left(40\right)$

Wherein, L is emulation step number.This criterion reflects the cumulative errors of wave filter.

Step 5: method for diagnosing faults is:

Design the strong tracking filfer under three groups of UUV states: fault model is promoted mainly on a left side, fault model is promoted mainly on the right side and UUV proper motion model.In order to obtain three groups of residual values like this, compare actual condition value and the state estimation of these three groups of strong tracking filfer, then contrast residual values and just can analyze immediately and obtain fault and which occurs in promote mainly.

For the diagnosis of the fault of main thruster, first whether should break down according to the actual conditions of the tracking of UUV flight path, course, the speed of a ship or plane and planning situation multilevel iudge main thruster, then judge it is which main thruster breaks down according to the speed residual error of three models, the comparative analysis of angular velocity residual error.

Step 6: fault-tolerance processing method is:

When a main thruster fault, in order to keep the normal operation of the correct of other operational factors of UUV and UUV, just must stop the work of fault main thruster, adding the normal operation of the control realization UUV that rudder substitutes with a main thruster.

Claims (3)

1., based on a UUV fault-tolerant control system for thruster fault, its composition comprises: UUV motion model, horizontal plane motion calculating system, fault model, strong tracking filfer, diagnostic module.

2. a kind of UUV fault-tolerant control system based on thruster fault according to claim 1, is characterized in that described fault diagnosis is by the strong tracking filfer under design three groups of UUV states: fault model is promoted mainly on a left side, fault model is promoted mainly on the right side and UUV proper motion model.In order to obtain three groups of residual values like this, compare actual condition value and the state estimation of these three groups of strong tracking filfer, then contrast residual values and just can analyze immediately and obtain fault and which occurs in promote mainly.

3. the UUV fault-tolerant control system based on thruster fault according to claim 1, it is characterized in that described fault-tolerant processing is when a main thruster fault, in order to keep the normal operation of the correct of other operational factors of UUV and UUV, just must stop the work of fault main thruster, add the normal operation of the control realization UUV that rudder substitutes with a main thruster.