CN111380519B - Navigation error correction method for ultrashort baseline/strapdown inertial navigation loose combination - Google Patents

Navigation error correction method for ultrashort baseline/strapdown inertial navigation loose combination Download PDF

Info

Publication number
CN111380519B
CN111380519B CN202010144251.2A CN202010144251A CN111380519B CN 111380519 B CN111380519 B CN 111380519B CN 202010144251 A CN202010144251 A CN 202010144251A CN 111380519 B CN111380519 B CN 111380519B
Authority
CN
China
Prior art keywords
coordinate system
navigation
underwater vehicle
strapdown inertial
axis
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
CN202010144251.2A
Other languages
Chinese (zh)
Other versions
CN111380519A (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.)
Harbin Engineering University
Original Assignee
Harbin Engineering University
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 Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN202010144251.2A priority Critical patent/CN111380519B/en
Publication of CN111380519A publication Critical patent/CN111380519A/en
Application granted granted Critical
Publication of CN111380519B publication Critical patent/CN111380519B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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/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/18—Stabilised platforms, e.g. by gyroscope
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/18—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using ultrasonic, sonic, or infrasonic waves
    • G01S5/22—Position of source determined by co-ordinating a plurality of position lines defined by path-difference measurements

Abstract

A navigation error correction method of an ultrashort baseline/strapdown inertial navigation loose combination belongs to the technical field of combined navigation and underwater sound positioning. The method solves the problem that when the array opening angle is large, the strapdown inertial navigation of the underwater vehicle needs to move to the effective working range of the ultra-short array for error correction, so that the working efficiency of the underwater vehicle is reduced. When the ultra-short baseline positioning system has a real number solution, the ratio of the relative depth and the slant range of the acoustic array and the beacon, namely the cosine value of the array opening angle, can be obtained, the filtering gain of the combined navigation is controlled by the value, and the proportion of low-quality observation information in state updating when the array opening angle is large can be reduced. By controlling the gain of the filter externally, strapdown inertial navigation errors can be effectively inhibited, and the working efficiency of the underwater vehicle under the condition of a large array opening angle is ensured. The invention can be applied to the fields of integrated navigation and underwater sound positioning.

Description

Navigation error correction method for ultrashort baseline/strapdown inertial navigation loose combination
Technical Field
The invention belongs to the technical field of integrated navigation and underwater sound positioning, and particularly relates to a filtering gain control method of an ultrashort baseline/strapdown inertial navigation loose assembly.
Background
With the increase of the array opening angle, the positioning error of the ultra-short baseline becomes larger, and the integrated navigation precision is influenced. In other words, the combination of the traditional ultra-short baseline and the strapdown inertial navigation can only be performed in the inner part and the surface of a cone with a certain array opening angle as a half cone angle, and when the underwater vehicle is in other position areas, the underwater vehicle only can depend on the strapdown inertial navigation, but the pure inertial navigation has the defect that the error is accumulated along with the time integral, so that the underwater vehicle needs to move to the effective working range of the ultra-short baseline at intervals to correct the accumulated error of the inertial navigation, and the working efficiency of the underwater vehicle is greatly reduced.
Disclosure of Invention
The invention aims to solve the problem that when the array opening angle is larger, the strapdown inertial navigation of an underwater vehicle needs to move to the effective working range of an ultra-short array for error correction, so that the working efficiency of the underwater vehicle is reduced, and provides a navigation error correction method of an ultra-short baseline/strapdown inertial navigation loose combination.
The technical scheme adopted by the invention for solving the technical problems is as follows: a navigation error correction method of an ultrashort baseline/strapdown inertial navigation loose combination comprises the following steps:
the method comprises the following steps that firstly, an ultra-short baseline acoustic array is inversely installed on an underwater vehicle, a gyroscope component and an accelerometer component of strapdown inertial navigation are fixedly connected on the underwater vehicle, and a synchronous beacon is distributed on the water surface; the position of the beacon under the navigation coordinate system is obtained through a GPS;
the navigation coordinate system is a geographical coordinate system of 'northeast sky', the geographical coordinate system takes the center of mass of the underwater vehicle as an origin o, the x axis points to the geographical east direction, the y axis points to the geographical north direction, the z axis is perpendicular to the xoy plane and points to the sky direction, and the x axis, the y axis and the z axis form a right-hand coordinate system;
secondly, calculating the position of the beacon and the relative depth of the ultra-short baseline acoustic array and the beacon by measuring the one-way propagation time delay and the effective sound velocity of the acoustic signal between the ultra-short baseline acoustic array and the beacon in an array coordinate system, and then calculating the ratio of the relative depth to the propagation distance of the acoustic signal between the underwater vehicle and the beacon, wherein the calculated ratio is the cosine value of the open angle of the array;
the array coordinate system takes the center of the ultra-short baseline acoustic array as an origin OaThe heading direction pointing to the underwater vehicle along the ultra-short baseline acoustic array plane is YaAxis, ZaAxis perpendicular to array plane upwards, XaAxis and YaAxis, ZaThe axes form a right-hand coordinate system;
thirdly, obtaining a positioning result of the ultra-short baseline on the underwater vehicle under the strapdown inertial navigation calculation navigation coordinate system according to the attitude information of the underwater vehicle provided by the strapdown inertial navigation and the position information of the beacon under the array coordinate system and the navigation coordinate system
Figure BDA0002400161440000021
Fourthly, positioning the underwater vehicle according to the ultra-short baseline
Figure BDA0002400161440000022
Underwater vehicle position calculated by strapdown inertial navigation
Figure BDA0002400161440000023
Establishing an extended Kalman filter;
step five, adjusting the filtering gain of the extended Kalman filter according to the cosine value of the open angle of the array calculated in the step two, and realizing state updating;
and step six, correcting the output of the strapdown inertial navigation according to the updated state value in the step five, and resetting the state of the extended Kalman filter.
The invention has the beneficial effects that: the invention provides a navigation error correction method of an ultra-short baseline/strapdown inertial navigation loose combination, which can obtain the ratio of the relative depth and the slant range of an acoustic array and a beacon, namely the cosine value of the array opening angle when an ultra-short baseline positioning system is positioned effectively, control the filter gain of combined navigation by the value, and reduce the proportion of low-quality observation information in state updating when the array opening angle is larger.
By controlling the gain of the filter externally, strapdown inertial navigation errors can be effectively inhibited, and the working efficiency of the underwater vehicle under the condition of a large array opening angle is ensured.
Drawings
FIG. 1 is a schematic diagram of a method of navigation error correction for ultrashort baseline/strapdown inertial navigation pine combinations when ultrashort baseline positioning is active.
Detailed Description
The first embodiment is as follows: as shown in fig. 1, a navigation error correction method of an ultra-short baseline/strapdown inertial navigation loose combination according to this embodiment includes the following steps:
the method comprises the following steps that firstly, an ultra-short baseline acoustic array is inversely installed on an underwater vehicle, a gyroscope component and an accelerometer component of strapdown inertial navigation are fixedly connected on the underwater vehicle, and a synchronous beacon is distributed on the water surface; the position of the beacon under the navigation coordinate system is obtained through a GPS;
the navigation coordinate system is an 'northeast' ('ENU') geographic coordinate system, the geographic coordinate system takes the center of mass of the underwater vehicle as an origin o, an x axis points to the east direction of geography, a y axis points to the north direction of geography, a z axis is perpendicular to an xoy plane and points to the sky direction, and the x axis, the y axis and the z axis form a right-hand coordinate system;
secondly, calculating the position of the beacon and the relative depth of the ultra-short baseline acoustic array and the beacon by measuring the one-way propagation time delay and the effective sound velocity of the acoustic signal between the ultra-short baseline acoustic array and the beacon in an array coordinate system, and then calculating the ratio of the relative depth to the propagation distance of the acoustic signal between the underwater vehicle and the beacon, wherein the calculated ratio is the cosine value of the open angle of the array;
the array coordinate system takes the center of the ultra-short baseline acoustic array as an origin OaThe heading direction pointing to the underwater vehicle along the ultra-short baseline acoustic array plane is YaAxis, ZaAxis perpendicular to array plane upwards, XaAxis and YaAxis, ZaThe axes form a right-hand coordinate system;
thirdly, obtaining a positioning result of the ultra-short baseline on the underwater vehicle under the strapdown inertial navigation calculation navigation coordinate system according to the attitude information of the underwater vehicle provided by the strapdown inertial navigation and the position information of the beacon under the array coordinate system and the navigation coordinate system
Figure BDA0002400161440000031
The strapdown inertial navigation calculation navigation coordinate system is as follows: the strapdown inertial navigation system realizes the recurrence of the geographical coordinate system of the northeast through resolving the attitude information of the underwater vehicle;
fourthly, positioning the underwater vehicle according to the ultra-short baseline
Figure BDA0002400161440000032
Establishing an extended Kalman filter based on position information fusion with the position of the underwater vehicle calculated by strapdown inertial navigation;
step five, adjusting the filtering gain of the extended Kalman filter according to the cosine value of the open angle of the array calculated in the step two, and realizing state updating;
and step six, correcting the output of the strapdown inertial navigation according to the updated state value in the step five, and resetting the state of the extended Kalman filter.
And after the state of the extended Kalman filter is reset, repeating the processes from the second step to the fifth step, correcting the output of the strapdown inertial navigation by using the updated state value, and continuously correcting the output of the strapdown inertial navigation.
Under any array opening angle, the invention can ensure the combined navigation precision and the working efficiency of the aircraft.
The second embodiment is as follows: the first difference between the present embodiment and the specific embodiment is: the effective sound velocity is the ratio of the geometric distance between the sound source and the ultra-short baseline acoustic array to the propagation time of the sound between the two points.
The third concrete implementation mode: the first difference between the present embodiment and the specific embodiment is: the array opening angle refers to a connecting line of the ultra-short baseline acoustic array and the beacon and Z under the array coordinate systemaThe included angle of the shaft in the positive direction.
The fourth concrete implementation mode: the first difference between the present embodiment and the specific embodiment is: in the second step, the position of the beacon is [ X ] in the matrix coordinate systema′ Ya′ Za′]TCosine of the base array opening angle is cos thetaZ；
Figure BDA0002400161440000033
Wherein: r is the propagation distance of the acoustic signal between the underwater vehicle and the beacon, XaIs a beacon at XaPosition in the axial direction, YaIs a beacon at YaPosition in the axial direction, ZaIs a beacon at ZaPosition in the axial direction; cos θXIs beacon at XaRatio of axial position to R, cos θYIs that the beacon is at YaThe ratio of the position in the axial direction to R;
cos2θX+cos2θY+cos2θZ＝1
wherein:
Figure BDA0002400161440000041
c is the sound velocity in water, tau is the mean value of one-way propagation delay measured by each element of the ultra-short baseline acoustic array, the one-way propagation distance is the distance between the underwater vehicle and the beacon, tauXAnd τYPropagation delay differences of two elements along the matrix coordinate system XaAxis and YaComponent of the axis, dXAnd dYIs that the corresponding two primitives are in XaAxis and YaPosition difference in the axial direction.
The fifth concrete implementation mode: the fourth difference between this embodiment and the specific embodiment is that: the specific process of the third step is as follows:
the attitude information of the underwater vehicle provided by the strapdown inertial navigation comprises a course angle A, a pitch angle K and a roll angle psi of the underwater vehicle;
establishing a carrier coordinate system obxbybzbOrigin of coordinates o of a carrier coordinate systembCentroid, coordinate axis x, of the underwater vehiclebIs directed to the right along the transverse axis of the underwater vehicle, and the coordinate axis ybIs directed forward along the longitudinal axis of the underwater vehicle, and the coordinate axis zbThe positive direction of the vector is directed upwards along the vertical axis of the underwater vehicle, and the definition of the vector coordinate system meets the right-hand rule;
relative to the carrier of the matrix coordinate systemThe angular installation deviations of the coordinate system are respectively alpha, beta and gamma (the angular deviations of three coordinate axes of the base matrix coordinate system and three coordinate axes of the carrier coordinate system), and the position deviation of the origin of the base matrix coordinate system relative to the origin of the carrier coordinate system
Figure BDA0002400161440000047
Comprises the following steps:
Figure BDA0002400161440000042
(ΔXb、ΔYband Δ ZbAll components in the vector coordinate system), Δ Xb、ΔYbAnd Δ ZbIs composed of
Figure BDA0002400161440000043
Component (b), the superscript T represents transposition;
conversion matrix from carrier coordinate system to strapdown inertial navigation calculation navigation coordinate system
Figure BDA0002400161440000044
And a conversion matrix from the base matrix coordinate system to the carrier coordinate system
Figure BDA0002400161440000045
Respectively as follows:
Figure BDA0002400161440000046
Figure BDA0002400161440000051
position of beacon under array coordinate system resolved according to ultra-short baseline acoustic positioning system
Figure BDA0002400161440000052
And location of GPS-provided beacons
Figure BDA0002400161440000053
To obtain
Figure BDA0002400161440000054
Wherein:
Figure BDA0002400161440000055
calculating a positioning result of the ultra-short base line to the underwater vehicle under a navigation coordinate system for the strapdown inertial navigation;
the navigation coordinate system n' calculated by strapdown inertial navigation and the three coordinate axes of the real navigation coordinate system n (the established navigation coordinate system) have an angle error phi: phi is ═ phiE φN φU]TLet phi be the strapdown inertial navigation misalignment angle error, phiE、φNAnd phiUAll of which are components in phi, a transformation matrix between a navigation coordinate system n' of the strapdown inertial navigation computation and a real navigation coordinate system n
Figure BDA0002400161440000056
Can be approximated as
Figure BDA0002400161440000057
Wherein: φ is an intermediate variable, I is an identity matrix;
Figure BDA0002400161440000058
calculating the difference between the true value of the underwater vehicle position in the real navigation coordinate system and the position of the underwater vehicle in the strapdown inertial navigation calculation navigation coordinate system n
Figure BDA0002400161440000059
Comprises the following steps:
Figure BDA00024001614400000510
wherein:
Figure BDA00024001614400000511
is a transformation matrix between a carrier coordinate system and a calculated navigation coordinate system n', wherein n is a Gaussian white noise sequence in the ultra-short baseline acoustic positioning system.
In the present invention, the navigation coordinate system, which is not particularly limited, refers to an established navigation coordinate system.
Alpha, beta and gamma are respectively the angle deviation of three coordinate axes of the matrix coordinate system relative to three coordinate axes of the carrier coordinate system;
the course angle is the included angle between the heading of the carrier (underwater vehicle) and the geographical north, and is defined as that north and west are positive, and the angle range is (-180 degrees and 180 degrees)](ii) a Roll angle gamma is carrier vertical axis zbWith the horizontal axis xbThe included angle of the vertical plane of the axis is positive when the carrier inclines rightwards, and the angle range is (-180 degrees and 180 degrees)](ii) a The pitch angle theta is the longitudinal axis y of the carrierbThe included angle between the horizontal projection line and the carrier is positive when the carrier is raised, and the angle range is (-90 degrees and 90 degrees)]。
And resolving the attitude, the speed and the position of the underwater vehicle under a navigation coordinate system by the strapdown inertial navigation. And converting the relative position of the beacon resolved by the ultra-short baseline under the acoustic array coordinate system by combining the installation deviation calibration result and the attitude information of the acoustic array to obtain the position of the underwater vehicle under the calculated navigation coordinate system.
The sixth specific implementation mode: the fifth embodiment is different from the fifth embodiment in that: the specific process of the step four is as follows:
the method comprises the steps of representing the position of an underwater vehicle output by strapdown inertial navigation in the form of latitude L, longitude lambda and height h, and converting the latitude L, the longitude lambda and the height h into an earth rectangular coordinate, wherein the earth rectangular coordinate system takes the geocenter as an origin, and x iseThe axis points to the intersection of the meridian and the equator, yeThe axis pointing at the intersection of the 90 ° meridian and the equator, zeAxis and xeAxis, yeThe axes form a right-hand coordinate system
Figure BDA0002400161440000061
In the formula, xe′、ye' and ze' X of the underwater vehicle respectively outputting strapdown inertial navigation in the earth rectangular coordinate systemeAxis, yeAxis and zePosition in the axial direction, RNIs the main curvature radius of the earth-unitary fourth of twelve earthly branches,
Figure BDA0002400161440000062
Reis the radius of the earth, e is the eccentricity of the earth,
Figure BDA0002400161440000063
a and b are the ellipse major and minor axis radii, respectively;
the differential conversion relationship of the above equation is:
Figure BDA0002400161440000064
the conversion matrix from the earth rectangular coordinate system to the strapdown inertial navigation calculation navigation coordinate system is as follows:
Figure BDA0002400161440000065
state variable of extended Kalman filter is formed by strapdown inertial navigation misalignment angle error phi ═ phiE φN φU]TStrapdown inertial navigation velocity error delta v ═ delta vE δvN δvU]TThe strapdown inertial navigation position error δ p ═ δ L δ λ δ h]TStrapdown inertial navigation gyro drift error epsilon ═ epsilonx εy εz]TStrapdown inertial navigation accelerometer bias error
Figure BDA0002400161440000078
Composition is carried out;
Figure BDA0002400161440000079
wherein: x is a state variable of the extended Kalman filter; delta vE、δvNAnd δ vUAre all components in δ ν, δ L, δ λ and δ h are all components in δ p, εx、εyAnd εzAre all components in the field of epsilon,
Figure BDA00024001614400000710
and
Figure BDA00024001614400000711
are all made of
Figure BDA00024001614400000712
The component (b);
the state equation of the extended kalman filter is:
Xk+1＝Fk+1/kXk+wk+1
wherein: xkExtending the state variable, X, of the Kalman filter for time kk+1Extending the state variable of the Kalman filter for the time k +1, Fk+1/kObtained from the error equation of the strapdown inertial navigation system as a state transition matrix, wk+1Is a process noise sequence of an extended Kalman filter system, generally in the form of white Gaussian noise;
underwater vehicle position [ L lambda h ] output by strapdown inertial navigation]TObtaining the position X 'of the underwater vehicle under the earth rectangular coordinate system through coordinate conversion'e(SINS)Then underwater vehicle position X'e(SINS)Position under strapdown inertial navigation computing navigation coordinate system
Figure BDA0002400161440000071
Comprises the following steps:
Figure BDA0002400161440000072
the observed quantity Z of the extended kalman filter is then:
Figure BDA0002400161440000073
wherein:
Figure BDA0002400161440000074
is that
Figure BDA0002400161440000075
The corresponding position error is determined by the position error,
Figure BDA0002400161440000076
and δ p ═ δ L δ λ δ h]TThe relationship of (1) is:
Figure BDA0002400161440000077
observation equation Z of extended Kalman Filterk+1Comprises the following steps:
Zk+1＝Hk+1Xk+1+vk+1
wherein: v. ofk+1Observation of a noise sequence for extended Kalman Filter systems, usually in the form of white Gaussian noise, Hk+1Is an observation matrix.
Figure BDA0002400161440000081
The combined navigation generally has a closed loop mode of feedback correction and an open loop mode of output correction, the project implementation of the former is complex, the output of the strapdown inertial navigation can be directly influenced when a filter has a fault, and the output correction does not relate to the interior of an independent navigation system and has stronger fault-tolerant capability, so the invention adopts the output correction mode.
The seventh embodiment: the sixth embodiment is different from the sixth embodiment in that: the concrete process of the step five is as follows:
when the positioning of the ultra-short baseline acoustic array is invalid, no measurement information enters the extended Kalman filter, and at the moment, the extended Kalman filter only updates the time:
according to
Figure BDA0002400161440000082
Calculating the predicted value of the state at the moment of k +1
Figure BDA0002400161440000083
Wherein
Figure BDA0002400161440000084
Is a state estimation value at the time k;
according to Pk+1/k＝Fk+1/kPkFT k+1/k+QkCalculating a state prediction error covariance matrix P at time k +1k+1/kIn which P iskEstimating an error covariance matrix, Q, for a state at time kkA system process noise covariance matrix at the moment k;
when the ultra-short baseline acoustic array positioning is effective, the measurement information enters an extended Kalman filter, and the extended Kalman filter completes the measurement updating:
according to Kk+1＝PkHT k+1(Hk+1PkHT k+1+Rk+1)-1Calculating the filter gain K at time K +1k+1Wherein R isk+1And Hk+1Respectively is a system observation noise covariance matrix and an observation matrix at the moment of k + 1; superscript-1 represents the inverse of the matrix;
according to Pk+1＝(I-Kk+1Hk+1)PkCalculating a state estimation error covariance matrix P at time k +1k+1；
According to
Figure BDA0002400161440000085
Calculating a state estimation value at the time of k +1
Figure BDA0002400161440000086
The extended Carl filter based on the position information is used for estimating the state variable by taking attitude error, speed error, position error, gyro drift and accelerometer bias of strapdown inertial navigation as the state variables and taking the difference between an ultra-short baseline and the position of an underwater vehicle output by the strapdown inertial navigation as observed quantity.
The extended Karl filter is used for performing linear approximation processing on a nonlinear state equation and an observation equation.
The Karl filter is linear Bayes estimation under the minimum mean square error criterion; karl filtering adopts a state equation to describe the relationship of state variables at adjacent moments, and adopts an observation equation to describe the relationship of the state variables and observed quantities.
The adjustment of the filter gain refers to the filter gain calculated by utilizing the cosine value of the array opening angle to control the theory, so that the filter gain under the condition of large array opening angle is properly reduced, and the influence degree of the low-quality ultrashort baseline acoustic positioning result on the state variable estimation precision is reduced.
And the correction of the strapdown inertial navigation output means that the estimated value of the state variable is subtracted from the original strapdown inertial navigation output to obtain a new inertial navigation output.
The resetting of the filtering state variable means that after the strapdown inertial navigation output is corrected, theoretically, the navigation information output by the strapdown inertial navigation at the moment has no error, and therefore the filtering state variable is zero.
|cosθzThe value range of | is [ 01 |)]In the case of a large opening angle, | cos θzThe filter gain can be properly adjusted, the influence of low-quality observation information on the update of the filter state is reduced, and the combined navigation precision is improved.
In the invention, the state equation of the extended Kalman filter is formed by an error equation of a strapdown inertial navigation system, but not limited to the state equation, such as a positioning error, a distance measurement error, an installation deviation and the like of an ultra-short baseline are added into the state equation.
The above-described calculation examples of the present invention are merely to explain the calculation model and the calculation flow of the present invention in detail, and are not intended to limit the embodiments of the present invention. It will be apparent to those skilled in the art that other variations and modifications of the present invention can be made based on the above description, and it is not intended to be exhaustive or to limit the invention to the precise form disclosed, and all such modifications and variations are possible and contemplated as falling within the scope of the invention.

Claims (7)

1. A method for correcting navigation error of ultrashort baseline/strapdown inertial navigation loose combination is characterized by comprising the following steps:
the method comprises the following steps that firstly, an ultra-short baseline acoustic array is inversely installed on an underwater vehicle, a gyroscope component and an accelerometer component of strapdown inertial navigation are fixedly connected on the underwater vehicle, and a synchronous beacon is distributed on the water surface; the position of the beacon under the navigation coordinate system is obtained through a GPS;
the navigation coordinate system is a geographical coordinate system of 'northeast sky', the geographical coordinate system takes the center of mass of the underwater vehicle as an origin o, the x axis points to the geographical east direction, the y axis points to the geographical north direction, the z axis is perpendicular to the xoy plane and points to the sky direction, and the x axis, the y axis and the z axis form a right-hand coordinate system;
secondly, calculating the position of the beacon and the relative depth of the ultra-short baseline acoustic array and the beacon by measuring the one-way propagation time delay and the effective sound velocity of the acoustic signal between the ultra-short baseline acoustic array and the beacon in an array coordinate system, and then calculating the ratio of the relative depth to the propagation distance of the acoustic signal between the underwater vehicle and the beacon, wherein the calculated ratio is the cosine value of the open angle of the array;
the array coordinate system takes the center of the ultra-short baseline acoustic array as an origin OaThe heading direction pointing to the underwater vehicle along the ultra-short baseline acoustic array plane is YaAxis, ZaAxis perpendicular to array plane upwards, XaAxis and YaAxis, ZaThe axes form a right-hand coordinate system;
thirdly, obtaining a positioning result of the ultra-short baseline on the underwater vehicle under the strapdown inertial navigation calculation navigation coordinate system according to the attitude information of the underwater vehicle provided by the strapdown inertial navigation and the position information of the beacon under the array coordinate system and the navigation coordinate system
Figure FDA0003139446290000011
Fourthly, positioning the underwater vehicle according to the ultra-short baseline
Figure FDA0003139446290000012
Establishing an extended Kalman filter with the position of the underwater vehicle calculated by strapdown inertial navigation;
step five, adjusting the filtering gain of the extended Kalman filter according to the cosine value of the open angle of the array calculated in the step two, and realizing state updating;
and step six, correcting the output of the strapdown inertial navigation according to the updated state value in the step five, and resetting the state of the extended Kalman filter.
2. The method as claimed in claim 1, wherein the effective sound velocity is a ratio of a geometric distance between the sound source and the ultra-short baseline acoustic matrix to a propagation time of the sound signal between the two points.
3. The method as claimed in claim 1, wherein the array opening angle is a Z-coordinate of a connection line between the ultra-short baseline acoustic array and the beaconaThe included angle of the shaft in the positive direction.
4. The method as claimed in claim 1, wherein in the second step, the position of the beacon is [ X'a Y′a Z′a]TCosine of the base array opening angle is cos thetaZ；
Figure FDA0003139446290000021
Wherein: r is an acoustic signalPropagation distance, X 'between the underwater vehicle and the beacon'aFor beacons at XaPosition in axial direction, Y'aFor beacons at YaPosition in the axial direction, Z'aFor beacons in ZaPosition in the axial direction; cos θXIs beacon at XaRatio of axial position to R, cos θYIs that the beacon is at YaThe ratio of the position in the axial direction to R;
cos2θX+cos2θY+cos2θZ＝1
wherein:
Figure FDA0003139446290000022
c is the speed of sound in water, τXAnd τYPropagation delay differences of two elements along the matrix coordinate system XaAxis and YaComponent of the axis, dXAnd dYIs that the corresponding two primitives are in XaAxis and YaPosition difference in the axial direction.
5. The method as claimed in claim 4, wherein the third step comprises the following steps:
the attitude information of the underwater vehicle provided by the strapdown inertial navigation comprises a course angle A, a pitch angle K and a roll angle psi of the underwater vehicle;
establishing a carrier coordinate system obxbybzbOrigin of coordinates o of a carrier coordinate systembCentroid, coordinate axis x, of the underwater vehiclebIs directed to the right along the transverse axis of the underwater vehicle, and the coordinate axis ybIs directed forward along the longitudinal axis of the underwater vehicle, and the coordinate axis zbThe positive direction of the vector is directed upwards along the vertical axis of the underwater vehicle, and the definition of the vector coordinate system meets the right-hand rule;
the angular installation deviation of the base matrix coordinate system relative to the carrier coordinate system is alpha, beta and gamma respectively, and the position deviation of the origin of the base matrix coordinate system relative to the origin of the carrier coordinate system
Figure FDA0003139446290000023
Comprises the following steps:
Figure FDA0003139446290000024
ΔXb、ΔYband Δ ZbIs composed of
Figure FDA0003139446290000025
Component (b), the superscript T represents transposition;
conversion matrix from carrier coordinate system to strapdown inertial navigation calculation navigation coordinate system
Figure FDA0003139446290000026
And a conversion matrix from the base matrix coordinate system to the carrier coordinate system
Figure FDA0003139446290000031
Respectively as follows:
Figure FDA0003139446290000032
Figure FDA0003139446290000033
according to the position of the beacon under the matrix coordinate system
Figure FDA0003139446290000034
And location of GPS-provided beacons
Figure FDA0003139446290000035
To obtain
Figure FDA0003139446290000036
Wherein:
Figure FDA0003139446290000037
calculating a positioning result of the ultra-short base line to the underwater vehicle under a navigation coordinate system for the strapdown inertial navigation;
an angle error phi exists between a navigation coordinate system n' calculated by strapdown inertial navigation and three coordinate axes of a real navigation coordinate system n: phi is ═ phiE φN φU]TLet phi be the strapdown inertial navigation misalignment angle error, phiE、φNAnd phiUAll of which are components in phi, a transformation matrix between a navigation coordinate system n' of the strapdown inertial navigation computation and a real navigation coordinate system n
Figure FDA0003139446290000038
Is composed of
Figure FDA0003139446290000039
Wherein: φ is an intermediate variable, I is an identity matrix;
Figure FDA00031394462900000310
the difference between the position of the underwater vehicle under the real navigation coordinate system and the position of the underwater vehicle under the strapdown inertial navigation calculation navigation coordinate system n
Figure FDA00031394462900000311
Comprises the following steps:
Figure FDA00031394462900000312
wherein:
Figure FDA00031394462900000313
is a transformation matrix between a carrier coordinate system and a calculated navigation coordinate system n', n being an ultra-short baseA gaussian white noise sequence in a line acoustic localization system.
6. The method as claimed in claim 5, wherein the specific procedure of the fourth step is as follows:
the method comprises the steps of representing the position of an underwater vehicle output by strapdown inertial navigation in the form of latitude L, longitude lambda and height h, and converting the latitude L, the longitude lambda and the height h into an earth rectangular coordinate, wherein the earth rectangular coordinate system takes the geocenter as an origin, and x iseThe axis points to the intersection of the meridian and the equator, yeThe axis pointing at the intersection of the 90 ° meridian and the equator, zeAxis and xeAxis, yeThe axes form a right-hand coordinate system
Figure FDA0003139446290000041
In formula (II), x'e、y′eAnd z'eX of underwater vehicle respectively outputting strapdown inertial navigation in earth rectangular coordinate systemeAxis, yeAxis and zePosition in the axial direction, RNIs the main curvature radius of the earth-unitary fourth of twelve earthly branches,
Figure FDA0003139446290000042
Reis the radius of the earth, e is the eccentricity of the earth,
Figure FDA0003139446290000043
a and b are the ellipse major and minor axis radii, respectively;
the differential conversion relationship of the above equation is:
Figure FDA0003139446290000044
the conversion matrix from the earth rectangular coordinate system to the strapdown inertial navigation calculation navigation coordinate system is as follows:
Figure FDA0003139446290000045
state variable of extended Kalman filter is formed by strapdown inertial navigation misalignment angle error phi ═ phiE φN φU]TStrapdown inertial navigation velocity error delta v ═ delta vE δvN δvU]TThe strapdown inertial navigation position error δ p ═ δ L δ λ δ h]TStrapdown inertial navigation gyro drift error epsilon ═ epsilonx εy εz]TStrapdown inertial navigation accelerometer bias error
Figure FDA0003139446290000046
Composition is carried out;
Figure FDA0003139446290000047
wherein: x is a state variable of the extended Kalman filter; delta vE、δvNAnd δ vUAre all components in δ ν, δ L, δ λ and δ h are all components in δ p, εx、εyAnd εzAre all components in the field of epsilon,
Figure FDA0003139446290000048
and
Figure FDA0003139446290000049
are all made of
Figure FDA00031394462900000410
The component (b);
the state equation of the extended kalman filter is:
Xk+1＝Fk+1/kXk+wk+1
wherein: xkThe state variables of the extended kalman filter for time k,Xk+1extending the state variable of the Kalman filter for the time k +1, Fk+1/kBeing a state transition matrix, wk+1A noise sequence is the process noise sequence of the extended Kalman filter system;
underwater vehicle position [ L lambda h ] output by strapdown inertial navigation]TObtaining the position X 'of the underwater vehicle under the earth rectangular coordinate system through coordinate conversion'e(SINS)Then underwater vehicle position X'e(SINS)Position under strapdown inertial navigation computing navigation coordinate system
Figure FDA0003139446290000051
Comprises the following steps:
Figure FDA0003139446290000052
the observed quantity Z of the extended kalman filter is then:
Figure FDA0003139446290000053
wherein:
Figure FDA0003139446290000054
is that
Figure FDA0003139446290000055
The corresponding position error is determined by the position error,
Figure FDA0003139446290000056
and δ p ═ δ L δ λ δ h]TThe relationship of (1) is:
Figure FDA0003139446290000057
observation equation Z of extended Kalman Filterk+1Comprises the following steps:
Zk+1＝Hk+1Xk+1+vk+1
wherein: v. ofk+1Observing a noise sequence for an extended Kalman Filter System, Hk+1Is an observation matrix.
7. The method as claimed in claim 6, wherein the detailed procedure of the fifth step is as follows:
when the positioning of the ultra-short baseline acoustic array is invalid, no measurement information enters the extended Kalman filter, and the extended Kalman filter only updates the time:
according to
Figure FDA0003139446290000058
Calculating the predicted value of the state at the moment of k +1
Figure FDA0003139446290000059
Wherein
Figure FDA00031394462900000510
Is a state estimation value at the time k;
according to Pk+1/k＝Fk+1/kPkFT k+1/k+QkCalculating a state prediction error covariance matrix P at time k +1k+1/kIn which P iskEstimating an error covariance matrix, Q, for a state at time kkA system process noise covariance matrix at the moment k;
when the ultra-short baseline acoustic array positioning is effective, the measurement information enters an extended Kalman filter, and the extended Kalman filter completes the measurement updating:
according to Kk+1＝PkHT k+1(Hk+1PkHT k+1+Rk+1)-1Calculating the filter gain K at time K +1k+1Wherein R isk+1And Hk+1Respectively is a system observation noise covariance matrix and an observation matrix at the moment of k + 1;
according to Pk+1＝(I-Kk+1Hk+1)PkCalculating a state estimation error covariance matrix P at time k +1k+1；
According to
Figure FDA0003139446290000061
Calculating a state estimation value at the time of k +1
Figure FDA0003139446290000062
CN202010144251.2A 2020-03-04 2020-03-04 Navigation error correction method for ultrashort baseline/strapdown inertial navigation loose combination Active CN111380519B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010144251.2A CN111380519B (en) 2020-03-04 2020-03-04 Navigation error correction method for ultrashort baseline/strapdown inertial navigation loose combination

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010144251.2A CN111380519B (en) 2020-03-04 2020-03-04 Navigation error correction method for ultrashort baseline/strapdown inertial navigation loose combination

Publications (2)

Publication Number Publication Date
CN111380519A CN111380519A (en) 2020-07-07
CN111380519B true CN111380519B (en) 2021-08-31

Family

ID=71222696

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010144251.2A Active CN111380519B (en) 2020-03-04 2020-03-04 Navigation error correction method for ultrashort baseline/strapdown inertial navigation loose combination

Country Status (1)

Country Link
CN (1) CN111380519B (en)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112836889A (en) * 2021-02-19 2021-05-25 鹏城实验室 Path optimization method, underwater vehicle and computer readable storage medium
CN113176539B (en) * 2021-04-25 2022-09-09 哈尔滨工程大学 Underwater sound signal noise multi-stage suppression and steady positioning system and positioning method
CN116068540B (en) * 2023-02-17 2023-09-12 哈尔滨工程大学 Radial beam angle correction method for acoustic Doppler velocity measurement

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8130142B2 (en) * 2009-09-21 2012-03-06 Appareo Systems, Llc GNSS ultra-short baseline heading determination system and method
CN105509765B (en) * 2014-09-23 2018-07-20 北京自动化控制设备研究所 A kind of inertia/DVL/USBL mounting error calibration methods
CN105486313A (en) * 2016-02-03 2016-04-13 东南大学 Positioning method based on low-cost USBL-assisted SINS
CN109737956B (en) * 2018-12-17 2022-06-21 东南大学 SINS/USBL phase difference tight combination navigation positioning method based on double transponders
CN109828296A (en) * 2019-03-08 2019-05-31 哈尔滨工程大学 A kind of non-linear tight integration synthesis correction method of INS/USBL

Also Published As

Publication number Publication date
CN111380519A (en) 2020-07-07

Similar Documents

Publication Publication Date Title
CN111380519B (en) Navigation error correction method for ultrashort baseline/strapdown inertial navigation loose combination
CN111380518B (en) SINS/USBL tight combination navigation positioning method introducing radial velocity
CN101788296B (en) SINS/CNS deep integrated navigation system and realization method thereof
CN110514203B (en) Underwater integrated navigation method based on ISR-UKF
CN108444476B (en) Polar region collaborative navigation method of multi-underwater unmanned vehicle considering underwater acoustic communication delay
CN101949703A (en) Strapdown inertial/satellite combined navigation filtering method
CN115200574A (en) Polar region transverse combination navigation method under earth ellipsoid model
CN111025229B (en) Underwater robot pure orientation target estimation method
CN111207773B (en) Attitude unconstrained optimization solving method for bionic polarized light navigation
CN110207698B (en) Polar region grid inertial navigation/ultra-short baseline tight combination navigation method
CN109708670A (en) The noise compensation method of polar region Transfer Alignment deflection deformation based on improved Sage-Husa adaptive Kalman filter
CN113155134B (en) Underwater acoustic channel tracking and predicting method based on inertia information assistance
CN112083425A (en) SINS/LBL tight combination navigation method introducing radial velocity
CN112833878A (en) Near-ground multi-source astronomical autonomous navigation method
CN117537811A (en) Cross-polar region navigation switching method under optimized earth ellipsoid model
CN110388942B (en) Vehicle-mounted posture fine alignment system based on angle and speed increment
CN111380520B (en) SINS/USBL loose combination navigation positioning method introducing radial velocity
CN112229421A (en) Strapdown inertial navigation shaking base rough alignment method based on lie group optimal estimation
CN114061574B (en) Position-invariant constraint and zero-speed correction-based coal mining machine pose-determining and orienting method
CN111721284B (en) SINS/USBL combined navigation positioning method in passive mode
CN109471103A (en) A kind of missile-borne Bistatic SAR data fusion positioning error correcting method
CN115790652A (en) Odometer/dual-antenna GNSS space online calibration method
CN111380517B (en) SINS/USBL deep combination navigation positioning method based on USBL software receiver
CN117470234B (en) Ship cross-polar region filtering switching method based on Psi angle error model
CN116068540B (en) Radial beam angle correction method for acoustic Doppler velocity measurement

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