CN102865880B - Method for predicting underwater vehicle navigation system circular error probability - Google Patents

Abstract

The invention relates to a method for predicting underwater vehicle navigation system circular error probability, which comprises the following steps: determining a sensor used by the navigation system and a standard difference of the error, then calculating the standard probability sigma (eH) of lateral deviation caused by course angle error according to the standard difference of eH of sigma and each course angle error of a course sensor; calculating the standard probability sigma (eL) of vertical deviation caused by speed proportion error according to the standard difference of of speed proportion error of eL of Lambda and a speed sensor, and calculating circular error probability according to the standard difference of the lateral deviation and the standard difference of the vertical deviation. According to the invention, during the design phase, the circular error probability of the navigation system can be effectively predicted; the circular error probability can be rapidly calculated during the test or usage phase, and the cost is almost free.

Description

A kind of method of estimating underwater vehicles navigation system circular error probable

Technical field

The present invention relates to underwater robot technical field, relate in particular to a kind of method of estimating underwater vehicles navigation system circular error probable.

Background technology

Circular error probable is the important indicator of weighing underwater robot navigation error, conventional method is to adopt the method for the test of underwater robot real navigation, accumulation lot of experiments sample to calculate circular error probable, also by mathematical computations, does not estimate the method for circular error probable.

Underwater vehicles navigation system is mainly comprised of heading sensor and speed pickup.These two kinds of sensors are measured respectively course and the speed of underwater robot current time, obtain the position of underwater robot by dead reckoning method, realize underwater navigation.Two error sources of speed proportional error that the course angle error that navigation error mainly causes from heading sensor and speed pickup cause.The course angle error that heading sensor causes mainly refers to course angle initial error.For inertia heading sensor, for example optical fibre gyro compass also has course angle drift error except course angle initial error.Each heading sensor that starts, its course angle initial error is all not identical, is a kind of stochastic error.In large sample statistical method, course angle initial error can be thought Normal Distribution, the error criterion that can provide with reference to heading sensor.Course angle initial error remains unchanged after starting heading sensor, by linear, additive relation, passes to course angle measured value.The course angle drift error of inertia heading sensor is caused by the drift of inertia device, affected by water environment of living in, inertia device internal noise in underwater robot navigation process.Course angle drift error in each navigation process is all not identical.In large sample statistical method, course angle drift error can be thought Normal Distribution.Speed pickup error mainly refers to speed proportional error.Speed proportional error is affected by the residing external environment of underwater robot navigation area, and the speed proportional error in navigation process is at every turn all not identical, and in large sample statistical method, speed proportional error can be thought Normal Distribution.

Course angle error and speed proportional error are from two kinds of different sensors, uncorrelated mutually, the caused lateral deviation of course angle error and the caused along track bias of speed proportional error are also uncorrelated mutually, and approximate Normal Distribution, meet circular error probable design conditions.

Summary of the invention

The object of the invention is to lack when there is design underwater robot in prior art circular error probable is effectively estimated, the index Design result that makes to navigate lacks the defect of confidence level, and a kind of method of estimating underwater vehicles navigation system circular error probable is provided.

The technical scheme that the present invention adopted is for achieving the above object: a kind of method of estimating underwater vehicles navigation system circular error probable, comprising:

The first step, determines sensor that navigational system is used and the standard deviation of error thereof;

Second step, according to

e _H＝φ (1)

In formula, e _hfor lateral deviation, φ is course angle error and 0 < | φ | and≤2 °, and the standard deviation of the every course angle error of heading sensor, calculate the standard deviation sigma (e of the caused lateral deviation of course angle error _h);

The 3rd step, according to

e _L＝λ (2)

In formula, e _lfor along track bias, λ is speed proportional error, and the standard deviation of the speed proportional error of speed pickup, the standard deviation sigma (e of the along track bias that computing velocity proportional error causes _l);

The 4th step, calculates circular error probable according to the standard deviation of the standard deviation of lateral deviation and along track bias.

The step of described second step comprises:

According to product description, determine the course angle initial error φ of the heading sensor of navigational system ₁standard deviation sigma (φ ₁);

If the heading sensor of navigational system adopts inertia heading sensor, course angle drift error φ ₂standard deviation sigma (φ ₂) > 0;

If the heading sensor of navigational system does not adopt inertia heading sensor, course angle drift error φ ₂standard deviation be σ (φ ₂)=0;

According to course angle initial error φ ₁standard deviation sigma (φ ₁) and course angle drift error φ ₂standard deviation sigma (φ ₂), use formula

$\mathrm{\&sigma;}\left(e_H\right)=\sqrt{{\left[\mathrm{\&sigma;}\left({\mathrm{\&phi;}}_1\right)\right]}^2+{\left[\mathrm{\&sigma;}\left({\mathrm{\&phi;}}_2\right)\right]}^2}---\left(3\right)$

Calculate course angle error φ=φ ₁+ φ ₂caused lateral deviation e _hstandard deviation.

The method of described the 3rd step is: according to product description, determine the standard deviation sigma (λ) of speed proportional error λ of the speed pickup of navigational system, can obtain as along track bias e _lstandard deviation

σ(e _L)＝σ(λ) (4)

The method of described the 4th step is: by the standard deviation sigma (e of lateral deviation _h) and the standard deviation sigma (e of along track bias _l) substitution circular error probable computing formula,

$P\left(r\right)=\frac{2\mathrm{\&sigma;}\left(e_H\right)\mathrm{\&sigma;}\left(e_L\right)}{{\left[\mathrm{\&sigma;}\left(e_H\right)\right]}^2+{\left[\mathrm{\&sigma;}\left(e_L\right)\right]}^2}{C}_0(\mathrm{\&alpha;},\mathrm{\&beta;})-0.5---\left(5\right)$

Wherein,

$\mathrm{\&alpha;}=\frac{r^2\{{\left[\mathrm{\&sigma;}\left(e_H\right)\right]}^2+{\left[\mathrm{\&sigma;}\left(e_L\right)\right]}^2\}}{4{\left[\mathrm{\&sigma;}\left(e_H\right)\right]}^2{\left[\mathrm{\&sigma;}\left(e_L\right)\right]}^2}---\left(6\right)$

$\mathrm{\&beta;}=\frac{|{\left[\mathrm{\&sigma;}\left(e_H\right)\right]}^2-{\left[\mathrm{\&sigma;}\left(e_L\right)\right]}^2|}{{\left[\mathrm{\&sigma;}\left(e_H\right)\right]}^2+{\left[\mathrm{\&sigma;}\left(e_L\right)\right]}^2}---\left(7\right)$

$C_0(\mathrm{\&alpha;},\mathrm{\&beta;})={\&Integral;}_0^{\mathrm{\&alpha;}}{e}^{-t}{I}_0\left(\mathrm{\&beta;t}\right)\mathrm{dt}---\left(8\right)$

$I_0\left(\mathrm{\&beta;t}\right)=\frac{1}{\mathrm{\&pi;}}{\&Integral;}_0^{\mathrm{\&pi;}}{e}^{\&PlusMinus;\mathrm{\&beta;}t\cos 2\mathrm{\&theta;}}\mathrm{d\&theta;}---\left(9\right)$

Obtain the circular error probable of underwater robot.

Described by the standard deviation sigma (e of lateral deviation _h) and the standard deviation sigma (e of along track bias _l) operation steps of substitution circular error probable computing formula is:

A. use the standard deviation sigma (e of lateral deviation _h) and the standard deviation sigma (e of along track bias _l) to formula (5) to (9) initialization, in formula (7), obtain the initial value β of β ₀, make the initial value α of α ₀=1;

B. by α ₀and β ₀substitution formula (9), obtains I ₀(β ₀t), then by I ₀(β ₀t) substitution formula (8) obtains C ₀the initial value C of (α, β) ₀(α ₀, β ₀), by C ₀(α ₀, β ₀), σ (e _h) and σ (e _l) substitution formula (5), set up circular error probable equation

$f\left(\mathrm{\&alpha;}\right)=\frac{2\mathrm{\&sigma;}\left(e_H\right)\mathrm{\&sigma;}\left(e_L\right)}{{\left[\mathrm{\&sigma;}\left(e_H\right)\right]}^2+{\left[\mathrm{\&sigma;}\left(e_L\right)\right]}^2}{C}_0(\mathrm{\&alpha;},\mathrm{\&beta;})-0.5---\left(10\right)$

C. solve circular error probable equation root, if met | f (α) |≤0.01, will in equation root substitution formula (6), obtain circular error probable r; If do not met | f (α) |≤0.01, according to newton's down-hill method, upgrade α, get back to step b.

Beneficial effect of the present invention is embodied in:

1. in the design phase: when design underwater robot, the circular error probable of navigational system was lacked to method for predicting, the index Design result that makes to navigate lacks confidence level in the past.This brings larger adverse effect to the design of underwater robot and development.This method can design person be selected suitable navigation sensor, effectively estimates the circular error probable of navigational system, and navigation error index is met design requirement, and can improve significantly underwater robot design level.

2. in test or operational phase: when the circular error probable of evaluation underwater vehicles navigation system, need carry out real navigation test to obtain circular error probable, sample size needs dozens of conventionally in the past.And this method only needs a few minutes can calculate circular error probable, almost there is no cost.

Accompanying drawing explanation

Fig. 1 is deviation schematic diagram of the present invention;

Fig. 2 is computing method process flow diagram of the present invention.

Embodiment

Below in conjunction with drawings and the specific embodiments, the present invention is elaborated.

Estimate a method for underwater vehicles navigation system circular error probable, as shown in Figure 1.

1. couple e _h=φ, the navigation error that course angle error causes and course angle are irrelevant, only relevant with course angle error, and only cause perpendicular to the deviation in the direction of underwater robot air route the proof of i.e. lateral deviation:

In unit interval, the North-East Bound displacement of underwater robot is

$\left[\begin{array}{c}{s}_E\\ s_N\end{array}\right]=\left[\begin{array}{cc}\cos \mathrm{\&psi;}& \sin \mathrm{\&psi;}\\ -\sin \mathrm{\&psi;}& \cos \mathrm{\&psi;}\end{array}\right]\left[\begin{array}{c}u\\ v\end{array}\right]\&CenterDot;T$

In formula, ψ is sail body course angle, and φ is course angle error, and u is forward speed, and v is dextrad speed, and T is the unit interval, S _efor east orientation displacement, S _nfor north orientation displacement.Distance to go is s, with vector, can be expressed as

$s=T{C}_b^{n}v$

The North-East Bound displacement that contains course angle error is

$\left[\begin{array}{c}{s}_E^{\&prime;}\\ s_N^{\&prime;}\end{array}\right]\left[\begin{array}{cc}\cos \mathrm{\&phi;}& \sin \mathrm{\&phi;}\\ -\sin \mathrm{\&phi;}& \cos \mathrm{\&phi;}\end{array}\right]\left[\begin{array}{cc}\cos \mathrm{\&psi;}& \sin \mathrm{\&psi;}\\ -\sin \mathrm{\&psi;}& \cos \mathrm{\&psi;}\end{array}\right]\left[\begin{array}{c}u\\ v\end{array}\right]\&CenterDot;T$

With vector representation, be

$s^{\&prime;}=T{C}_n^{n^{\&prime;}}{C}_b^{n}v$

Position deviation in unit interval is

$\mathrm{\&Delta;s}=s^{\&prime;}-s$

$=(C_n^{n^{\&prime;}}-I)T{C}_b^{n}v$

$=(C_n^{n^{\&prime;}}-I)s$

$\left[\begin{array}{c}\mathrm{\&Delta;}{s}_E\\ \mathrm{\&Delta;}{s}_N\end{array}\right]=\left[\begin{array}{cc}\cos \mathrm{\&phi;}-1& \sin \mathrm{\&phi;}\\ -\sin \mathrm{\&phi;}& \cos \mathrm{\&phi;}-1\end{array}\right]\left[\begin{array}{c}{s}_E\\ S_N\end{array}\right]$

Due to 0 < | φ |≤2 °, φ is little angle, so

$\left[\begin{array}{c}\mathrm{\&Delta;}{s}_E\\ \mathrm{\&Delta;}{s}_N\end{array}\right]=\left[\begin{array}{cc}0& \mathrm{\&phi;}\\ -\mathrm{\&phi;}& 0\end{array}\right]\left[\begin{array}{c}{s}_E\\ s_N\end{array}\right]$

Displacement in the above formula constituent parts time is added up, through t=NT after the time,

$\mathrm{\&Delta;}{S}_E=\mathrm{\&phi;}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{s}_{\mathrm{Ni}},$ $S_N=-\mathrm{\&phi;}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{s}_{\mathrm{Ei}}$

Position deviation is

$\left[\begin{array}{c}\mathrm{\&Delta;}{S}_E\\ \mathrm{\&Delta;}{S}_N\end{array}\right]=\left[\begin{array}{cc}0& \mathrm{\&phi;}\\ -\mathrm{\&phi;}& 0\end{array}\right]\left[\begin{array}{c}{S}_E\\ S_N\end{array}\right]$

ΔS＝φS

Therefore position deviation Δ S is caused by course angle error φ, irrelevant with course angle.

The position deviation Δ S that course angle error is caused is decomposed into lateral deviation and along track bias.Choose the horizontal direction identical with φ direction for just, the lateral deviation that course angle causes is expressed as by relative value

$e_{\mathrm{\&phi;H}}=\frac{S\&CenterDot;\sin \mathrm{\&phi;}}{S}\&ap;\frac{S\&CenterDot;\mathrm{\&phi;}}{S}=\mathrm{\&phi;}$

And the along track bias size that course angle causes is expressed as by relative value

$\left|e_{\mathrm{\&phi;L}}\right|=\left|\frac{S-S\cos \mathrm{\&phi;}}{S}\right|=|2\&CenterDot;{\sin }^2(\mathrm{\&phi;}/2)|\&ap;\frac{{\mathrm{\&phi;}}^2}{2}$

Because φ is little angle, and the infinitesimal than φ high-order, therefore have

e _φL＝0

So course angle error only causes lateral deviation, its value is

e _H＝φ

It is formula (1).

2. couple e _l=λ, the navigation error that speed proportional coefficient causes and course angle are irrelevant, only relevant with course angle error, and only cause perpendicular to the deviation in the direction of underwater robot air route the proof of i.e. lateral deviation:

In unit interval, the North-East Bound displacement of underwater robot is

$\left[\begin{array}{c}{s}_E\\ s_N\end{array}\right]=\left[\begin{array}{cc}\cos \mathrm{\&psi;}& \sin \mathrm{\&psi;}\\ -\sin \mathrm{\&psi;}& \cos \mathrm{\&psi;}\end{array}\right]\left[\begin{array}{c}u\\ v\end{array}\right]\&CenterDot;T$

In formula, sail body course angle is ψ, speed proportional error λ, and forward speed is u, and dextrad speed is v, and the unit interval is T, with vector, can be expressed as

$s=T{C}_b^{n}v$

North-East Bound displacement containing speed proportional error is

$\left[\begin{array}{c}{s}_E^{\&prime;}\\ s_N^{\&prime;}\end{array}\right]=(1-\mathrm{\&lambda;})\left[\begin{array}{cc}\cos \mathrm{\&psi;}& \sin \mathrm{\&psi;}\\ -\sin \mathrm{\&psi;}& \cos \mathrm{\&psi;}\end{array}\right]\left[\begin{array}{c}u\\ v\end{array}\right]\&CenterDot;T$

$s^{\&prime;}=\mathrm{\&lambda;T}{C}_b^{n}v$

Position deviation in unit interval is

$\mathrm{\&Delta;s}=s^{\&prime;}-s$

$=\mathrm{\&lambda;T}{C}_b^{n}v$

$=\mathrm{\&lambda;s}$

$\left[\begin{array}{c}\mathrm{\&Delta;}{s}_E\\ \mathrm{\&Delta;}{s}_N\end{array}\right]=\mathrm{\&lambda;}\left[\begin{array}{c}{s}_E\\ s_N\end{array}\right]$

Displacement in the above formula constituent parts time is added up, through t=NT after the time,

$\mathrm{\&Delta;}{S}_E=-\mathrm{\&lambda;}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{s}_{\mathrm{Ei}},$ $\mathrm{\&Delta;}{S}_N=-\mathrm{\&lambda;}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{s}_{\mathrm{Ni}},$

Therefore position deviation is

$\left[\begin{array}{c}\mathrm{\&Delta;}{S}_E\\ \mathrm{\&Delta;}{S}_N\end{array}\right]=\mathrm{\&lambda;}\left[\begin{array}{c}{S}_E\\ S_N\end{array}\right]$

ΔS＝λS

Therefore speed proportional error only causes the position deviation Δ S in the S direction of air route, only causes along track bias, irrelevant with course angle.Along track bias size is

$\left|e_L\right|=\frac{\left|\mathrm{\&Delta;S}\right|}{\left|S\right|}=\left|\mathrm{\&lambda;}\right|$

When λ > 0, Δ S is identical with S direction, e _l> 0; When λ < 0, Δ S and S opposite direction, e _l< 0; λ=0 o'clock, Δ S is zero.So

e _L＝λ

Be formula (2).

Below in conjunction with drawings and the specific embodiments, the present invention is elaborated.

Embodiment 1: the navigational system of underwater robot A of take is example, adopts this method to estimate the circular error probable of underwater robot.

The heading sensor of navigational system adopts inertia heading sensor, and the course angle initial error that product description is listed and course angle drift error index are: course angle initial error φ ₁standard deviation be 0.2 °, be designated as σ (φ ₁)=0.2 °; Course angle drift error φ ₂standard deviation be 0.3 °, be designated as σ (φ ₂)=0.3 °.The standard deviation of the speed proportional error λ that speed pickup product description is listed is 0.5%, is designated as σ (λ)=0.005.If distance to go is S, course angle initial error φ ₁the standard deviation of the lateral deviation causing is

Course angle drift error φ ₂the standard deviation of the lateral deviation causing is

Due to φ ₁and φ ₂not similar error, uncorrelated mutually, course angle error φ=φ ₁+ φ ₂the standard deviation of caused lateral deviation is

$\mathrm{\&sigma;}\left(e_H\right)=\mathrm{\&sigma;}\left(\mathrm{\&phi;}\right)=\mathrm{\&sigma;}({\mathrm{\&phi;}}_1+{\mathrm{\&phi;}}_2)$

$=\sqrt{{\left[\mathrm{\&sigma;}\left({\mathrm{\&phi;}}_1\right)\right]}^2+{\left[\mathrm{\&sigma;}\left({\mathrm{\&phi;}}_2\right)\right]}^2}$

$=\sqrt{{0.0035}^2+{0.0052}^2}$

$=0.0063$

The standard deviation of the along track bias that speed proportional error causes is

σ(e _L)＝σ(λ)＝0.005

By σ (e _h)=0.0063 and σ (e _l)=0.005 substitution formula (7), obtains the initial value β of β ₀=0.227.Make the initial value α of α ₀=1, and β ₀in=0.227 substitution formula (9), obtain I ₀(β ₀t), then by I ₀(β ₀t) substitution formula (8) obtains C ₀the initial value of (α, β)

C ₀(α ₀，β ₀)＝0.6342

By C ₀(α ₀, β ₀), σ _x=σ (e _h) and σ _y=σ (e _l) in substitution formula (5), obtain together

f(α)＝0.9739C ₀(α ₀，β ₀)-0.5＝0.1178

With newton's down-hill method, obtain equation root

α＝0.6755

By operation steps, b calculates, and obtains

|f(α)|＝0.0208＞0.01

Root with newton's down-hill method renewal equation

α＝0.7173

By operation steps, b calculates, and obtains

|f(α)|＝0.0004＜0.01

By α=0.7173, σ (e _h)=0.0063 and σ (e _l)=0.005 substitution formula (6), the circular error probable that obtains underwater robot A is 0.66%.

Embodiment 2: the navigational system of underwater robot B of take is example, adopt this method to estimate the circular error probable of underwater robot.

The heading sensor of navigational system adopts fluxgate compass, without course angle drift error.The course angle initial error φ that product description is listed ₁standard deviation requirement be 2 °, the standard deviation of the speed proportional error λ that speed pickup product description is listed is 1%, the lateral deviation that course angle error φ causes is

The along track bias that speed proportional error causes is

σ(e _L)＝σ(λ)＝0.01

By σ (e _h)=0.035 and σ (e _l)=0.01 substitution formula (7), obtains the initial value β of β ₀=0.8491.Make the initial value α of α ₀=1, and β ₀in=0.8491 substitution formula (9), obtain I ₀(β ₀t), then by I ₀(β ₀t) substitution formula (8) obtains C ₀the initial value of (α, β)

C ₀(α ₀，β ₀)＝0.6617

By C ₀(α ₀, β ₀), σ _x=σ (e _h) and σ _y=σ (e _l) in substitution formula (5), obtain together

f(α)＝0.9739C0(α ₀，β ₀)-0.5＝-0.1494

With newton's down-hill method, obtain equation root

α＝1.6453

By operation steps, b calculates, and obtains

|f(α)|＝0.0261＞0.01

Root with newton's down-hill method renewal equation

α＝1.8101

By operation steps, b calculates, and obtains

|f(α)|＝0.0011＜0.01

By α=1.8101, σ (e _h)=0.035 and σ (e _l)=0.01 substitution formula (6), the circular error probable that obtains underwater robot B is 2.6%.

Claims (3)

1. a method of estimating underwater vehicles navigation system circular error probable, is characterized in that, comprising:

The first step, determines sensor that navigational system is used and the standard deviation of error thereof;

Second step, according to the standard deviation of formula (1) and the every course angle error of heading sensor, calculates the standard deviation sigma (e of the caused lateral deviation of course angle error _h),

e _H＝φ (1)

In formula, e _hfor lateral deviation, φ is course angle error and 0 < | φ | and≤2 °;

The 3rd step, according to the standard deviation of the speed proportional error of formula (2) and speed pickup, the standard deviation sigma (e of the along track bias that computing velocity proportional error causes _l),

e _L＝λ (2)

In formula, e _lfor along track bias, λ is speed proportional error;

The 4th step, calculates circular error probable according to the standard deviation of the standard deviation of lateral deviation and along track bias;

The method of described the 4th step is: by the standard deviation sigma (e of lateral deviation _h) and the standard deviation sigma (e of along track bias _l) substitution circular error probable computing formula,

Wherein,

Obtain the circular error probable of underwater robot;

Described by the standard deviation sigma (e of lateral deviation _h) and the standard deviation sigma (e of along track bias _l) operation steps of substitution circular error probable computing formula is:

A. use the standard deviation sigma (e of lateral deviation _h) and the standard deviation sigma (e of along track bias _l) to formula (5) to (9) initialization, in formula (7), obtain the initial value β of β ₀, make the initial value α of α ₀=1;

B. by α ₀and β ₀substitution formula (9), obtains I ₀(β ₀t), then by I ₀(β ₀t) substitution formula (8) obtains C ₀the initial value C of (α, β) ₀(α ₀, β ₀), by C ₀(α ₀, β ₀), σ (e _h) and σ (e _l) substitution formula (5), set up circular error probable equation

C. solve circular error probable equation root, if met | f (α) |≤0.01, will in equation root substitution formula (6), obtain circular error probable r; If do not met | f (α) |≤0.01, according to newton's down-hill method, upgrade α, get back to step b.

2. a kind of method of estimating underwater vehicles navigation system circular error probable according to claim 1, is characterized in that, the step of described second step comprises:

According to product description, determine the course angle initial error φ of the heading sensor of navigational system ₁standard deviation sigma (φ ₁);

If the heading sensor of navigational system adopts inertia heading sensor, course angle drift error φ ₂standard deviation sigma (φ ₂) > 0;

If the heading sensor of navigational system does not adopt inertia heading sensor, course angle drift error φ ₂standard deviation be σ (φ ₂)=0;

According to course angle initial error φ ₁standard deviation sigma (φ ₁) and course angle drift error φ ₂standard deviation sigma (φ ₂), use formula

Calculate course angle error φ=φ ₁+ φ ₂caused lateral deviation e _hstandard deviation.

3. a kind of method of estimating underwater vehicles navigation system circular error probable according to claim 1, it is characterized in that, the method of described the 3rd step is: according to product description, determine the standard deviation sigma (λ) of speed proportional error λ of the speed pickup of navigational system, can obtain as along track bias e _lstandard deviation

σ(e _L)＝σ(λ) (4)。