CN115855049A - SINS/DVL navigation method based on particle swarm optimization robust filtering - Google Patents

SINS/DVL navigation method based on particle swarm optimization robust filtering Download PDF

Info

Publication number
CN115855049A
CN115855049A CN202310069689.2A CN202310069689A CN115855049A CN 115855049 A CN115855049 A CN 115855049A CN 202310069689 A CN202310069689 A CN 202310069689A CN 115855049 A CN115855049 A CN 115855049A
Authority
CN
China
Prior art keywords
representing
matrix
error
navigation
sins
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
CN202310069689.2A
Other languages
Chinese (zh)
Other versions
CN115855049B (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.)
Hohai University HHU
Original Assignee
Hohai University HHU
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 Hohai University HHU filed Critical Hohai University HHU
Priority to CN202310069689.2A priority Critical patent/CN115855049B/en
Publication of CN115855049A publication Critical patent/CN115855049A/en
Application granted granted Critical
Publication of CN115855049B publication Critical patent/CN115855049B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Feedback Control In General (AREA)
  • Navigation (AREA)

Abstract

The invention provides a particle swarm optimization robust filtering-based SINS/DVL navigation method, belongs to the technical field of underwater robot navigation positioning, and mainly aims to enhance the anti-interference capability of an SINS/DVL combined navigation system in a complex underwater environment and further improve the combined navigation precision of the SINS/DVL system. The method mainly comprises the following steps: the method comprises the steps of navigation model construction based on a state transformation principle, filtering state updating and noise matrix construction, fitness function construction, a self-adaptive particle swarm optimization process and filtering measurement updating. The method can solve the problem of poor positioning accuracy caused by interference of an SINS/DVL combined navigation system in a complex underwater environment. Compared with a Kalman filtering method and a Huber robust filtering method. The method can improve the anti-interference capability of the system and further improve the combined navigation precision of the SINS/DVL.

Description

SINS/DVL navigation method based on particle swarm optimization robust filtering
Technical Field
The invention belongs to the navigation and positioning technology of underwater robots, and particularly relates to a SINS/DVL navigation method based on particle swarm optimization robust filtering.
Background
Ocean resources are abundant, and the underwater robot is an effective carrier for developing and utilizing the ocean resources. Meanwhile, the underwater robot can not operate efficiently and can not leave high-precision navigation positioning information. Therefore, underwater navigation positioning plays an irreplaceable role in the field of underwater robots. Currently, common underwater navigation methods include a Strapdown Inertial Navigation System (SINS), a doppler log (DVL), an underwater acoustic positioning system, a geophysical navigation system, and the like. Although the underwater acoustic positioning system has the characteristic of high precision, a matrix needs to be laid in advance under water, the maintenance is complex, and the underwater acoustic positioning system cannot be used in unknown sea areas. The geophysical navigation method comprises the following steps: terrain matching, geomagnetic matching and the like, and such methods require the establishment of huge terrain and geomagnetic databases in advance and require complex periodic updating operations. Therefore, an integrated navigation mode taking the strapdown inertial navigation system as the main Doppler log as the auxiliary Doppler log becomes a common and autonomous navigation mode of the underwater robot.
Due to the complexity of the underwater environment, there is uncertainty and interference in the propagation of acoustic signals. Therefore, the research of the SINS/DVL-based robust filtering algorithm is urgent. The Kalman filtering algorithm is the most classical filtering algorithm of the SINS/DVL combination, but the Kalman filtering algorithm cannot deal with the problem of external interference; the Huber robust filtering algorithm proposed in recent years can improve the anti-interference capability of the integrated navigation system, however, when external interference is continuous or strong nonlinear noise interference exists, the Huber robust filtering algorithm still has the problem of poor positioning longitude.
Aiming at the problems, the invention provides an SINS/DVL navigation method based on particle swarm optimization robust filtering, on one hand, in order to improve the robustness of the model, an SINS/DVL navigation model is established based on the state transformation principle; on the other hand, in order to improve the robustness of the algorithm, on the basis of the Huber robust filtering algorithm, a noise variance matrix is respectively improved, and a self-adaptive particle swarm optimization algorithm is introduced, so that the robust processing of external interference measurement information is realized, and the SINS/DVL navigation accuracy is further improved.
Disclosure of Invention
The purpose of the invention is as follows: the invention aims to solve the problem of underwater SINS/DVL combined navigation in a complex environment, and provides a SINS/DVL navigation method based on particle swarm optimization robust filtering.
The invention relates to a SINS/DVL navigation method based on particle swarm optimization robust filtering, wherein the known quantity of SINS/DVL navigation is as follows:
doppler log, abbreviated DVL, outputting three-dimensional velocity information under carrier system
Figure SMS_1
, wherein ,/>
Figure SMS_2
In a coordinate system representing the carrierxDirection speed information, <' > based on the direction of the vehicle>
Figure SMS_3
In a coordinate system representing the carrieryThe speed information of the direction of the vehicle,
Figure SMS_4
in a coordinate system representing the carrierzSpeed information of the direction;
a strapdown inertial navigation system, abbreviated as SINS, outputs three-dimensional velocity information under a navigation coordinate system:
Figure SMS_5
, wherein ,/>
Figure SMS_6
Indicates east speed, and>
Figure SMS_7
indicates a north speed, and->
Figure SMS_8
Representing the speed in the direction of the day;
the method comprises the following steps:
step 1, constructing a combined navigation model constructed based on a state transformation principle;
step 2, updating the filtering state and the system noise variance matrix on the basis of the step 1Q k Sum measure noise variance matrixS k The structure of (1);
step 3, introducing a self-adaptive particle filter algorithm to carry out system noise variance matrixQ k Sum measure noise variance matrixS k Correcting, wherein the process comprises two steps, namely particle filtering fitness function construction and self-adaptive particle swarm optimization algorithm;
and 4, updating the filtering measurement.
Further, step 1 specifically includes the following steps:
firstly, establishing an SINS/DVL combined navigation state equation based on a state transformation principle:
Figure SMS_9
wherein ,
Figure SMS_10
represents a combined navigation status vector, <' > based on the navigation state information>
Figure SMS_11
Representing a combined navigation state transition matrix; />
Figure SMS_12
Representing combined navigation system noise; />
Figure SMS_13
A combined navigation state vector representing 15 dimensions may be represented as follows:
Figure SMS_14
wherein ,
Figure SMS_15
respectively representing an east direction speed error, a north direction speed error and a sky direction speed error;
Figure SMS_16
respectively representxThe direction attitude error,yError in orientation and attitudezA directional attitude error; />
Figure SMS_17
Respectively representing a longitude error, a latitude error and an altitude error; />
Figure SMS_18
Respectively representxZero offset error of the direction accelerometer,yDirection accelerometer zero offset error sumzThe direction accelerometer has zero offset error;/>
Figure SMS_19
respectively representxZero offset error of the directional gyroscope,yZero bias error sum of directional gyroscopezZero bias error of the directional gyroscope;
combined navigation state transition matrix
Figure SMS_20
The device consists of four parts, specifically as follows:
Figure SMS_21
wherein ,
Figure SMS_22
representing a state transition sub-matrix corresponding to the speed error; />
Figure SMS_23
Representing a state transition sub-matrix corresponding to the attitude error; />
Figure SMS_24
Representing a state transition sub-matrix corresponding to the position error; />
Figure SMS_25
Represents a zero matrix of 6 rows and 15 columns, in which case>
Figure SMS_26
The expression is as follows:
Figure SMS_27
wherein ,
Figure SMS_28
representing the rotation angular velocity of the earth system relative to the inertial system; />
Figure SMS_29
Representing a rotational angular velocity of the navigation system relative to the earth system; />
Figure SMS_30
Representing the earth gravity information under the navigation coordinate system; />
Figure SMS_31
Representing a posture transfer matrix from a carrier coordinate system to a navigation coordinate system; symbol->
Figure SMS_32
An antisymmetric matrix representing the matrix;
Figure SMS_33
the expression is as follows:
Figure SMS_34
/>
wherein ,
Figure SMS_35
intermediate variables defined merely for convenience of presentation do not represent any meaning, and>
Figure SMS_36
are respectively:
Figure SMS_37
Figure SMS_38
Figure SMS_39
Figure SMS_40
representing the angular rate of rotation of the earth;Landhrespectively representing the geographical latitude and the height of the position of the carrier;R M andR N individual watchIndicating the radius of the meridian and the radius of the prime unit at the position of the carrier;
Figure SMS_41
the expression is as follows:
Figure SMS_42
wherein ,
Figure SMS_43
intermediate variables defined only for convenience of presentation and not representing any meaning, and +>
Figure SMS_44
Are respectively:
Figure SMS_45
Figure SMS_46
then, a measurement equation of the SINS/DVL integrated navigation system is given:
Figure SMS_47
wherein ,
Figure SMS_48
representing a measurement transfer matrix; />
Figure SMS_49
Representing a measurement noise matrix; />
Figure SMS_50
The measurement matrix is expressed as follows:
Figure SMS_51
wherein ,
Figure SMS_52
representing a posture transfer matrix of the navigation system to the carrier system; />
Figure SMS_53
Three-dimensional velocity information under a vector system representing DVL output, measuring a transfer matrix ≧ or>
Figure SMS_54
Can be expressed as follows:
Figure SMS_55
。
further, the step 2 specifically comprises the following steps:
the filter state update is performed as follows:
Figure SMS_56
wherein ,
Figure SMS_57
to representkA combined navigation state transition matrix of the time; />
Figure SMS_58
To representk-a combined navigation state vector estimate at time 1, -a->
Figure SMS_59
Representing a one-step predicted state vector;
the state vector mean square error matrix is updated as follows:
Figure SMS_60
wherein ,
Figure SMS_61
a mean square error matrix representing the state vector; />
Figure SMS_62
To representkThe system noise variance matrix at the time of day,
Figure SMS_63
representing a one-step predictive covariance matrix, superscriptTRepresents a transpose of a matrix;
based on Huber M estimation principle, measurement noise variance array is constructed
Figure SMS_64
：
Figure SMS_65
wherein ,
Figure SMS_66
representing the measured noise variance matrix.
Further, step 3 specifically includes the following steps:
first fitness function constructs
Array of system noise variancesQ k Sum measure noise variance matrixS k As particles, the performance of the SINS/DVL combined navigation system depends mainly on the position accuracy, so the mean square error of the position error of the filter output is taken as the fitness function:
Figure SMS_67
wherein ,f（Q k ,S k ) Is thatQ k ,S k The mean square error of the position error of the SINS/DVL integrated navigation system when the parameter is the SINS/DVL integrated navigation system;
Figure SMS_68
is an estimated position value;Mto estimate the length; />
Figure SMS_69
Is the actual position value of the reference;
then, carrying out adaptive particle swarm optimization, specifically comprising the following processes:
Figure SMS_70
initializing particle swarm parameters, initializing the position and the speed of the particle swarm in a parameter interval, randomly generating a plurality of particles and initializing optimal particles; />
Figure SMS_71
Evaluating the fitness of each particle according to the fitness function, comparing the fitness with the fitness value of the optimal particle, and replacing the optimal particle if the fitness is better;
Figure SMS_72
particlest=（Q k ,S k ) Iteratively updating the speed and the position of each particle, adjusting the track of each particle to approach to the optimal point according to the experience of each particle and the group, and updating according to the following equation:
Figure SMS_73
Figure SMS_74
in the formula ,Gthe current iteration number is;
Figure SMS_76
are particlestIn the first placeGPosition of generation->
Figure SMS_78
Are particlestIn the first placeGPosition in the +1 passage +>
Figure SMS_80
Are particlestIn the first placeGSpeed of +1 generation; />
Figure SMS_77
Are particlestFirst, theGBest position searched by generation;
Figure SMS_79
for the whole particle swarmGThe best position searched up to the generation, <' > or>
Figure SMS_81
Is the inertial weight; />
Figure SMS_82
Is a learning factor;
Figure SMS_75
random numbers between 0~1;
the improvement to the inertial weight is as follows:
Figure SMS_83
wherein ,u start andu end the initial value and the final value of the inertia weight are respectively, and the value range is as follows: 0.4-0.4 ≤u end ≤u start ≤0.9；
Figure SMS_84
Is the maximum value of the iteration times;
Figure SMS_85
repeat->
Figure SMS_86
and />
Figure SMS_87
And updating the optimal particles until the iteration times or the precision requirement is met, and generating the optimal particles.
Further, step 4 specifically includes the following steps:
and (3) performing filtering measurement updating:
Figure SMS_88
wherein ,
Figure SMS_89
representkThe status vector of the moment in time>
Figure SMS_90
RepresentkA measurement vector at a moment in time>
Figure SMS_91
To representk-a state estimation weight at time 1; />
Figure SMS_92
Intermediate variables, which are defined for convenience only and do not represent any meaning, are specifically identified below:
Figure SMS_93
Figure SMS_94
Figure SMS_95
the values of each element in the weight matrix are as follows: />
Figure SMS_96
wherein ,
Figure SMS_97
representing a symbolic function; />
Figure SMS_98
A representation factor;
the mean square error matrix is updated as follows:
Figure SMS_99
wherein ,
Figure SMS_100
an identity matrix representing 15 rows and 15 columns; />
Figure SMS_101
RepresentkThe filter gain at that moment->
Figure SMS_102
To representkCovariance matrix of moments +>
Figure SMS_103
To representkThe measurement of the time shift matrix->
Figure SMS_104
To representkMeasurement of the first in the vector matrix of the time of dayiA plurality of elements +>
Figure SMS_105
To representkThe first in the time matrix AiElement(s) is/are present>
Figure SMS_106
To representkThe measurement of the time shifts the matrix.
Compared with the prior art, the invention has the following advantages:
(1) Aiming at the robustness problem of an SINS/DVL navigation model, the navigation model based on the state transformation principle is constructed by using the earth gravity information which is not easy to be interfered to replace the acceleration measurement information which is easy to be interfered, a state equation and a measurement equation are deduced, and the robustness of the model is further improved.
(2) On the basis of a Huber robust filtering algorithm, the method considers the strong nonlinearity problem of system noise and measurement noise, introduces a particle swarm optimization algorithm, designs a fitness function, and further improves the processing capacity of the algorithm on interference noise.
(3) On the basis of a particle swarm optimization algorithm, the invention provides an improved inertial weight calculation method aiming at the influence of the weight on the particle swarm search speed and convergence, and the self-adaptability of the particle swarm optimization algorithm is further improved.
Drawings
FIG. 1 is a flow chart of a SINS/DVL navigation method based on particle swarm optimization robust filtering described in the present invention;
FIG. 2 is a schematic diagram of the SINS/DVL navigation method based on particle swarm optimization robust filtering described in the present invention;
FIG. 3 is a SINS/DVL combined navigation east position error curve of the method of the present invention;
FIG. 4 is a SINS/DVL combined navigation north position error plot of the method of the present invention.
Detailed Description
The technical solution of the present invention is described in detail below, but the scope of the present invention is not limited to the embodiments.
The invention relates to a SINS/DVL navigation method based on particle swarm optimization robust filtering, which comprises the following steps of:
doppler log, abbreviated DVL, outputting three-dimensional velocity information under carrier system
Figure SMS_107
, wherein ,/>
Figure SMS_108
In a coordinate system representing a carrierxSpeed information of the direction->
Figure SMS_109
In a coordinate system representing the carrieryThe speed information of the direction is determined,
Figure SMS_110
in a coordinate system representing a carrierzSpeed information of the direction;
a strapdown inertial navigation system, abbreviated as SINS, outputs three-dimensional velocity information under a navigation coordinate system:
Figure SMS_111
, wherein ,/>
Figure SMS_112
Indicating east speed, <' > based on>
Figure SMS_113
Indicates a north speed, and->
Figure SMS_114
Representing the speed in the direction of the day; />
The method comprises the following steps:
step 1, constructing a combined navigation model constructed based on a state transformation principle;
firstly, establishing an SINS/DVL integrated navigation state equation based on the state transformation principle:
Figure SMS_115
wherein ,
Figure SMS_116
represents a combined navigation status vector, <' > based on the navigation state information>
Figure SMS_117
Representing a combined navigation state transition matrix; />
Figure SMS_118
Representing combined navigation system noise; />
Figure SMS_119
Representing a 15-dimensional combined navigation state vector, can be expressed as follows:
Figure SMS_120
wherein ,
Figure SMS_121
respectively representing an east direction speed error, a north direction speed error and a sky direction speed error;
Figure SMS_122
respectively representxThe direction attitude error,yError in attitude of direction andza directional attitude error;/>
Figure SMS_123
respectively representing a longitude error, a latitude error and an altitude error; />
Figure SMS_124
Respectively representxZero offset error of the direction accelerometer,yDirection accelerometer zero offset error sumzZero offset error of the direction accelerometer; />
Figure SMS_125
Respectively representxZero offset error of the directional gyroscope,yZero offset error sum of directional gyroscopezZero bias error of the directional gyroscope;
combined navigation state transition matrix
Figure SMS_126
The device consists of four parts, specifically as follows:
Figure SMS_127
wherein ,
Figure SMS_128
representing a state transition sub-matrix corresponding to the speed error; />
Figure SMS_129
Representing a state transition sub-matrix corresponding to the attitude error; />
Figure SMS_130
Representing a state transition sub-matrix corresponding to the position error; />
Figure SMS_131
Represents a zero matrix of 6 rows and 15 columns, in which case>
Figure SMS_132
The expression is as follows:
Figure SMS_133
wherein ,
Figure SMS_134
representing the rotational angular velocity of the earth system relative to the inertial system; />
Figure SMS_135
Representing a rotational angular velocity of the navigation system relative to the earth system; />
Figure SMS_136
Representing the earth gravity information under the navigation coordinate system; />
Figure SMS_137
Representing a posture transfer matrix from a carrier coordinate system to a navigation coordinate system; symbol->
Figure SMS_138
An antisymmetric matrix representing the matrix;
Figure SMS_139
is expressed as follows:
Figure SMS_140
wherein ,
Figure SMS_141
intermediate variables defined merely for convenience of presentation do not represent any meaning, and>
Figure SMS_142
are respectively: />
Figure SMS_143
Figure SMS_144
Figure SMS_145
Figure SMS_146
Representing the angular rate of rotation of the earth;Landhrespectively representing the geographical latitude and the height of the position of the carrier;R M andR N respectively representing the radius of a meridian and the radius of a prime unit at the position of the carrier;
Figure SMS_147
is expressed as follows:
Figure SMS_148
wherein ,
Figure SMS_149
an intermediate variable defined merely for convenience of presentation, does not represent any meaning, and->
Figure SMS_150
Are respectively:
Figure SMS_151
Figure SMS_152
then, a measurement equation of the SINS/DVL integrated navigation system is given:
Figure SMS_153
wherein ,
Figure SMS_154
representing a measurement transfer matrix; />
Figure SMS_155
Representing a measurement noise matrix; />
Figure SMS_156
The measurement matrix is expressed as follows:
Figure SMS_157
wherein ,
Figure SMS_158
representing a posture transfer matrix of the navigation system to the carrier system; />
Figure SMS_159
Three-dimensional velocity information under a vector system representing DVL output, measuring a transfer matrix ≧ or>
Figure SMS_160
Can be expressed as follows:
Figure SMS_161
。
the above is the combined navigation model based on the state transformation principle proposed by the present invention.
Step 2, updating the system noise variance matrix in the filtering state based on the step 1Q k Sum measure noise variance matrixS k The structure of (2);
the filter state update is performed as follows:
Figure SMS_162
wherein ,
Figure SMS_163
to representkA combined navigation state transition matrix of the time; />
Figure SMS_164
To representk-combined navigation state vector estimate at time 1, -based on the combined navigation state vector estimate in the vehicle navigation system and the vehicle navigation system>
Figure SMS_165
Representing a one-step predicted state vector;
the state vector mean square error matrix is updated as follows:
Figure SMS_166
wherein ,
Figure SMS_167
a mean square error matrix representing the state vector; />
Figure SMS_168
To representkThe system noise variance matrix at a time instant,
Figure SMS_169
representing a one-step predictive covariance matrix, superscriptTRepresents a transpose of a matrix;
based on Huber M estimation principle, measurement noise variance array is constructed
Figure SMS_170
：
Figure SMS_171
wherein ,
Figure SMS_172
representing the measured noise variance matrix.
Step 3, in order to better meet the requirement of the filtering algorithm on robustness, on the basis of the two noise variance matrixes, the invention introduces the adaptive particle filtering algorithm to the system noise variance matrixQ k Sum measure noise variance matrixS k And correcting to further improve the robustness of filtering. The process comprises two steps, namely particle filtering fitness function construction and a self-adaptive particle swarm optimization algorithm;
array of system noise variancesQ k Sum measure noise variance matrixS k As particles, the performance of the SINS/DVL combined navigation system depends mainly on the position accuracy, so the mean square error of the position error of the filter output is taken as the fitness function:
Figure SMS_173
wherein ,f（Q k ,S k ) Is thatQ k ,S k The mean square error of the position error of the SINS/DVL integrated navigation system when the parameter is the SINS/DVL integrated navigation system;
Figure SMS_174
is an estimated position value;Mto estimate the length; />
Figure SMS_175
Is the actual position value of the reference;
then, carrying out adaptive particle swarm optimization, specifically comprising the following processes:
Figure SMS_176
initializing particle swarm parameters, initializing the position and the speed of the particle swarm in a parameter interval, randomly generating a plurality of particles and initializing optimal particles;
Figure SMS_177
evaluating the fitness of each particle according to the fitness function, comparing the fitness with the fitness value of the optimal particle, and replacing the optimal particle if the fitness is better;
Figure SMS_178
particlest=（Q k ,S k ) Iteratively updating the speed and the position of each particle, adjusting the track of each particle to approach to the optimal point according to the experience of each particle and the group, and carrying out the following stepsThe equations are updated:
Figure SMS_179
Figure SMS_180
in the formula ,Gthe current iteration number is;
Figure SMS_182
are particlestIn the first placeGPosition of generation->
Figure SMS_185
Are particlestIn the first placeGPosition in the +1 passage +>
Figure SMS_187
Are particlestIn the first placeGSpeed of +1 generation; />
Figure SMS_183
Are particlestFirst, theGBest position searched by generation;
Figure SMS_184
for the whole particle groupGBest position searched up to generation, <' > based on time>
Figure SMS_186
Is the inertial weight; />
Figure SMS_188
Is a learning factor;
Figure SMS_181
random numbers between 0~1;
to more reasonably change the inertial weight, a better result is obtained. The invention improves the inertial weight as follows:
Figure SMS_189
wherein ,u start andu end the initial value and the final value of the inertia weight are respectively, and the value range is as follows: 0.4 ≤u end ≤u start ≤0.9；
Figure SMS_190
Is the maximum value of the iteration times;
Figure SMS_191
repeat->
Figure SMS_192
and />
Figure SMS_193
And updating the optimal particles until the iteration times or the precision requirement is met, and generating the optimal particles.
And 4, on the basis of the filter state updating and the noise matrix correction, performing filter measurement updating:
Figure SMS_194
wherein ,
Figure SMS_195
to representkStatus vector of time instant>
Figure SMS_196
To representkThe measured vector at the moment is greater or less than>
Figure SMS_197
To representk-a state estimation weight at time 1; />
Figure SMS_198
Intermediate variables, which are defined for convenience only and do not represent any meaning, are specifically identified below:
Figure SMS_199
Figure SMS_200
Figure SMS_201
the values of each element in the weight matrix are as follows: />
Figure SMS_202
wherein ,
Figure SMS_203
representing a symbolic function; />
Figure SMS_204
A representation factor;
the mean square error matrix is updated as follows:
Figure SMS_205
wherein ,
Figure SMS_206
an identity matrix representing 15 rows and 15 columns; />
Figure SMS_207
To representkThe filter gain at that moment->
Figure SMS_208
To representkThe covariance matrix of the moment, < > >>
Figure SMS_209
To representkThe measurement of the time shift matrix->
Figure SMS_210
To representkMeasurement of the first in the vector matrix of the time of dayiElement(s) is/are present>
Figure SMS_211
RepresentkThe first in the time matrix AiElement(s) is/are present>
Figure SMS_212
To representkThe measurement of the time shifts the matrix.
The simulation parameters are set as follows:
zero offset error of gyroscope
Figure SMS_213
(ii) a The zero offset error of the accelerometer is 100ug; the data output frequency of the strapdown inertial navigation system is 200Hz; DVL scale factor error 0.003; the DVL output frequency is 2Hz. FIG. 3 shows the east position error of the SINS/DVL combined navigation corresponding to the three methods; FIG. 4 shows the error curves of the SINS/DVL integrated navigation north position corresponding to the three methods; from fig. 3 and fig. 4, it can be seen that the navigation accuracy of the method of the present invention is superior to that of the kalman filtering method and the Huber robust filtering method.
It is to be understood that these examples are for illustrative purposes only and are not intended to limit the scope of the present invention, which is to be given the full breadth of the appended claims and any and all equivalent modifications thereto by those of ordinary skill in the art after reading this disclosure.

Claims (5)

1. A SINS/DVL navigation method based on particle swarm optimization robust filtering, the known quantity of SINS/DVL navigation:
doppler log, abbreviated DVL, outputting three-dimensional velocity information under carrier system
Figure QLYQS_1
, wherein ,/>
Figure QLYQS_2
In a coordinate system representing a carrierxSpeed information of the direction->
Figure QLYQS_3
In a coordinate system representing a carrieryThe speed information of the direction is determined,
Figure QLYQS_4
in a coordinate system representing a carrierzSpeed information of the direction;
a strapdown inertial navigation system, abbreviated as SINS, outputs three-dimensional velocity information under a navigation coordinate system:
Figure QLYQS_5
, wherein ,/>
Figure QLYQS_6
Indicates east speed, and>
Figure QLYQS_7
indicates a north speed, and->
Figure QLYQS_8
Representing the speed in the direction of the day;
the method is characterized by comprising the following steps:
step 1, constructing a combined navigation model constructed based on a state transformation principle;
step 2, updating the filtering state and the system noise variance matrix on the basis of the step 1Q k Sum measure noise variance matrixS k The structure of (1);
step 3, introducing a self-adaptive particle filter algorithm to carry out system noise variance matrixQ k Sum measure noise variance matrixS k Correcting, wherein the process is divided into two steps, namely, particle filtering fitness function construction and a self-adaptive particle swarm optimization algorithm;
and 4, updating the filtering measurement.
2. The SINS/DVL navigation method based on particle swarm optimization robust filtering according to claim 1, wherein the step 1 specifically comprises the following steps:
firstly, establishing an SINS/DVL combined navigation state equation based on a state transformation principle:
Figure QLYQS_9
wherein ,
Figure QLYQS_10
represents a combined navigation status vector, <' > based on the navigation state information>
Figure QLYQS_11
Representing a combined navigation state transition matrix; />
Figure QLYQS_12
Representing combined navigation system noise; />
Figure QLYQS_13
Representing a 15-dimensional combined navigation state vector, can be expressed as follows:
Figure QLYQS_14
wherein ,
Figure QLYQS_15
respectively representing an east direction speed error, a north direction speed error and a sky direction speed error;
Figure QLYQS_16
respectively representxThe direction attitude error,yError in orientation and attitudezA directional attitude error; />
Figure QLYQS_17
Respectively representing longitude error, latitude error and altitude error; />
Figure QLYQS_18
Respectively representxZero offset error of the direction accelerometer,yDirection accelerometer zero offset error sumzThe direction accelerometer has zero offset error; />
Figure QLYQS_19
Respectively representxZero offset error of the directional gyroscope,yZero offset error sum of directional gyroscopezZero bias error of the directional gyroscope;
combined navigation state transition matrix
Figure QLYQS_20
The device consists of four parts, specifically as follows:
Figure QLYQS_21
wherein ,
Figure QLYQS_22
representing a state transition sub-matrix corresponding to the speed error; />
Figure QLYQS_23
Representing a state transition sub-matrix corresponding to the attitude error; />
Figure QLYQS_24
Representing a state transition sub-matrix corresponding to the position error; />
Figure QLYQS_25
Represents a zero matrix of 6 rows and 15 columns, in which case>
Figure QLYQS_26
Is expressed as follows:
Figure QLYQS_27
wherein ,
Figure QLYQS_28
representing the rotational angular velocity of the earth system relative to the inertial system; />
Figure QLYQS_29
Representing a rotational angular velocity of the navigation system relative to the earth system; />
Figure QLYQS_30
Representing the earth gravity information under the navigation coordinate system; />
Figure QLYQS_31
Representing a posture transfer matrix from a carrier coordinate system to a navigation coordinate system; symbol->
Figure QLYQS_32
An antisymmetric matrix representing the matrix;
Figure QLYQS_33
is expressed as follows:
Figure QLYQS_34
wherein ,
Figure QLYQS_35
intermediate variables defined only for convenience of expression do not represent any meaning, and,
Figure QLYQS_36
are respectively:
Figure QLYQS_37
Figure QLYQS_38
Figure QLYQS_39
Figure QLYQS_40
representing the angular rate of rotation of the earth;Landhrespectively representing the geographical latitude and the height of the position of the carrier;R M andR N respectively representing the radius of a meridian and the radius of a prime unit at the position of the carrier;
Figure QLYQS_41
the expression is as follows:
Figure QLYQS_42
wherein ,
Figure QLYQS_43
intermediate variables defined only for convenience of presentation and not representing any meaning, and +>
Figure QLYQS_44
Are respectively: />
Figure QLYQS_45
Figure QLYQS_46
Then, a measurement equation of the SINS/DVL integrated navigation system is given:
Figure QLYQS_47
wherein ,
Figure QLYQS_48
representing a measurement transfer matrix;/>
Figure QLYQS_49
representing a measurement noise matrix; />
Figure QLYQS_50
The measurement matrix is expressed as follows:
Figure QLYQS_51
wherein ,
Figure QLYQS_52
representing a posture transfer matrix of the navigation system to the carrier system; />
Figure QLYQS_53
Three-dimensional velocity information under a vector system representing DVL output, measuring a transfer matrix ≧ or>
Figure QLYQS_54
Can be expressed as follows:
Figure QLYQS_55
。
3. the SINS/DVL navigation method based on particle swarm optimization robust filtering according to claim 2, wherein the step 2 specifically comprises the following steps:
the filter state update is performed as follows:
Figure QLYQS_56
wherein ,
Figure QLYQS_57
to representkA combined navigation state transition matrix of the time; />
Figure QLYQS_58
To representk-a combined navigation state vector estimate at time 1, -a->
Figure QLYQS_59
Representing a one-step predicted state vector;
the state vector mean square error matrix is updated as follows:
Figure QLYQS_60
wherein ,
Figure QLYQS_61
a mean square error matrix representing the state vector; />
Figure QLYQS_62
To representkThe system noise variance matrix at a time @>
Figure QLYQS_63
Representing a one-step predictive covariance matrix, superscriptTRepresents a transpose of a matrix;
based on Huber M estimation principle, measurement noise variance array is constructed
Figure QLYQS_64
：
Figure QLYQS_65
wherein ,
Figure QLYQS_66
representing the measured noise variance matrix.
4. The SINS/DVL navigation method based on particle swarm optimization robust filtering according to claim 3, wherein the step 3 specifically comprises the following steps:
first fitness function constructs
Array of system noise variancesQ k Sum measure noise variance matrixS k As particles, the performance of the SINS/DVL combined navigation system depends mainly on the position accuracy, so the mean square error of the position error of the filter output is taken as the fitness function:
Figure QLYQS_67
wherein ,f（Q k , S k ) Is thatQ k , S k The mean square error of the position error of the SINS/DVL integrated navigation system is taken as a parameter;
Figure QLYQS_68
is an estimated position value;M to estimate the length; />
Figure QLYQS_69
Is the actual position value of the reference;
then, carrying out adaptive particle swarm optimization, specifically comprising the following processes:
Figure QLYQS_70
initializing particle swarm parameters, initializing the position and the speed of the particle swarm in a parameter interval, randomly generating a plurality of particles and initializing optimal particles;
Figure QLYQS_71
evaluating the fitness of each particle according to the fitness function, comparing the fitness with the fitness value of the optimal particle, and replacing the optimal particle if the fitness is better;
Figure QLYQS_72
particlest=（Q k ,S k ) Iteratively updating the speed and the position of each particle, adjusting the track of each particle to approach to the optimal point according to the experience of each particle and the group, and updating according to the following equation:
Figure QLYQS_73
Figure QLYQS_74
in the formula ,Gthe current iteration number is;
Figure QLYQS_76
are particlestIn the first placeGPosition of generation->
Figure QLYQS_79
Are particlestIn the first placeGPosition in the +1 passage +>
Figure QLYQS_81
Are particlestIn the first placeGSpeed of +1 generation; />
Figure QLYQS_77
Are particlestFirst, theGBest position searched by generation; />
Figure QLYQS_78
For the whole particle swarmGThe best position searched up to the generation, <' > or>
Figure QLYQS_80
Is the inertial weight; />
Figure QLYQS_82
Is a learning factor; />
Figure QLYQS_75
Is 0 toA random number between 1;
the improvement to the inertial weight is as follows:
Figure QLYQS_83
wherein ,u start andu end the initial value and the final value of the inertia weight are respectively, and the value range is as follows: 0.4-0.4 ≤u end ≤u start ≤0.9；
Figure QLYQS_84
Is the maximum value of the iteration times;
Figure QLYQS_85
repeat->
Figure QLYQS_86
and />
Figure QLYQS_87
And updating the optimal particles until the iteration times or the precision requirement is met, and generating the optimal particles.
5. The SINS/DVL navigation method based on particle swarm optimization robust filtering according to claim 4, wherein the step 4 specifically comprises the following steps:
and (3) performing filtering measurement updating:
Figure QLYQS_88
wherein ,
Figure QLYQS_89
to representkThe status vector of the moment in time>
Figure QLYQS_90
To representkThe measured vector at the moment is greater or less than>
Figure QLYQS_91
Representk-a state estimation weight at time 1; />
Figure QLYQS_92
Intermediate variables, which are defined for convenience only and do not represent any meaning, are specifically identified below:
Figure QLYQS_93
Figure QLYQS_94
Figure QLYQS_95
the values of each element in the weight matrix are as follows:
Figure QLYQS_96
wherein ,
Figure QLYQS_97
representing a symbolic function; />
Figure QLYQS_98
A representation factor;
the mean square error matrix is updated as follows:
Figure QLYQS_99
wherein ,
Figure QLYQS_100
an identity matrix representing 15 rows and 15 columns;/>
Figure QLYQS_101
representkThe filter gain at that moment->
Figure QLYQS_102
To representkThe covariance matrix of the moment, < > >>
Figure QLYQS_103
To representkThe measurement of the time shift matrix->
Figure QLYQS_104
To representkMeasurement of the first in the vector matrix of the time of dayiThe number of the elements is one,
Figure QLYQS_105
to representkThe first in the time matrix AiElement(s) is/are present>
Figure QLYQS_106
To representkThe measurement of the time shifts the matrix. />
CN202310069689.2A 2023-02-07 2023-02-07 SINS/DVL navigation method based on particle swarm optimization robust filtering Active CN115855049B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310069689.2A CN115855049B (en) 2023-02-07 2023-02-07 SINS/DVL navigation method based on particle swarm optimization robust filtering

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310069689.2A CN115855049B (en) 2023-02-07 2023-02-07 SINS/DVL navigation method based on particle swarm optimization robust filtering

Publications (2)

Publication Number Publication Date
CN115855049A true CN115855049A (en) 2023-03-28
CN115855049B CN115855049B (en) 2023-05-12

Family

ID=85657690

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310069689.2A Active CN115855049B (en) 2023-02-07 2023-02-07 SINS/DVL navigation method based on particle swarm optimization robust filtering

Country Status (1)

Country Link
CN (1) CN115855049B (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116255988A (en) * 2023-05-11 2023-06-13 北京航空航天大学 Composite anti-interference self-adaptive filtering method based on ship dynamics combined navigation
CN116295538A (en) * 2023-05-24 2023-06-23 河海大学 USBL (universal serial bus) installation error calibration method based on improved particle filtering

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111024064A (en) * 2019-11-25 2020-04-17 东南大学 SINS/DVL combined navigation method for improving Sage-Husa adaptive filtering
CN112556697A (en) * 2020-12-08 2021-03-26 江苏科技大学 Shallow coupling data fusion navigation method based on federated structure
WO2022028286A1 (en) * 2020-08-04 2022-02-10 东南大学 Motion constraint-aided underwater integrated navigation method employing improved sage-husa adaptive filtering
CN114459477A (en) * 2022-03-09 2022-05-10 东南大学 Improved PSO-ANFIS-assisted SINS/DVL tightly combined navigation method

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111024064A (en) * 2019-11-25 2020-04-17 东南大学 SINS/DVL combined navigation method for improving Sage-Husa adaptive filtering
WO2022028286A1 (en) * 2020-08-04 2022-02-10 东南大学 Motion constraint-aided underwater integrated navigation method employing improved sage-husa adaptive filtering
CN112556697A (en) * 2020-12-08 2021-03-26 江苏科技大学 Shallow coupling data fusion navigation method based on federated structure
CN114459477A (en) * 2022-03-09 2022-05-10 东南大学 Improved PSO-ANFIS-assisted SINS/DVL tightly combined navigation method

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
CHENGCHENG LIANG等: "SINS/DVL Integrated Based on Interacting Multiple Model Approach by Integrating Feedback Particle Filter for ship navigation" *
徐晓苏;董亚;童金武;代维;: "基于5阶球面最简相径的改进型容积卡尔曼滤波在SINS/DVL组合导航中的应用" *
王博;刘泾洋;刘沛佳;: "SINS/DVL组合导航技术综述" *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116255988A (en) * 2023-05-11 2023-06-13 北京航空航天大学 Composite anti-interference self-adaptive filtering method based on ship dynamics combined navigation
CN116295538A (en) * 2023-05-24 2023-06-23 河海大学 USBL (universal serial bus) installation error calibration method based on improved particle filtering

Also Published As

Publication number Publication date
CN115855049B (en) 2023-05-12

Similar Documents

Publication Publication Date Title
CN106871928B (en) Strap-down inertial navigation initial alignment method based on lie group filtering
CN112097763B (en) Underwater vehicle combined navigation method based on MEMS IMU/magnetometer/DVL combination
CN104655131B (en) Inertial navigation Initial Alignment Method based on ISTSSRCKF
CN115855049A (en) SINS/DVL navigation method based on particle swarm optimization robust filtering
CN111024064B (en) SINS/DVL combined navigation method for improving Sage-Husa adaptive filtering
CN110579740A (en) unmanned ship integrated navigation method based on adaptive federal Kalman filtering
CN110779518B (en) Underwater vehicle single beacon positioning method with global convergence
CN110514203B (en) Underwater integrated navigation method based on ISR-UKF
CN110231029B (en) Underwater robot multi-sensor fusion data processing method
CN105509739A (en) Tightly coupled INS/UWB integrated navigation system and method adopting fixed-interval CRTS smoothing
CN105424036A (en) Terrain-aided inertial integrated navigational positioning method of low-cost underwater vehicle
CN113834483B (en) Inertial/polarization/geomagnetic fault-tolerant navigation method based on observability degree
CN112284384A (en) Cooperative positioning method of clustered multi-deep-sea submersible vehicle considering measurement abnormity
CN104374405A (en) MEMS strapdown inertial navigation initial alignment method based on adaptive central difference Kalman filtering
CN105547300A (en) All-source navigation system and method used for AUV (Autonomous Underwater Vehicle)
CN111207773B (en) Attitude unconstrained optimization solving method for bionic polarized light navigation
CN108508463B (en) Fourier-Hermite orthogonal polynomial based extended ellipsoid collective filtering method
CN111323049B (en) Coarse alignment method of particle swarm algorithm
CN111156986A (en) Spectrum red shift autonomous integrated navigation method based on robust adaptive UKF
CN110221331B (en) Inertia/satellite combined navigation dynamic filtering method based on state transformation
CN111964675A (en) Intelligent aircraft navigation method for blackout area
CN111722295A (en) Underwater strapdown gravity measurement data processing method
CN107944467A (en) A kind of vehicle-mounted MIMUs/GPS information fusion methods and system of Adaboost optimizations
CN110873577B (en) Underwater rapid-acting base alignment method and device
CN110375773B (en) Attitude initialization method for MEMS inertial navigation system

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