CN112747748A - Pilot AUV navigation data post-processing method based on reverse solution - Google Patents

Pilot AUV navigation data post-processing method based on reverse solution Download PDF

Info

Publication number
CN112747748A
CN112747748A CN202011529538.3A CN202011529538A CN112747748A CN 112747748 A CN112747748 A CN 112747748A CN 202011529538 A CN202011529538 A CN 202011529538A CN 112747748 A CN112747748 A CN 112747748A
Authority
CN
China
Prior art keywords
auv
sins
dvl
navigation
equation
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.)
Pending
Application number
CN202011529538.3A
Other languages
Chinese (zh)
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.)
China Academy of Aerospace Aerodynamics CAAA
Original Assignee
China Academy of Aerospace Aerodynamics CAAA
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 China Academy of Aerospace Aerodynamics CAAA filed Critical China Academy of Aerospace Aerodynamics CAAA
Priority to CN202011529538.3A priority Critical patent/CN112747748A/en
Publication of CN112747748A publication Critical patent/CN112747748A/en
Pending legal-status Critical Current

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/20—Instruments for performing navigational calculations
    • G01C21/203—Specially adapted for sailing ships
    • 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
    • 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
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • 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
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Abstract

The navigation AUV navigation data post-processing method based on reverse solution fully utilizes GPS information of the navigation AUV after water is produced by reversely solving navigation data stored in the navigation process of the navigation AUV and corrects positioning errors accumulated underwater, so that the positioning performance of the whole navigation formation is improved, and finally, the feasibility and the effectiveness of the method are subjected to simulation verification through simulation. The method can be used in the fields of submarine topography scanning, chart drawing, underwater detection and the like, can further improve the position precision of underwater task data acquired in AUV formation, and improves the accuracy and the usefulness of the data.

Description

Pilot AUV navigation data post-processing method based on reverse solution
Technical Field
The invention relates to a pilot AUV navigation data post-processing method based on reverse resolving.
Background
Autonomous Underwater Vehicles (AUVs) are important tools for performing marine military missions and for marine development. Currently, the underwater navigation technology is a key technical challenge facing long-time underwater navigation of the AUV. In the multi-AUV collaborative navigation system, AUVs form an underwater communication network by assembling underwater acoustic equipment, so that information interaction and sharing can be performed among the AUVs. The multi-AUV cooperative operation can bear complex tasks which are difficult to or impossible to complete by a single AUV, and has the advantages of high efficiency and good reliability, and wide application prospect. In recent years, a multi-AUV collaborative navigation system has become a key research direction in the field of underwater navigation. Generally, there are two ways of cooperating a multi-AUV system: parallel mode and master-slave mode. In the master-slave multi-AUV collaborative navigation system, the master AUV can obtain higher positioning accuracy by assembling a high-accuracy inertial navigation device to become a reference standard of other sub AUVs, and the slave AUV is only provided with a low-accuracy navigation device. The multi-AUV collaborative navigation system is constructed by a small number of main AUVs equipped with high-precision navigation equipment and other slave AUVs equipped with low-precision navigation equipment, so that the high-precision positioning of the whole system can be completed, and the number of the slave AUVs is not limited theoretically, so that the multi-AUV collaborative navigation system has the characteristics of high precision and low cost, and is always the key point of research.
In the master-slave multi-AUV collaborative navigation system, the positioning information of the pilot AUV is mainly used as a reference, and the accumulated positioning errors of other AUVs are corrected at any time in the process of sailing. In the field of underwater Navigation technology, as a relatively mature integrated Navigation mode, a Strap-down Inertial Navigation System (SINS) and Doppler Velocity meter (DVL) integrated Navigation System corrects the Velocity information resolved by the SINS by using the Velocity information provided by the DVL, so as to inhibit error accumulation of the Strap-down Inertial Navigation System, and the integrated Navigation System is an underwater integrated Navigation technology which is relatively widely applied at present. Although the DVL is used as a navigation aid to correct the positioning error of the SINS, the DVL itself has a measurement error, and when the AUV performs a long-time underwater task, a large error is still accumulated, so that the navigation data acquired underwater has a large error, and the position information of the acquired task data cannot be reflected really. Therefore, although the piloting AUV is provided with the high-precision navigation equipment, the positioning precision of the piloting AUV is inevitably dispersed after long-time navigation due to the fact that the piloting AUV cannot receive external information, so that the positioning error resolved by the piloting AUV is gradually increased, and the positioning performance of the whole navigation formation is influenced.
Aiming at the problems, a pilot AUV navigation data post-processing method based on the combination of SINS/DVL and SINS/DVL/GPS is provided, and forward filtering is carried out on the system underwater by adopting an SINS/DVL combined navigation mode; after water flows out, the system receives GPS signals, two navigation modes of SINS/DVL and SINS/DVL/GPS are adopted for independent filtering, positioning errors of the system are obtained through reverse calculation after filtering is finished, and finally the positioning errors accumulated underwater of the system are corrected through combination of the positioning errors of the system obtained through the forward calculation and the reverse calculation, so that the overall positioning performance of the system is improved. By the method, the underwater accumulated positioning error of the piloted AUV can be corrected, the positioning precision of the piloted AUV is improved, and the positioning performance of the whole system is improved. Finally, the feasibility and the effectiveness of the method are verified through simulation analysis.
Disclosure of Invention
The technical problem to be solved by the invention is as follows: the method overcomes the defects of the prior art, provides a pilot AUV navigation data post-processing method based on reverse resolving, and solves the problem of contradiction between the dynamic performance of a carrier tracking loop and low noise.
The technical solution of the invention is as follows:
a pilot AUV navigation data post-processing method based on reverse resolving comprises the following steps:
step 1: establishing a mathematical model of SINS and DVL, comprising:
(1) establishing an SINS error state equation;
(2) establishing an error model of the DVL;
(3) establishing a measurement equation of SINS/DVL;
step 2: establishing an SINS/DVL/GPS measurement equation according to the mathematical model of the SINS and the DVL established in the step 1;
and step 3: establishing a posture updating differential equation of the strapdown inertial navigation system;
and 4, step 4: establishing a speed updating differential equation of the strapdown inertial navigation system;
and 5: establishing a position updating differential equation of the strapdown inertial navigation system;
step 6: discretizing the attitude, speed and position updating differential equation to obtain a discretization equation;
and 7: and establishing a reverse navigation equation according to the discretization equation, and performing reverse solution on the piloting AUV navigation data to complete post-processing of the piloting AUV navigation data.
Further, the establishing of the SINS error state equation specifically includes:
and selecting a geographic coordinate system as a reference system, and selecting position error, speed error, attitude error, gyroscope drift and accelerometer zero offset as state quantities.
Figure BDA0002851808540000031
Wherein, XSINSRepresents the state vector of the SINS,
Figure BDA0002851808540000032
north, sky and east misalignment angles, respectively; delta VN、δVU、δVENorth, sky and east speed errors, respectively; δ L, δ λ are latitude and longitude errors, respectively; epsilonbx、εby、εbzGyroscope drift in x, y, z directions, respectively; deltabx、Δby、ΔbzAccelerometer bias in x, y, z directions for the accelerometer, respectively;
to obtain the SINS error state equation of
Figure BDA0002851808540000033
In the above formula, FSINSRepresenting system state transition matrix, WSINSIs system noise and is zero mean white gaussian noise. FSINSAs shown below.
Figure BDA0002851808540000034
In the above formula, RM,RNThe curvature radiuses of the earth meridian and the prime unit circle at the position of the carrier are respectively, and the approximate calculation formula is as follows: rM≈Re(1-2e+3esin2L) and RN≈Re(1+esin2L)；ReThe semiaxis is the long semiaxis of the earth reference ellipsoid, and e is the ellipticity of the reference ellipsoid; vN、VU、VEThe velocity of the AUV in the north-east direction, L and h respectively represent the longitude and depth of the carrier; omegaieIs the earth rotation rate; f. ofN、fU、fERespectively the specific force of the carrier in the north-east direction;
Figure BDA0002851808540000041
the state transition matrix is from an AUV coordinate system b to a geographic coordinate system n;
Figure BDA0002851808540000042
wherein phi, psi and theta respectively represent the roll angle, the course angle and the yaw angle of the AUV.
Further, the establishing of the error model of the DVL specifically includes:
assuming a forward velocity v of the AUV, the transducer is moved relative to the seafloor along the beam axis OA at a velocity v cos α, given the transmit frequency f of the DVL0The frequency of the sound wave received by the stationary reflecting point is f1The propagation velocity of sound wave in water is v0Then there is
Figure BDA0002851808540000043
Alpha is the inclination angle of the transducer;
at the same time, the frequency f is reflected from the sea bottom reflection point1The sound wave frequency received by a DVL velocimeter receiving transducer on the AUV is f2Then, then
Figure BDA0002851808540000044
Figure BDA0002851808540000045
The above formula is simplified into
Figure BDA0002851808540000046
Obtaining a DVL frequency shift of
Figure BDA0002851808540000047
Obtained by equation conversion
Figure BDA0002851808540000048
Under an AUV coordinate system, the vector equation form of the DVL velocity measurement error equation is expressed as
δVb＝ΔC·Vb+u；
Wherein, δ VbRepresenting the speed measurement error of the DVL under the carrier system; u is measurement noise; Δ C is the scale coefficient error;
by adopting an SINS/DVL combined navigation mode, the speed relation between a DVL coordinate system and an AUV coordinate system is expressed as
Figure BDA0002851808540000049
Wherein the content of the first and second substances,
Figure BDA00028518085400000410
a transformation matrix from a DVL coordinate system d system to an AUV coordinate system b system; vbRepresents the velocity in the AUV coordinate system; vdRepresenting the velocity in the DVL coordinate system.
Further, the establishing of the SINS/DVL measurement equation specifically includes:
the equation of velocity measured by SINS in the northeast direction is
VE＝VE0+δVE
VN＝VN0+δVN
VU＝VU0+δVU
Wherein, VN、VU、VEThe velocity of the AUV in the north-east direction; vN0、VU0、VE0Respectively measuring the ideal speed of the AUV in the north east direction by the SINS; delta VN、δVU、δVERespectively the velocity error of the SINS in the north-east direction;
velocity measurement of DVL under geographic coordinate system n
Figure BDA0002851808540000051
Is shown as
Figure BDA0002851808540000052
Figure BDA0002851808540000053
Figure BDA0002851808540000054
Wherein the content of the first and second substances,
Figure BDA0002851808540000055
respectively representing the velocity of the DVL measured in the north-east direction under a geographic coordinate system n;
Figure BDA0002851808540000056
respectively representing the projections of the measurement errors in the east direction, the north direction and the sky direction,
Figure BDA0002851808540000057
Figure BDA0002851808540000058
respectively representing the projections of random interference errors of the measured values in the east direction, the north direction and the sky direction;
the calculated velocity of the SINS is subtracted from the projection of the measured velocity of the DVL in the geographic coordinate system to obtain
Figure BDA0002851808540000059
Therefore, the measurement equation Z (t) is
Figure BDA00028518085400000510
Wherein u isnFor generalized noise, H (t) is an observation matrix, which can be split into two parts
Figure BDA00028518085400000511
The two parts in the above formula are
Figure BDA0002851808540000061
Wherein the content of the first and second substances,
Figure BDA0002851808540000062
an observation matrix representing the SINS is shown,
Figure BDA0002851808540000063
an observation matrix representing the DVL;
Figure BDA0002851808540000064
Figure BDA0002851808540000065
is a state transition matrix
Figure BDA0002851808540000066
9 variable values.
Further, the step 2 establishes an SINS/DVL/GPS measurement equation according to the established mathematical model of SINS and DVL, specifically:
in the SINS/DVL/GPS integrated navigation system, the state quantity of the system is unchanged, the state equation is the same as the SINS/DVL integrated navigation state equation, the position information of the carrier calculated by the SINS is differed with the position information acquired by the GPS, and the difference is added into the measurement equation of the SINS/DVL, so that the measurement equation of the SINS/DVL/GPS integrated navigation system is obtained as follows:
Figure BDA0002851808540000067
wherein the observed quantity Z is a direct observed value of the state quantity; the observation matrix H is a constant matrix H ═ I5×5 0](ii) a X is a state variable of SINS; v is measurement noise; l isSINS、λSINSRepresenting latitude information and latitude information obtained by SINS resolving; l isGPS、λGPSIndicating latitude information and longitude information measured by the GPS.
Further, the establishing of the attitude update differential equation of the strapdown inertial navigation system in the step 3 specifically includes:
Figure BDA0002851808540000068
each variable is represented as follows
Figure BDA0002851808540000069
Figure BDA00028518085400000610
Figure BDA00028518085400000611
Wherein the content of the first and second substances,
Figure BDA00028518085400000612
for state transition matrix
Figure BDA00028518085400000613
In the form of a differential of (a),
Figure BDA00028518085400000614
is the measured angular rate of the AUV;
Figure BDA00028518085400000615
AUV angular velocity measured for a gyroscope;
Figure BDA00028518085400000616
is the earth rotation rate;
Figure BDA00028518085400000617
is the position velocity;
Figure BDA00028518085400000618
representing the transition matrix from the navigation coordinate system n to the AUV coordinate system b.
Further, the step 4 of establishing a velocity update differential equation of the strapdown inertial navigation system specifically includes:
Figure BDA0002851808540000071
Figure BDA0002851808540000072
gn＝[0 -g 0]T
wherein f isnLinear acceleration under a navigation coordinate system; f. ofbLinear acceleration measured by an accelerometer under an AUV coordinate system; gnAcceleration in a spatial direction; g is the acceleration of gravity.
Further, the step 5 of establishing a position update differential equation of the strapdown inertial navigation system specifically includes:
Figure BDA0002851808540000073
wherein the content of the first and second substances,
Figure BDA0002851808540000074
in differential form, L, λ, h, respectively.
Further, the step 6 performs discretization processing on the attitude, velocity and position update differential equation to obtain a discretization equation, specifically:
Figure BDA0002851808540000075
Figure BDA0002851808540000076
Figure BDA0002851808540000077
Figure BDA0002851808540000078
Figure BDA0002851808540000079
wherein
Figure BDA00028518085400000710
Figure BDA00028518085400000711
Figure BDA00028518085400000712
Wherein the content of the first and second substances,
Figure BDA00028518085400000713
respectively represent at the k, k-State transition matrix at time 1;
Figure BDA00028518085400000714
respectively representing the speed of the AUV at the k and k-1 moments; l isk、Lk-1Respectively showing the latitude of the AUV at the k and k-1 times; lambda [ alpha ]k、λk-1Respectively indicates the longitude of AUV at the k and k-1 time; h isk、hk-1Respectively showing the navigation depth of the AUV at the k and k-1 moments; t issIs a discrete period;
Figure BDA00028518085400000715
acceleration of the AUV measured by the accelerometer at the k moment;
Figure BDA00028518085400000716
Figure BDA0002851808540000081
angular rate of AUV at time k;
Figure BDA0002851808540000082
the angular velocity of the AUV measured by the gyroscope at the kth moment;
Figure BDA0002851808540000083
the earth rotation rate at the k and k-1 moments;
Figure BDA0002851808540000084
the position rate of the kth and k-1 time;
Figure BDA0002851808540000085
respectively represents the velocity components of the AUV measured in three directions in the northeast on the k and k-1 moments.
Further, the step 7 establishes a reverse navigation equation according to the discretization equation, specifically:
recording the earth coordinate system as an e system; the navigation coordinate system is an n system; AUV coordinate system b system, supposing AUV at TmSailing to T at any momentnAt the moment, the navigation point navigates from the M point to the N point and navigates forwardsThe program is composed of TmResolving to TnI.e. resolving from point M to point N; the starting time of the reverse calculation process is the end point of the forward calculation, and then the reverse calculation is carried out to the initial point of the forward calculation process, namely from TnBackward recursion of time to TmThe equation for updating the attitude, the speed and the position of the reverse strapdown inertial navigation at the moment is as follows
Figure BDA0002851808540000086
Figure BDA0002851808540000087
Figure BDA0002851808540000088
Figure BDA0002851808540000089
Figure BDA00028518085400000810
Let p-k-1, p-1-k, and replace and simplify the above formula variables to get
Figure BDA00028518085400000811
Figure BDA00028518085400000812
Figure BDA00028518085400000813
Figure BDA00028518085400000814
Figure BDA00028518085400000815
Wherein the content of the first and second substances,
Figure BDA00028518085400000816
Figure BDA0002851808540000091
are respectively as
Figure BDA0002851808540000092
Figure BDA0002851808540000093
Lk-1、Lk、hk-1、hk、λk-1、λk、
Figure BDA0002851808540000094
And (b) represents the same;
and recording and reversely processing navigation information from M points to N points acquired by the AUV, so that reverse calculation from N points to M points is realized.
Compared with the prior art, the invention has the beneficial effects that:
the invention provides a pilot AUV navigation data post-processing method based on reverse solution. Aiming at the problem that after a multi-AUV collaborative navigation system sails underwater for a long time, a piloting AUV provided with high-precision navigation equipment cannot receive external information, and the positioning error of the multi-AUV collaborative navigation system is gradually accumulated along with time, so that the positioning error of the multi-AUV system is dispersed integrally along with the divergence of the positioning error of the piloting AUV. And finally, forward and reverse combined calculation is carried out on the navigation data in 3600s time through simulation, and finally the feasibility and the effectiveness of the method are verified. The method can be effectively applied to the fields of mapping of sea maps, submarine topography analysis, underwater light/acoustic data processing and the like, and is used for improving the accuracy and the usefulness of underwater task data acquired by AUV formation.
Drawings
FIG. 1 is a schematic diagram of a velocity measurement principle of a Doppler velocity meter;
FIG. 2 is a block diagram of the SINS/DVL/GPS integrated navigation system filtering principle
FIG. 3 is a schematic diagram of forward and reverse combined navigation data post-processing
FIG. 4 pilot AUV navigation trajectory route;
FIG. 5 shows a positioning error curve after forward calculation of a piloted AUV;
FIG. 6 shows a positioning error curve after reverse calculation of a piloted AUV;
FIG. 7 illustrates a positioning error curve after forward and reverse combination of piloting AUV;
Detailed Description
The features of the invention will now be further described with reference to the examples, the accompanying drawings and the attached tables:
in a master-slave type multi-Autonomous Underwater Vehicle (AUV), a piloting AUV is equipped with a high-precision navigation device, and therefore, the positioning information of the piloting AUV is generally used as a reference, and the positioning error is not considered. In practical application, after the piloted AUV navigates underwater for a long time, the positioning error still diverges, and the positioning error of the piloted AUV is in direct proportion to the course. Therefore, when the AUV formation is underway underwater for a long time, the piloting AUV still accumulates larger positioning errors due to no correction of external information, and the positioning performance of the whole sailing formation is further influenced.
In order to ensure the underwater navigation accuracy of the multi-AUV collaborative navigation system, GPS positioning information is received by regular water outlet of a piloting AUV to correct the current positioning, but the method can only correct the position information after the water outlet time of the piloting AUV, but cannot correct the positioning error accumulated in the underwater navigation stage, and can bring the problems of discontinuity and concealment of underwater operation.
Aiming at the problem, the invention provides a pilot AUV navigation data post-processing method based on the combination of SINS/DVL and SINS/DVL/GPS integrated navigation. The method can be used in the fields of submarine topography scanning, chart drawing, underwater detection and the like, can further improve the position precision of underwater task data acquired in AUV formation, and improves the accuracy and the usefulness of the data.
The invention provides a pilot AUV navigation data post-processing method based on reverse resolving, which comprises the following steps:
step 1: establishing a mathematical model of SINS and DVL, comprising:
(1) establishing an SINS error state equation;
specifically, an SINS error state equation is established, specifically:
and selecting a geographic coordinate system as a reference system, and selecting position error, speed error, attitude error, gyroscope drift and accelerometer zero offset as state quantities.
Figure BDA0002851808540000111
Wherein, XSINSRepresents the state vector of the SINS,
Figure BDA0002851808540000112
north, sky and east misalignment angles, respectively; delta VN、δVU、δVENorth, sky and east speed errors, respectively; δ L, δ λ are latitude and longitude errors, respectively; epsilonbx、εby、εbzGyroscope drift in x, y, z directions, respectively; deltabx、Δby、ΔbzAccelerometer bias in x, y, z directions for the accelerometer, respectively;
to obtain the SINS error state equation of
Figure BDA0002851808540000113
In the above formula, FSINSRepresenting system state transition matrix, WSINSIs system noise and is zero mean white gaussian noise. FSINSAs shown below.
Figure BDA0002851808540000114
In the above formula, RM,RNThe curvature radiuses of the earth meridian and the prime unit circle at the position of the carrier are respectively, and the approximate calculation formula is as follows: rM≈Re(1-2e+3esin2L) and RN≈Re(1+esin2L)；ReThe semiaxis is the long semiaxis of the earth reference ellipsoid, and e is the ellipticity of the reference ellipsoid; vN、VU、VEThe speed of the carrier in the north-east direction, L and h respectively represent the longitude and the depth of the carrier; omegaieIs the earth rotation rate; f. ofN、fU、fERespectively the specific force of the carrier in the north-east direction;
Figure BDA0002851808540000115
the state transition matrix is from AUV coordinate system b to geographic coordinate system n.
Figure BDA0002851808540000116
Wherein phi, psi and theta respectively represent the roll angle, the course angle and the yaw angle of the AUV.
(2) Establishing an error model of the DVL;
the establishing of the error model of the DVL specifically comprises the following steps:
as shown in fig. 1, assuming that the AUV forward speed is v,the transducer is moved relative to the seafloor along the beam axis OA at a velocity vcos alpha, given the transmit frequency f of the DVL0The frequency of the sound wave received by the stationary reflecting point is f1The propagation velocity of sound wave in water is v0Then there is
Figure BDA0002851808540000121
α is the transducer tilt angle.
At the same time, the frequency f is reflected from the sea bottom reflection point1The sound wave frequency received by a DVL velocimeter receiving transducer on the AUV is f2Then, then
Figure BDA0002851808540000122
Figure BDA0002851808540000123
The above formula is simplified into
Figure BDA0002851808540000124
Obtaining a DVL frequency shift of
Figure BDA0002851808540000125
Obtained by equation conversion
Figure BDA0002851808540000126
In AUV coordinate system, the vector equation form of DVL velocity measurement error equation is expressed as delta Vb＝ΔC·Vb+u；
Wherein, δ VbRepresenting the speed measurement error of the DVL under the carrier system; u is measurement noise; Δ C is the scale factor error.
When combined navigation is performed by DVL and SINS, the DVL coordinate system and the AUV coordinate system do not coincide, that is, there is spatial inconsistency in the navigation information, so it is necessary to estimate the transformation matrix between the two coordinate systems. Therefore, by using the SINS/DVL integrated navigation method, the velocity relationship between the DVL coordinate system and the AUV coordinate system can be expressed as
Figure BDA0002851808540000127
Wherein the content of the first and second substances,
Figure BDA0002851808540000128
a transformation matrix from a DVL coordinate system d system to an AUV coordinate system b system; vbRepresents the velocity in the AUV coordinate system; vdRepresenting the velocity in the DVL coordinate system.
(3) Establishing a measurement equation of SINS/DVL;
the establishing of the SINS/DVL measurement equation specifically comprises the following steps:
the equation of velocity measured by SINS in the northeast direction is
VE＝VE0+δVE
VN＝VN0+δVN
VU＝VU0+δVU
Wherein, VN、VU、VERespectively measuring the actual speed of the AUV in the north-east direction by the SINS; vN0、VU0、VE0Respectively measuring the ideal speed of the AUV in the north east direction by the SINS; delta VN、δVU、δVERespectively, the velocity error of the SINS in the north-east direction.
Velocity measurement of DVL under geographic coordinate system n
Figure BDA0002851808540000131
Is shown as
Figure BDA0002851808540000132
Figure BDA0002851808540000133
Figure BDA0002851808540000134
Wherein the content of the first and second substances,
Figure BDA0002851808540000135
respectively representing the velocity of the DVL measured in the north-east direction under a geographic coordinate system n;
Figure BDA0002851808540000136
respectively representing the projections of the measurement errors in the east direction, the north direction and the sky direction,
Figure BDA0002851808540000137
Figure BDA0002851808540000138
respectively, represent the projections of the measured values random interference errors in the east, north and sky directions.
The calculated velocity of the SINS is subtracted from the projection of the measured velocity of the DVL in the geographic coordinate system to obtain
Figure BDA0002851808540000139
Therefore, the measurement equation Z (t) is
Figure BDA00028518085400001310
Wherein u isnFor generalized noise, H (t) is an observation matrix, which can be split into two parts
Figure BDA00028518085400001311
The two parts in the above formula are
Figure BDA00028518085400001312
Wherein the content of the first and second substances,
Figure BDA00028518085400001313
an observation matrix representing the SINS is shown,
Figure BDA00028518085400001314
an observation matrix representing the DVL;
Figure BDA00028518085400001315
Figure BDA0002851808540000141
is a state transition matrix
Figure BDA0002851808540000142
9 variable values.
Step 2: establishing an SINS/DVL/GPS measurement equation according to the mathematical model of the SINS and the DVL established in the step 1;
the method specifically comprises the following steps:
as shown in fig. 2, in the SINS/DVL/GPS integrated navigation system, since the state quantity of the system is not changed and the state equation is the same as the SINS/DVL integrated navigation state equation, the position information of the carrier calculated by the SINS and the position information acquired by the GPS are only required to be subtracted and added into the measurement equation of the SINS/DVL, and the state variable is subjected to feedback correction through kalman filtering to obtain the optimal estimation of the navigation parameter. The measurement equation of the SINS/DVL/GPS integrated navigation system is as follows:
Figure BDA0002851808540000143
wherein the observed quantity Z is a direct observed value of the state quantity; the observation matrix H is a constant matrix H ═ I5×5 0](ii) a X is a state variable of SINS; v is measurement noise; l isSINS、λSINSRepresenting latitude information and latitude information obtained by SINS resolving; l isGPS、λGPSIndicating latitude information and longitude information measured by the GPS.
And step 3: establishing a posture updating differential equation of the strapdown inertial navigation system;
the method specifically comprises the following steps:
Figure BDA0002851808540000144
each variable is represented as follows
Figure BDA0002851808540000145
Figure BDA0002851808540000146
Figure BDA0002851808540000147
Wherein the content of the first and second substances,
Figure BDA0002851808540000148
for state transition matrix
Figure BDA0002851808540000149
In the form of a differential of (a),
Figure BDA00028518085400001410
is the measured angular rate of the AUV;
Figure BDA00028518085400001411
AUV angular velocity measured for a gyroscope;
Figure BDA00028518085400001412
is the earth rotation rate;
Figure BDA00028518085400001413
is the position velocity;
Figure BDA00028518085400001414
representing the transition matrix from the navigation coordinate system n to the AUV coordinate system b.
And 4, step 4: establishing a speed updating differential equation of the strapdown inertial navigation system;
the method specifically comprises the following steps:
Figure BDA0002851808540000151
Figure BDA0002851808540000152
gn＝[0 -g 0]T
wherein f isnLinear acceleration under a navigation coordinate system; f. ofbLinear acceleration measured by an accelerometer under an AUV coordinate system; gnAcceleration in a spatial direction; g is the acceleration of gravity.
And 5: establishing a position updating differential equation of the strapdown inertial navigation system;
the method specifically comprises the following steps:
Figure BDA0002851808540000153
wherein the content of the first and second substances,
Figure BDA0002851808540000154
in differential form, L, λ, h, respectively.
Step 6: discretizing the attitude, speed and position updating differential equation to obtain a discretization equation;
the method specifically comprises the following steps:
Figure BDA0002851808540000155
Figure BDA0002851808540000156
Figure BDA0002851808540000157
Figure BDA0002851808540000158
Figure BDA0002851808540000159
wherein
Figure BDA00028518085400001510
Figure BDA00028518085400001511
Figure BDA00028518085400001512
Wherein the content of the first and second substances,
Figure BDA00028518085400001513
respectively representing state transition matrixes at k and k-1;
Figure BDA00028518085400001514
respectively representing the speed of the AUV at the k and k-1 moments; l isk、Lk-1Respectively showing the latitude of the AUV at the k and k-1 times; lambda [ alpha ]k、λk-1Respectively indicates the longitude of AUV at the k and k-1 time; h isk、hk-1Respectively showing the navigation depth of the AUV at the k and k-1 moments; t issIs a discrete period;
Figure BDA0002851808540000161
acceleration of the AUV measured by the accelerometer at the k moment;
Figure BDA0002851808540000162
angular rate of AUV at time k;
Figure BDA0002851808540000163
the angular velocity of the AUV measured by the gyroscope at the kth moment;
Figure BDA0002851808540000164
the earth rotation rate at the k and k-1 moments;
Figure BDA0002851808540000165
the position rate of the kth and k-1 time;
Figure BDA0002851808540000166
respectively represents the velocity components of the AUV measured in three directions in the northeast on the k and k-1 moments.
And 7: and establishing a reverse navigation equation according to the discretization equation, and performing reverse solution on the piloting AUV navigation data to complete post-processing of the piloting AUV navigation data.
The method specifically comprises the following steps:
recording the earth coordinate system as an e system; the navigation coordinate system is an n system; AUV coordinate system b system, supposing AUV at TmSailing to T at any momentnAt the moment, the navigation point navigates from the M point to the N point, and the forward navigation process is from TmResolving to TnI.e. resolving from point M to point N; the starting time of the reverse calculation process is the end point of the forward calculation, and then the reverse calculation is carried out to the initial point of the forward calculation process, namely from TnBackward recursion of time to TmThe equation for updating the attitude, the speed and the position of the reverse strapdown inertial navigation at the moment is as follows
Figure BDA0002851808540000167
Figure BDA0002851808540000168
Figure BDA0002851808540000169
Figure BDA00028518085400001610
Figure BDA00028518085400001611
(Note: the above formula is a conversion of the formula in claim 9, defined)
Let p-k-1, p-1-k, and replace and simplify the above formula variables to get
Figure BDA00028518085400001612
Figure BDA00028518085400001613
Figure BDA0002851808540000171
Figure BDA0002851808540000172
Figure BDA0002851808540000173
Wherein the content of the first and second substances,
Figure BDA0002851808540000174
Figure BDA0002851808540000175
are respectively as
Figure BDA0002851808540000176
Figure BDA0002851808540000177
Lk-1、Lk、hk-1、hk、λk-1、λk、
Figure BDA0002851808540000178
And (3) represents the same meaning.
Through the derivation of the formula, the navigation information from the M point to the N point acquired by the AUV is recorded and reversely processed, so that the reverse calculation from the N point to the M point is realized, as shown in FIG. 3. During forward and reverse solution, the position coordinates, attitude matrix and speed of the vehicle are the same at the same time, while the speed is in the opposite direction. The embodiment of the invention comprises the following steps:
(1) and initializing navigation parameters.
1) Initializing state variables of the SINS and DVL integrated navigation system, and setting an initial state value x0 as [ phi; dvn, respectively; dpos; ed; db ], the initial values of the state variables in x0 are shown below.
phi＝[10；60；-10]*(pi/180/60)
dpos＝[10/6378160；10/6378160]
dvn＝[0.5；0.5；0.5]
eb＝[0.02；0.02；0.02]*(pi/180/3600)
db＝[100；100；100]*(0.000001*9.78)
Wherein the white noise measured by the gyroscope and the white noise measured by the accelerometer are
web＝[0.02；0.02；0.02]*(pi/180/3600)
wdb＝[50；50；50]*(0.000001*9.78)
The white noise mean square error w in the system equation can be obtained as web; wdb, respectively; webq; wdbq ], and the white noise mean square error between SINS and DVL in the observation equation is v ═ 0.01; 0.01; 0.01], wherein
webq＝[web(1,1)^2；web(2,1)^2；web(3,1)^2]
wdbq＝[wdb(1,1)^2；wdb(2,1)^2；wdb(3,1)^2]
2) And initializing the attitude, speed and position information of the system. The initial attitude angle, initial velocity, and initial position of the carrier are set as follows.
att＝[0；0；0]*(pi/180)
vb＝[2；0；0]
pos＝[24*(pi/180)+35*(pi/180/60)；120*(pi/180)+58*(pi/180/60)；-10]
(2) And establishing a Kalman filtering equation of the discrete system.
Figure BDA0002851808540000181
Figure BDA0002851808540000182
Figure BDA0002851808540000183
Pk\k-1＝Φk,k-1Pk-1ΦT k,k-1+Γk-1Qk-1ΓT k-1
Pk＝(I-KkHk)Pk|k-1(I-KkHk)T+KkRkKT k
In the above formula, the first and second carbon atoms are,
Figure BDA0002851808540000184
is a state estimation;
Figure BDA0002851808540000185
predicting for one step; kkIs a filter gain array; zkIs a measured value; hkIs a measuring array; phik,k-1A one-step transfer matrix from k-1 to k;
Figure BDA0002851808540000186
estimating the state of the previous moment; pk\k-1To estimate the mean square error; pk-1To estimate the mean square error; gamma-shapedk-1A noise driving array; rkTo measure the noise variance matrix.
(3) Initial conditions for SINS/DVL combined navigation system filtering are created.
X0＝0
P0＝diag{(0.1m/s)2,(0.1m/s)2,(0.5m/s)2,(0.3')2,(0.3')2,(0.1°)2,(0.1°)2,(0.2°)2,
(0.1°/h)2,(0.1°/h)2,(0.2°/h)2,(0.001)2,(0.001)2,(0.001)2}
Q＝diag{(0.01m/s)2,(0.01m/s)2,(0.01m/s)2,0,0,(0.01°/h)2,(0.01°/h)2,(0.01°/h)2,
0,0,0,0,0,0,0,0}
R＝diag{(0.01m/s)2,(0.01m/s)2,(0.01m/s)2}
(4) And creating initial conditions for filtering of the SINS/DVL/GPS combined navigation system.
X0＝0
P0＝diag{(0.1m/s)2,(0.1m/s)2,(0.5m/s)2,(0.8')2,(0.8')2,(0.1°)2,(0.1°)2,(0.2°)2,
(0.1°/h)2,(0.1°/h)2,(0.2°/h)2,(0.001)2,(0.001)2,(0.001)2}
Q＝diag{(0.01m/s)2,(0.01m/s)2,(0.01m/s)2,0,0,(0.01°/h)2,(0.01°/h)2,(0.01°/h)2,
0,0,0,0,0,0,0,0}
R＝diag{(0.01m/s)2,(0.01m/s)2,(0.01m/s)2,(0.8')2,(0.8')2}
(5) Setting and explaining initial conditions of navigation AUV track planning.
The initial position of the piloting AUV is [24 ° 35'; 102 ° 58'; -30] sailing speed of 2 m/s. At the beginning of navigation of the AUV, the piloting AUV navigates linearly at a yaw angle of 0 degrees and enters a floating state after a period of time, after the system goes out of water, the piloting AUV receives GPS information and continues to navigate for a period of time to the end, and the total navigation time of the system from the beginning to the end of navigation is 3600 s. The trajectory parameter settings for the pilot AUV are shown in table 1.
TABLE 1 piloting AUV navigation trajectory parameter settings
Figure BDA0002851808540000191
Fig. 4 is a track route obtained by simulation operation according to the navigation parameters set in the step (5) of the embodiment. The co-location error of the pilot AUV obtained by forward solution and the co-location error of the pilot AUV obtained by reverse solution are analyzed, as shown in fig. 5 and 6. And finally, combining the two calculation modes to obtain the navigation AUV positioning error after forward and reverse combination calculation, as shown in FIG. 7. It can be clearly seen from comparison that, in the curve shown in fig. 7, the positioning error is obviously converged by adopting a forward and reverse combined solution mode. The positioning error obtained by adopting the traditional forward calculation method is continuously accumulated along with time and finally diverged. Therefore, the feasibility and the accuracy of the method are proved by simulation. The method can be widely applied to the fields of mapping of sea maps, submarine topography analysis, underwater optical/acoustic data processing and the like, and is used for improving the accuracy and the usefulness of underwater task data.
Those skilled in the art will appreciate that the details of the invention not described in detail in this specification are well within the skill of those in the art.

Claims (10)

1. A pilot AUV navigation data post-processing method based on reverse resolving is characterized by comprising the following steps:
step 1: establishing a mathematical model of SINS and DVL, comprising:
(1) establishing an SINS error state equation;
(2) establishing an error model of the DVL;
(3) establishing a measurement equation of SINS/DVL;
step 2: establishing an SINS/DVL/GPS measurement equation according to the mathematical model of the SINS and the DVL established in the step 1;
and step 3: establishing a posture updating differential equation of the strapdown inertial navigation system;
and 4, step 4: establishing a speed updating differential equation of the strapdown inertial navigation system;
and 5: establishing a position updating differential equation of the strapdown inertial navigation system;
step 6: discretizing the attitude, speed and position updating differential equation to obtain a discretization equation;
and 7: and establishing a reverse navigation equation according to the discretization equation, and performing reverse solution on the piloting AUV navigation data to complete post-processing of the piloting AUV navigation data.
2. The reverse-solution-based piloting AUV navigation data post-processing method according to claim 1, characterized in that: the establishing of the SINS error state equation specifically comprises the following steps:
and selecting a geographic coordinate system as a reference system, and selecting position error, speed error, attitude error, gyroscope drift and accelerometer zero offset as state quantities.
Figure FDA0002851808530000011
Wherein, XSINSRepresents the state vector of the SINS,
Figure FDA0002851808530000012
north, sky and east misalignment angles, respectively; delta VN、δVU、δVENorth, sky and east speed errors, respectively; δ L, δ λ are latitude and longitude errors, respectively; epsilonbx、εby、εbzGyroscope drift in x, y, z directions, respectively; deltabx、Δby、ΔbzAccelerometer bias in x, y, z directions for the accelerometer, respectively;
to obtain the SINS error state equation of
Figure FDA0002851808530000021
In the above formula, FSINSRepresenting system state transition matrix, WSINSIs system noise and is zero mean white gaussian noise. FSINSAs shown below.
Figure FDA0002851808530000022
In the above formula, RM,RNThe curvature radiuses of the earth meridian and the prime unit circle at the position of the carrier are respectively, and the approximate calculation formula is as follows: rM≈Re(1-2e+3esin2L) and RN≈Re(1+esin2L)；ReThe semiaxis is the long semiaxis of the earth reference ellipsoid, and e is the ellipticity of the reference ellipsoid; vN、VU、VEThe velocity of the AUV in the north-east direction, L and h respectively represent the longitude and depth of the carrier; omegaieIs the earth rotation rate; f. ofN、fU、fERespectively the specific force of the carrier in the north-east direction;
Figure FDA0002851808530000023
the state transition matrix is from an AUV coordinate system b to a geographic coordinate system n;
Figure FDA0002851808530000024
wherein phi, psi and theta respectively represent the roll angle, the course angle and the yaw angle of the AUV.
3. The reverse-solution-based piloting AUV navigation data post-processing method according to claim 2, characterized in that: the establishing of the error model of the DVL specifically comprises the following steps:
assuming a forward velocity v of the AUV, the transducer is moved relative to the seafloor along the beam axis OA at a velocity v cos α, given by DVLTransmitting at a frequency f0The frequency of the sound wave received by the stationary reflecting point is f1The propagation velocity of sound wave in water is v0Then there is
Figure FDA0002851808530000025
Alpha is the inclination angle of the transducer;
at the same time, the frequency f is reflected from the sea bottom reflection point1The sound wave frequency received by a DVL velocimeter receiving transducer on the AUV is f2Then, then
Figure FDA0002851808530000026
Figure FDA0002851808530000031
The above formula is simplified into
Figure FDA0002851808530000032
Obtaining a DVL frequency shift of
Figure FDA0002851808530000033
Obtained by equation conversion
Figure FDA0002851808530000034
Under an AUV coordinate system, the vector equation form of the DVL velocity measurement error equation is expressed as
δVb＝ΔC·Vb+u；
Wherein, δ VbRepresenting the speed measurement error of the DVL under the carrier system; u is measurement noise; Δ C is the scale coefficient error;
by adopting an SINS/DVL combined navigation mode, the speed relation between a DVL coordinate system and an AUV coordinate system is expressed as
Figure FDA0002851808530000035
Wherein the content of the first and second substances,
Figure FDA0002851808530000036
a transformation matrix from a DVL coordinate system d system to an AUV coordinate system b system; vbRepresents the velocity in the AUV coordinate system; vdRepresenting the velocity in the DVL coordinate system.
4. The reverse-solution-based piloted AUV navigation data post-processing method according to claim 3, characterized in that: the establishing of the SINS/DVL measurement equation specifically comprises the following steps:
the equation of velocity measured by SINS in the northeast direction is
VE＝VE0+δVE
VN＝VN0+δVN
VU＝VU0+δVU
Wherein, VN、VU、VEThe velocity of the AUV in the north-east direction; vN0、VU0、VE0Respectively measuring the ideal speed of the AUV in the north east direction by the SINS; delta VN、δVU、δVERespectively the velocity error of the SINS in the north-east direction;
velocity measurement of DVL under geographic coordinate system n
Figure FDA0002851808530000037
Is shown as
Figure FDA0002851808530000038
Figure FDA0002851808530000039
Figure FDA00028518085300000310
Wherein the content of the first and second substances,
Figure FDA0002851808530000041
respectively representing the velocity of the DVL measured in the north-east direction under a geographic coordinate system n;
Figure FDA0002851808530000042
respectively representing the projections of the measurement errors in the east direction, the north direction and the sky direction,
Figure FDA0002851808530000043
Figure FDA0002851808530000044
respectively representing the projections of random interference errors of the measured values in the east direction, the north direction and the sky direction;
the calculated velocity of the SINS is subtracted from the projection of the measured velocity of the DVL in the geographic coordinate system to obtain
Figure FDA0002851808530000045
Therefore, the measurement equation Z (t) is
Figure FDA0002851808530000046
Wherein u isnFor generalized noise, H (t) is an observation matrix, which can be split into two parts
Figure FDA0002851808530000047
The two parts in the above formula are
Figure FDA0002851808530000048
Wherein the content of the first and second substances,
Figure FDA0002851808530000049
an observation matrix representing the SINS is shown,
Figure FDA00028518085300000410
an observation matrix representing the DVL;
Figure FDA00028518085300000411
Figure FDA00028518085300000412
is a state transition matrix
Figure FDA00028518085300000413
9 variable values.
5. The reverse-solution-based piloted AUV navigation data post-processing method according to claim 4, characterized in that: step 2, establishing an SINS/DVL/GPS measurement equation according to the established mathematical model of SINS and DVL, specifically:
in the SINS/DVL/GPS integrated navigation system, the state quantity of the system is unchanged, the state equation is the same as the SINS/DVL integrated navigation state equation, the position information of the carrier calculated by the SINS is differed with the position information acquired by the GPS, and the difference is added into the measurement equation of the SINS/DVL, so that the measurement equation of the SINS/DVL/GPS integrated navigation system is obtained as follows:
Figure FDA0002851808530000051
wherein the observed quantity Z is a direct observed value of the state quantity; the observation matrix H is a constant matrix H ═ I5×5 0](ii) a X is a state variable of SINS; v is measurement noise; l isSINS、λSINSRepresenting latitude information and latitude information obtained by SINS resolving; l isGPS、λGPSIndicating latitude information and longitude information measured by the GPS.
6. The reverse-solution-based piloted AUV navigation data post-processing method according to claim 5, characterized in that: the step 3 of establishing a posture updating differential equation of the strapdown inertial navigation system specifically comprises the following steps:
Figure FDA0002851808530000052
each variable is represented as follows
Figure FDA0002851808530000053
Figure FDA0002851808530000054
Figure FDA0002851808530000055
Wherein the content of the first and second substances,
Figure FDA0002851808530000056
for state transition matrix
Figure FDA0002851808530000057
In the form of a differential of (a),
Figure FDA0002851808530000058
is the measured angular rate of the AUV;
Figure FDA0002851808530000059
AUV angular velocity measured for a gyroscope;
Figure FDA00028518085300000510
is the earth rotation rate;
Figure FDA00028518085300000511
is the position velocity;
Figure FDA00028518085300000512
representing the transition matrix from the navigation coordinate system n to the AUV coordinate system b.
7. The reverse-solution-based piloted AUV navigation data post-processing method according to claim 6, characterized in that: step 4, establishing a velocity update differential equation of the strapdown inertial navigation system, specifically:
Figure FDA00028518085300000513
Figure FDA00028518085300000514
gn＝[0 -g 0]T
wherein f isnLinear acceleration under a navigation coordinate system; f. ofbLinear acceleration measured by an accelerometer under an AUV coordinate system; gnAcceleration in a spatial direction; g is the acceleration of gravity.
8. The reverse-solution-based piloted AUV navigation data post-processing method according to claim 7, characterized in that: the step 5 of establishing a position updating differential equation of the strapdown inertial navigation system specifically comprises the following steps:
Figure FDA0002851808530000061
wherein the content of the first and second substances,
Figure FDA0002851808530000062
in differential form, L, λ, h, respectively.
9. The reverse-solution-based piloting AUV navigation data post-processing method according to claim 8, characterized in that: step 6, discretizing the attitude, speed and position update differential equation to obtain a discretization equation, specifically:
Figure FDA0002851808530000063
Figure FDA0002851808530000064
Figure FDA0002851808530000065
Figure FDA0002851808530000066
Figure FDA0002851808530000067
wherein
Figure FDA0002851808530000068
Figure FDA0002851808530000069
Figure FDA00028518085300000610
Wherein the content of the first and second substances,
Figure FDA00028518085300000611
respectively representing state transition matrixes at k and k-1;
Figure FDA00028518085300000612
respectively representing the speed of the AUV at the k and k-1 moments; l isk、Lk-1Respectively showing the latitude of the AUV at the k and k-1 times; lambda [ alpha ]k、λk-1Respectively indicates the longitude of AUV at the k and k-1 time; h isk、hk-1Respectively showing the navigation depth of the AUV at the k and k-1 moments; t issIs a discrete period;
Figure FDA00028518085300000613
acceleration of the AUV measured by the accelerometer at the k moment;
Figure FDA00028518085300000614
Figure FDA00028518085300000615
angular rate of AUV at time k;
Figure FDA00028518085300000616
the angular velocity of the AUV measured by the gyroscope at the kth moment;
Figure FDA00028518085300000617
the earth rotation rate at the k and k-1 moments;
Figure FDA00028518085300000618
the position rate of the kth and k-1 time;
Figure FDA00028518085300000619
respectively represents the velocity components of the AUV measured in three directions in the northeast on the k and k-1 moments.
10. The reverse-solution-based piloting AUV navigation data post-processing method according to claim 9, characterized in that: and 7, establishing a reverse navigation equation according to the discretization equation, specifically:
recording the earth coordinate system as an e system; the navigation coordinate system is an n system; AUV coordinate system b system, supposing AUV at TmSailing to T at any momentnAt the moment, the navigation point navigates from the M point to the N point, and the forward navigation process is from TmResolving to TnI.e. resolving from point M to point N; the starting time of the reverse calculation process is the end point of the forward calculation, and then the reverse calculation is carried out to the initial point of the forward calculation process, namely from TnBackward recursion of time to TmThe equation for updating the attitude, the speed and the position of the reverse strapdown inertial navigation at the moment is as follows
Figure FDA0002851808530000071
Figure FDA0002851808530000072
Figure FDA0002851808530000073
Figure FDA0002851808530000074
Figure FDA0002851808530000075
Let p-k-1, p-1-k, and replace and simplify the above formula variables to get
Figure FDA0002851808530000076
Figure FDA0002851808530000077
Figure FDA0002851808530000078
Figure FDA0002851808530000079
Figure FDA00028518085300000710
Wherein the content of the first and second substances,
Figure FDA00028518085300000711
Figure FDA00028518085300000712
are respectively as
Figure FDA00028518085300000713
Figure FDA00028518085300000714
Lk-1、Lk、hk-1、hk、λk-1、λk、
Figure FDA00028518085300000715
And (b) represents the same;
and recording and reversely processing navigation information from M points to N points acquired by the AUV, so that reverse calculation from N points to M points is realized.
CN202011529538.3A 2020-12-22 2020-12-22 Pilot AUV navigation data post-processing method based on reverse solution Pending CN112747748A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011529538.3A CN112747748A (en) 2020-12-22 2020-12-22 Pilot AUV navigation data post-processing method based on reverse solution

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011529538.3A CN112747748A (en) 2020-12-22 2020-12-22 Pilot AUV navigation data post-processing method based on reverse solution

Publications (1)

Publication Number Publication Date
CN112747748A true CN112747748A (en) 2021-05-04

Family

ID=75648217

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011529538.3A Pending CN112747748A (en) 2020-12-22 2020-12-22 Pilot AUV navigation data post-processing method based on reverse solution

Country Status (1)

Country Link
CN (1) CN112747748A (en)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114440925A (en) * 2021-12-29 2022-05-06 宜昌测试技术研究所 DVL calibration method for neglecting horizontal attitude of AUV integrated navigation system
CN114894222A (en) * 2022-07-12 2022-08-12 深圳元戎启行科技有限公司 External parameter calibration method of IMU-GNSS antenna and related method and equipment
WO2024074473A1 (en) * 2022-10-03 2024-04-11 Safran Electronics & Defense Collaborative navigation method for vehicles having navigation solutions of different accuracies

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20090102714A1 (en) * 2005-05-26 2009-04-23 Nicholas Charles Talbot GNSS post positioning with selected precision
CN107063245A (en) * 2017-04-19 2017-08-18 东南大学 A kind of SINS/DVL integrated navigation filtering methods based on 5 rank SSRCKF
CN109443379A (en) * 2018-09-28 2019-03-08 东南大学 A kind of underwater anti-shake dynamic alignment methods of the SINS/DVL of deep-sea submariner device
CN110031882A (en) * 2018-08-02 2019-07-19 哈尔滨工程大学 A kind of outer measurement information compensation method based on SINS/DVL integrated navigation system

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20090102714A1 (en) * 2005-05-26 2009-04-23 Nicholas Charles Talbot GNSS post positioning with selected precision
CN107063245A (en) * 2017-04-19 2017-08-18 东南大学 A kind of SINS/DVL integrated navigation filtering methods based on 5 rank SSRCKF
CN110031882A (en) * 2018-08-02 2019-07-19 哈尔滨工程大学 A kind of outer measurement information compensation method based on SINS/DVL integrated navigation system
CN109443379A (en) * 2018-09-28 2019-03-08 东南大学 A kind of underwater anti-shake dynamic alignment methods of the SINS/DVL of deep-sea submariner device

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
董权威等: "基于逆向解算的领航AUV导航数据后处理方法", 《水下无人系统学报》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114440925A (en) * 2021-12-29 2022-05-06 宜昌测试技术研究所 DVL calibration method for neglecting horizontal attitude of AUV integrated navigation system
CN114894222A (en) * 2022-07-12 2022-08-12 深圳元戎启行科技有限公司 External parameter calibration method of IMU-GNSS antenna and related method and equipment
CN114894222B (en) * 2022-07-12 2022-10-28 深圳元戎启行科技有限公司 External parameter calibration method of IMU-GNSS antenna and related method and equipment
WO2024074473A1 (en) * 2022-10-03 2024-04-11 Safran Electronics & Defense Collaborative navigation method for vehicles having navigation solutions of different accuracies

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
Lee et al. Simulation of an inertial acoustic navigation system with range aiding for an autonomous underwater vehicle
WO2022028286A1 (en) Motion constraint-aided underwater integrated navigation method employing improved sage-husa adaptive filtering
Grenon et al. Enhancement of the inertial navigation system for the morpheus autonomous underwater vehicles
Hegrenas et al. Model-aided inertial navigation for underwater vehicles
CN112747748A (en) Pilot AUV navigation data post-processing method based on reverse solution
CN103697910B (en) The correction method of autonomous underwater aircraft Doppler log installation error
CN111595348B (en) Master-slave mode cooperative positioning method of autonomous underwater vehicle combined navigation system
CN104316045A (en) AUV (autonomous underwater vehicle) interactive auxiliary positioning system and AUV interactive auxiliary positioning method based on SINS (strapdown inertial navigation system)/LBL (long base line)
CN111829512B (en) AUV navigation positioning method and system based on multi-sensor data fusion
CN102829777A (en) Integrated navigation system for autonomous underwater robot and method
CN111947651B (en) Underwater combined navigation information fusion method and system and autonomous underwater vehicle
LaPointe Virtual long baseline (VLBL) autonomous underwater vehicle navigation using a single transponder
CN111596333B (en) Underwater positioning navigation method and system
CN101393025A (en) AUV combined navigation system non-tracing switch method
CN107797125B (en) A method of reducing deep-sea detecting type AUV navigation positioning errors
CN108303079B (en) Data smoothing method for underwater USBL reverse application
CN112284384A (en) Cooperative positioning method of clustered multi-deep-sea submersible vehicle considering measurement abnormity
CN111982105B (en) Underwater navigation positioning method and system based on SINS/LBL tight combination
CN112556697A (en) Shallow coupling data fusion navigation method based on federated structure
CN112015086B (en) Feedback control method for limited-time path tracking output of under-actuated surface ship
CN110133695A (en) A kind of double antenna GNSS location delay time dynamic estimation system and method
CN104061930A (en) Navigation method based on strapdown inertial guidance and Doppler log
CN111735455A (en) Improved Gaussian distance iterative algorithm based butt joint recovery integrated navigation method

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
RJ01 Rejection of invention patent application after publication

Application publication date: 20210504