CN104280026A - Adaptive unscented Kalman filter (AUKF)-based deepwater robot long-baseline combined navigation method - Google Patents

Abstract

The invention relates to an adaptive unscented Kalman filter (AUKF)-based deepwater robot long-baseline combined navigation method. The method comprises the following steps of acquiring a deepwater robot initial absolute position as a track estimation starting point by a global positioning system, acquiring initial information of the deepwater robot, constructing an unscented Kalman filtering main filter, carrying out filtering estimation on the acquired initial information, constructing an unscented Kalman assistant filter, further carrying out filtering estimation on the information subjected to unscented Kalman main filter-based filtering estimation and carrying out data fusion on the acquired initial information by an AUKF method to obtain fused information. The AUKF-based deepwater robot long-baseline combined navigation method improves a navigation precision of a deepwater robot adopting a long-baseline positioning system and can smooth a course and depth needed by a deepwater robot control system and speed information under a carrier coordinate system.

Description

Based on the deepwater robot Long baselines Combinated navigation method of AUKF

Technical field

The present invention relates to a kind of deepwater robot airmanship in oceanographic engineering field, specifically a kind of Long baselines Combinated navigation method based on self-adaptation uncented Kalman filter.

Background technology

Accurate homing capability is the key of underwater robot effective operation and safe retrieving.Navigational system must provide accurate location in remote and Long time scale, speed and attitude information.But due to the impact of the factors such as the complicacy, robot own vol, weight, the energy and the disguise that are subject to underwater environment, realize the navigation of high-precision underwater robot and remain a difficult task.

Existing underwater navigation method mainly multi-sensor combined navigation method, carries out filtering to multi-sensor information.But for the navigation problem of deepwater robot, when system noise information is unknown, if the noise parameter arranged has big difference with actual noise, Biased estimator even the dispersing of filtering algorithm will be caused.Marine environment is complicated and changeable, is difficult to make prior estimate comparatively accurately to noise, therefore will design and adopt the filtering algorithm being more suitable for real-time estimating system noise information.Long baselines locator data outlier is more, it is comparatively large to postpone, and working state abnormal in some cases, therefore also most important to raising navigation accuracy to the correct process of Long baselines data.

Summary of the invention

For the deficiencies in the prior art; the invention provides a kind of deepwater robot Combinated navigation method based on self-adaptation uncented Kalman filter; object is the navigation accuracy improving the deepwater robot using Long baselines positioning system, simultaneously can course smoothly needed for deepwater robot control system, velocity information under the degree of depth and carrier coordinate system.

The technical scheme that the present invention is adopted for achieving the above object is: a kind of deepwater robot Long baselines Combinated navigation method based on AUKF, utilize GPS to obtain the initial point of initial absolute position as reckoning of deepwater robot, and gather the initial information of deepwater robot;

Build uncented Kalman filter senior filter and carry out filtering estimation to the initial information collected, building colourless Kalman's extension filter, after estimating senior filter filtering, the further filtering of information is estimated;

Adopt the method for self-adaptation uncented Kalman filter to carry out data fusion to the initial information collected, draw the information after fusion.

Described initial information comprises the linear velocity information that Doppler log gathers, the attitude information that motion sensor gathers, the positional information that Long baselines acoustic positioning device gathers and the depth information that depthometer gathers.

Described Long baselines acoustic positioning device obtains positional information by the method resolving deepwater robot absolute position voluntarily.

The described method resolving deepwater robot absolute position voluntarily, step is as follows:

Long baselines data pack protocol parses the range information of deepwater robot to each beacon, judges the validity of each bootstrap information, according to the absolute location information of each effective beacon and deepwater robot to the range information of beacon, sets up ternary quadratic equation group:

${(x_o-x_A)}^2+{(y_o-y_A)}^2+{(z_o-z_A)}^2=l_A^2$

${(x_o-x_B)}^2+{(y_o-y_B)}^2+{(z_o-z_B)}^2=l_B^2$

${(x_o-x_C)}^2+{(y_o-y_C)}^2+{(z_o-z_C)}^2=l_C^2$

(x in above-mentioned system of equations _a, y _a, z _a), (x _b, y _b, z _b), (x _c, y _c, z _c) be respectively the absolute location coordinates of each effective beacon, l _a, l _b, l _cbe respectively the distance of deepwater robot to each beacon, separate this system of equations and obtain (x _o, y _o, z _o) be the absolute location coordinates of deepwater robot; obtain two groups of solutions of robot absolute location coordinates; the efficient solution in two groups of solutions is judged, as the east orientation position detection value in measurement vector and north orientation position detection value according to the depth information of current time depthometer collection.

The implementation procedure that colourless Kalman's senior filter filtering is estimated is:

To a upper moment random vector by estimator average and variance after non-linear colourless conversion, obtain discrete Sigma point, state equation through senior filter calculates and upgrades Sigma point, then calculate the state average and variance that upgrade Sigma point, state average and variance are calculated to state average and the variance of a step estimation; The state average estimate a step and variance calculate renewal through the measurement equation of colourless conversion and senior filter again, obtain the estimated value of the average of deepwater robot Observable state, variance and covariance; Read sensor gathers observation information, and calculates with the estimated value of state average, variance, observer state and gain matrix and estimate, obtains the estimated value of system average, variance, and is newly ceased; The observation signal of measured value as extension filter is calculated according to the new breath of senior filter.

The implementation procedure that extension filter carries out filtering estimation to systematic procedure noise is as follows:

A upper moment estimates the diagonal entry and the variance that obtain the system noise variance matrix of extension filter, obtains a step estimated value of diagonal entry and variance with the state equation of extension filter to diagonal entry and variance after calculating renewal; The observed reading estimated is obtained after calculating renewal with the measurement equation of extension filter according to variance, state variance and gain matrix that a step of senior filter is estimated, the measured value of the step estimated value, observed reading and the senior filter that obtain is calculated estimated value and the variance estimating to obtain the diagonal entry of systematic procedure noise variance matrix, when there being new observation information, continue the performing step of next control cycle senior filter.

The state equation of described extension filter is set up as follows:

${\stackrel{\‾}{q}}_k={\stackrel{\‾}{q}}_{k-1}+w_{q_k}$

Wherein for the system noise in extension filter, for system noise variance matrix diagonal element.

The measurement establishing equation of described extension filter is as follows:

$\stackrel{\‾}{S}{q}_k=g\left({\stackrel{\‾}{q}}_k\right)=\mathrm{vdiag}\left[{\left(K_k^T{K}_k\right)}^{-1}{K}_k^T(P_{k|k-1}-P_k)K_k{\left(K_k^T{K}_k\right)}^{-1}\right]$

Wherein, vdiag () represents a vector, and its component is the elements in a main diagonal of matrix (), system noise variance matrix diagonal element as the observation signal of system, P _k|k-1for the variance that a step of senior filter state vector is estimated, P _kfor the variance of senior filter state vector, K _kfor the gain matrix of senior filter, for the observed reading estimated.

The measurement equation of described senior filter switches according to the significant instant of Long baselines locator data:

When the observed reading that Long baselines acoustic positioning device provides is effective, measures equation and be defined as:

y _k＝x _k+N ⁿ

Wherein, y=[u ^y, v ^y, w ^y, r ^y, ξ ^y, η ^y, ζ ^y, ψ ^y] ^tfor measurement vector, wherein u ^y, v ^y, w ^y, r ^yforward speed, side velocity, vertical velocity and the observed quantity turning bow angular velocity respectively, ξ ^y, η ^y, ζ ^y, ψ ^ythe observed quantity to angle of north orientation position, east orientation position, the degree of depth and bow, N ⁿfor observation noise;

When the observed reading that Long baselines acoustic positioning device provides is invalid, measures equation and be defined as:

$y_1=x_1+N_1^n$

$y_2=x_2+N_2^n$

$y_3=x_3+N_3^n$

$y_4=x_4+N_4^n$

$y_5=x_7+N_5^n$

$y_6=x_8+N_6^n$

Wherein, y=[u ^y, v ^y, w ^y, r ^y, ζ ^y, ψ ^y] ^tfor measurement vector.

Described significant instant is Long baselines Data Update and is not moment of outlier.

The present invention has following beneficial effect and advantage:

1. the present invention proposes according to Long baselines data pack protocol, resolved the method for robot absolute position by robot to beacon distance, solve Long baselines positioning system in some cases and the problem of locating information comparatively accurately can not be provided.

2. the present invention proposes and switch observation equation according to the validity of Long baselines locator data, solve the more and Long baselines acoustic positioning device of Long baselines locator data outlier and the inconsistent problem of other sensor image data turnover rates.

3. navigation accuracy of the present invention is high; because the extension filter in AUKF algorithm can be estimated in real time to the noise information of system; and can restrain faster, solve system noise that marine environment complicated and changeable and deepwater robot execute the task and cause and be difficult to obtain prior imformation and then cause the Biased estimator even problem of filtering divergence comparatively accurately.

Accompanying drawing explanation

Fig. 1 is the fundamental diagram of Combinated navigation method of the present invention;

Fig. 2 is the process flow diagram of self-adaptation uncented Kalman filter algorithm of the present invention;

Fig. 3 is the embodiment result of the deepwater robot Long baselines integrated navigation based on AUKF.

Embodiment

Below in conjunction with drawings and Examples, the present invention is described in further detail.

Based on a deepwater robot Combinated navigation method of AUKF, mainly comprise the following steps:

GPS is utilized to obtain the initial point of initial absolute location information as reckoning of deepwater robot, utilize Doppler log gathering line velocity information, utilize motion sensor to gather attitude information, utilize Long baselines acoustic positioning device and depthometer collection position information; Self-adaptation uncented Kalman filter method is adopted to carry out data fusion to above-mentioned information, get the velocity under carrier coordinate system and the position vector under earth coordinates and bow to angle as filter status vector, get the information of each sensor collection as wave filter measurement vector, in some cases Long baselines positioning system can not provide comparatively accurately locating information time, resolve the observed reading of robot location as positional information voluntarily according to beacon distance information, select Long baselines data significant instant to switch observation equation.Filter result is directly used by deepwater robot control system and navigational system.

Further, if Long baselines positioning system is because acoustic velocity value in water is arranged, the reason such as inaccurate causes positioning error larger, the invention still further relates to a kind of method resolving robot absolute position according to the robot in Long baselines packet far from the range information of subsea beacon (being no less than 3) voluntarily, step is as follows:

Each moment receiving Long baselines packet, the range information of robot to each beacon (lay and should be no less than 3 beacons) is parsed under water according to Long baselines data protocol, the validity of each bootstrap information is judged by data pack protocol, according to the absolute location information of each effective beacon and the robot range information to beacon, set up the ternary quadratic equation group that corresponding ball intersects, separate this system of equations, obtain two groups of solutions of robot absolute coordinates, judge the efficient solution in two groups of solutions according to the depth information of current time depthometer collection.

Described employing self-adaptation uncented Kalman filter method step is as follows:

On the basis of deepwater robot kinematics model, get the velocity under carrier coordinate system, position vector under earth coordinates and bow to angle as filter status vector, set up system state equation;

The senior filter choice criteria UKF wave filter of AUKF method, to system state transmission and renewal, estimates the status information of system in real time, obtains the end value average of system estimation with variance P _k;

The new breath information ξ estimated is calculated according to senior filter _kcalculate the measured value Sq of extension filter _k; Set up the state equation of extension filter and measure equation, according to measured value Sq _kas the observation signal of system, adopt the noise information of extension filter KF to system of AUKF to estimate in real time, obtain the diagonal entry of system noise covariance battle array Q and variance estimated value, noise information here comprises process noise v _kwith measurement noises n _k, their covariance matrix is respectively Q ^vand Q ⁿ.

Further, because the data updating rate of Long baselines acoustic positioning device and Doppler log, motion sensor, depthometer is inconsistent, the outlier of Long baselines acoustic positioning device is more, selects Long baselines data significant instant to switch observation equation.

Be illustrated in figure 1 the fundamental diagram of Combinated navigation method of the present invention; utilize Doppler log gathering line velocity information; utilize Long baselines acoustic positioning device acquisition plane positional information; utilize depthometer sampling depth information; motion sensor is utilized to gather attitude information; data fusion method is utilized to merge above-mentioned information; obtain deepwater robot at marine 3 D motion trace, simultaneously can the smoothly course needed for deepwater robot control system and the velocity information under carrier coordinate system.

(1) get the velocity under carrier coordinate system and the position vector under earth coordinates and bow to angle as filter status vector, be defined as: x=[u ^x, v ^x, w ^x, r ^x, ξ ^x, η ^x, ζ ^x, ψ ^x] ^t, wherein u ^x, v ^x, w ^xand r ^xforward speed respectively, side velocity, vertical velocity and the quantity of state turning bow angular velocity, ξ ^x, η ^x, ζ ^xand ψ ^xnorth orientation position, east orientation position, the degree of depth and the bow quantity of state to angle respectively.

State equation is defined as:

$u_{k+1}^x=u_k^x+N_1^v$

$v_{k+1}^x=v_k^x+N_2^v$

$w_{k+1}^x=w_k^x+N_3^v$

$r_{k+1}^x=r_k^x+N_4^v$

${\mathrm{\ξ}}_{k+1}^x={\mathrm{\ξ}}_k^x+[\sin ({\mathrm{\ψ}}_k^x+N_8^v)\×(u_k^x+N_1^v)+\cos ({\mathrm{\ψ}}_k^x+N_8^v)\×(v_k^x+N_2^v)]\×\mathrm{\Δt}+N_5^v$

${\mathrm{\η}}_{k+1}^x={\mathrm{\η}}_k^x+[\cos ({\mathrm{\ψ}}_k^x+N_8^v)\×(u_k^x+N_1^v)-\sin ({\mathrm{\ψ}}_k^x+N_8^v)\×(v_k^x+N_2^v)]\×\mathrm{\Δt}+N_6^v$

${\mathrm{\ζ}}_{k+1}^x={\mathrm{\ζ}}_k^x+(w_k^x+N_3^v)\×\mathrm{\Δt}+N_7^v$

${\mathrm{\ψ}}_{k+1}^x={\mathrm{\ψ}}_k^x+(r_k^x+N_4^v)\×\mathrm{\Δt}+N_8^v$

N ^vfor system noise, Δ t is sensor sample interval.

(2) observation equation switches according to the validity of Long baselines locator data, and Long baselines data significant instant refers to Long baselines Data Update and is not moment of outlier.

(3) senior filter of AUKF selects UKF to carry out filtering estimation to state equation.The Changing Pattern of system noise distribution is unknown, is now usually regarded as the uncorrelated random drift amount that noise drives, therefore selects KF as the extension filter of AUKF.

(4) structure of extension filter state equation in AUKF algorithm:

By deepwater robot system noise variance matrix Q diagonal entry the state equation of extension filter can be expressed as:

${\stackrel{\‾}{q}}_k=f_q\left({\stackrel{\‾}{q}}_{k-1}\right)+w_{q_k}$

Wherein for the system noise in extension filter, be zero mean Gaussian white noise be zero mean Gaussian white noise;

Due to changing Pattern unknown, the incoherent random drift vector that noise drives can be regarded as, therefore the state equation of extension filter is defined as:

${\stackrel{\‾}{q}}_k={\stackrel{\‾}{q}}_{k-1}+w_{q_k}.$

(5) in AUKF algorithm, the structure of equation measured by extension filter:

Extension filter is the new breath ξ with senior filter _kthe diagonal entry of variance matrix as the observation signal of system, the variance P that the step according to senior filter state vector is estimated _k|k-1, the variance P of senior filter state vector _k, the gain matrix K of senior filter _kthe observation equation of composition extension filter is defined as:

$\stackrel{\‾}{S}{q}_k=g\left({\stackrel{\‾}{q}}_k\right)=\mathrm{vdiag}\left[{\left(K_k^T{K}_k\right)}^{-1}{K}_k^T(P_{k|k-1}-P_k)K_k{\left(K_k^T{K}_k\right)}^{-1}\right]$

Wherein vdiag is a vector of the elements in a main diagonal composition getting matrix in bracket.

Be illustrated in figure 2 the realization flow of self-adaptation uncented Kalman filter algorithm.Self-adaptation uncented Kalman filter is the filtering implementation procedure that senior filter and extension filter combine.

The implementation procedure that senior filter filtering is estimated:

To a upper moment random vector by estimator average with variance P _k-1after non-linear colourless conversion, one group that obtains discrete Sigma point χ _k-1, calculate through system state equation and upgrade Sigma point, then calculate the state average upgrading Sigma point with variance P _k|k-1, to state average with variance P _k|k-1calculate state average and the variance of a step estimation; The state average estimate a step and variance calculate renewal through colourless conversion with measurement equation again, obtain the average of deepwater robot Observable state its variance and covariance estimated value; Read sensor gathers observable information y _kwith state average with variance P _k|k-1with estimated value and the gain matrix K of Observable state _kestimate as calculated, obtain system average variance P _kestimated value, therefrom newly ceased ξ _k; According to the new breath ξ of senior filter _kcalculate measured value Sq _kas the observation signal of extension filter, then continue extension filter program step.

Extension filter KF carries out the implementation procedure of filtering estimation to systematic procedure noise:

A upper moment estimates the diagonal entry obtaining the system noise variance matrix of extension filter with its variance with the state equation pair of extension filter with calculate after upgrading and obtain diagonal entry with its variance a step estimated value; With the variance P that the measurement equation of extension filter is estimated according to a step of senior filter _k|k-1, state variance P _kwith gain matrix K _kthe observed reading estimated is obtained after upgrading as calculated the step estimated value obtained and the observed reading of estimation with the measured value Sq of senior filter _kestimate as calculated to obtain systematic procedure noise variance matrix Q ^vthe estimated value of diagonal entry with its variance , when there being new observation information y _ktime, continue next control cycle senior filter program step.

Long baselines positioning system can not provide locating information comparatively accurately in some cases, and the method step that described employing resolves robot absolute position according to the robot in Long baselines packet voluntarily far from the range information of subsea beacon (being no less than 3) is as follows:

Each moment receiving Long baselines packet, the range information of robot to each beacon (lay and should be no less than 3 beacons) is parsed under water according to Long baselines data protocol, the validity of each bootstrap information is judged by data pack protocol, according to the absolute location information of each effective beacon and robot to the range information of beacon, set up the ternary quadratic equation group that corresponding ball intersects:

${(x_o-x_A)}^2+{(y_o-y_A)}^2+{(z_o-z_A)}^2=l_A^2$

${(x_o-x_B)}^2+{(y_o-y_B)}^2+{(z_o-z_B)}^2=l_B^2$

${(x_o-x_C)}^2+{(y_o-y_C)}^2+{(z_o-z_C)}^2=l_C^2$

(x in above-mentioned system of equations _a, y _a, z _a), (x _b, y _b, z _b), (x _c, y _c, z _c) be the absolute location coordinates of each effective beacon, l _al _bl _cfor robot is to the distance of each beacon, separates this system of equations and obtain (x _o, y _o, z _o) be the absolute location coordinates of robot.Obtain two groups of solutions of robot absolute coordinates, judge the efficient solution in two groups of solutions according to the depth information of current time depthometer collection, this solution is as the east orientation in measurement vector and north orientation position detection value.

Embodiment result if Fig. 3 is the deepwater robot Long baselines integrated navigation based on AUKF:

In outfield experiments, robot is to be no more than the speed of 1.5m/s at surface navigation.Fig. 3 gives the result utilizing the deepwater robot Long baselines Combinated navigation method that the present invention is based on self-adaptation uncented Kalman filter to carry out robot navigation, and makes comparisons with the positioning result of Long baselines equipment and the navigation results of dead reckoning.Wherein solid line represents the absolute position that GPS obtains; dot-and-dash line represents the navigation results of dead reckoning; point represents the positioning result of Long baselines equipment, and dotted line represents the navigation results of the deepwater robot Long baselines Combinated navigation method based on self-adaptation uncented Kalman filter.Can obviously find out from figure; the track of the robot adopting the deepwater robot Long baselines Combinated navigation method based on self-adaptation uncented Kalman filter to estimate can overcome the cumulative errors being forbidden to increase in time with dead reckoning in Long baselines equipment location in some cases, and closer to the absolute position that GPS obtains.This embodiment shows that the deepwater robot Long baselines Combinated navigation method that the present invention is based on self-adaptation uncented Kalman filter can significantly improve integrated navigation system precision.

Information after fusion comprises: plan position information, depth information, attitude information, linear velocity information, angular velocity information.

Claims (10)

1., based on a deepwater robot Long baselines Combinated navigation method of AUKF, it is characterized in that:

Utilize GPS to obtain the initial point of initial absolute position as reckoning of deepwater robot, and gather the initial information of deepwater robot;

Build uncented Kalman filter senior filter and carry out filtering estimation to the initial information collected, building colourless Kalman's extension filter, after estimating senior filter filtering, the further filtering of information is estimated;

Adopt the method for self-adaptation uncented Kalman filter to carry out data fusion to the initial information collected, draw the information after fusion.

2. the deepwater robot Long baselines Combinated navigation method based on AUKF according to claim 1; it is characterized in that: described initial information comprises the linear velocity information that Doppler log gathers; the attitude information that motion sensor gathers, the positional information that Long baselines acoustic positioning device gathers and the depth information that depthometer gathers.

3. the deepwater robot Long baselines Combinated navigation method based on AUKF according to claim 2, is characterized in that: described Long baselines acoustic positioning device obtains positional information by the method resolving deepwater robot absolute position voluntarily.

4. the deepwater robot Long baselines Combinated navigation method based on AUKF according to claim 3, it is characterized in that: the described method resolving deepwater robot absolute position voluntarily, step is as follows:

Long baselines data pack protocol parses the range information of deepwater robot to each beacon, judges the validity of each bootstrap information, according to the absolute location information of each effective beacon and deepwater robot to the range information of beacon, sets up ternary quadratic equation group:

${(x_o-x_A)}^2+{(y_o-y_A)}^2+{(z_o-z_A)}^2=l_A^2$

${(x_o-x_B)}^2+{(y_o-y_B)}^2+{(z_o-z_B)}^2=l_B^2$

${(x_o-x_C)}^2+{(y_o-y_C)}^2+{(z_o-z_C)}^2=l_C^2$

(x in above-mentioned system of equations _a, y _a, z _a), (x _b, y _b, z _b), (x _c, y _c, z _c) be respectively the absolute location coordinates of each effective beacon, l _a, l _b, l _cbe respectively the distance of deepwater robot to each beacon, separate this system of equations and obtain (x _o, y _o, z _o) be the absolute location coordinates of deepwater robot; obtain two groups of solutions of robot absolute location coordinates; the efficient solution in two groups of solutions is judged, as the east orientation position detection value in measurement vector and north orientation position detection value according to the depth information of current time depthometer collection.

5. the deepwater robot Long baselines Combinated navigation method based on AUKF according to claim 1, is characterized in that: the implementation procedure that colourless Kalman's senior filter filtering is estimated is:

To a upper moment random vector by estimator average and variance after non-linear colourless conversion, obtain discrete Sigma point, state equation through senior filter calculates and upgrades Sigma point, then calculate the state average and variance that upgrade Sigma point, state average and variance are calculated to state average and the variance of a step estimation; The state average estimate a step and variance calculate renewal through the measurement equation of colourless conversion and senior filter again, obtain the estimated value of the average of deepwater robot Observable state, variance and covariance; Read sensor gathers observation information, and calculates with the estimated value of state average, variance, observer state and gain matrix and estimate, obtains the estimated value of system average, variance, and is newly ceased; The observation signal of measured value as extension filter is calculated according to the new breath of senior filter.

6. the deepwater robot Long baselines Combinated navigation method based on AUKF according to claim 1, is characterized in that: the implementation procedure that extension filter carries out filtering estimation to systematic procedure noise is as follows:

A upper moment estimates the diagonal entry and the variance that obtain the system noise variance matrix of extension filter, obtains a step estimated value of diagonal entry and variance with the state equation of extension filter to diagonal entry and variance after calculating renewal; The observed reading estimated is obtained after calculating renewal with the measurement equation of extension filter according to variance, state variance and gain matrix that a step of senior filter is estimated, the measured value of the step estimated value, observed reading and the senior filter that obtain is calculated estimated value and the variance estimating to obtain the diagonal entry of systematic procedure noise variance matrix, when there being new observation information, continue the performing step of next control cycle senior filter.

7. the deepwater robot Long baselines Combinated navigation method based on AUKF according to claim 6, is characterized in that: the state equation of described extension filter is set up as follows:

${\stackrel{\‾}{q}}_k={\stackrel{\‾}{q}}_{k-1}+w_{q_k}$

Wherein for the system noise in extension filter, for system noise variance matrix diagonal element.

8. the deepwater robot Long baselines Combinated navigation method based on AUKF according to claim 6, is characterized in that: the measurement establishing equation of described extension filter is as follows:

$\stackrel{\‾}{S}{q}_k=g\left({\stackrel{\‾}{q}}_k\right)=\mathrm{vdiag}\left[{\left(K_k^T{K}_k\right)}^{-1}{K}_k^T(P_{k|k-1}-P_k)K_k{\left(K_k^T{K}_k\right)}^{-1}\right]$

Wherein, vdiag () represents a vector, and its component is the elements in a main diagonal of matrix (), system noise variance matrix diagonal element as the observation signal of system, P _k|k-1for the variance that a step of senior filter state vector is estimated, P _kfor the variance of senior filter state vector, K _kfor the gain matrix of senior filter, for the observed reading estimated.

9. the deepwater robot Long baselines Combinated navigation method based on AUKF according to claim 5, is characterized in that: the measurement equation of described senior filter switches according to the significant instant of Long baselines locator data:

When the observed reading that Long baselines acoustic positioning device provides is effective, measures equation and be defined as:

y _k＝x _k+N ⁿ

Wherein, y=[u ^y, v ^y, w ^y, r ^y, ξ ^y, η ^y, ζ ^y, ψ ^y] ^tfor measurement vector, wherein u ^y, v ^y, w ^y, r ^yforward speed, side velocity, vertical velocity and the observed quantity turning bow angular velocity respectively, ξ ^y, η ^y, ζ ^y, ψ ^ythe observed quantity to angle of north orientation position, east orientation position, the degree of depth and bow, N ⁿfor observation noise;

When the observed reading that Long baselines acoustic positioning device provides is invalid, measures equation and be defined as:

$y_1=x_1+N_1^n$

$y_2=x_2+N_2^n$

$y_3=x_3+N_3^n$

$y_4=x_4+N_4^n$

$y_5=x_7+N_5^n$

$y_6=x_8+N_6^n$

Wherein, y=[u ^y, v ^y, w ^y, r ^y, ζ ^y, ψ ^y] ^tfor measurement vector.

10. the deepwater robot Long baselines Combinated navigation method based on AUKF according to claim 9, is characterized in that: described significant instant is Long baselines Data Update and is not moment of outlier.