阅读说明：本技术 一种无源模式下的sins/usbl组合导航定位方法 (SINS/USBL combined navigation positioning method in passive mode ) 是由 郑翠娥 刘云 孙大军 张居成 韩云峰 崔宏宇 张殿伦 于 2020-03-04 设计创作，主要内容包括：一种无源模式下的SINS/USBL组合导航定位方法,涉及组合导航及水声定位技术领域,针对现有技术中无源模式下SINS/USBL紧组合导航定位精度低的问题,本发明引入深度计信息,设计USBL无源工作模式下的时钟误差的计算方法,补偿USBL单通道的时延误差,以提高组合导航定位的精度。本发明能在有限的基阵开角范围内提高SINS/USBL紧组合导航定位精度。(Aiming at the problem that SINS/USBL tightly combined navigation positioning precision is low in the prior art, the SINS/USBL combined navigation positioning method in the passive mode introduces depth meter information, designs a clock error calculation method in the USBL passive working mode, and compensates time delay error of a USBL single channel so as to improve the precision of combined navigation positioning. The invention can improve the SINS/USBL tight combination navigation positioning precision in the limited range of the array opening angle.)

1. A SINS/USBL combined navigation positioning method in a passive mode is characterized by comprising the following steps:

the method comprises the following steps: the USBL acoustic array is inversely installed on an underwater vehicle, a gyro component, an addition component and a depth meter of the SINS are fixedly connected on the underwater vehicle, an asynchronous beacon is arranged on the water surface, and a GPS is arranged to acquire the position information of the beacon under a navigation coordinate system;

step two: establishing an acoustic matrix coordinate system, and determining the position of a primitive in the matrix coordinate system;

step three: measuring propagation delay and delay difference of an acoustic signal between a beacon and a primitive by using USBL (universal serial bus), obtaining a measured value of the USBL propagation delay and a measured value of the USBL propagation delay difference, and resolving the direction angle cosine of an aircraft relative to the beacon under a matrix coordinate system by combining an effective sound velocity, wherein the effective sound velocity is the ratio of the geometric distance between the beacon and a receiving matrix to the propagation time of the acoustic signal between two points;

step four: acquiring depth information of an aircraft, judging whether a relative position relation between the aircraft and a beacon meets a matrix open angle condition by combining direction angle cosine, if so, calculating to obtain a predicted value of the slant distance between the beacon and the aircraft and a predicted value of the propagation delay of an acoustic signal between the beacon and a matrix center, if so, calculating to obtain a predicted value of the slant distance and a predicted value of the propagation delay of two continuous sampling moments, executing a step five, if not, judging whether a clock error estimated value of an adjacent moment exists, if so, executing a step six after using the estimated value, if not, continuing to use a USBL propagation delay measured value obtained in the step three, and executing a step seven, wherein the matrix open angle is an included angle between a connecting line of an origin of a matrix coordinate system and the beacon and a positive direction of a z axis;

step five: establishing a clock error model, estimating error model parameters by using the difference between the propagation delay predicted values of two continuous sampling moments and the propagation delay mean value of each element measured by the corresponding USBL, and calculating a clock error estimated value;

step six: compensating the propagation delay of the USBL measurement by using the clock error estimated value to obtain a new USBL propagation delay measurement value;

step seven: calculating the attitude and the position of the underwater vehicle by using the SINS, and converting the relative position of the primitive under the matrix coordinate system into a navigation coordinate system used by the SINS according to the obtained attitude and position;

step eight: calculating propagation delay and propagation delay difference of acoustic signals between each element and a beacon according to the position information of the SINS converted elements in a navigation coordinate system, and obtaining SINS propagation delay measurement values and propagation delay difference measurement values;

step nine: establishing an information fusion extended Kalman filter based on the difference between the USBL propagation delay measurement value and the SINS propagation delay measurement value and the difference between the USBL propagation delay measurement value and the SINS propagation delay measurement value in the step eight according to the USBL propagation delay measurement value and the SINS propagation delay measurement value in the step eight;

step ten: calculating SINS errors by using the information fusion extended Kalman filter, correcting navigation output, resetting the state of the extended Kalman filter, and re-executing the third step to the tenth step.

2. The SINS/USBL combined navigation and positioning method under the passive mode as recited in claim 1, wherein the specific step of establishing the acoustic matrix coordinate system in the second step is:

firstly, taking the center of a basic array as an original point, then taking the heading direction pointing to an underwater vehicle along the plane of the basic array as a y axis, wherein a z axis is vertical to the plane of the basic array and faces upwards, and the x axis, the y axis and the z axis form a right-hand coordinate system;

and in the step eight, the navigation coordinate system is a northeast geographical coordinate system, the northeast geographical coordinate system takes the center of mass of the underwater vehicle as an origin, the x axis points to the geographical east direction, the y axis points to the geographical north direction, and the z axis is perpendicular to the xoy plane and points to the sky direction, so that a right-hand coordinate system is formed.

3. The SINS/USBL combined navigation and positioning method in the passive mode as recited in claim 1, wherein the information fusion extended Kalman filter takes SINS attitude error, velocity error, position error, gyro drift, and accelerometer bias as state variables, and takes USBL/SINS propagation delay difference and USBL/SINS propagation delay difference as observed quantities, and under the minimum mean square error criterion, a state equation and an observation equation describing the system are established.

4. The SINS/USBL combined navigation and positioning method in passive mode as claimed in claim 1, wherein the position of the primitive in the matrix coordinate system is:

wherein i is the primitive sequence number and r is the distance between primitives 1 and 3 or primitives 2 and 4.

5. The method as claimed in claim 4, wherein the cosine of the direction angle in the three steps is:

wherein c is the effective sound velocity, d is the corresponding element pitch,

6. The SINS/USBL combined navigation positioning method under the passive mode as claimed in claim 5, wherein the specific steps of estimating error model parameters, calculating clock error estimation values, and compensating the propagation delay error of the USBL acoustic array in the sixth step using the difference between the propagation delay prediction value of the adjacent time and the acoustic signal propagation delay mean value measured by the corresponding USBL underwater acoustic positioning system are as follows:

firstly, a clock error tau in a USBL passive working mode and an acoustic signal propagation delay tau measured by the USBL meet the condition that tau is a tau + b, a is the change rate of the clock error, b is a constant error, and the combination type is used

when array opening angle theta_zIs less than 60 degrees in absolute value,

wherein, tau_iIs the true value of the delay measurement of the cell i, tau is the true value of the clock error,is a compensated clock error, n_iIs the measurement noise, τ, of the element i₃₁Is the real value of the delay difference of the elements 1,3, n₃₁Is the delay difference measurement noise, τ, of the elements 1,3₄₂Is the real value of the delay difference of the primitives 2,4, n₄₂Is the delay difference measurement noise for primitives 4, 2.

7. The SINS/USBL combined navigation and positioning method in the passive mode as recited in claim 6, wherein the specific steps of the seventh step are:

firstly, combining the installation deviation calibration results of a base array coordinate system and a carrier coordinate system to obtain the position of a basic element under a navigation coordinate system, wherein the attitude of the aircraft comprises a course angle A, a pitch angle K and a roll angle psi, the angle installation deviation of the carrier coordinate system corresponding to the acoustic base array and the aircraft is α, β and gamma respectively, and the installation deviation of the position is that [ delta X delta y delta z ═ is]^TTransformation matrix from carrier coordinate system to navigation coordinate systemConversion matrix from acoustic matrix coordinate system to carrier coordinate systemRespectively as follows:

the position of the ith primitive in the navigation coordinate system is represented as

8. The SINS/USBL combined navigation and positioning method in passive mode as recited in claim 7, wherein said step eight propagation delay

the calculated primitive positions for the SINS,in order to be the true position of the primitive,is composed of

the propagation delay difference is:

9. the SINS/USBL combined navigation and positioning method in the passive mode as recited in claim 8, wherein the specific steps of said ninth step are:

the SINS position is in the form of latitude L, longitude lambda and height h, and is converted into an earth rectangular coordinate, and then

Wherein R is_NIs the main curvature radius of the earth-unitary fourth of twelve earthly branches,

the differential form of the above equation is:

note the book

The transformation matrix from the earth rectangular coordinate system to the SINS calculation navigation coordinate system is as follows:

the extended Kalman filter state variable is set as [ phi ] by SINS attitude error phi_xφ_yφ_z]^TVelocity error v ═ v_xv_yv_z]^TThe position error p ═ L λ h]^TThe drift error of gyro ═ 2_{x y z}]^TBias error of accelerometer

The equation of state is

X_k+1＝F_k+1/kX_k+w_k+1

Wherein, F_k+1/kFor the state transition matrix, obtained from the error equation of SINS, w_k+1Is an extended Carl filtering process noise vector; the observed quantity of the extended Kalman filter is

The observation equation is

Z_k+1＝H_k+1X_k+1+v_k+1

Wherein v is_k+1Is a sequence of white gaussian noise, and is,

propagation delay

note the book

Observation matrix H_k+1Is composed of

10. The SINS/USBL combined navigation and positioning method in the passive mode as recited in claim 9, wherein the specific steps of the tenth step are:

step eleven: according toCalculating the predicted value of the state at the moment of k +1

Step twelve: according to P_k+1/k＝F_k+1/kP_kF^T _k+1/k+Q_kCalculating a state prediction error covariance matrix P at time k +1_k+1/kIn which P is_kEstimating an error covariance matrix, Q, for a state at time k_kA system process noise covariance matrix at the moment k;

step thirteen: according to K_k+1＝P_kH^T _k+1(H_k+1P_kH^T _k+1+R_k+1)^-1Calculating the filter gain K at time K +1_k+1Wherein R is_k+1And H_k+1Respectively is a system observation noise covariance matrix and an observation matrix at the moment of k + 1;

fourteen steps: according to P_k+1＝(I-K_k+1H_k+1)P_kCalculating a state estimation error covariance matrix P at time k +1_k+1；

Step fifteen: according to

Sixthly, the steps are as follows: according to

Technical Field

The invention relates to the technical field of integrated navigation and underwater acoustic positioning, in particular to a SINS/USBL integrated navigation positioning method in a passive mode.

Background

The passive working mode of the USBL underwater acoustic positioning system means that a sound source clock and a measuring system clock are not synchronous, so that acoustic signal propagation time delays measured by all channels of the USBL acoustic receiving array all contain the same clock error. When the time delay difference of every two channels is utilized, the influence of clock errors can be mutually counteracted, the time delay precision of a single channel is still influenced by the clock errors, and the ranging and positioning precision of the USBL is reduced. The traditional SINS/USBL tightly-combined navigation is established in an USBL active working mode, propagation delay and delay difference (slope distance and slope distance difference) are used as the basis of data fusion, and the research on the SINS/USBL tightly-combined navigation in a passive mode is less.

Disclosure of Invention

The purpose of the invention is: aiming at the problem of low SINS/USBL tight combination navigation positioning precision in the prior art in the passive mode, a SINS/USBL combination navigation positioning method in the passive mode is provided.

The technical scheme adopted by the invention to solve the technical problems is as follows:

a SINS/USBL combined navigation positioning method in a passive mode comprises the following steps:

the method comprises the following steps: the USBL acoustic array is inversely installed on an underwater vehicle, a gyro component, an addition component and a depth meter of the SINS are fixedly connected on the underwater vehicle, an asynchronous beacon is arranged on the water surface, and a GPS is arranged to acquire the position information of the beacon under a navigation coordinate system;

step two: establishing an acoustic matrix coordinate system, and determining the position of a primitive in the matrix coordinate system;

step three: measuring propagation delay and delay difference of an acoustic signal between a beacon and a primitive by using USBL (universal serial bus), obtaining a measured value of the USBL propagation delay and a measured value of the USBL propagation delay difference, and resolving the direction angle cosine of an aircraft relative to the beacon under a matrix coordinate system by combining an effective sound velocity, wherein the effective sound velocity is the ratio of the geometric distance between the beacon and a receiving matrix to the propagation time of the acoustic signal between two points;

step four: acquiring depth information of an aircraft, judging whether a relative position relation between the aircraft and a beacon meets a matrix open angle condition by combining direction angle cosine, if so, calculating to obtain a predicted value of the slant distance between the beacon and the aircraft and a predicted value of the propagation delay of an acoustic signal between the beacon and a matrix center, if so, calculating to obtain a predicted value of the slant distance and a predicted value of the propagation delay of two continuous sampling moments, executing a step five, if not, judging whether a clock error estimated value of an adjacent moment exists, if so, executing a step six after using the estimated value, if not, continuing to use a USBL propagation delay measured value obtained in the step three, and executing a step seven, wherein the matrix open angle is an included angle between a connecting line of an origin of a matrix coordinate system and the beacon and a positive direction of a z axis;

step five: establishing a clock error model, estimating error model parameters by using the difference between the propagation delay predicted values of two continuous sampling moments and the propagation delay mean value of each element measured by the corresponding USBL, and calculating a clock error estimated value;

step six: compensating the propagation delay of the USBL measurement by using the clock error estimated value to obtain a new USBL propagation delay measurement value;

step seven: calculating the attitude and the position of the underwater vehicle by using the SINS, and converting the relative position of the primitive under the matrix coordinate system into a navigation coordinate system used by the SINS according to the obtained attitude and position;

step eight: calculating propagation delay and propagation delay difference of acoustic signals between each element and a beacon according to the position information of the SINS converted elements in a navigation coordinate system, and obtaining SINS propagation delay measurement values and propagation delay difference measurement values;

step nine: establishing an information fusion extended Kalman filter based on the difference between the USBL propagation delay measurement value and the SINS propagation delay measurement value and the difference between the USBL propagation delay measurement value and the SINS propagation delay measurement value in the step eight according to the USBL propagation delay measurement value and the SINS propagation delay measurement value in the step eight;

step ten: calculating SINS errors by using the information fusion extended Kalman filter, correcting navigation output, resetting the state of the extended Kalman filter, and re-executing the third step to the tenth step.

Further, the specific steps of establishing the acoustic matrix coordinate system in the second step are as follows:

firstly, taking the center of a basic array as an original point, then taking the heading direction pointing to an underwater vehicle along the plane of the basic array as a y axis, wherein a z axis is vertical to the plane of the basic array and faces upwards, and the x axis, the y axis and the z axis form a right-hand coordinate system;

and in the step eight, the navigation coordinate system is a northeast geographical coordinate system, the northeast geographical coordinate system takes the center of mass of the underwater vehicle as an origin, the x axis points to the geographical east direction, the y axis points to the geographical north direction, and the z axis is perpendicular to the xoy plane and points to the sky direction, so that a right-hand coordinate system is formed.

Furthermore, the information fusion extended Kalman filter takes attitude error, speed error, position error, gyro drift and accelerometer bias of SINS as state variables, takes the difference between the propagation delay of USBL and the propagation delay of SINS and the difference between the propagation delay differences as observed quantities, and establishes a state equation and an observation equation of a description system under the minimum mean square error criterion.

Further, the positions of the primitives in the matrix coordinate system are as follows:

wherein i is the primitive sequence number and r is the distance between primitives 1 and 3 or primitives 2 and 4.

Further, the cosine of the direction angle in the third step is:

wherein c is the effective sound velocity, d is the corresponding element pitch,the propagation delay of the acoustic signal between each primitive and beacon measured for the USBL positioning system,andis the propagation delay difference.

Further, the specific steps of estimating error model parameters, calculating a clock error estimation value and compensating the propagation delay error of the USBL acoustic array by using the difference between the propagation delay prediction value of the adjacent time and the acoustic signal propagation delay mean value measured by the corresponding USBL underwater acoustic positioning system in the sixth step are as follows:

firstly, a clock error tau in a USBL passive working mode and an acoustic signal propagation delay tau measured by the USBL meet the condition that tau is a tau + b, a is the change rate of the clock error, b is a constant error, and the combination type is usedFormula (II)And formula

Calculating estimated values of a and b

And

when array opening angle theta_zIs less than 60 degrees in absolute value,and

the value of the adjacent update is kept unchanged,calculating a clock error estimate

Namely, the formula for compensating the propagation delay error of the USBL acoustic array is as follows:

wherein, tau_iIs the true value of the delay measurement of the cell i, tau is the true value of the clock error,is a compensated clock error, n_iIs the measurement noise, τ, of the element i₃₁Is the real value of the delay difference of the elements 1,3, n₃₁Is the delay difference measurement noise, τ, of the elements 1,3₄₂Is the real value of the delay difference of the primitives 2,4, n₄₂Is the delay difference measurement noise for primitives 4, 2.

Further, the specific steps of the seventh step are as follows:

firstly, combining the installation deviation calibration results of a base array coordinate system and a carrier coordinate system to obtain the position of a basic element under a navigation coordinate system, wherein the attitude of the aircraft comprises a course angle A, a pitch angle K and a roll angle psi, the angle installation deviation of the carrier coordinate system corresponding to the acoustic base array and the aircraft is α, β and gamma respectively, and the installation deviation of the position is that [ delta X delta y delta z ═ is]^TTransformation matrix from carrier coordinate system to navigation coordinate systemConversion matrix from acoustic matrix coordinate system to carrier coordinate system

Respectively as follows:

the position of the ith primitive in the navigation coordinate system is represented as

Further, the propagation delay in the step eightComprises the following steps:

the calculated primitive positions for the SINS,in order to be the true position of the primitive,is composed of

Andthe error between the two-dimensional data of the two-dimensional data,is the beacon position, c is the effective speed of sound;

the propagation delay difference is:

further, the step nine comprises the following specific steps:

the SINS position is in the form of latitude L, longitude lambda and height h, and is converted into an earth rectangular coordinate, and then

Wherein R is_NIs the main curvature radius of the earth-unitary fourth of twelve earthly branches,R_eis the radius of the earth, e is the eccentricity of the earth,wherein a and b are the radii of the major and minor axes of the ellipse, respectively;

the differential form of the above equation is:

note the book

The transformation matrix from the earth rectangular coordinate system to the SINS calculation navigation coordinate system is as follows:

the extended Kalman filter state variable is set as [ phi ] by SINS attitude error phi_xφ_yφ_z]^TVelocity error v ═ v_xv_yv_z]^TThe position error p ═ L λ h]^TThe drift error of gyro ═ 2_{x y z}]^TBias error of accelerometerIs composed of, i.e.

The equation of state is

X_k+1＝F_k+1/kX_k+w_k+1

Wherein, F_k+1/kFor the state transition matrix, obtained from the error equation of SINS, w_k+1Is an extended Carl filtering process noise vector; the observed quantity of the extended Kalman filter is

The observation equation is

Z_k+1＝H_k+1X_k+1+v_k+1

Wherein v is_k+1Is a sequence of white gaussian noise, and is,

propagation delayThe full differential of (d) is expressed as:

note the book

Observation matrix H_k+1Is composed of

Further, the specific steps of the tenth step are as follows:

step eleven: according to

Calculating the predicted value of the state at the moment of k +1

Step twelve: according to P_k+1/k＝F_k+1/kP_kF^T _k+1/k+Q_kCalculating a state prediction error covariance matrix P at time k +1_k+1/kIn which P is_kEstimating an error covariance matrix, Q, for a state at time k_kA system process noise covariance matrix at the moment k;

step thirteen: according to K_k+1＝P_kH^T _k+1(H_k+1P_kH^T _k+1+R_k+1)^-1Calculating the filter gain K at time K +1_k+1Wherein R is_k+1And H_k+1Respectively is a system observation noise covariance matrix and an observation matrix at the moment of k + 1;

fourteen steps: according to P_k+1＝(I-K_k+1H_k+1)P_kCalculating a state estimation error covariance matrix P at time k +1_k+1；

Step fifteen: according toCalculating a state estimation value at the time of k +1

Sixthly, the steps are as follows: according to

Correcting SINS output according to the calculation result, resetting the state of the extended Kalman filter, and re-executing the third step to the tenth step.

The invention has the beneficial effects that:

according to the invention, depth meter information is introduced, a clock error calculation method in a USBL passive working mode is designed, and a time delay error of a single channel of the USBL is compensated, so that the precision of integrated navigation positioning is improved. The invention can improve the SINS/USBL tight combination navigation positioning precision in the limited range of the array opening angle.

Drawings

FIG. 1 is a schematic diagram of USBL clock error estimation in passive mode;

FIG. 2 is a diagram of the relative position of the aircraft and the beacon in the matrix coordinate system;

fig. 3 is a schematic diagram of a matrix coordinate system, a carrier coordinate system, and a navigation coordinate system.

Detailed Description