CN103940416B - The AUV multiprogram of a kind of electromagnet log auxiliary resolves air navigation aid parallel - Google Patents

Abstract

The present invention is to provide the AUV multiprogram of a kind of electromagnet log auxiliary and resolve air navigation aid parallel.Gather the output data of gyroscope, accelerometer and electromagnet log；The gyroscope collected and the output data run inertial navigation program of accelerometer, the computing speed of output AUV and position；The speed that the speed resolve inertial navigation program and electromagnet log are measured exports the velocity information of optimization after virtual log processes；The output data degree of being accelerated of the velocity information accelerometer to collecting optimized compensates, using the input as strapdown compass program of the value after compensating；Strapdown compass system is carried out angular rate compensation by speed and the latitude value of inertial navigation program resolving after optimization；By inertial navigation program output position information, strapdown compass program output attitude information, and virtual log output speed information.The present invention, under the premise not improving cost, reduces ocean current to the AUV impact navigated, it is thus achieved that navigation information complete, high-precision.

Description

The AUV multiprogram of a kind of electromagnet log auxiliary resolves air navigation aid parallel

Technical field

The present invention relates to a kind of AUV underwater navigation method, the AUV multiprogram of a kind of electromagnet log auxiliary resolves air navigation aid parallel specifically.

Background technology

Autonomous Underwater Vehicle (AutonomousUnderwaterVehicles, AUV) be widely used in marine resources reconnoitre with develop, recover and salvaging etc. in seafloor topography investigation, ocean.For current development, navigation problem remains one of major technical challenge encountered for AUV, and navigation system must provide for being accurately positioned in remote and Long time scale, speed and attitude information.

For much need homing capability from AUV, generally require satisfied two conflicting requirements, namely high accuracy and low cost.At present, it is comparatively ripe that inertial navigation system has developed, but there is also certain gap when meeting navigation long-range, long-time and high-precision requirement.Based on the characteristic that the error of inertial navigation accumulates in time, at present widely used be the mode utilizing external auxiliary equipment to be combined navigation to improve overall navigation precision and the navigation performance of AUV, including GPS, acoustic navigation, Doppler log etc..But adopting needs AUV to float during GPS auxiliary, and gps signal is subject to external interference；Acoustic navigation needs accurately to lay, in AUV working sea area, the transducer array that operating distance is limited, is suitable only for using at the AUV of finite region, specific marine site execution task；Adopting Doppler log auxiliary is that application is maximum at present, but Doppler log can to emission signal, it is impossible to meets and has concealed task.Publication number is in the patent document of CN102818567A, disclose " the AUV Combinated navigation method that a kind of Ensemble Kalman Filter-particle filter combines ", GPS, Doppler log and electronic compass is utilized to assist, compound filter is adopted to estimate position and attitude, this method relatively costly, auxiliary information is subject to external interference and does not possess disguise, adopts filtering method to add the amount of calculation of system simultaneously.

Summary of the invention

It is an object of the invention to provide a kind of under the premise not improving cost, ocean current can be reduced on the AUV impact navigated, it is thus achieved that the AUV multiprogram of the electromagnet log auxiliary of navigation information complete, high-precision resolves air navigation aid parallel.

The object of the present invention is achieved like this:

Step 1: be arranged on the inertial measurement cluster on AUV and electromagnet log is fully warmed-up the output data of rear continuous acquisition gyroscope, accelerometer and electromagnet log；

Step 2: utilize the output data run inertial navigation program of gyroscope and the accelerometer collected, the computing speed of output AUV and position；

Step 3: the speed that the speed resolve inertial navigation program and electromagnet log are measured exports the velocity information of optimization after virtual log processes；

Step 4: utilize the output data degree of being accelerated of the velocity information accelerometer to collecting optimized to compensate, and using the input as strapdown compass program of the value after compensating；

Step 5: utilize the latitude value that the speed after optimizing and inertial navigation program resolve that strapdown compass system is carried out angular rate compensation；

Step 6: last by inertial navigation program output position information, strapdown compass program output attitude information, and virtual log output speed information.

Present invention additionally comprises following feature:

1, the described velocity information exporting optimization after virtual log processes specifically includes:

The speed that the speed that electromagnet log is measured by virtual log resolves with inertial navigation program carries out integrated treatment, the AUV speed through water after being optimized,

V_Log=V_AUV-V_current+δV_Log

V_SINS=V_AUV+δV_SINS

V_out=V_AUV-V_current

Wherein, V_LogRepresent the speed that electromagnet log is measured, V_AUVRepresent AUV true velocity, V_currentRepresent ocean current speed, δ V_LogRepresent electromagnet log range rate error, V_SINSRepresent the speed that inertial navigation program resolves, δ V_SINSRepresent the velocity error that inertial navigation program resolves, V_outRepresent the output speed after virtual log processes；

By the HMM/KF(of time delay hidden markov model/Kalman filtering) digital filter, by δ V_LogWith δ V_SINSFilter,

Discrete HMM model representation is:

$\left\{\begin{array}{c}{X}_{k+1}={\mathrm{FX}}_k+{\mathrm{\ξ}}_k\\ Z_{k+1}={\mathrm{HX}}_k+v_k\end{array}\right.$

Wherein: X is state vector, Z is observation vector, and F is state-transition matrix, and H is observing matrix, ξ_kFor system noise vector, ν_kFor measurement noise vector, X ∈ R^N, Z ∈ R^M, And F_ij,H_ij> 0；

The filtering equations of HMM/KF wave filter is:

$\left\{\begin{array}{c}{\hat{X}}_{k/k-1}=F_{k,k-1}{\hat{X}}_{k-1}\\ {\hat{X}}_k={\hat{X}}_{k/k-1}+K_{\mathrm{off}}(Z_k-H_k{\hat{X}}_{k/k-1})\end{array}\right.$

Wherein:Respectively k-1 and k moment Filtering Estimation,For k moment predicted state vector, Z_kFor k moment observation, F_k,k-1For Matrix of shifting of a step, H_kFor measuring battle array, K_offFor filtering gain；

K_offCalculating process as follows:

$\left\{\begin{array}{c}{P}_{k-1}^{-1}=P_{k-1/k-2}^{-1}+H_{k-1}^T{R}_{k-1}^{-1}{H}_{k-1}\\ P_{k/k-1}=F_{k/k-1}{P}_{k-1}{F}_{k/k-1}^T+Q_{k-1}\\ K_{\mathrm{off}}=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}\end{array}\right.$

Wherein: P_kFor estimating mean square error, P_k/k-1For one-step prediction mean square error, Q_kSystem noise variance matrix, R_kFor measuring noise square difference battle array.

2, the described accelerometer data degree of being accelerated to collecting of the speed after optimizing that utilizes compensates, and the value after compensation is specifically included as the input of strapdown compass program:

The harmful acceleration compensation value caused by speed is:

The acceleration of motion offset caused by speed can be obtained by the velocity differentials after virtual log processes:

$A={\stackrel{\·}{V}}_{\mathrm{out}}$

Value after acceleration compensation is:

f′=f^b-A-B

Wherein: f ' represents the accelerometer data after acceleration compensation, f^bRepresenting the accelerometer data collected, B represents the harmful acceleration caused by speed, and A represents the acceleration of motion caused by speed, and Ω represents rotational-angular velocity of the earth, and R represents earth radius,Representing local latitude, subscript E and N is illustrated respectively in the projection of east orientation and north orientation.

Strapdown compass system is carried out angular rate compensation and specifically includes by the latitude value that 3, the described speed after utilization optimization and inertial navigation program resolve:

The rotational-angular velocity of the earth offset that latitude change causes is:

The angular rate compensation value that involves that speed and latitude change cause is:

Wherein:Represent and changed, by latitude, the rotational-angular velocity of the earth changing value caused,Represent and involved angular velocity changing value by what speed and latitude change caused,Represent latitude changing value

The method of the present invention has the advantage that (1) effectively overcomes ocean current to the AUV impact navigated.(2) navigation information complete, high-precision (including speed, position, attitude information) is obtained.(3) not to emission signal, possesses disguise.(4) cost is low, it is only necessary to electromagnet log is assisted.

Beneficial effects of the present invention is illustrated as follows:

In order to verify the practicality of the present invention, carry out Matlab emulation experiment.

Gyroscope constant value drift is: ε_i=0.01 °/h (i=x, y, z), Gyro Random migration is: ε_ri=0.005 °/h (i=x, y, z), scale factor error is: δ K_gi=10ppm (i=x, y, z)；

Accelerometer bias is:, accelerometer noise is:, scale factor error is: δ K_ai=10ppm (i=x, y, z)；

Initial position is: longitude is 126.6705 °, and latitude is 45.7796 °；

AUV 50m uniform rectilinear under water navigates by water, and speed is 5kn, and hours underway is 15 hours.

Simulation result shows that multiprogram resolves the velocity error of output parallel and inhibits low frequency periodic to vibrate compared with the velocity error that independent SINS resolves, multiprogram resolve parallel position error that the position error of output resolves compared to independent SINS by 15 hours 4 nautical miles be reduced to 15 hours 1.3 nautical miles, multiprogram resolves the attitude error of output parallel can reduce the attitude saltus step caused in AUV mobile process compared with the attitude error of independent strapdown compass system, strengthens the robustness of system.

Accompanying drawing explanation

Fig. 1 is the design frame chart of the present invention；

Fig. 2 is the algorithm principle figure of the present invention；

Fig. 3 (a)-Fig. 3 (d) is velocity error correlation curve, wherein: Fig. 3 (a)-Fig. 3 (b) is SINS, and Fig. 3 (c)-Fig. 3 (d) resolves parallel for multiprogram；

Fig. 4 (a)-Fig. 4 (b) is position error correlation curve, and Fig. 4 (a) is SINS, and Fig. 4 (b) resolves parallel for multiprogram；

Fig. 5 (a)-Fig. 5 (f) is attitude error correlation curve, and Fig. 5 (a)-Fig. 5 (c) is SINS, and Fig. 5 (c)-Fig. 5 (f) resolves parallel for multiprogram.

Detailed description of the invention

The present invention comprises the following steps:

Step 1: be arranged on AUV by inertial measurement cluster and electromagnet log, is fully warmed-up rear continuous acquisition gyroscope, accelerometer and electromagnet log output data.

Step 2: utilize the gyroscope gathered and accelerometer data to run inertial navigation program, the computing speed of output AUV and position.

Step 3: the speed that the speed resolve inertial navigation program and electromagnet log are measured exports the velocity information of optimization after virtual log processes.

The speed that the effect of virtual log is speed and the inertial navigation program resolving measured by electromagnet log carries out integrated treatment, the AUV speed through water after being optimized.

V_Log=V_AUV-V_current+δV_Log

V_SINS=V_AUV+δV_SINS

V_out=V_AUV-V_current

Wherein, V_LogRepresent the speed that electromagnet log is measured, V_AUVRepresent AUV true velocity, V_currentRepresent ocean current speed, δ V_LogRepresent electromagnet log range rate error, V_SINSRepresent the speed that inertial navigation program resolves, δ V_SINSRepresent the velocity error that inertial navigation program resolves, V_outRepresent the output speed after virtual log processes.

The essence of virtual log is a digital filter in fact, it is therefore an objective to by δ V_LogWith δ V_SINSFiltering, what design here is the HMM/KF(hidden markov model/Kalman filtering of non-time delay) digital filter.

Discrete HMM model is represented by:

$\left\{\begin{array}{c}{X}_{k+1}={\mathrm{FX}}_k+{\mathrm{\ξ}}_k\\ Z_{k+1}={\mathrm{HX}}_k+v_k\end{array}\right.$

Wherein, X is state vector, and Z is observation vector, and F is state-transition matrix, and H is observing matrix, ξ_kFor system noise vector, ν_kFor measurement noise vector, X ∈ R^N, Z ∈ R^M, And F_ij,H_ij>0。

The filtering equations of HMM/KF wave filter is:

$\left\{\begin{array}{c}{\hat{X}}_{k/k-1}=F_{k,k-1}{\hat{X}}_{k-1}\\ {\hat{X}}_k={\hat{X}}_{k/k-1}+K_{\mathrm{off}}(Z_k-H_k{\hat{X}}_{k/k-1})\end{array}\right.$

Wherein,Respectively k-1 and k moment Filtering Estimation,For k moment predicted state vector, Z_kFor k moment observation, F_k,k-1For Matrix of shifting of a step, H_kFor measuring battle array, K_offFor filtering gain.

K_offCalculating process as follows:

$\left\{\begin{array}{c}{P}_{k-1}^{-1}=P_{k-1/k-2}^{-1}+H_{k-1}^T{R}_{k-1}^{-1}{H}_{k-1}\\ P_{k/k-1}=F_{k/k-1}{P}_{k-1}{F}_{k/k-1}^T+Q_{k-1}\\ K_{\mathrm{off}}=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}\end{array}\right.$

Wherein, P_kFor estimating mean square error, P_k/k-1For one-step prediction mean square error, Q_kSystem noise variance matrix, R_kFor measuring noise square difference battle array.

The input of virtual log is electromagnet log measuring speed and SINS computing speed, is output as AUV speed through water accurate measurements.

Step 4: utilize the accelerometer data degree of being accelerated to gathering of the speed after optimizing to compensate, and using the value after compensating as the input of strapdown compass program；

The harmful acceleration compensation value caused by speed is:

The acceleration of motion offset caused by speed can be obtained by the velocity differentials after virtual log processes:

$A={\stackrel{\·}{V}}_{\mathrm{out}}$

Value after acceleration compensation is:

f′=f^b-A-B

Wherein, f ' represents the accelerometer data after acceleration compensation, f^bRepresenting the accelerometer data collected, B represents the harmful acceleration caused by speed, and A represents the acceleration of motion caused by speed, and Ω represents rotational-angular velocity of the earth, and R represents earth radius,Representing local latitude, subscript E and N is illustrated respectively in the projection of east orientation and north orientation.

Step 5: utilize the latitude value that the speed after optimizing and inertial navigation program resolve that strapdown compass system is carried out angular rate compensation；

The rotational-angular velocity of the earth offset that latitude change causes is:

The angular rate compensation value that involves that speed and latitude change cause is:

Wherein,Represent and changed, by latitude, the rotational-angular velocity of the earth changing value caused,Represent and involved angular velocity changing value by what speed and latitude change caused,Represent latitude changing value.

Step 6: last by inertial navigation program output position information, strapdown compass program output attitude information, and virtual log output speed information.

Claims (5)

1. the AUV multiprogram of electromagnet log auxiliary resolves an air navigation aid parallel, it is characterized in that:

Step 1: be arranged on the inertial measurement cluster on AUV and electromagnet log is fully warmed-up the output data of rear continuous acquisition gyroscope, accelerometer and electromagnet log；

Step 2: utilize the output data run inertial navigation program of gyroscope and the accelerometer collected, the computing speed of output AUV and position；

Step 3: the speed that the speed resolve inertial navigation program and electromagnet log are measured exports the velocity information of optimization after virtual log processes；

Step 4: utilize the output data degree of being accelerated of the velocity information accelerometer to collecting optimized to compensate, and using the input as strapdown compass program of the value after compensating；

Step 5: utilize the latitude value that the speed after optimizing and inertial navigation program resolve that strapdown compass system is carried out angular rate compensation；

Step 6: last by inertial navigation program output position information, strapdown compass program output attitude information, and virtual log output speed information.

2. the AUV multiprogram of electromagnet log according to claim 1 auxiliary resolves air navigation aid parallel, it is characterized in that the described velocity information exporting optimization after virtual log processes specifically includes:

The speed that the speed that electromagnet log is measured by virtual log resolves with inertial navigation program carries out integrated treatment, the AUV speed through water after being optimized,

V_Log=V_AUV-V_current+δV_Log

V_SINS=V_AUV+δV_SINS

V_out=V_AUV-V_current

Wherein, V_LogRepresent the speed that electromagnet log is measured, V_AUVRepresent AUV true velocity, V_currentRepresent ocean current speed, δ V_LogRepresent electromagnet log range rate error, V_SINSRepresent the speed that inertial navigation program resolves, δ V_SINSRepresent the velocity error that inertial navigation program resolves, V_outRepresent the output speed after virtual log processes；

By the hidden markov model of time delay/Kalman filtering digital filter, by δ V_LogWith δ V_SINSFilter,

Discrete hidden markov model model representation is:

$\left\{\begin{array}{c}{X}_{k+1}=F{X}_k+{\mathrm{\ξ}}_k\\ Z_{k+1}={\mathrm{HX}}_k+v_k\end{array}\right.$

Wherein: X is state vector, Z is observation vector, and F is state-transition matrix, and H is observing matrix, ξ_kFor system noise vector, ν_kFor measurement noise vector, X ∈ R^N, Z ∈ R^M,And F_ij,H_ij> 0；

The filtering equations of hidden markov model/Kalman filtering wave filter is:

$\left\{\begin{array}{c}{\hat{X}}_{k/k-1}=F_{k,k-1}{\hat{X}}_{k-1}\\ {\hat{X}}_k={\hat{X}}_{k/k-1}+K_{off}(Z_k-H_k{\hat{X}}_{k/k-1})\end{array}\right.$

Wherein:Respectively k-1 and k moment Filtering Estimation,For k moment predicted state vector, Z_kFor k moment observation, F_k,k-1For Matrix of shifting of a step, H_kFor measuring battle array, K_offFor filtering gain；

K_offCalculating process as follows:

$\left\{\begin{array}{c}{P}_{k-1}^{-1}=P_{k-1/k-2}^{-1}+H_{k-1}^T{R}_{k-1}^{-1}{H}_{k-1}\\ P_{k/k-1}=F_{k/k-1}{P}_{k-1}{F}_{k/k-1}^T+Q_{k-1}\\ K_{off}=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}\end{array}\right.$

Wherein: P_kFor estimating mean square error, P_k/k-1For one-step prediction mean square error, Q_kSystem noise variance matrix, R_kFor measuring noise square difference battle array.

3. the AUV multiprogram of electromagnet log according to claim 1 and 2 auxiliary resolves air navigation aid parallel, it is characterized in that the described output data degree of being accelerated utilizing the velocity information the optimized accelerometer to collecting compensates, and the value after compensation specifically included as the input of strapdown compass program:

The harmful acceleration compensation value caused by speed is:

The acceleration of motion offset caused by speed can be obtained by the velocity differentials after virtual log processes:

$A={\stackrel{\·}{V}}_{out}$

Value after acceleration compensation is:

F '=f^b-A-B

Wherein: f ' represents the accelerometer data after acceleration compensation, f^bRepresenting the accelerometer data collected, B represents the harmful acceleration caused by speed, and A represents the acceleration of motion caused by speed, and Ω represents rotational-angular velocity of the earth, and R represents earth radius,Representing local latitude, subscript E and N is illustrated respectively in the projection of east orientation and north orientation.

4. the AUV multiprogram of electromagnet log according to claim 1 and 2 auxiliary resolves air navigation aid parallel, it is characterized in that strapdown compass system is carried out angular rate compensation and specifically includes by the described latitude value utilizing the speed after optimizing and the resolving of inertial navigation program:

The rotational-angular velocity of the earth offset that latitude change causes is:

The angular rate compensation value that involves that speed and latitude change cause is:

Wherein:Represent and changed, by latitude, the rotational-angular velocity of the earth changing value caused,Represent and involved angular velocity changing value by what speed and latitude change caused,Represent latitude changing value.

5. the AUV multiprogram of electromagnet log according to claim 3 auxiliary resolves air navigation aid parallel, it is characterized in that strapdown compass system is carried out angular rate compensation and specifically includes by the described latitude value utilizing the speed after optimizing and the resolving of inertial navigation program:

The rotational-angular velocity of the earth offset that latitude change causes is:

The angular rate compensation value that involves that speed and latitude change cause is:

Wherein:Represent and changed, by latitude, the rotational-angular velocity of the earth changing value caused,Represent and involved angular velocity changing value by what speed and latitude change caused,Represent latitude changing value.