CN114166203B - Intelligent underwater robot multi-source combined navigation method based on improved S-H self-adaptive federal filtering - Google Patents
Intelligent underwater robot multi-source combined navigation method based on improved S-H self-adaptive federal filtering Download PDFInfo
- Publication number
- CN114166203B CN114166203B CN202111354287.4A CN202111354287A CN114166203B CN 114166203 B CN114166203 B CN 114166203B CN 202111354287 A CN202111354287 A CN 202111354287A CN 114166203 B CN114166203 B CN 114166203B
- Authority
- CN
- China
- Prior art keywords
- error
- improved
- adaptive
- underwater robot
- navigation
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 60
- 238000001914 filtration Methods 0.000 title claims abstract description 32
- 230000003044 adaptive effect Effects 0.000 claims abstract description 16
- 230000004927 fusion Effects 0.000 claims abstract description 11
- 239000011159 matrix material Substances 0.000 claims description 19
- 238000005259 measurement Methods 0.000 claims description 14
- 230000008569 process Effects 0.000 claims description 12
- 238000006243 chemical reaction Methods 0.000 claims description 10
- 230000033001 locomotion Effects 0.000 claims description 4
- 230000008859 change Effects 0.000 claims description 3
- 230000004069 differentiation Effects 0.000 claims description 3
- 230000008054 signal transmission Effects 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 5
- 230000001133 acceleration Effects 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 238000009825 accumulation Methods 0.000 description 2
- 238000012938 design process Methods 0.000 description 2
- 238000009434 installation Methods 0.000 description 2
- 230000010354 integration Effects 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses an intelligent underwater robot multi-source combined navigation method based on improved S-H self-adaptive federal filtering, and belongs to the field of intelligent underwater robots. The improved S-H adaptive federal filtering method comprises the following steps: s100, modeling an underwater multisource integrated navigation system to obtain a navigation sensor and an error model thereof; s200, based on an error model, an improved S-H self-adaptive federal filtering method is provided. The intelligent underwater robot multi-source combined navigation method based on the improved S-H self-adaptive federal filtering can correct errors of multiple sensors, correct the errors of the sensors based on the characteristics of multiple sources, select federal filters to perform data fusion on a multi-source combined navigation system, and has the advantages of small calculated amount, simple structure, good fault tolerance and real-time performance and the like.
Description
Technical Field
The invention relates to an intelligent underwater robot multi-source combined navigation method based on improved S-H self-adaptive federal filtering, and belongs to the field of intelligent underwater robots.
Background
The high-precision navigation system is a key point and a difficult point in the AUV design process of the intelligent underwater robot, and is also a guarantee that the AUV can successfully complete tasks and successfully return. At present, the strapdown inertial navigation system SINS has high navigation precision in a short time, high information updating speed and no need of external information input, and becomes a commonly used underwater navigation sensor, but errors of the SINS can be accumulated continuously along with the increase of time, can not independently complete navigation tasks, and needs to be combined with other navigation sensors, such as an ultra-short baseline positioning system (Ultra Short Baseline Positioning System, USBL), a Doppler velocimeter (Doppler Velocity Log, DVL) and a Magnetic heading instrument (MCP), so as to inhibit errors generated by accumulation of time.
Along with the development of science and technology, the variety and functions of the underwater navigation sensor are diversified, and how to effectively perform information fusion on a multi-sensor combined navigation system to obtain optimal navigation information becomes a key point of research.
The high-precision navigation system is an important point and a difficult point in the AUV design process, and is also a guarantee that the AUV can successfully complete tasks and successfully return. At present, the SINS has high navigation accuracy in a short time, high information updating speed and no need of external information input, and becomes a commonly used underwater navigation sensor, but the errors of the SINS are accumulated continuously along with the increase of time, so that the SINS cannot independently complete navigation tasks, and needs to be combined with other navigation sensors, such as USBL, DVL, MCP, to restrain the errors generated by accumulation along with time.
Meanwhile, the key point of the high-precision navigation system is how to design a proper navigation filter, and the verification proves that the S-H self-adaptive filtering method has the best effect aiming at the characteristics of unknown or time-varying measurement noise statistics caused by complex and changeable underwater environment, and is applied to multi-source combined navigation data fusion, and the analysis and improvement are needed because the system has the characteristics of overlarge dimension and the like, so that the system is easy to diverge, and is suitable for being applied to a federal filtering structure.
Disclosure of Invention
The invention provides an intelligent underwater robot multi-source combined navigation method based on improved S-H self-adaptive federal filtering, which solves the problems in the prior art.
An intelligent underwater robot multi-source integrated navigation method based on improved S-H adaptive federal filtering, the improved S-H adaptive federal filtering method comprising the steps of:
s100, modeling an underwater multisource integrated navigation system to obtain a navigation sensor and an error model thereof;
s200, based on an error model, an improved S-H self-adaptive federal filtering method is provided.
Further, in S100, the method specifically includes the following steps:
s110, firstly, establishing a common coordinate system and a coordinate conversion relation of a navigation system;
s120, obtaining error models of the SINS, USBL, DVL and MCP sensors on the basis of S110.
Further, in S110, the method specifically includes the following steps:
s111, assuming that four sensors SINS, USBL, DVL and MCP are all arranged at the center of a carrier of the underwater robot, the data obtained by the four sensors are all based on a carrier coordinate system, and the data are required to be unified into a navigation coordinate system;
s112, navigation coordinate System around OZ n Axis rotation ψ, about OX n1 Axis rotates θ, then around OY n2 Rotating gamma, namely:
wherein, psi is course angle, the geographic north direction is taken as starting point, the clockwise movement is positive, and the range is 0-360 degrees; θ is pitch angle, with horizontal plane as reference, positive upwards, negative downwards, in the range-90 ° to 90 °; gamma is a roll angle, right inclination is positive, left inclination is negative and the range is-90 degrees to 90 degrees by taking a vertical plane as a reference;
the conversion matrices correspond to:
the transformation matrix between the navigation coordinate system and the carrier coordinate system is:
further, in S120, specific:
the errors of the SINS include velocity, position and attitude errors, wherein,
the attitude error equation is:
in the method, in the process of the invention,indicating the misalignment angle of the east, north and sky three-way platform,>for the rotation angular velocity of the earth>Indicating the angular velocity caused by the change in position of the underwater robot carrier,
in the above, v= [ V E V N V U ] T Respectively representing the east, north and sky three-way speeds of the carrier in a geographic coordinate system, L, lambda and h respectively representing longitude, latitude and altitude, R M 、R N Respectively represent the curvature radius of each point on the earth reference ellipsoidal meridian and ellipsoidal mortise unitary circle,
expanding an attitude error equation to obtain:
velocity error equation:
in the above, f n The specific force is expressed and developed as follows:
position error equation:
the error of the USBL is as follows:
establishing a matrix coordinate system oxyz, fixing an acoustic beacon on an underwater robot carrier, and setting the position of the acoustic beacon as (X) a ,Y a ,Z a ) R is a position vector, three hydrophones are respectively arranged on an origin, an x axis and a y axis, and the angle between the connecting line between the acoustic beacon and the origin and the coordinate axis is theta mx 、θ my And theta m ，
The distance R between the beacon and the matrix is:
while
X a ＝Rcosθ mx (15)
Y a ＝Rcosθ my (16)
Is obtained by the following two formulas:
substituting formula (12) into formula (13) and formula (14) to obtain:
and substituting the formula (15) into the formula (16) to obtain:
and (3) solving to obtain:
the same principle is obtained:
θ mx 、θ my obtained by means of the phase difference,
the distance between two matrixes is d, the wavelength of sound wave is lambda, the phase difference of sound wave signals received by each matrix is phi, and the incident angle of the signals to the matrixes is theta m ：
The phase difference obtained by this method is obtained by the following formula:
c is the propagation speed of sound wave under water, which is the known quantity, T is the time difference between the signal transmission and the signal reception of the modem, and R is:
R＝0.5cT (26)
finally, the method comprises the following steps:
the error of the ultra-short baseline positioning system is analyzed below, and X is obtained from the above a 、Y a 、Z a Expressed as X a For example, Y a 、Z a Similarly, for X a And (3) performing full differentiation to obtain:
the relative positioning accuracy of the positions is as follows:
the variance of the relative skew R of the X-axis direction error is:
and (3) the same principle:
wherein DeltaT, deltac, deltaphi 12 、Δφ 13 Respectively a time measurement error, a sound velocity measurement error and a phase measurement error;
error equation for DVL:
the DVL transmits a beam of frequency f to the front, the back, the left and the right of the seabed 0 Wave speed c 0 Is provided that the AUV moves forward at a velocity V,
in the above, f d1 ,f d2 ,f d3 ,f d4 Respectively, the acoustic wave frequency shifts in four directions, V x ,V y ,V z Representing the three-axis velocity component of the AUV,
the DVL error includes a velocity offset error δV EDVL And δV NDVL The scale factor error delta C and the drift angle error delta are expressed by a first order Markov process,the error equation is:
wherein beta is -1 EDVL 、β -1 NDVL 、β -1 Δ Respectively represent the related time, w Ed 、w Nd 、w Δ To excite white noise;
error equation for MCP:
after compensation, the error approximation of the MCP is considered a first order markov process:
in the above, δψ MCP Represents the MCP heading angle error,represents the correlation time, w MCP Representing the corresponding driving white noise.
Further, in S200, the method includes the following steps:
s210, an improved S-H adaptive filtering method is provided;
s220, performing time updating and measurement updating on three sub-filters in the federal filter by adopting the improved S-H adaptive filtering method;
s230, the main filter performs information fusion on the output of each sub-filter.
Further, in S210, the improved S-H adaptive filtering method includes the steps of:
s211, initial value: x (0), P (0), R (0), Q (0), k=1;
s212, a prediction equation:
s213, prediction error covariance:
s214, self-adaptive estimation:
s215, calculating a gain matrix:
s216, calculating an estimated value:
s217, updating a covariance matrix: p (P) k ＝(I-K k H k )P k|k-1 。
Further, in S230, the main filter performs information fusion on the outputs of the respective sub-filters:
the invention has the following beneficial effects: the intelligent underwater robot multi-source combined navigation method based on the improved S-H self-adaptive federal filtering can correct errors of multiple sensors, correct the errors of the sensors based on the characteristics of multiple sources, select federal filters to perform data fusion on a multi-source combined navigation system, and has the advantages of small calculated amount, simple structure, good fault tolerance and real-time performance and the like.
Drawings
FIG. 1 is a diagram showing the conversion relationship between b and n;
FIG. 2 is a schematic diagram of a strapdown inertial navigation system;
FIG. 3 is a schematic diagram of an ultra-short baseline positioning system;
FIG. 4 is a schematic diagram of the Doppler velocimeter operation;
fig. 5 is a general block diagram of a federal filter.
Detailed Description
The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
The invention provides an intelligent underwater robot multi-source combined navigation method based on improved S-H self-adaptive federal filtering, which comprises the following steps:
s100, modeling an underwater multisource integrated navigation system to obtain a navigation sensor and an error model thereof;
s200, based on an error model, an improved S-H self-adaptive federal filtering method is provided.
Further, in S100, the method specifically includes the following steps:
s110, firstly, establishing a common coordinate system and a coordinate conversion relation of a navigation system;
s120, obtaining error models of the SINS, USBL, DVL and MCP sensors on the basis of S110.
Further, in S110, the method specifically includes the following steps:
s111, under the general condition, assuming that each navigation sensor is installed at the center of a carrier of the underwater robot, the data obtained by the sensors are all based on a carrier coordinate system, and for the convenience of subsequent calculation, the data are required to be unified under the navigation coordinate system, so that coordinate conversion is required;
s112, as shown in FIG. 1, the navigation coordinate system surrounds the OZ n Axis rotation ψ, about OX n1 Axis rotates θ, then around OY n2 Rotating gamma, namely:
wherein, psi is course angle, the geographic north direction is taken as starting point, the clockwise movement is positive, and the range is 0-360 degrees; θ is pitch angle, with horizontal plane as reference, positive upwards, negative downwards, in the range-90 ° to 90 °; gamma is a roll angle, right inclination is positive, left inclination is negative and the range is-90 degrees to 90 degrees by taking a vertical plane as a reference;
the conversion matrices correspond to:
the transformation matrix between the navigation coordinate system and the carrier coordinate system is:
specifically, the geographic coordinate system is also called g system, and is the coordinate system describing the position of the object on the earth surface, and its origin O takes the position point or projection point of the carrier on the sphere, ox g 、Oy g 、Oz g The coordinate system is respectively directed to the east, north and the sky, also called northeast and north (ENU), and the g system is generally regarded as a navigation coordinate system; the navigation coordinate system, also called n-system, is a coordinate system which is virtualized to facilitate solving navigation parameters and is related to the position. Since the navigation parameters of the SINS are not generally solved in the b system, the inertial device signals are decomposed into the navigation coordinate system for solving.
Further, in S120, specific:
the geographic coordinate system is selected as a navigation coordinate system, as can be seen from fig. 2, the strapdown inertial navigation system realizes the conversion between the n system and the b system by establishing a virtual platform, the accelerometer measures the specific force in the motion process, then the three-way linear acceleration information is calculated according to Newton's second law, the acceleration under the n system can be obtained through a conversion matrix, the speed can be obtained through integration, the position can be obtained through speed integration, the gyroscope obtains the strapdown inertial navigation matrix through measuring the angular acceleration, so that the attitude information is obtained, the error of the inertial navigation system mainly comes from an inertial device arranged in the inertial navigation system, the error is continuously increased along with the increase of time, and in the long-time underwater navigation process, the error is liable to be caused to be larger, and the requirement of navigation precision is not met.
The error of strapdown inertial navigation mainly comes from errors generated by an internal inertial device, including scale errors, installation errors, drift errors and the like, and the drift errors become main consideration because the former two errors can be basically compensated through calibration.
Drift error epsilon of gyroscope is composed of three-way component epsilon i (i=x, y, z) composition including constant drift error ε bi And random error epsilon wi Wherein the random error can be seen as consisting of gaussian white noise and a first order markov process:
ε i ＝ε bi +ε wi i＝x,y,z (37)
ε n ＝[ε E ε N ε U ] T representing the projection of the gyroscope drift error in the navigation coordinate system.
Drift error of accelerationFrom three-way component->Composition, including constant drift error->And random error->
Representing the projection of accelerometer drift errors in the navigation coordinate system.
The errors of the SINS include velocity, position and attitude errors, wherein,
the attitude error equation is:
in the method, in the process of the invention,indicating the misalignment angle of the east, north and sky three-way platform,>for the rotation angular velocity of the earth>Indicating the angular velocity caused by the change in position of the underwater robot carrier,
in the above, v= [ V E V N V U ] T Respectively representing the east, north and sky three-way speeds of the underwater robot carrier in a geographic coordinate system, L, lambda and h respectively representing longitude, latitude and altitude, R M 、R N Respectively represent the curvature radius of each point on the earth reference ellipsoidal meridian and ellipsoidal mortise unitary circle,
expanding an attitude error equation to obtain:
velocity error equation:
in the above, f n The specific force is indicated as such,
it was developed as follows:
position error equation:
the error of the USBL is as follows:
the ultra-short baseline system USBL has a baseline length of several centimeters to several minutes meters, is composed of an acoustic modem and a transponder, and the modem is fixedly connected to a mother ship. Although the positioning precision is slightly smaller than that of the other two acoustic navigation systems, the positioning precision is not required to be set up in advance on the seabed, so that the acoustic navigation system has the characteristics of easiness in installation, small size and the like, meets the requirement of underwater positioning precision, and is one of the navigation sensors commonly used by AUVs.
Referring to fig. 3, an array coordinate system oxyz is established, and an acoustic beacon is fixed to an underwater robot carrier, the position of which is set as (X a ,Y a ,Z a ) R is a position vector, three hydrophones are respectively arranged on an origin, an x axis and a y axis, and the angle between the connecting line between the acoustic beacon and the origin and the coordinate axis is theta mx 、θ my And theta m ，
The distance R between the beacon and the matrix is:
while
X a ＝Rcosθ mx (15)
Y a ＝Rcosθ my (16)
Is obtained by the following two formulas:
substituting formula (12) into formula (13) and formula (14) to obtain:
and substituting the formula (15) into the formula (16) to obtain:
and (3) solving to obtain:
the same principle is obtained:
θ mx 、θ my obtained by means of the phase difference,
the distance between two matrixes is d, the wavelength of sound wave is lambda, the phase difference of sound wave signals received by each matrix is phi, and the incident angle of the signals to the matrixes is theta m ：
The phase difference obtained by this method is obtained by the following formula:
c is the propagation speed of sound wave under water, which is the known quantity, T is the time difference between the signal transmission and the signal reception of the modem, and R is:
R＝0.5cT (26)
finally, the method comprises the following steps:
the error of the ultra-short baseline positioning system is analyzed below, and X is obtained from the above a 、Y a 、Z a Expressed as X a For example, Y a 、Z a Similarly, for X a And (3) performing full differentiation to obtain:
the relative positioning accuracy of the positions is as follows:
the variance of the relative skew R of the X-axis direction error is:
and (3) the same principle:
wherein DeltaT, deltac, deltaphi 12 、Δφ 13 Respectively a time measurement error, a sound velocity measurement error and a phase measurement error;
error equation for DVL:
referring to fig. 4, a doppler velocimeter (DopplerVelocity Log, DVL) is used in the invention to assist in correcting the speed information of the SINS, which is an important underwater speed sensor designed based on the doppler effect, and has high speed measurement accuracy. The Doppler effect is the fact that when the transmitting source is displaced relative to the medium, the frequency of the signal received by the observer is inconsistent with the frequency at the time of transmission, and the difference between the two is called Doppler shift.
The DVL transmits a beam of frequency f to the front, the back, the left and the right of the seabed 0 Wave speed c 0 Is provided that the AUV moves forward at a velocity V,
in the above, f d1 ,f d2 ,f d3 ,f d4 Respectively, the acoustic wave frequency shifts in four directions, V x ,V y ,V z Representing the three-axis velocity component of the AUV,
the DVL error includes a velocity offset error δV EDVL And δV NDVL The scale factor error delta C and the drift angle error delta are expressed by a first order Markov process, and then the error equation is as follows:
wherein beta is -1 EDVL 、β -1 NDVL 、β -1 Δ Respectively represent the related time, w Ed 、w Nd 、w Δ To excite white noise;
error equation for MCP:
after compensation, the error approximation of the MCP is considered a first order markov process:
/>
in the above, δψ MCP Represents the MCP heading angle error,represents the correlation time, w MCP Representing the corresponding driving white noise.
Further, in S200, the method includes the following steps:
s210, an improved S-H adaptive filtering method is provided;
s220, performing time updating and measurement updating on three sub-filters in the federal filter by adopting the improved S-H adaptive filtering method;
s230, the main filter performs information fusion on the output of each sub-filter.
Further, in S210, the improved S-H adaptive filtering method includes the steps of:
s211, initial value: x (0), P (0), R (0), Q (0), k=1, where X (0) is an initial value of the state variable X, and is generally set to 0, and initial values of P (0), R (0), and Q (0) are determined according to the performance of the sensor. In this embodiment, there are 4 kinds of sensors, and in other embodiments, referring to fig. 5, a plurality of kinds of sensors of other kinds may be selected to perform the output information fusion of the sub-filters;
s212, a prediction equation:
s213, prediction error covariance:
s214, self-adaptive estimation:
s215, calculating a gain matrix:
s216, calculating an estimated value:
s217, updating a covariance matrix: p (P) k ＝(I-K k H k )P k|k-1 。
Further, in S230, the main filter performs information fusion on the outputs of the respective sub-filters:
/>
Claims (5)
1. an intelligent underwater robot multi-source combined navigation method based on improved S-H self-adaptive federal filtering is characterized in that the improved S-H self-adaptive federal filtering method comprises the following steps:
s100, modeling an underwater multisource integrated navigation system to obtain a navigation sensor and an error model thereof;
s200, based on an error model, an improved S-H self-adaptive federal filtering method is provided;
in S200, the steps of:
s210, an improved S-H adaptive filtering method is provided;
s220, performing time updating and measurement updating on three sub-filters in the federal filter by adopting the improved S-H adaptive filtering method;
s230, the main filter performs information fusion on the output of each sub-filter;
in S210, the improved S-H adaptive filtering method includes the steps of:
s211, setting an initial value: x (0), P (0), R (0), Q (0), k=1;
s212, predicting the occurrence of the state based on the previous state, and predicting an equation:
s213, prediction error covariance:
s214, self-adaptive estimation:
s215, calculating a gain matrix:
s216, calculating an estimated value:
s217, updating a covariance matrix: p (P) k ＝(I-K k H k )P k|k-1 。
2. The intelligent underwater robot multi-source integrated navigation method based on the improved S-H adaptive federal filtering according to claim 1, wherein in S100, the method specifically comprises the following steps:
s110, firstly, establishing a common coordinate system and a coordinate conversion relation of a navigation system;
s120, obtaining error models of the SINS, USBL, DVL and MCP sensors on the basis of S110.
3. The intelligent underwater robot multi-source integrated navigation method based on the improved S-H adaptive federal filtering according to claim 2, wherein in S110, the method specifically comprises the following steps:
s111, assuming that four sensors SINS, USBL, DVL and MCP are all arranged at the center of a carrier of the underwater robot, the data obtained by the four sensors are all based on a carrier coordinate system, and the data are required to be unified into a navigation coordinate system;
s112, navigation coordinate System around OZ n Axis rotation ψ, about OX n1 Axis rotates θ, then around OY n2 Rotating gamma, namely:
wherein, psi is course angle, the geographic north direction is taken as starting point, the clockwise movement is positive, and the range is 0-360 degrees; θ is pitch angle, with horizontal plane as reference, positive upwards, negative downwards, in the range-90 ° to 90 °; gamma is a roll angle, right inclination is positive, left inclination is negative and the range is-90 degrees to 90 degrees by taking a vertical plane as a reference;
the conversion matrices correspond to:
the transformation matrix between the navigation coordinate system and the underwater robot carrier coordinate system is:
4. an improved S-H adaptive federal filtering-based intelligent multi-source integrated navigation method for underwater robot according to claim 3, wherein in S120, the following are specified:
the errors of the SINS include velocity, position and attitude errors, wherein,
the attitude error equation is:
in the method, in the process of the invention,indicating the misalignment angle of the east, north and sky three-way platform,>for the rotation angular velocity of the earth>Indicating the angular velocity caused by the change in position of the underwater robot carrier,
in the above, v= [ V E V N V U ] T Respectively representing the east, north and sky three-way speeds of the underwater robot carrier in a geographic coordinate system, L, lambda and h respectively representing longitude, latitude and altitude, R M 、R N Respectively represent the curvature radius of each point on the earth reference ellipsoidal meridian and ellipsoidal mortise unitary circle,
expanding an attitude error equation to obtain:
velocity error equation:
in the above, f n The specific force is indicated as such,
it was developed as follows:
position error equation:
the error of the USBL is as follows:
establishing a matrix coordinate system oxyz, fixing an acoustic beacon on an underwater robot carrier, and setting the position of the acoustic beacon as (X) a ,Y a ,Z a ) R is a position vector, three hydrophones are respectively arranged on an origin, an x axis and a y axis, and the angle between the connecting line between the acoustic beacon and the origin and the coordinate axis is theta mx 、θ my And theta m ，
The distance R between the beacon and the matrix is:
while
X a ＝Rcosθ mx (15)
Y a ＝Rcosθ my (16)
Is obtained by the following two formulas:
substituting formula (12) into formula (13) and formula (14) to obtain:
and substituting the formula (15) into the formula (16) to obtain:
and (3) solving to obtain:
the same principle is obtained:
θ mx 、θ my obtained by means of the phase difference,
the distance between two matrixes is d, the wavelength of sound wave is lambda, the phase difference of sound wave signals received by each matrix is phi, and the incident angle of the signals to the matrixes is theta m ：
The phase difference obtained by this method is obtained by the following formula:
c is the propagation speed of sound wave under water, which is the known quantity, T is the time difference between the signal transmission and the signal reception of the modem, and R is:
R＝0.5cT (26)
finally, the method comprises the following steps:
the error of the ultra-short baseline positioning system is analyzed below, and X is obtained from the above a 、Y a 、Z a Expressed as X a For example, Y a 、Z a Similarly, for X a And (3) performing full differentiation to obtain:
the relative positioning accuracy of the positions is as follows:
the variance of the relative skew R of the X-axis direction error is:
and (3) the same principle:
wherein DeltaT, deltac, deltaphi 12 、Δφ 13 Respectively a time measurement error, a sound velocity measurement error and a phase measurement error;
error equation for DVL:
the DVL transmits a beam of frequency f to the front, the back, the left and the right of the seabed 0 Wave speed c 0 Is provided that the AUV moves forward at a velocity V,
in the above, f d1 ,f d2 ,f d3 ,f d4 Respectively, the acoustic wave frequency shifts in four directions, V x ,V y ,V z Representing the three-axis velocity component of the AUV,
the DVL error includes a velocity offset error δV EDVL And δV NDVL The scale factor error delta C and the drift angle error delta are expressed by a first order Markov process, and then the error equation is as follows:
wherein beta is -1 EDVL 、β -1 NDVL 、β -1 Δ Respectively represent the related time, w Ed 、w Nd 、w Δ To excite white noise;
error equation for MCP:
after compensation, the error approximation of the MCP is considered a first order markov process:
in the above, δψ MCP Represents the MCP heading angle error,represents the correlation time, w MCP Representing the corresponding driving white noise.
5. The intelligent underwater robot multi-source integrated navigation method based on the improved S-H adaptive federal filtering of claim 1, wherein in S230, the main filter performs information fusion on the outputs of the sub-filters:
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111354287.4A CN114166203B (en) | 2021-11-16 | 2021-11-16 | Intelligent underwater robot multi-source combined navigation method based on improved S-H self-adaptive federal filtering |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111354287.4A CN114166203B (en) | 2021-11-16 | 2021-11-16 | Intelligent underwater robot multi-source combined navigation method based on improved S-H self-adaptive federal filtering |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114166203A CN114166203A (en) | 2022-03-11 |
| CN114166203B true CN114166203B (en) | 2024-02-09 |
Family
ID=80479270
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111354287.4A Active CN114166203B (en) | 2021-11-16 | 2021-11-16 | Intelligent underwater robot multi-source combined navigation method based on improved S-H self-adaptive federal filtering |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114166203B (en) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108827310A (en) * | 2018-07-12 | 2018-11-16 | 哈尔滨工程大学 | A kind of star sensor secondary gyroscope online calibration method peculiar to vessel |
| CN109459019A (en) * | 2018-12-21 | 2019-03-12 | 哈尔滨工程大学 | A kind of vehicle mounted guidance calculation method based on cascade adaptive robust federated filter |
| WO2020087845A1 (en) * | 2018-10-30 | 2020-05-07 | 东南大学 | Initial alignment method for sins based on gpr and improved srckf |
| CN111947651A (en) * | 2020-07-17 | 2020-11-17 | 中国人民解放军海军工程大学 | Underwater combined navigation information fusion method and system and autonomous underwater vehicle |
| CN112254718A (en) * | 2020-08-04 | 2021-01-22 | 东南大学 | Motion constraint assisted underwater combined navigation method based on improved Sage-Husa adaptive filtering |
-
2021
- 2021-11-16 CN CN202111354287.4A patent/CN114166203B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108827310A (en) * | 2018-07-12 | 2018-11-16 | 哈尔滨工程大学 | A kind of star sensor secondary gyroscope online calibration method peculiar to vessel |
| WO2020087845A1 (en) * | 2018-10-30 | 2020-05-07 | 东南大学 | Initial alignment method for sins based on gpr and improved srckf |
| CN109459019A (en) * | 2018-12-21 | 2019-03-12 | 哈尔滨工程大学 | A kind of vehicle mounted guidance calculation method based on cascade adaptive robust federated filter |
| CN111947651A (en) * | 2020-07-17 | 2020-11-17 | 中国人民解放军海军工程大学 | Underwater combined navigation information fusion method and system and autonomous underwater vehicle |
| CN112254718A (en) * | 2020-08-04 | 2021-01-22 | 东南大学 | Motion constraint assisted underwater combined navigation method based on improved Sage-Husa adaptive filtering |
Non-Patent Citations (6)
| Title |
|---|
| An improved self-adaptive Kalman filter for underwater integrated navigation system based on DR;yushan sun;2011 2nd International Conference on Intelligent Control and Information Processing;20110828;993-998 * |
| 基于多源融合的水下自主航行器定位方法研究;杨一鹏;中国优秀硕士学位论文全文数据库(第01期);1-79 * |
| 杨一鹏.基于多源融合的水下自主航行器定位方法研究.中国优秀硕士学位论文全文数据库.2021,(第01期),1-79. * |
| 水下潜航器的惯导/超短基线/多普勒测速信息融合及容错验证;徐博;郝芮;王超;张勋;张娇;;光学精密工程(第09期);全文 * |
| 水下航行器组合导航***与信息融合技术研究;牟宏伟;中国博士学位论文全文数据库(第04期);1-185 * |
| 自主式水下航行器导航算法研究;周吉雄;中国优秀硕士学位论文全文数据库(第01期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114166203A (en) | 2022-03-11 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109443379B (en) | SINS/DV L underwater anti-shaking alignment method of deep-sea submersible vehicle | |
| CN109324330B (en) | USBL/SINS tight combination navigation positioning method based on mixed derivative-free extended Kalman filtering | |
| CN112097763B (en) | Underwater vehicle combined navigation method based on MEMS IMU/magnetometer/DVL combination | |
| CN103697910B (en) | The correction method of autonomous underwater aircraft Doppler log installation error | |
| CN111947651B (en) | Underwater combined navigation information fusion method and system and autonomous underwater vehicle | |
| CN111829512B (en) | AUV navigation positioning method and system based on multi-sensor data fusion | |
| CN108345021B (en) | Doppler radar assisted GPS/INS vehicle speed measurement method | |
| CN111380518B (en) | SINS/USBL tight combination navigation positioning method introducing radial velocity | |
| CN111323012B (en) | INS-assisted DVL speed measurement error compensation method under high dynamic environment of carrier | |
| CN109579850B (en) | Deepwater intelligent navigation method based on auxiliary inertial navigation to water velocity | |
| CN112284384A (en) | Cooperative positioning method of clustered multi-deep-sea submersible vehicle considering measurement abnormity | |
| CN111854747B (en) | DVL-assisted SINS (strapdown inertial navigation system) coarse alignment method under large-mobility condition of carrier | |
| CN112556697A (en) | Shallow coupling data fusion navigation method based on federated structure | |
| CN112747748A (en) | Pilot AUV navigation data post-processing method based on reverse solution | |
| CN116295386A (en) | Multi-sensor fusion-based underwater vehicle navigation control system and method | |
| CN115096302A (en) | Strapdown inertial base navigation system information filtering robust alignment method, system and terminal | |
| CN104061930A (en) | Navigation method based on strapdown inertial guidance and Doppler log | |
| CN111982126B (en) | Design method of full-source BeiDou/SINS elastic state observer model | |
| CN116222578B (en) | Underwater integrated navigation method and system based on self-adaptive filtering and optimal smoothing | |
| CN117029872A (en) | AUV navigation method and system based on INS/DVL/LBL tight combination | |
| CN114166203B (en) | Intelligent underwater robot multi-source combined navigation method based on improved S-H self-adaptive federal filtering | |
| CN112083425A (en) | SINS/LBL tight combination navigation method introducing radial velocity | |
| CN113670302B (en) | Inertial/ultrashort baseline integrated navigation method under influence of motion effect | |
| CN116358544A (en) | Method and system for correcting inertial navigation error based on acoustic feature matching positioning | |
| CN103697887B (en) | A kind of optimization air navigation aid based on SINS and Doppler log |
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 |