CN111504324A - Underwater integrated navigation method of noise adaptive filtering - Google Patents

Underwater integrated navigation method of noise adaptive filtering Download PDF

Info

Publication number
CN111504324A
CN111504324A CN202010345972.XA CN202010345972A CN111504324A CN 111504324 A CN111504324 A CN 111504324A CN 202010345972 A CN202010345972 A CN 202010345972A CN 111504324 A CN111504324 A CN 111504324A
Authority
CN
China
Prior art keywords
auv
sins
navigation
error
speed
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.)
Granted
Application number
CN202010345972.XA
Other languages
Chinese (zh)
Other versions
CN111504324B (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.)
Northwestern Polytechnical University
Original Assignee
Northwestern Polytechnical 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 Northwestern Polytechnical University filed Critical Northwestern Polytechnical University
Priority to CN202010345972.XA priority Critical patent/CN111504324B/en
Publication of CN111504324A publication Critical patent/CN111504324A/en
Application granted granted Critical
Publication of CN111504324B publication Critical patent/CN111504324B/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/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
    • 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

Landscapes

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

Abstract

The invention relates to an underwater integrated navigation method of noise adaptive filtering, which adopts a depth sensor as navigation depth in an AUV integrated navigation system; simplifying SINS related error equation in filtering, and based on residual χ2The method has the advantages that the rotation speed of the AUV brushless direct current propulsion motor is used as a tachometer, the AUV hardware cost is slightly increased, the information of the tachometer is used for SINS/tachometer combined navigation in a DV L abnormal state, the divergence speed of errors such as AUV positions is restrained, and the AUV navigation positioning precision is improved.

Description

Underwater integrated navigation method of noise adaptive filtering
Technical Field
The invention belongs to the field of multi-sensor integrated navigation, and relates to an underwater integrated navigation method of noise adaptive filtering, aiming at the problem of high-efficiency information fusion of SINS/DV L/tachometer/depth sensor.
Background
An Autonomous Underwater Vehicle (AUV) is a main tool for human beings to search ocean, and accurate Underwater navigation and positioning are guarantees for determining whether the Underwater Vehicle can work normally and stably, and are particularly important for the Underwater Vehicle working under the water for long voyage.
Currently, navigation systems available in the AUV mainly include a Strapdown Inertial Navigation System (SINS), a doppler log (DV L), a tachometer, a long baseline system (L B L), a short baseline system (SB L), a Global Positioning System (GPS), a magnetic heading Machine (MCP), and the like.
The underwater combined navigation system cannot receive signals from the water surface when working underwater due to the particularity of the underwater combined navigation system, so that a GPS is useless, and navigation equipment based on acoustic long base lines and short base lines needs to be arranged in advance, so that the application range is limited.
For the SINS/DV L/depth sensor combined navigation system, there are several problems that the SINS generates accumulated error along with the increase of time when carrying out autonomous dead reckoning, and secondly, under the condition of complex hydrology environment or the condition that the movement of AUV is in a large pitch angle or roll angle, the accuracy of speed information provided by acoustic DV L may be reduced instantly, even effective data cannot be provided, and the like.
In conclusion, the problems of the SINS/DV L/depth sensor integrated navigation system are solved, the positioning accuracy and reliability of the integrated navigation system are improved, and the method has strong theoretical significance and engineering application value.
Disclosure of Invention
Technical problem to be solved
In order to avoid the defects of the prior art, the invention provides the underwater integrated navigation method of the noise adaptive filtering, which solves the problem that the navigation precision of the AUV integrated navigation system is rapidly reduced when the measured data output by DV L is abnormal, improves the robustness and navigation positioning precision of the integrated navigation system, and realizes the effective fusion of the measured data of SINS/DV L/tachometer/depth sensor.
Technical scheme
An underwater combined navigation method of noise adaptive filtering is characterized by comprising the following steps:
step 1: in the AUV integrated navigation system, the measured value of a depth sensor is used as the navigation depth, and an error equation of the SINS in filtering is simplified to obtain a simplified SINS error model:
position error equation:
Figure BDA0002470182070000021
Figure BDA0002470182070000022
the velocity error equation:
Figure BDA0002470182070000023
Figure BDA0002470182070000024
Figure BDA0002470182070000025
attitude error equation:
Figure BDA0002470182070000031
Figure BDA0002470182070000032
Figure BDA0002470182070000033
Figure BDA0002470182070000034
wherein L and λ represent latitude error and longitude error of AUV, respectivelyE、VN、VURespectively representing east, north and sky speed errors of the AUV; phi is aE、φN、φURespectively representing the projections of the attitude angle error of the AUV in the east direction, the north direction and the sky direction, L, lambda and h respectively representing the latitude, longitude and navigation depth of the position of the AUV, and RM、RNRespectively the curvature radius of the earth meridian and the prime meridian at the position of AUV; vE、VN、VURespectively representing the navigation speeds of the AUV in the east direction, the north direction and the sky direction; omegaieIs the earth rotation angular rate; f. ofE、fN、fU▽ showing the measured ratio values of the accelerometer in east, north and sky directions respectivelyE、▽N、▽URespectively representing the projections of the bias error of the accelerometer in the east direction, the north direction and the sky direction;E、N、Urespectively representing the projections of the random drift errors of the gyroscope in the east direction, the north direction and the sky direction;
Figure BDA0002470182070000036
a coordinate transformation matrix from a carrier coordinate system to a navigation coordinate system;bx、by、bzrespectively representing the random drift errors of the gyroscopes of the x axis, the y axis and the z axis; w is agx、wgy、wgzZero mean Gaussian white noise of respective three-axis gyroscope ▽bx、▽by、▽bzRespectively representing the bias errors of the accelerometer on an x axis, a y axis and a z axis; w is aax、way、wazZero mean gaussian white noise of the respective triaxial accelerometers;
taking the state variable as
X＝[L λ VEVNVUφEφNφU bx by bz▽bx▽by▽bz]T
w＝[wgxwgxwgxwgxwgxwgx]T
The system state equation is
Figure BDA0002470182070000035
In the formula: f and G are deterministic time-varying matrices with respect to the time parameter t;
step 2: establishing a measurement equation of combined filtering of the SINS/Doppler velocimeter:
the Doppler velocimeter measures the speed difference V of an AUV carrier coordinate systemdx、Vdy、VdzUsing coordinate transformation matrices
Figure BDA0002470182070000041
Converting the vector coordinate system into a navigation coordinate system; and (3) obtaining a measurement equation of the SINS/Doppler velocity instrument combination by utilizing the difference between the velocity of the SINS under the navigation system and the velocity of the Doppler velocity instrument converted into the navigation system, which is obtained by calculation of the SINS:
Figure BDA0002470182070000042
wherein HDMeasurement array for SINS/Doppler velocimeter combination, vDWhite Gaussian noise with zero mean value is used as the measured noise;
and step 3: discretizing a system state equation and a measurement equation according to the calculation period T of the SINS to obtain
Figure BDA0002470182070000043
Wherein:
Figure BDA0002470182070000044
measurement noise variance matrix RkComprises the following steps:
Figure BDA0002470182070000045
in the formula (I), the compound is shown in the specification,
Figure BDA0002470182070000046
taking an initial value of β01 and 0 < b < 1 is called an extinction factor, and as k increases, approximately βk1-b is approximately distributed; the value range of the fading factor b is 0.9-0.999; when the ith scalar sequential measurement update is carried out by adopting sequential filtering, a scalar measurement equation is as follows:
Figure BDA0002470182070000051
for brevity, the following notes:
Figure BDA0002470182070000052
further using the lower limit condition shown by the following formula
Figure BDA0002470182070000053
To ensure
Figure BDA0002470182070000054
Is positive while utilizing an upper limit condition
Figure BDA0002470182070000055
To quickly reduce the measurement
Figure BDA0002470182070000056
Reliability of (2):
Figure BDA0002470182070000057
by the above processing, the noise is measured
Figure BDA0002470182070000058
Is always limited in the interval
Figure BDA0002470182070000059
Within;
and 4, step 4: utilizing the statistical characteristics of the information in the combined filtering process of the SINS/Doppler velocimeter and passing through a fault function lambdakJudging whether the measurement data output by the Doppler velocimeter is abnormal:
Figure BDA00024701820700000510
according to a given false alarm probability α, by ×2Distributed to obtain threshold value TDThe method for judging the working state of the Doppler velocimeter comprises the following steps: when lambda isk≤TDWhen the Doppler velocity meter works normally; when lambda isk＞TDWhen the Doppler velocity meter works abnormally;
1. when the Doppler velocimeter works normally, the SINS/Doppler velocimeter is adopted to carry out combined navigation, Kalman filtering is used for carrying out state estimation, the position, speed and attitude obtained by SINS calculation are subjected to error correction by utilizing the position, speed and attitude error estimation result obtained by filtering each time, and the corrected navigation parameters such as the position, speed and attitude are output.
Kalman filter formula
State one-step prediction
Figure BDA00024701820700000511
State one-step prediction mean square error array
Figure BDA00024701820700000512
Filter gain
Figure BDA00024701820700000513
State estimation
Figure BDA0002470182070000061
State estimation mean square error matrix Pk＝(I-KkHk)Pk,k-1
Calculating the flow velocity V of the seawater around the AUV relative to the seabedwk. The average speed of AUV output by the preprocessed tachometer relative to the surrounding seawater at the k moment in the y-axis direction of the carrier system is V in one filtering period Tpy_kVelocity V of seawater k time around AUVw_kThe calculation formula is as follows:
Figure BDA0002470182070000062
calculating the flow velocity V of the seawater around the AUV relative to the seabed by an exponential weighted moving average methodwkThe details are as follows
Vwk＝βVw_k+(1-β)Vw_(k-1)
β can be set according to the change speed of the current around AUV, when the change speed is faster, β value is larger, such as β is 0.6, when the change speed is slower, β value is smaller, such as β is 0.2;
2. when the Doppler velocimeter works abnormally, the SINS/tachometer is adopted to carry out combined navigation, and the measurement equation is
Figure BDA0002470182070000063
In the formula, VpyProjection of AUV output by tachometer with respect to speed of surrounding seawater in y-axis direction of carrier system, VwIs the estimated value of the flow velocity of the seawater around the AUV before the abnormal work of the Doppler velocimeter, HWMeasurement array for SINS/tachometer combination, vWMeasure the noise for it, which is zero-mean white gaussian noise;
and performing state estimation on the SINS/tachometer combined navigation by using Kalman filtering, performing error correction on the SINS calculated position, speed and attitude by using the position, speed and attitude error estimation result obtained by filtering each time, and outputting the corrected position, speed and attitude navigation parameters.
Advantageous effects
The invention provides an underwater integrated navigation method of noise adaptive filtering, which adopts a depth sensor as navigation depth in an AUV integrated navigation system; simplifying SINS related error equation in filtering, and based on residual χ2The method has the advantages that the rotation speed of the AUV brushless direct current propulsion motor is used as a tachometer, the AUV hardware cost is slightly increased, the information of the tachometer is used for SINS/tachometer combined navigation in a DV L abnormal state, the divergence speed of errors such as AUV positions is restrained, and the AUV navigation positioning precision is improved.
Drawings
FIG. 1 is a schematic flow chart of the method of the present invention
FIG. 2 is a flow chart of an embodiment
Detailed Description
The invention will now be further described with reference to the following examples and drawings:
the technical scheme of the invention comprises the following steps:
step 1: in the AUV integrated navigation system, a depth sensor with higher measurement precision is adopted, and the measurement value of the depth sensor is directly used as the navigation depth;
based on the characteristic of high measurement accuracy of the depth sensor, neglecting the existing measurement error, taking the output value as a depth true value, and simplifying an error equation related to SINS in filtering;
step 2: establishing a measurement equation of combined filtering of the SINS/Doppler velocimeter:
the Doppler velocimeter measures the speed difference V of an AUV carrier coordinate systemdx、Vdy、VdzUsing coordinate transformation matrices
Figure BDA0002470182070000071
Converting the vector coordinate system into a navigation coordinate system; and (3) obtaining a measurement equation of the SINS/Doppler velocity instrument combination by utilizing the difference between the velocity of the SINS under the navigation system and the velocity of the Doppler velocity instrument converted into the navigation system, which is obtained by calculation of the SINS:
Figure BDA0002470182070000081
wherein HDMeasurement array for SINS/Doppler velocimeter combination, vDThe noise is measured as white gaussian noise with zero mean.
And step 3: discretizing a system state equation and a measurement equation according to the calculation period T of the SINS to obtain
Figure BDA0002470182070000082
Wherein:
Figure BDA0002470182070000083
considering that the speed measurement noise of the Doppler velocimeter may change in the working process, the measurement noise variance matrix RkThe determination method comprises the following steps:
Figure BDA0002470182070000084
in the formula (I), the compound is shown in the specification,
Figure BDA0002470182070000085
taking an initial value of β01 and 0 < b < 1 is called an extinction factor, and as k increases, approximately βk1-b is approximately distributed, and the value range of the fading factor b is 0.9-0.999 according to the conditions of noise measurement and the like of the Doppler velocimeter; meanwhile, the sequential filtering method is adopted to RkLimiting the size of each element of the diagonal, i.e. usingSequential filtering when the ith scalar sequential measurement update is performed, the scalar measurement equation is:
Figure BDA0002470182070000086
for brevity, the following notes:
Figure BDA0002470182070000091
further using the lower limit condition shown by the following formula
Figure BDA0002470182070000092
To ensure
Figure BDA0002470182070000093
Is positive while utilizing an upper limit condition
Figure BDA0002470182070000094
To quickly reduce the measurement
Figure BDA0002470182070000095
The reliability of (2).
Figure BDA0002470182070000096
By the above processing, the measured noise can be measured
Figure BDA0002470182070000097
Is always limited in the interval
Figure BDA0002470182070000098
Therefore, the method has better self-adaptive capability and filtering reliability.
And 4, step 4: utilizing the statistical characteristics of the information in the combined filtering process of the SINS/Doppler velocimeter and passing through a fault function lambdakJudging whether the measurement data output by the Doppler velocimeter is abnormal:
Figure BDA0002470182070000099
according to a given false alarm probability α, by ×2Distributed to obtain threshold value TDThe method for judging the working state of the Doppler velocimeter comprises the following steps: when lambda isk≤TDWhen the Doppler velocity meter works normally; when lambda isk＞TDIn time, the doppler velocimeter is abnormal in operation.
And 5: when the Doppler velocimeter works normally, the SINS/Doppler velocimeter is adopted to carry out combined navigation, Kalman filtering is used for carrying out state estimation, the position, speed and attitude obtained by SINS calculation are subjected to error correction by utilizing the position, speed and attitude error estimation result obtained by filtering each time, and the corrected navigation parameters such as the position, speed and attitude are output.
Kalman filter formula
State one-step prediction
Figure BDA00024701820700000910
State one-step prediction mean square error array
Figure BDA00024701820700000911
Filter gain
Figure BDA00024701820700000912
State estimation
Figure BDA00024701820700000913
State estimation mean square error matrix Pk＝(I-KkHk)Pk,k-1
Calculating the flow velocity V of the seawater around the AUV relative to the seabedwk. The average speed of AUV output by the preprocessed tachometer relative to the surrounding seawater at the k moment in the y-axis direction of the carrier system is V in one filtering period Tpy_kVelocity V of seawater k time around AUVw_kThe calculation formula is as follows:
Figure BDA0002470182070000101
based on the characteristic that the flow velocity change of the ocean current around the AUV is relatively slow, in order to reduce the measurement noise influence of the tachometer, the flow velocity V of the ocean water around the AUV relative to the seabed is calculated by an exponential weighted moving average methodwkThe details are as follows
Vwk＝βVw_k+(1-β)Vw_(k-1)
Wherein β can be set according to the change speed of the current around AUV, β value is larger when the change speed is faster, such as β is 0.6, and β value is smaller when the change speed is slower, such as β is 0.2.
Step 6: when the Doppler velocimeter works abnormally, the SINS/tachometer is adopted to carry out combined navigation, and the measurement equation is
Figure BDA0002470182070000102
In the formula, VpyProjection of AUV output by tachometer with respect to speed of surrounding seawater in y-axis direction of carrier system, VwIs the estimated value of the flow velocity of the seawater around the AUV before the abnormal work of the Doppler velocimeter, HWMeasurement array for SINS/tachometer combination, vWThe measured noise is white Gaussian noise with zero mean, and mainly includes the measured noise of the tachometer.
And performing state estimation on the SINS/tachometer combined navigation by using Kalman filtering, performing error correction on the position, speed and attitude obtained by SINS calculation by using the position, speed and attitude error estimation result obtained by filtering each time, and outputting the corrected navigation parameters such as the position, speed and attitude.
Error model for SINS:
selecting state variables of the filter, namely taking error quantities of all states of the AUV, including positioning errors (latitude errors and longitude errors) L and lambda, and east-direction, north-direction and sky-direction speed errors V of the AUVE、VN、VUThe attitude angle error is in east, north and sky directionsProjection phi ofE、φN、φURandom drift error of gyroscope in x-axis, y-axis and z-axisbx、by、bzBias error ▽ for x-axis, y-axis, and z-axis accelerometersbx、▽by、▽bzAssuming that the attitude angle error is a small angle and neglecting the gravity field model error, an approximate linear error model of the SINS can be obtained:
position error equation:
Figure BDA0002470182070000111
Figure BDA0002470182070000112
the velocity error equation:
Figure BDA0002470182070000113
Figure BDA0002470182070000114
Figure BDA0002470182070000115
attitude error equation:
Figure BDA0002470182070000116
Figure BDA0002470182070000117
Figure BDA0002470182070000118
l, lambda and h respectively represent the latitude, longitude and navigation depth of the position of AUV, RM、RNRespectively the ground where AUV is locatedRadius of curvature of meridian and unit fourth of twelve earthly branches, VE、VN、VURespectively shows the navigation speed, omega, of the AUV in the east, north and sky directionsieIs the angular rate of rotation of the earth, fE、fN、fURespectively representing the specific force values of the accelerometer measured in the east, north and sky directions, ▽E、▽N、▽URespectively representing the projections of the bias error of the accelerometer in the east direction, the north direction and the sky direction,E、N、Urespectively representing the projections of the random drift error of the gyroscope in the east direction, the north direction and the sky direction.
Figure BDA0002470182070000121
Wherein:
Figure BDA0002470182070000122
for a coordinate transformation matrix, w, from the carrier coordinate system to the navigation coordinate systemgx、wgy、wgzZero mean white Gaussian noise, w, of respective triaxial gyroscopesax、way、wazZero mean gaussian white noise for respective tri-axial accelerometers.
Taking the state variable as
X＝[L λ VEVNVUφEφNφU bx by bz▽bx▽by▽bz]T
w＝[wgxwgxwgxwgxwgxwgx]T
System equation of state
Figure BDA0002470182070000123
The determinacy of the time parameter t is a time-varying matrix, and the F and the G can be obtained by arranging according to a position error equation, a speed error equation and an attitude error equation of the SINS.
DV L measures AUV carrier coordinate system speedPin Vdx、Vdy、VdzUsing coordinate transformation matrices
Figure BDA0002470182070000124
Converting the speed of the vehicle from a carrier coordinate system to a navigation coordinate system, calculating the difference between the speed of the navigation system obtained by SINS and the speed of DV L converted to the navigation system, and establishing a measurement equation
Figure BDA0002470182070000125
Wherein HDMeasurement array for SINS/DV L combination, vDThe measured noise is white gaussian noise with zero mean, which mainly includes the measured noise of DV L.
Discretizing a system state equation and a measurement equation according to the calculation period T of the SINS to obtain
Figure BDA0002470182070000131
Wherein:
Figure BDA0002470182070000132
DV L working state discrimination method based on residual error
The DV L abnormal working states can be divided into two types, namely, DV L speed measurement accuracy is reduced, actions such as the AUV is impacted by sea currents or turns are mainly shown as measurement noise is increased and can be divided into noise sudden change types, and the DV L abnormal working states are mainly shown as output information changes, and output speed information can be 0 when sound waves of strong sound absorption materials cannot return or are in the conditions of large pitch angle and roll angle, and can be divided into information sudden change types.
Residual-based χ2A detection method for determining whether DV L works normally and the type of abnormal state by using the statistical characteristics of information, wherein the measurement error is zero mean Gaussian white noise when DV L works normally, and when DV L works in abnormal stateThe measurement error is no longer zero mean gaussian white noise.
When the DV L works normally, the residual vector in the filtering process is a zero-mean gaussian white noise process, and the covariance matrix is:
Figure BDA0002470182070000133
let the residual sequence, i.e. innovation, be:
Figure BDA0002470182070000134
the fault detection function is:
Figure BDA0002470182070000135
from statistical properties, λkObey degree of freedom m (vector Z)kDimension) of x2And (4) distribution. Let λkGreater than a certain threshold value TDHas a probability of α, i.e.:
Figure BDA0002470182070000141
where α is the allowable false alarm probability.
According to a given false alarm probability α, the probability of X2Distributed to obtain threshold value TDAt this time, the detection process is as follows:
when lambda isk≤TDWhen the DV L works normally;
when lambda isk＞TDIn time, DV L works abnormally.
Conventional residual-based χ2The detection method can quickly determine whether the system has a mutation type fault, but cannot distinguish the fault type. In order to solve the problem, the mean value of the residual errors is introduced into a fault detection system as a criterion, and the mean value of the residual errors of the noise mutant type oscillates near 0 and is close to 0; if the type of the mutation is information, the residual mean value has a certain deviation from the 0 value. Residual mean value calculation methodComprises the following steps:
Figure BDA0002470182070000142
in the above formula, the first and second carbon atoms are,
Figure BDA0002470182070000143
denotes the mean residual, m is the sliding window length, λiThe residual error of the combined system at the moment i after the fault occurs can be obtained through the SINS/DV L
Figure BDA0002470182070000144
Determine what error DV L has generated when
Figure BDA0002470182070000145
Close to 0, the error is a noise sudden change type error, namely DV L measurement data contains large disturbance noise, otherwise, the error is an information sudden change type error, and DV L fails in a short time.
When DV L works normally or works in an abnormal state with noise mutation type errors, adaptive filtering is adopted to fuse SINS/DV L combined navigation data, and the measurement prediction error formula is as follows:
Figure BDA0002470182070000146
measurement noise variance matrix RkThe acquisition method comprises the following steps:
Figure BDA0002470182070000147
wherein the content of the first and second substances,
Figure BDA0002470182070000148
taking an initial value of β01 and 0 < b < 1 is called an extinction factor, with increasing k, approximately βkApproximately equals to 1-b, the noise of the fading factor b is measured according to DV L, and the value range is 0.9-0.999.
Using sequential filtering method to RkSize of each element of diagonal lineAnd (4) limiting. Let R bekFor diagonal matrix, when the ith scalar sequential measurement update is performed by adopting sequential filtering, the scalar measurement equation is as follows:
Figure BDA0002470182070000151
for brevity, the following notes:
Figure BDA0002470182070000152
further using the lower limit condition shown by the following formula
Figure BDA0002470182070000153
To ensure
Figure BDA0002470182070000154
Is positive while utilizing an upper limit condition
Figure BDA0002470182070000155
To quickly reduce the measurement
Figure BDA0002470182070000156
The reliability of (2).
Figure BDA0002470182070000157
By the above processing, the measured noise can be measured
Figure BDA0002470182070000158
Is always limited in the interval
Figure BDA0002470182070000159
Therefore, the method has better self-adaptive capability and filtering reliability.
In the combined navigation process of the AUV by adopting SINS/DV L, the flow velocity V of the seawater around the AUV relative to the sea bottom is calculated simultaneouslywk. In a filtering period T, the AUV output by the preprocessed tachometer is relative to the k time of the surrounding seawater in the y-axis direction of the carrier systemHas an average speed of Vpy_kVelocity V of seawater k time around AUVw_kThe calculation formula is as follows:
Figure BDA00024701820700001510
based on the characteristic that the flow velocity change of the ocean current around the AUV is relatively slow, in order to reduce the measurement noise influence of the tachometer, the flow velocity V of the ocean water around the AUV relative to the seabed is calculated by an exponential weighted moving average methodwkThe details are as follows
Vwk＝βVw_k+(1-β)Vw_(k-1)
Wherein β can be set according to the change speed of the current around AUV, β value is larger when the change speed is faster, such as β is 0.6, and β value is smaller when the change speed is slower, such as β is 0.2.
When the DV L data error type is an information mutation type error, the speed information output by the DV L is completely invalid, the integrated navigation system SINS/DV L cannot work normally, and the integrated navigation system SINS/tachometer integrated navigation needs to be switched to, and after the DV L works normally, the integrated navigation system SINS/DV L is switched back to.
Measuring equation of SINS/tachometer integrated navigation:
Figure BDA0002470182070000161
wherein, VwThe calculated AUV velocity relative to the water bottom from the last measured velocity data before DV L failure, HWMeasurement array for SINS/tachometer combination, vWThe measured noise is white Gaussian noise with zero mean value, and mainly comprises the measured noise of a tachometer and the like.

Claims (1)

1. An underwater combined navigation method of noise adaptive filtering is characterized by comprising the following steps:
step 1: in the AUV integrated navigation system, the measured value of a depth sensor is used as the navigation depth, and an error equation of the SINS in filtering is simplified to obtain a simplified SINS error model:
position error equation:
Figure FDA0002470182060000011
Figure FDA0002470182060000012
the velocity error equation:
Figure FDA0002470182060000013
Figure FDA0002470182060000014
Figure FDA0002470182060000015
attitude error equation:
Figure FDA0002470182060000016
Figure FDA0002470182060000017
Figure FDA0002470182060000018
Figure FDA0002470182060000019
wherein L and λ represent latitude error and longitude error of AUV, respectivelyE、VN、VURespectively representing east, north and sky speed errors of the AUV; phi is aE、φN、φURespectively representing the projections of the attitude angle error of the AUV in the east direction, the north direction and the sky direction, L, lambda and h respectively representing the latitude, longitude and navigation depth of the position of the AUV, and RM、RNRespectively the curvature radius of the earth meridian and the prime meridian at the position of AUV; vE、VN、VURespectively representing the navigation speeds of the AUV in the east direction, the north direction and the sky direction; omegaieIs the earth rotation angular rate; f. ofE、fN、fU▽ showing the measured ratio values of the accelerometer in east, north and sky directions respectivelyE、▽N、▽URespectively representing the projections of the bias error of the accelerometer in the east direction, the north direction and the sky direction;E、N、Urespectively representing the projections of the random drift errors of the gyroscope in the east direction, the north direction and the sky direction;
Figure FDA0002470182060000023
a coordinate transformation matrix from a carrier coordinate system to a navigation coordinate system;bx、by、bzrespectively representing the random drift errors of the gyroscopes of the x axis, the y axis and the z axis; w is agx、wgy、wgzZero mean Gaussian white noise of respective three-axis gyroscope ▽bx、▽by、▽bzRespectively representing the bias errors of the accelerometer on an x axis, a y axis and a z axis; w is aax、way、wazZero mean gaussian white noise of the respective triaxial accelerometers;
taking the state variable as
X＝[L λ VEVNVUφEφNφU bx by bz▽bx▽by▽bz]T
w＝[wgxwgxwgxwgxwgxwgx]T
The system state equation is
Figure FDA0002470182060000021
In the formula: f and G are deterministic time-varying matrices with respect to the time parameter t;
step 2: establishing a measurement equation of combined filtering of the SINS/Doppler velocimeter:
the Doppler velocimeter measures the speed difference V of an AUV carrier coordinate systemdx、Vdy、VdzUsing coordinate transformation matrices
Figure FDA0002470182060000022
Converting the vector coordinate system into a navigation coordinate system; and (3) obtaining a measurement equation of the SINS/Doppler velocity instrument combination by utilizing the difference between the velocity of the SINS under the navigation system and the velocity of the Doppler velocity instrument converted into the navigation system, which is obtained by calculation of the SINS:
Figure FDA0002470182060000031
wherein HDMeasurement array for SINS/Doppler velocimeter combination, vDWhite Gaussian noise with zero mean value is used as the measured noise;
and step 3: discretizing a system state equation and a measurement equation according to the calculation period T of the SINS to obtain
Figure FDA0002470182060000032
Wherein:
Figure FDA0002470182060000033
measurement noise variance matrix RkComprises the following steps:
Figure FDA0002470182060000034
in the formula (I), the compound is shown in the specification,
Figure FDA0002470182060000035
taking an initial value of β01 and 0 < >b < 1 is called an fading factor, and as k is gradually increased, is approximately βk1-b is approximately distributed; the value range of the fading factor b is 0.9-0.999; when the ith scalar sequential measurement update is carried out by adopting sequential filtering, a scalar measurement equation is as follows:
Figure FDA0002470182060000036
for brevity, the following notes:
Figure FDA0002470182060000037
further using the lower limit condition shown by the following formula
Figure FDA0002470182060000038
To ensure
Figure FDA0002470182060000039
Is positive while utilizing an upper limit condition
Figure FDA00024701820600000310
To quickly reduce the measurement
Figure FDA0002470182060000041
Reliability of (2):
Figure FDA0002470182060000042
by the above processing, the noise is measured
Figure FDA0002470182060000043
Is always limited in the interval
Figure FDA0002470182060000044
Within;
and 4, step 4: utilizing the statistical characteristics of the information in the combined filtering process of the SINS/Doppler velocimeter and passing through a fault function lambdakJudging whether the measurement data output by the Doppler velocimeter is abnormal:
Figure FDA0002470182060000045
according to a given false alarm probability α, by ×2Distributed to obtain threshold value TDThe method for judging the working state of the Doppler velocimeter comprises the following steps: when lambda isk≤TDWhen the Doppler velocity meter works normally; when lambda isk＞TDWhen the Doppler velocity meter works abnormally;
1. when the Doppler velocimeter works normally, performing combined navigation by adopting the SINS/Doppler velocimeter, performing state estimation by using Kalman filtering, performing error correction on the SINS to obtain position, speed and attitude by using the position, speed and attitude error estimation result obtained by filtering each time, and outputting the corrected position, speed and attitude navigation parameters; kalman filter formula
State one-step prediction
Figure FDA0002470182060000046
State one-step prediction mean square error array
Figure FDA0002470182060000047
Filter gain
Figure FDA0002470182060000048
State estimation
Figure FDA0002470182060000049
State estimation mean square error matrix Pk＝(I-KkHk)Pk,k-1
Calculating the flow velocity V of the seawater around the AUV relative to the seabedwkAUV output by the preprocessed tachometer in the y-axis of the carrier system relative to the surrounding seawater during a filter period TThe average velocity of the direction at the time k is Vpy_kVelocity V of seawater k time around AUVw_kThe calculation formula is as follows:
Figure FDA0002470182060000051
calculating the flow velocity V of the seawater around the AUV relative to the seabed by an exponential weighted moving average methodwkThe details are as follows
Vwk＝βVw_k+(1-β)Vw_(k-1)
β can be set according to the change speed of the current around AUV, when the change speed is faster, β value is larger, such as β is 0.6, when the change speed is slower, β value is smaller, such as β is 0.2;
2. when the Doppler velocimeter works abnormally, the SINS/tachometer is adopted to carry out combined navigation, and the measurement equation is
Figure FDA0002470182060000052
In the formula, VpyProjection of AUV output by tachometer with respect to speed of surrounding seawater in y-axis direction of carrier system, VwIs the estimated value of the flow velocity of the seawater around the AUV before the abnormal work of the Doppler velocimeter, HWMeasurement array for SINS/tachometer combination, vWMeasure the noise for it, which is zero-mean white gaussian noise;
and performing state estimation on the SINS/tachometer combined navigation by using Kalman filtering, performing error correction on the SINS calculated position, speed and attitude by using the position, speed and attitude error estimation result obtained by filtering each time, and outputting the corrected position, speed and attitude navigation parameters.
CN202010345972.XA 2020-04-27 2020-04-27 Underwater integrated navigation method of noise adaptive filtering Active CN111504324B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010345972.XA CN111504324B (en) 2020-04-27 2020-04-27 Underwater integrated navigation method of noise adaptive filtering

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010345972.XA CN111504324B (en) 2020-04-27 2020-04-27 Underwater integrated navigation method of noise adaptive filtering

Publications (2)

Publication Number Publication Date
CN111504324A true CN111504324A (en) 2020-08-07
CN111504324B CN111504324B (en) 2022-07-26

Family

ID=71871456

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010345972.XA Active CN111504324B (en) 2020-04-27 2020-04-27 Underwater integrated navigation method of noise adaptive filtering

Country Status (1)

Country Link
CN (1) CN111504324B (en)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112179347A (en) * 2020-09-18 2021-01-05 西北工业大学 Combined navigation method based on spectrum red shift error observation equation
CN112710304A (en) * 2020-12-17 2021-04-27 西北工业大学 Underwater autonomous vehicle navigation method based on adaptive filtering
CN112947117A (en) * 2021-02-22 2021-06-11 湖南大学 Multi-ROV underwater cooperative operation simulation system
WO2022088797A1 (en) * 2020-10-26 2022-05-05 东南大学 Measurement abnormality-considered cooperative localization method for cluster type multi-deep-sea underwater vehicle

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
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
CN110146075A (en) * 2019-06-06 2019-08-20 哈尔滨工业大学（威海） A kind of SINS/DVL combined positioning method of gain compensation adaptive-filtering
CN110514203A (en) * 2019-08-30 2019-11-29 东南大学 A kind of underwater Combinated navigation method based on ISR-UKF
CN111024064A (en) * 2019-11-25 2020-04-17 东南大学 SINS/DVL combined navigation method for improving Sage-Husa adaptive filtering

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
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
WO2020062791A1 (en) * 2018-09-28 2020-04-02 东南大学 Sins/dvl-based underwater anti-shaking alignment method for deep-sea underwater vehicle
CN110146075A (en) * 2019-06-06 2019-08-20 哈尔滨工业大学（威海） A kind of SINS/DVL combined positioning method of gain compensation adaptive-filtering
CN110514203A (en) * 2019-08-30 2019-11-29 东南大学 A kind of underwater Combinated navigation method based on ISR-UKF
CN111024064A (en) * 2019-11-25 2020-04-17 东南大学 SINS/DVL combined navigation method for improving Sage-Husa adaptive filtering

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
查月等: "基于自适应滤波器的AUV组合导航系统研究", 《舰船科学技术》 *
袁东玉等: "基于SINS的水下组合导航系统研究", 《网络新媒体技术》 *

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112179347A (en) * 2020-09-18 2021-01-05 西北工业大学 Combined navigation method based on spectrum red shift error observation equation
WO2022088797A1 (en) * 2020-10-26 2022-05-05 东南大学 Measurement abnormality-considered cooperative localization method for cluster type multi-deep-sea underwater vehicle
CN112710304A (en) * 2020-12-17 2021-04-27 西北工业大学 Underwater autonomous vehicle navigation method based on adaptive filtering
CN112710304B (en) * 2020-12-17 2022-12-13 西北工业大学 Underwater autonomous vehicle navigation method based on adaptive filtering
CN112947117A (en) * 2021-02-22 2021-06-11 湖南大学 Multi-ROV underwater cooperative operation simulation system

Also Published As

Publication number Publication date
CN111504324B (en) 2022-07-26

Similar Documents

Publication Publication Date Title
CN111504324B (en) Underwater integrated navigation method of noise adaptive filtering
CN109443379B (en) SINS/DV L underwater anti-shaking alignment method of deep-sea submersible vehicle
Whitcomb et al. Advances in Doppler-based navigation of underwater robotic vehicles
CN111024064B (en) SINS/DVL combined navigation method for improving Sage-Husa adaptive filtering
US7778111B2 (en) Methods and systems for underwater navigation
Hegrenaes et al. Doppler water-track aided inertial navigation for autonomous underwater vehicle
CN106679662A (en) Combined underwater robot navigation method based on TMA (target motion analysis) technology and single beacon
Lee et al. Underwater navigation system based on inertial sensor and doppler velocity log using indirect feedback kalman filter
Kinsey et al. Towards in-situ calibration of gyro and doppler navigation sensors for precision underwater vehicle navigation
CN110940340A (en) Multi-sensor information fusion method based on small UUV platform
CN111982105B (en) Underwater navigation positioning method and system based on SINS/LBL tight combination
CN112747748A (en) Pilot AUV navigation data post-processing method based on reverse solution
KR101763911B1 (en) Heading estimation apparatus of auv in severe magnetic disturbance environment and the method thereof
Allotta et al. Localization algorithm for a fleet of three AUVs by INS, DVL and range measurements
Zhao et al. The experimental study on GPS/INS/DVL integration for AUV
CN112697154A (en) Self-adaptive multi-source fusion navigation method based on vector distribution
Sun et al. Fine resolution position estimation using Kalman filtering
Lager et al. Underwater terrain navigation using standard sea charts and magnetic field maps
Bennamoun et al. The development of an integrated GPS/INS/sonar navigation system for autonomous underwater vehicle navigation
US20210318452A1 (en) Method for identifying a static phase of a vehicle
CN114964235A (en) Combined navigation method based on inertia/Doppler log and damping state
Emel’yantsev et al. Specific features of constructing a dual-mode GNSS gyrocompass as a tightly-coupled integrated system
Liu et al. A correction method for DVL measurement error by pitch dynamics
Belge et al. Sensor fusion based on integrated navigation data of sea surface vehicle with machine learning method
Troni et al. Experimental evaluation of an inertial navigation system for underwater robotic vehicles

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