CN113959434B - Adjustable SINS, DVL, USBL integrated navigation method - Google Patents

Adjustable SINS, DVL, USBL integrated navigation method Download PDF

Info

Publication number
CN113959434B
CN113959434B CN202111108406.8A CN202111108406A CN113959434B CN 113959434 B CN113959434 B CN 113959434B CN 202111108406 A CN202111108406 A CN 202111108406A CN 113959434 B CN113959434 B CN 113959434B
Authority
CN
China
Prior art keywords
sins
dvl
usbl
state
error
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202111108406.8A
Other languages
Chinese (zh)
Other versions
CN113959434A (en
Inventor
靳聪慧
韩袁昭
可伟
姜校亮
马西保
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hebei Hanguang Heavy Industry Ltd
Original Assignee
Hebei Hanguang Heavy Industry Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hebei Hanguang Heavy Industry Ltd filed Critical Hebei Hanguang Heavy Industry Ltd
Priority to CN202111108406.8A priority Critical patent/CN113959434B/en
Publication of CN113959434A publication Critical patent/CN113959434A/en
Application granted granted Critical
Publication of CN113959434B publication Critical patent/CN113959434B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

The invention provides an adjustable SINS, DVL, USBL integrated navigation method which can adjust the operation mode according to various actual conditions, realize complementary advantages, improve the working adaptability of the whole system and finish integrated navigation under various conditions. In actual use of SINS, DVL, USBL integrated navigation, the invention obtains accurate estimation of inertial navigation errors by adopting an adjustable flexible integrated navigation mode for different conditions. The integrated navigation can realize advantage complementation, improve the working adaptability of the whole system, finish the integrated navigation under various conditions, effectively reduce inertial navigation errors by an adjustable flexible integrated navigation mode and improve the precision of the integrated navigation system.

Description

Adjustable SINS, DVL, USBL integrated navigation method
Technical Field
The invention relates to the technical field of navigation, in particular to an adjustable SINS, DVL, USBL combined navigation method.
Background
Several navigation devices in common use today include Strapdown Inertial Navigation Systems (SINS), doppler Velocimetry (DVL), and ultra short baseline positioning systems (USBL).
The Strapdown Inertial Navigation System (SINS) consists of a gyroscope, an accelerometer, a navigation computer and the like, and can realize autonomous navigation, the position and speed information of the carrier are obtained through the navigation calculation of the navigation computer by utilizing the measurement of the accelerometer, the attitude and heading information of the carrier are output by utilizing the measurement of the gyroscope, the attitude, speed and position information can be provided for the underwater carrier, but the navigation error is accumulated with time, the navigation precision of the system is reduced, and the system cannot work independently for a long time; the Doppler Velocimeter (DVL) is navigation equipment for measuring the carrier speed according to the Doppler effect, and the speed of the DVL relative to the water bottom is measured through the Doppler effect, so that high-precision continuous speed information can be provided, and the Doppler velocimeter is a common auxiliary navigation equipment, however, the speed measurement error of the SINS and DVL integrated navigation system device is easily influenced by factors such as water temperature, seawater salinity and beam width, and the like, and the problem that DVL sound waves cannot reach the water bottom or cannot return when the water bottom geology is a substance with strong wave absorption such as silt and the like exists; the ultra-short baseline positioning system (USBL) provides three-dimensional position information of a carrier according to an acoustic positioning principle, is high in positioning accuracy, simple to operate, high in portability and low in cost, and comprises a matrix and a transponder, wherein the position of the transponder is precisely known, the position information of the USBL is obtained by measuring relative position information (comprising angle information and inclined distance information) of the transponder relative to the matrix, the position information can be used as an auxiliary navigation system to inhibit divergence of errors of an inertial navigation system, however, the phenomenon of data jump or data loss can occur due to influence of marine environment, and SINS and USBL combination cannot easily realize long-distance combined navigation, so that the system can generally only work in a specific water area.
In conclusion, the existing navigation system has low working adaptability and can not complete navigation under various conditions.
Disclosure of Invention
In view of this, the invention provides an adjustable SINS, DVL, USBL integrated navigation method, which can adjust the operation mode according to various actual conditions, realize advantage complementation, improve the working adaptability of the whole system and complete integrated navigation under various conditions.
In order to achieve the above purpose, the technical scheme of the invention is as follows:
According to the adjustable SINS, DVL, USBL integrated navigation method, initial alignment is achieved by collecting SINS information and global positioning system information;
Synchronously acquiring the position and speed of the SINS, the speed of the DVL and the position data of the USBL after alignment is completed, and performing integrated navigation; SINS, DVL and USBL judge whether the respective output information is valid and whether the accuracy is higher than the set value when the output information is valid in real time, choose different filtering modes according to the specific situation, specifically as follows:
When the DVL and USBL measurement data are effective and the accuracy is higher than the set value, carrying out SINS, DVL, USBL integrated navigation in a centralized filtering mode; the method comprises the steps of measuring the quantity as the difference between the velocity of DVL and the velocity of SINS and the difference between the position of USBL and the position of SINS, calculating by adopting a classical Kalman filtering equation to obtain a state estimation value, feeding back the gesture, the velocity and the position, and setting the estimated state value to zero; unified calculation is carried out on the data when the data fusion is carried out by the centralized filtering, so that the optimal estimation of the state is realized;
When the DVL and USBL measurement data are effective and the accuracy of at least one measurement data is not higher than a set value, converting the measurement data into a federal filtering form to perform SINS, DVL, SINS and USBL integrated navigation; the federal filtering adopts a main filter and a sub-filter mode, the input information of the two sub-filters and the measurement of main inertial navigation are information fused in the main filter to obtain a state estimation value, the gesture, the speed and the position are fed back, and the estimated state value is set to zero;
When the DVL measurement data are valid and the USBL measurement data are invalid, the DVL measurement data are converted into SINS and DVL integrated navigation, the measurement is measured as the difference between the speed of the DVL and the speed of the SINS, a classical Kalman filtering equation is adopted to calculate and obtain a state estimation value, the gesture, the speed and the position are fed back, and the estimated state value is set to zero;
When the DVL measurement data are invalid and the USBL measurement data are valid, the method is converted into SINS and USBL integrated navigation, the measurement is the difference between the position of the USBL and the position of the SINS, a classical Kalman filtering equation is adopted to calculate and obtain a state estimation value, the gesture, the speed and the position are fed back, and the estimated state value is set to zero;
When the DVL and USBL measurement data are invalid, the integrated navigation is not carried out, the integrated navigation is converted into SINS navigation, and the integrated navigation state is entered when the DVL or USBL measurement is valid again.
In the integrated navigation method, the state variables of integrated navigation comprise attitude errors, speed errors, position errors, gyro drift and accelerometer zero offset; the state variables of the filters under different conditions are all 15 dimensions, as follows;
Wherein, Respectively representing east misalignment angle, north misalignment angle and sky misalignment angle error in inertial navigation misalignment angle error, and δV E、δVN、δVU respectively representing east speed error, north speed error and sky speed error in inertial navigation speed error; δl, δλ, δh represent the latitude error, longitude error, and altitude error, respectively, among the inertial navigation position errors; epsilon E、εN、εU represents random constant drift of the x-axis gyroscope, the y-axis gyroscope and the z-axis gyroscope respectively; /(I)The random constant zero offset of the x-axis accelerometer, the y-axis accelerometer and the z-axis accelerometer are represented respectively;
The system equation of the Kalman filter system is:
Wherein X is a system error state variable, W is a system noise variable, F is a system state transition matrix, and G is a system noise transition matrix;
W＝[wgx wgy wgz wax way waz]T
wherein w gx、wgy、wgz is the noise of the x-axis gyroscope, the y-axis gyroscope and the z-axis gyroscope respectively; w ax、way、waz and noise of the x-axis accelerometer, the y-axis accelerometer and the z-axis accelerometer, respectively;
Wherein, Is a posture transformation matrix from a carrier system to a navigation system;
Each submatrix in the system state transition matrix is represented as follows:
Wherein R Mh is the principal radius of curvature of the meridian of the earth, R Nh is the principal radius of curvature of the mortise circle of the earth, ω ie is the rotational angular velocity of the earth, L is the local latitude, East, north and Tian, respectively, of inertial navigation,/>The specific forces in three directions under the inertial navigation geographic coordinate system are respectively;
the measurement equation is:
Z＝HX+V；
discretization of the system state equation and the measurement equation is as follows:
Xk＝φk,k-1Xk-1+Γk-1Wk-1
Zk＝HkXk+Vk
Wherein X k is a state vector at k time, namely an estimated vector, X k-1 is a state vector at k-1 time, phi k,k-1 is a one-step state transition matrix from k-1 time to k time, Γ k-1 is a system noise input matrix, and W k-1 is system noise at k-1 time; z k is a measurement sequence at k time, H k is a measurement matrix at k time, and V k is a measurement noise sequence at k time;
State transition matrix: phi k,k-1＝I+Fk-1 delta t + …
Wherein I is an information matrix and is a P -1 matrix;
The optimal estimate of the state is calculated using the standard kalman filter equation as follows:
state one-step prediction vector: x k/k-1＝φk,k-1Xk-1;
filtering gain:
State estimation calculation: x k＝Xk/k-1+Kk(Zk-HkXk/k-1);
One-step prediction of the mean square error matrix:
Estimating a mean square error equation:
wherein R k is the variance matrix of the measured noise sequence, and Q k-1 is the variance matrix of the measured noise sequence.
When the federal filtering is adopted, the filtering is realized through a first sub-filter, a second sub-filter and a main filter;
The method comprises the steps of measuring the quantity of a first sub-filter as the difference between the speed of DVL and the speed of SINS, measuring the quantity of a second sub-filter as the difference between the position of USBL and the position of SINS, adopting the first sub-filter to realize SINS and DVL combined filtering, adopting the second sub-filter to realize SINS and USBL combined filtering, and respectively obtaining a state estimation value and an estimated mean square error array which are obtained through the first sub-filter as X 1 and P 1 and respectively obtaining a state estimation value and an estimated mean square error array which are obtained through the sub-filter 2 as X 2 and P 2. Calculating a fusion estimation error variance matrix, wherein the formula is Wherein P 1 -1 is the inverse of P 1,/>Is the inverse of P 2. Calculating the main filter state estimation value, wherein the formula is/>The attitude, velocity and position are fed back from X g and the state estimates X 1 and X 2 are zeroed out.
Where concentrated filtering is used, Z and H are as follows:
when the federal filtering is adopted, the first sub-filter realizes SINS and DVL combined filtering, and the second sub-filter realizes SINS and USBL combined filtering, Z and H are respectively as follows:
when the USBL is invalid and SINS and DVL combined filtering is adopted, Z and H are as follows:
When DVL is invalid, when SINS and USBL combined filtering is adopted, Z and H are as follows:
under different filtering conditions, the error feedback correction is the same, and specifically:
posture error correction:
Position error correction: [ Lλh ] T＝[L′ λ′ h′]T-[δL δλ δh]T
Speed error correction ：[VE VN VU]T＝[V′E V′N V′U]T-[δVE δVN δVU]T
Wherein,Correcting an attitude transformation matrix at the previous moment for an attitude error; l ', lambda', h 'are latitude, longitude and altitude at the time immediately before the position error correction, and V' E、V′N、V′U is the east, north and sky speed at the time immediately before the speed error correction.
The beneficial effects are that:
According to the adjustable SINS, DVL, USBL integrated navigation method, in the practical use of SINS, DVL, USBL integrated navigation, an adjustable flexible integrated navigation mode is adopted for different conditions, so that accurate estimation of inertial navigation errors is obtained. The integrated navigation can realize advantage complementation, improve the working adaptability of the whole system, finish the integrated navigation under various conditions, effectively reduce inertial navigation errors by an adjustable flexible integrated navigation mode and improve the precision of the integrated navigation system.
Drawings
FIG. 1 is a schematic diagram of a combined navigation flow according to the present invention.
Detailed Description
The invention will now be described in detail by way of example with reference to the accompanying drawings.
The invention discloses an adjustable SINS, DVL, USBL combined navigation method, which is shown in a flow chart in fig. 1 and comprises the following steps:
Initial alignment is achieved by collecting SINS information and Global Positioning System (GPS) information;
After alignment is completed, the SINS position and speed, the DVL speed and the USBL position data are synchronously acquired to carry out combined navigation under various conditions, SINS, DVL, USBL can judge whether the valid bit (valid and invalid) of the output information and the corresponding precision of the output information are higher than a set value (the higher precision is higher than the set value and the lower precision is not higher than the set value) in real time, and different filtering modes are selected according to specific conditions. The method comprises the following steps:
Under normal conditions, DVL and USBL measurement data are effective, and under the condition that the DVL and USBL measurement data are higher than a set value, SINS, DVL, USBL integrated navigation is performed in a centralized filtering mode. The quantity is measured as the difference between the velocity of DVL and the velocity of SINS and the difference between the position of USBL and the position of SINS, carrying out SINS, DVL, USBL integrated navigation, solving by adopting a classical Kalman filtering equation to obtain a state estimation value, feeding back the gesture, the velocity and the position, and setting the estimated state value to zero.
When the DVL and USBL measurement data are effective and the accuracy of at least one measurement data is not higher than a set value, converting into a federal filtering mode to perform SINS, DVL, SINS and USBL integrated navigation, performing information fusion on the input information of the two sub-filters and the measurement of the main inertial navigation in the main filter to obtain a state estimation value, feeding back the gesture, speed and position, and setting the estimated state value to zero.
When the DVL measurement data is effective and the USBL fault causes the measurement data to be invalid, the method is converted into SINS and DVL integrated navigation; when SINS and DVL integrated navigation is adopted, the quantity is measured as the difference between the speed of DVL and the speed of SINS, a classical Kalman filtering equation is adopted to calculate and obtain a state estimation value, the gesture, the speed and the position are fed back, and the estimated state value is set to zero.
When the DVL fault causes invalid measurement data and effective USBL measurement data, the measurement data are converted into SINS and USBL integrated navigation; when SINS and USBL combined navigation is adopted, the quantity is measured as the difference between the position of the USBL and the position of the SINS, a classical Kalman filtering equation is adopted to calculate and obtain a state estimation value, the gesture, the speed and the position are fed back, and the estimated state value is set to zero.
Specifically, when federal filtering is adopted in this embodiment, the quantity of the sub-filter 1 is measured as the difference between the velocity of DVL and the velocity of SINS, the quantity of the sub-filter 2 is measured as the difference between the position of USBL and the position of SINS, SINS and DVL combination are implemented by using the sub-filter 1, SINS and USBL combination are implemented by using the sub-filter 2, the state estimation value and the estimated mean square error matrix obtained by the sub-filter 1 are X 1 and P 1, respectively, and the state estimation value and the estimated mean square error matrix obtained by the sub-filter 2 are X 2 and P 2, respectively. And calculating a fusion error estimation variance matrix, a main filter state estimation value, feeding back the gesture, the speed and the position from the main filter state estimation value, and setting X 1 and X 2 to zero.
When the DVL and USBL measurement data are invalid, the integrated navigation is not carried out, the SINS (pure inertial) navigation is converted, the pure inertial calculation can maintain the accuracy for a period of time, and the integrated navigation state is entered when the DVL or USBL measurement is valid again.
The specific process of the invention is as follows:
after the system is powered on and the initial alignment is completed through GPS assistance, according to the error characteristics of the strapdown inertial navigation system during long-term operation, attitude errors, speed errors, position errors, gyro drift and accelerometer zero offset are selected as state variables of combined navigation. The state variables of the filters under different conditions are all 15 dimensions, as follows;
Wherein, Respectively representing east misalignment angle, north misalignment angle and sky misalignment angle error in inertial navigation misalignment angle error, and δV E、δVN、δVU respectively representing east speed error, north speed error and sky speed error in inertial navigation speed error; δl, δλ, δh represent the latitude error, longitude error, and altitude error, respectively, among the inertial navigation position errors; epsilon E、εN、εU represents random constant drift of the x-axis gyroscope, the y-axis gyroscope and the z-axis gyroscope respectively; /(I)Representing the random constant zero offset of the x-axis accelerometer, the y-axis accelerometer and the z-axis accelerometer, respectively.
Establishing a system equation of a Kalman filtering system:
Wherein X is a system error state variable, W is a system noise variable, F is a system state transition matrix, and G is a system noise transition matrix.
W＝[wgx wgy wgz wax way waz]T
Wherein w gx、wgy、wgz is the noise of the x-axis gyro, the y-axis gyro and the z-axis gyro respectively. w ax、way、waz and noise for the x-axis accelerometer, y-axis accelerometer, and z-axis accelerometer, respectively.
Wherein,Is a carrier to navigation system pose transformation matrix.
Each submatrix in the system state transition matrix is represented as follows:
In the above formulas, R Mh、RNh is the principal radius of curvature of the meridian of the earth, the principal radius of curvature of the mortise circle, ω ie is the earth rotation angular velocity, L is the local latitude, The east speed, the north speed and the sky speed of inertial navigation respectively,The specific forces in three directions under the inertial navigation geographic coordinate system are respectively.
Establishing a measurement equation:
Z＝HX+V；
discretizing a system state equation and a measurement equation:
Xk＝φk,k-1Xk-1+Γk-1Wk-1
Zk＝HkXk+Vk
Wherein X k is a state vector at k time, namely an estimated vector, X k-1 is a state vector at k-1 time, phi k,k-1 is a one-step state transition matrix from k-1 time to k time, Γ k-1 is a system noise input matrix, and W k-1 is system noise at k-1 time; z k is the measurement sequence at time k, H k is the measurement matrix at time k, and V k is the measurement noise sequence at time k.
State transition matrix: phi k,k-1＝I+Fk-1 delta t + …
Wherein I is an information matrix and is a P -1 matrix;
The optimal estimate of the state is calculated using the standard kalman filter equation as follows:
a state one-step predictive vector; x k/k-1＝φk,k-1Xk-1;
Filtering gain:
State estimation calculation, X k＝Xk/k-1+Kk(Zk-HkXk/k-1);
One-step prediction of the mean square error matrix:
Estimating a mean square error equation:
wherein R k is the variance matrix of the measured noise sequence, and Q k-1 is the variance matrix of the measured noise sequence.
When the centralized filtering is adopted, the quantity is measured as the difference between the velocity of DVL and the velocity of SINS and the difference between the position of USBL and the position of SINS, the integrated navigation of SINS, DVL, USBL is carried out, the classical Kalman filtering equation is adopted to calculate and obtain the state estimation value, the gesture, the velocity and the position are fed back, and the estimated state value is set to zero. Z and H are as follows:
When federal filtering is adopted, the quantity of the sub-filter 1 is measured as the difference between the velocity of DVL and the velocity of SINS, the quantity of the sub-filter 2 is measured as the difference between the position of USBL and the position of SINS, SINS and DVL combination are realized by adopting the sub-filter 1, SINS and USBL combination are realized by adopting the sub-filter 2, the state estimation value and the estimated mean square error matrix obtained by the sub-filter 1 are respectively X 1 and P 1, and the state estimation value and the estimated mean square error matrix obtained by the sub-filter 2 are respectively X 2 and P 2. Calculating a fusion estimation error variance matrix, wherein the formula is Wherein P 1 -1 is the inverse of P 1,/>Is the inverse of P 2. Calculating the main filter state estimation value, wherein the formula is/>The attitude, velocity and position are fed back from X g and the state estimates X 1 and X 2 are zeroed out. Z and H are each as follows:
When SINS and DVL combined filtering is adopted, the quantity is measured as the difference between the speed of DVL and the speed of SINS, a classical Kalman filtering equation is adopted to calculate and obtain a state estimation value, the gesture, the speed and the position are fed back, and the estimated state value is set to zero. Z and H are as follows:
When SINS and USBL combined filtering is adopted, the quantity is measured as the difference between the position of the USBL and the position of the SINS, a classical Kalman filtering equation is adopted to calculate and obtain a state estimation value, the gesture, the speed and the position are fed back, and the estimated state value is set to zero. Z and H are as follows:
The error feedback correction was the same in each case, as follows:
posture error correction:
Position error correction: [ Lλh ] T＝[L′ λ′ h′]T-[δL δλ δh]T
Speed error correction ：[VE VN VU]T＝[V′E V′N V′U]T-[δVE δVN δVU]T
Wherein,Correcting an attitude transformation matrix at the previous moment for an attitude error; l ', lambda', h 'are latitude, longitude and altitude at the time immediately before the position error correction, and V' E、V′N、V′U is the east, north and sky speed at the time immediately before the speed error correction.
When the DVL and USBL measurement data are invalid, the pure inertia solution can maintain the accuracy for a period of time, and the combined navigation state is entered when the DVL or USBL measurement is valid again.
In summary, the above embodiments are only preferred embodiments of the present invention, and are not intended to limit the scope of the present invention. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims (6)

1. An adjustable SINS, DVL, USBL integrated navigation method is characterized in that initial alignment is realized by collecting SINS information and global positioning system information;
Synchronously acquiring the position and speed of the SINS, the speed of the DVL and the position data of the USBL after alignment is completed, and performing integrated navigation; SINS, DVL and USBL judge whether the respective output information is valid and whether the accuracy is higher than the set value when the output information is valid in real time, choose different filtering modes according to the specific situation, specifically as follows:
When the DVL and USBL measurement data are effective and the accuracy is higher than the set value, carrying out SINS, DVL, USBL integrated navigation in a centralized filtering mode; the method comprises the steps of measuring the quantity as the difference between the velocity of DVL and the velocity of SINS and the difference between the position of USBL and the position of SINS, calculating by adopting a classical Kalman filtering equation to obtain a state estimation value, feeding back the gesture, the velocity and the position, and setting the estimated state value to zero; unified calculation is carried out on the data when the data fusion is carried out by the centralized filtering, so that the optimal estimation of the state is realized;
When the DVL and USBL measurement data are effective and the accuracy of at least one measurement data is not higher than a set value, converting the measurement data into a federal filtering form to perform SINS, DVL, SINS and USBL integrated navigation; the federal filtering adopts a main filter and a sub-filter mode, the input information of the two sub-filters and the measurement of main inertial navigation are information fused in the main filter to obtain a state estimation value, the gesture, the speed and the position are fed back, and the estimated state value is set to zero;
When the DVL measurement data are valid and the USBL measurement data are invalid, the DVL measurement data are converted into SINS and DVL integrated navigation, the measurement is measured as the difference between the speed of the DVL and the speed of the SINS, a classical Kalman filtering equation is adopted to calculate and obtain a state estimation value, the gesture, the speed and the position are fed back, and the estimated state value is set to zero;
When the DVL measurement data are invalid and the USBL measurement data are valid, the method is converted into SINS and USBL integrated navigation, the measurement is the difference between the position of the USBL and the position of the SINS, a classical Kalman filtering equation is adopted to calculate and obtain a state estimation value, the gesture, the speed and the position are fed back, and the estimated state value is set to zero;
When the DVL and USBL measurement data are invalid, not performing integrated navigation, converting into SINS navigation, and entering an integrated navigation state when the DVL or USBL measurement is valid again;
in the integrated navigation method, the state variables of integrated navigation comprise attitude errors, speed errors, position errors, gyro drift and accelerometer zero offset; the state variables of the filters under different conditions are all 15 dimensions, as follows;
Wherein, Respectively representing east misalignment angle, north misalignment angle and sky misalignment angle error in inertial navigation misalignment angle error, and δV E、δVN、δVU respectively representing east speed error, north speed error and sky speed error in inertial navigation speed error; δl, δλ, δh represent the latitude error, longitude error, and altitude error, respectively, among the inertial navigation position errors; epsilon E、εN、εU represents random constant drift of the x-axis gyroscope, the y-axis gyroscope and the z-axis gyroscope respectively; /(I)The random constant zero offset of the x-axis accelerometer, the y-axis accelerometer and the z-axis accelerometer are represented respectively;
The system equation of the Kalman filter system is:
Wherein X is a system error state variable, W is a system noise variable, F is a system state transition matrix, and G is a system noise transition matrix;
W＝[wgx wgy wgz wax way waz]T
wherein w gx、wgy、wgz is the noise of the x-axis gyroscope, the y-axis gyroscope and the z-axis gyroscope respectively; w ax、way、waz and noise of the x-axis accelerometer, the y-axis accelerometer and the z-axis accelerometer, respectively;
Wherein, Is a posture transformation matrix from a carrier system to a navigation system;
Each submatrix in the system state transition matrix is represented as follows:
Wherein R Mh is the principal radius of curvature of the meridian of the earth, R Nh is the principal radius of curvature of the mortise circle of the earth, ω ie is the rotational angular velocity of the earth, L is the local latitude, East, north and Tian, respectively, of inertial navigation,/>The specific forces in three directions under the inertial navigation geographic coordinate system are respectively;
the measurement equation is:
Z＝HX+V；
discretization of the system state equation and the measurement equation is as follows:
Xk＝φk,k-1Xk-1+Γk-1Wk-1
Zk＝HkXk+Vk
Wherein X k is a state vector at k time, namely an estimated vector, X k-1 is a state vector at k-1 time, phi k,k-1 is a one-step state transition matrix from k-1 time to k time, Γ k-1 is a system noise input matrix, and W k-1 is system noise at k-1 time; z k is a measurement sequence at k time, H k is a measurement matrix at k time, and V k is a measurement noise sequence at k time;
State transition matrix: phi k,k-1＝I+Fk-1 delta t + …
Wherein I is an information matrix and is a P -1 matrix;
The optimal estimate of the state is calculated using the standard kalman filter equation as follows:
state one-step prediction vector: x k/k-1＝φk,k-1Xk-1;
filtering gain:
State estimation calculation: x k＝Xk/k-1+Kk(Zk-HkXk/k-1);
One-step prediction of the mean square error matrix:
Estimating a mean square error equation:
wherein R k is a variance matrix of the measurement noise sequence, and Q k-1 is a variance matrix of the measurement noise sequence;
when the centralized filtering is adopted, Z and H are as follows:
2. the integrated navigation method of claim 1, wherein the federal filtering is implemented by a first sub-filter, a second sub-filter, and a main filter;
The method comprises the steps of measuring the quantity of a first sub-filter as the difference between the speed of DVL and the speed of SINS, measuring the quantity of a second sub-filter as the difference between the position of USBL and the position of SINS, adopting the first sub-filter to realize SINS and DVL combined filtering, adopting the second sub-filter to realize SINS and USBL combined filtering, and respectively obtaining a state estimation value and an estimated mean square error array which are X 1 and P 1 through the first sub-filter, and obtaining a state estimation value and an estimated mean square error array which are X 2 and P 2 through the sub-filter 2; calculating a fusion estimation error variance matrix, wherein the formula is Wherein P 1 -1 is the inverse of P 1,/>Is the inverse of P 2; calculating the main filter state estimation value, wherein the formula is/>The attitude, velocity and position are fed back from X g and the state estimates X 1 and X 2 are zeroed out.
3. The integrated navigation method of claim 1 or 2, wherein when federal filtering is adopted, a first sub-filter implements SINS and DVL combined filtering, and a second sub-filter implements SINS and USBL combined filtering, Z and H are respectively as follows:
4. the integrated navigation method of claim 1, wherein when USBL is inactive, Z and H are as follows when SINS, DVL combined filtering is employed:
5. The integrated navigation method of claim 1, wherein when DVL is invalid, Z and H are as follows when SINS, USBL combined filtering is employed:
6. the integrated navigation method of claim 1,2, 3, 4 or 5, wherein the error feedback corrections are the same under different filtering conditions, specifically:
posture error correction:
Position error correction: [ Lλh ] T＝[L' λ' h']T-[δL δλ δh]T
Speed error correction ：[VE VN VU]T＝[V'E V'N V'U]T-[δVE δVN δVU]T
Wherein,Correcting an attitude transformation matrix at the previous moment for an attitude error; l ', lambda', h 'are latitude, longitude and altitude at the time immediately before the position error correction, and V' E、V'N、V'U is the east, north and sky speed at the time immediately before the speed error correction.
CN202111108406.8A 2021-09-22 2021-09-22 Adjustable SINS, DVL, USBL integrated navigation method Active CN113959434B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111108406.8A CN113959434B (en) 2021-09-22 2021-09-22 Adjustable SINS, DVL, USBL integrated navigation method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111108406.8A CN113959434B (en) 2021-09-22 2021-09-22 Adjustable SINS, DVL, USBL integrated navigation method

Publications (2)

Publication Number Publication Date
CN113959434A CN113959434A (en) 2022-01-21
CN113959434B true CN113959434B (en) 2024-06-14

Family

ID=79462327

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111108406.8A Active CN113959434B (en) 2021-09-22 2021-09-22 Adjustable SINS, DVL, USBL integrated navigation method

Country Status (1)

Country Link
CN (1) CN113959434B (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116026324B (en) * 2023-02-10 2023-06-20 北京大学 Cross-domain navigation system and method for water-air cross-medium craft

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6449559B2 (en) * 1998-11-20 2002-09-10 American Gnc Corporation Fully-coupled positioning process and system thereof
CN102829777B (en) * 2012-09-10 2015-09-16 江苏科技大学 Autonomous underwater vehicle combined navigation system and method
CN107024226B (en) * 2016-02-01 2021-03-16 北京自动化控制设备研究所 Inertial navigation error estimation method based on inertial navigation/DVL/USBL combination
CN109737959A (en) * 2019-03-20 2019-05-10 哈尔滨工程大学 A kind of polar region Multi-source Information Fusion air navigation aid based on federated filter
CN110006433A (en) * 2019-04-22 2019-07-12 哈尔滨工程大学 The integrated navigation and location system and method for sea-bottom oil-gas pipe detection robot
CN111829512B (en) * 2020-06-08 2024-04-09 中国航天空气动力技术研究院 AUV navigation positioning method and system based on multi-sensor data fusion
CN112556697A (en) * 2020-12-08 2021-03-26 江苏科技大学 Shallow coupling data fusion navigation method based on federated structure

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
一种基于集中滤波的SINS/DVL/USBL水下组合导航算法;张亚文 等;《导航定位与授时》;20170131;全文 *
联邦滤波器的SINS/GNS/DVS水下组合导航;刘明雍;周志远;赵涛;;火力与指挥控制;20091215(第12期);全文 *

Also Published As

Publication number Publication date
CN113959434A (en) 2022-01-21

Similar Documents

Publication Publication Date Title
CN107655476B (en) Pedestrian high-precision foot navigation method based on multi-information fusion compensation
WO2020062791A1 (en) Sins/dvl-based underwater anti-shaking alignment method for deep-sea underwater vehicle
CN109782323B (en) Navigation positioning and calibrating method for autonomous underwater vehicle in deep sea
CN102829777B (en) Autonomous underwater vehicle combined navigation system and method
US6860023B2 (en) Methods and apparatus for automatic magnetic compensation
Kinsey et al. A survey of underwater vehicle navigation: Recent advances and new challenges
CN103235328B (en) GNSS (global navigation satellite system) and MEMS (micro-electromechanical systems) integrated navigation method
CN102538792B (en) Filtering method for position attitude system
CN102169184B (en) Method and device for measuring installation misalignment angle of double-antenna GPS (Global Position System) in integrated navigation system
CN109324330A (en) Based on USBL/SINS tight integration navigation locating method of the mixing without derivative Extended Kalman filter
CN106643709B (en) Combined navigation method and device for offshore carrier
Wang et al. Quadratic extended Kalman filter approach for GPS/INS integration
CN103389092B (en) A kind of kite balloon airship attitude measuring and measuring method
CN110514203B (en) Underwater integrated navigation method based on ISR-UKF
CN104181574A (en) Strapdown inertial navigation system/global navigation satellite system combined based navigation filter system and method
CN103744098A (en) Ship's inertial navigation system (SINS)/Doppler velocity log (DVL)/global positioning system (GPS)-based autonomous underwater vehicle (AUV) combined navigation system
Arnold et al. Robust model-aided inertial localization for autonomous underwater vehicles
CN107966145B (en) AUV underwater navigation method based on sparse long baseline tight combination
CN110849360B (en) Distributed relative navigation method for multi-machine collaborative formation flight
CN103712621B (en) Polarised light and infrared sensor are assisted inertial navigation system method for determining posture
CN114459476B (en) Underwater unmanned submarine current measuring DVL/SINS integrated navigation method based on virtual speed measurement
CN113959434B (en) Adjustable SINS, DVL, USBL integrated navigation method
CN104061930A (en) Navigation method based on strapdown inertial guidance and Doppler log
CN112525204B (en) Spacecraft inertia and solar Doppler speed combined navigation method
AU2020104248A4 (en) Integrated gps and inertial navigation system for industrial robots

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant