CN115265532A - Auxiliary filtering method for marine integrated navigation - Google Patents

Auxiliary filtering method for marine integrated navigation Download PDF

Info

Publication number
CN115265532A
CN115265532A CN202210898321.2A CN202210898321A CN115265532A CN 115265532 A CN115265532 A CN 115265532A CN 202210898321 A CN202210898321 A CN 202210898321A CN 115265532 A CN115265532 A CN 115265532A
Authority
CN
China
Prior art keywords
error
ship
neural network
navigation system
ins
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202210898321.2A
Other languages
Chinese (zh)
Inventor
李伟峰
史国友
王庆武
马麟
刘姿含
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Dalian Maritime University
Original Assignee
Dalian Maritime 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 Dalian Maritime University filed Critical Dalian Maritime University
Priority to CN202210898321.2A priority Critical patent/CN115265532A/en
Publication of CN115265532A publication Critical patent/CN115265532A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

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

Abstract

The invention discloses an auxiliary filtering method for marine integrated navigation, which comprises the following steps: s1, when a GPS signal in an INS/GPS integrated navigation system is interrupted, establishing an error state model of an inertial navigation system; s2: establishing a state equation of a marine INS/GPS integrated navigation system; s3: establishing a radial basis function neural network model, optimizing the radial basis function neural network module through a particle swarm algorithm, and obtaining a nonlinear relation between a ship track obtained through an inertial navigation system and a real ship track; s4: and acquiring the position, speed and attitude angle of the ship obtained by the inertial navigation system when the GPS signal is interrupted. The method predicts the ship position, the ship speed and the attitude angle output by the INS/GPS integrated navigation when the navigation is unlocked due to the interruption of the GPS signal, trains the stability and can effectively improve the resolving precision of the navigation.

Description

Auxiliary filtering method for marine integrated navigation
Technical Field
The invention relates to the technical field of navigation, in particular to an auxiliary filtering method for marine integrated navigation.
Background
In marine integrated navigation, the combined mode of Inertial Navigation (INS) and Global Positioning System (GPS) structures is most widely applied, and has the advantages of high precision, low cost and all-weather work. However, under severe marine weather conditions, the calculation accuracy of the GPS receiver will be sharply reduced due to electromagnetic interference, shielding of a receiving antenna, and the like, and filtering is divergent, so that the combined navigation performance operating in the pure inertial navigation mode is seriously deteriorated.
Disclosure of Invention
The invention provides an auxiliary filtering method for marine integrated navigation, which aims to overcome the technical problem.
In order to achieve the purpose, the technical scheme of the invention is as follows:
an auxiliary filtering method used in marine integrated navigation comprises the following steps:
s1, when a GPS signal in an INS/GPS integrated navigation system is interrupted, establishing an error state model of an inertial navigation system, wherein the error state model comprises a position error state equation, a speed error state equation and an attitude error state equation;
s2: establishing a state equation of a marine INS/GPS integrated navigation system to obtain a marine INS/GPS integrated navigation system observation equation according to the state equation of the marine INS/GPS integrated navigation system;
s3: establishing a radial basis function neural network model, optimizing the radial basis function neural network module through a particle swarm algorithm, obtaining the optimized radial basis function neural network model, and obtaining a nonlinear relation between a ship track obtained through an inertial navigation system and a ship real track through the optimized radial basis function neural network model and a ship INS/GPS combined navigation system observation equation;
s4: and acquiring the ship position, the ship speed and the attitude angle obtained by the inertial navigation system when the GPS signal is interrupted according to the nonlinear relation between the ship track and the ship real track and the error state model of the inertial navigation system.
Further, the error state model of inertial navigation is established in S1 as follows:
Figure BDA0003769908930000021
in the formula:
Figure BDA0003769908930000022
the error vectors of the ship in the latitude, longitude and altitude directions are shown, wherein delta L is the error of the ship in the latitude direction, delta lambda is the error of the ship in the longitude direction, and delta h is the error of the ship in the altitude direction;
Figure BDA0003769908930000023
is the speed error vector of the ship,
Figure BDA0003769908930000024
is a ship attitude error vector;
Figure BDA0003769908930000025
error vectors of a gyroscope in an inertial measurement unit in three axes;
Figure BDA0003769908930000026
error vectors of an accelerometer in the inertial measurement unit in three axes;
the position error state equation is established as follows:
Figure BDA0003769908930000027
in the formula: delta L is the geodetic latitude error of the ship; δ λ is the geodetic longitude error of the ship; δ h is the geodetic height error of the ship; rMIs the radius of curvature of the meridian of the earth; r isNThe curvature radius of the earth fourth prime circle is; delta vnIs the speed of northAn error; delta veIs east speed error; delta vdIs the ground direction velocity error; h is the height of the ship; λ is the geodetic longitude of the vessel;
Figure BDA0003769908930000028
is a derivation operation;
the velocity error state equation is established as follows:
Figure BDA0003769908930000031
in the formula, L is the geodetic latitude of the ship; v. ofnIs the north velocity; v. ofeEast speed; v. ofdIs the ground speed; delta psi is a course angle error, delta theta is a pitch angle error, and delta gamma is a roll angle error; c11…C33For the attitude transfer matrix component, fdIs ground specific force; f. ofnA north specific force; omegaeEast angular velocity; δ fxIs the specific force error of the accelerometer along the x-axis; δ fyThe specific force error of the accelerometer along the y-axis; δ fzIs the specific force error of the accelerometer along the z-axis;
establishing the attitude error state equation as follows:
Figure BDA0003769908930000032
in the formula: delta omegaxMeasuring angular rate error for the x-axis of the gyroscope; delta omegayMeasuring an angular rate error for a y-axis of the gyroscope; delta omegazAngular rate error is measured for the x-axis of the gyroscope.
Further, the step of establishing the marine INS/GPS integrated navigation system state equation in S2 is as follows:
establishing a state equation of the marine INS/GPS integrated navigation system can be written as:
Figure BDA0003769908930000041
in the formula: f (-) is a non-linear continuous function, w (t) is the sum of accelerometer noise and gyroscope noise; x is a navigation system state variable; t is time;
the state equation of the marine INS/GPS integrated navigation system is discretized to obtain:
xk＝F(tk-tk-1)xk-1+wk-1 (6)
in the formula: f is a state transition matrix, wk-1White gaussian noise associated with the inertial sensor; t is tkThe operation time of k time after dispersion; x is the number ofk-1Is a state variable at the moment of k-1; k is the moment after dispersion;
the discrete form of the observation equation of the marine INS/GPS integrated navigation system is established as follows:
zk＝Hkxk+vk (7)
in the formula: hkIs an observation matrix of discrete k times, VkTo measure a noise vector; z is a radical ofkThe difference between the position, the speed and the attitude angle output by the inertial navigation and the position, the speed and the attitude angle output by the GNSS navigation;
the observation matrix H is specifically represented as follows:
Figure BDA0003769908930000042
wherein, I3×3Is a third order identity matrix.
Further, in S3, the radial basis function neural network model is established as follows:
the activation function for building the radial basis function neural network model is as follows:
Figure BDA0003769908930000043
in the formula: x is an input vector, oiIs the central parameter of the ith function, i is the number of the function in the hidden layer; sigmaiIs the variance of the ith function;
at this time, the output of the ith neuron of the hidden layer is:
Figure BDA0003769908930000051
wherein, | | · | | is a european norm;
the output of the radial basis function neural network model is:
Figure BDA0003769908930000052
in the formula of omegaijThe adjustment weight value from the ith neuron of the hidden layer to the jth neuron of the output layer is shown.
Further, o in the radial basis function neural network modeliAnd ωijRespectively as the speed and position of the particle swarm in the particle swarm optimization algorithm
And in the S3, the radial basis function neural network model is optimized through a particle swarm algorithm to establish:
Figure BDA0003769908930000053
in the formula:
Figure BDA0003769908930000054
is the velocity of the lambda particle in the d dimension at the gamma +1 iteration;
Figure BDA0003769908930000055
is the position of the lambada particle in the d-dimension at the gamma iteration;
Figure BDA0003769908930000056
the position of an individual extreme value of the lambada particle in the d dimension at the gamma iteration is taken as the position of the lambada particle in the gamma iteration;
Figure BDA0003769908930000057
setting the position of the global extreme value of the d-dimension as the particle swarm; rand () is a random number generated within the interval (0, 1), c1A non-negative acceleration constant that is a maximum step size for adjusting flight in the pbest direction; c. C2Determining the effect of group experience on the particle trajectory in order to adjust a non-negative acceleration constant of the maximum step length of the flight towards the gbest direction; w is aλA non-negative inertia factor for the λ -th particle; gamma is the iteration number of the particle swarm optimization algorithm; lambda is the particle number of the particle swarm optimization algorithm; d is the dimension of the search space;
the updating formula of the self speed and position of particle swarm optimization is as follows:
Figure BDA0003769908930000058
Figure BDA0003769908930000059
wherein the content of the first and second substances,
Figure BDA00037699089300000510
is the position of the lambda particle in the d-dimension at the gamma iteration.
Has the beneficial effects that: according to the auxiliary filtering method for the marine integrated navigation, the nonlinear relation between the ship track obtained by the inertial navigation system and the real ship track is established by establishing an error state model of the inertial navigation system when a GPS signal in an INS/GPS integrated navigation system is interrupted and a radial basis function neural network optimized based on a particle swarm optimization, the ship position, the ship speed and the attitude angle output by the INS/GPS integrated navigation when the navigation is unlocked due to the interruption of the GPS signal are predicted, the stability is trained, and the resolving precision of the navigation can be effectively improved.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings needed to be used in the description of the embodiments or the prior art will be briefly introduced below, and it is obvious that the drawings in the following description are some embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to these drawings without creative efforts.
FIG. 1 is a flow chart of the marine integrated navigation assistance filtering method of the present invention;
FIG. 2 is a diagram of a topology of an RBF neural network according to an embodiment of the present invention;
FIG. 3 is a flow chart of neural network assisted filtering in an embodiment of the present invention;
FIG. 4 is a graph comparing longitude error curves of BP neural network, RBF neural network, PSO-RBF neural network assisted filtering in an embodiment of the present invention;
FIG. 5 is a comparison graph of latitude error curves of auxiliary filtering of the BP neural network, the RBF neural network and the PSO-RBF neural network in an embodiment of the present invention;
FIG. 6 is a diagram comparing east velocity error curves of BP neural network, RBF neural network, PSO-RBF neural network assisted filtering in an embodiment of the present invention;
FIG. 7 is a comparison graph of error curves of the north velocity assisted filtering of the BP neural network, the RBF neural network, and the PSO-RBF neural network in the embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are some, but not all, embodiments of the present invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
The embodiment provides an auxiliary filtering method for marine integrated navigation, as shown in fig. 1, including the following steps:
s1, when a GPS signal in an INS/GPS integrated navigation system is interrupted, establishing an error state model of an inertial navigation system, wherein the error state model comprises a position error state equation, a speed error state equation and an attitude error state equation;
establishing an error state model of Inertial Navigation (INS) in S1 as follows:
in this embodiment, because the tight-coupling integrated navigation structure using the pseudo range and the pseudo range rate as the marine INS/GPS integrated navigation measurement vector cannot suppress the filtering divergence when the GPS satellite has no output, and has the disadvantages of large calculation amount and complex design, the loose-coupling closed-loop structure that is easier to implement is selected in this embodiment. The combined navigation coordinate system adopts a local geographical coordinate system (NED), the carrier coordinate system is a 'front-right-lower' coordinate system of the advancing direction of the ship, and the difference between the positions output by the INS and the GPS is taken as an observed quantity. Adopting a geographical coordinate system (NED) as a navigation coordinate system, and establishing a position error state equation, a speed error state equation and an attitude error state equation of the inertial navigation system;
error state variables are used instead of the normal state vector:
Figure BDA0003769908930000071
in the formula:
Figure BDA0003769908930000072
error vectors of the ship in three directions of latitude, longitude and altitude are provided, wherein δ L is an error of the ship in the latitude direction, δ λ is an error of the ship in the longitude direction, and δ h is an error of the ship in the altitude direction;
Figure BDA0003769908930000073
the speed error vector of the ship is provided, and the components of the speed error vector are errors in three directions under a local horizontal coordinate system respectively;
Figure BDA0003769908930000074
is a ship attitude error vector;
Figure BDA0003769908930000075
for gyroscopes in inertial measurement unitsError vectors of three axes;
Figure BDA0003769908930000076
error vectors of an accelerometer in the inertial measurement unit in three axes;
the position error state equation is established as follows:
Figure BDA0003769908930000081
in the formula: delta L is the geodetic latitude error of the ship; δ λ is the geodetic longitude error of the ship; δ h is the geodetic height error of the ship; rMIs the radius of curvature of the meridian of the earth; rNThe curvature radius of the earth fourth prime circle is; delta vnIs the north velocity error; delta veEast-direction velocity error; delta vdIs the ground direction velocity error; h is the height of the ship; λ is the geodetic longitude of the vessel;
Figure BDA0003769908930000082
is a derivation operation;
the speed error state equation is established as follows:
Figure BDA0003769908930000083
in the formula, L is the geodetic latitude of the ship; v. ofnIs the north velocity; v. ofeEast speed; v. ofdIs the ground speed; delta psi is a course angle error, delta theta is a pitch angle error, and delta gamma is a roll angle error; c11…C33For the attitude transfer matrix component, fdIs ground specific force; f. ofnNorth direction specific force; omegaeEast angular velocity; δ fxSpecific force errors distributed along the x-axis for the accelerometer; δ fyThe specific force error of the accelerometer along the y-axis; δ fzA specific force error for the accelerometer along the z-axis, wherein the specific force error model assumes a first order Markov process;
the attitude error state equation is established as follows:
Figure BDA0003769908930000091
in the formula: delta omegaxMeasuring angular rate error for the x-axis of the gyroscope; delta omegayMeasuring an angular rate error for a y-axis of the gyroscope; delta omegazMeasuring angular rate error for the x-axis of the gyroscope; wherein the gyroscope angular rate error model assumes a first order Markov process;
s2: taking the difference between the position output by the GPS before the interruption of the GPS signal and the position output by the INS as an observed quantity, selecting a proper feedback structure and an inertial navigation parameter correction mode according to needs, and fusing navigation information of the INS and the GPS by using a Kalman filter, namely the ship speed, the ship position and the attitude angle output by the INS and the GPS so as to establish a state equation of the marine INS/GPS integrated navigation system and obtain an observation equation of the marine INS/GPS integrated navigation system according to the state equation of the marine INS/GPS integrated navigation system;
the steps of the state equation of the marine INS/GPS integrated navigation system established in the S2 are as follows:
establishing a state equation of the marine INS/GPS integrated navigation system can be written as:
Figure BDA0003769908930000092
in the formula: f (-) is a non-linear continuous function, w (t) is the sum of accelerometer noise and gyroscope noise; x is a navigation system state variable; t is time;
discretizing the state equation of the marine INS/GPS integrated navigation system to obtain the following result:
xk＝F(tk-tk-1)xk-1+wk-1 (6)
in the formula: f is a state transition matrix, wk-1White gaussian noise associated with the inertial sensor; t is tkThe operation time of k time after dispersion; x is the number ofk-1Is a state variable at the moment of k-1; k is the moment after dispersion;
specifically, the present embodiment uses the position of the GPS navigation, the acceleration obtained by the accelerometer in the INS, and the angular velocity of the gyroscope as corresponding measurement vectors.
Since the kalman filter is linear, discretization processing is required for use. Therefore, the discrete form of the ship INS/GPS integrated navigation system observation equation is established as follows:
zk＝Hkxk+vk (7)
in the formula: hkIs an observation matrix of discrete k times, VkTo measure the noise vector. z is a radical ofkThe difference between the position, the speed and the attitude angle output by the inertial navigation and the position, the speed and the attitude angle output by the GNSS navigation;
h is an observation matrix, which is specifically represented as follows:
Figure BDA0003769908930000101
wherein, I3×3Is a third order identity matrix.
S3: establishing a radial basis function neural network model, optimizing the radial basis function neural network module through a particle swarm algorithm, obtaining the optimized radial basis function neural network model, obtaining a nonlinear relation between a ship track obtained through an inertial navigation system and a ship real track through the optimized radial basis function neural network model and a ship INS/GPS combined navigation system observation equation, and correcting a navigation result of inertial navigation;
in S3, establishing a RBF neural network model (radial basis function neural network model) to construct a nonlinear relation between a ship track obtained by an inertial navigation system and a ship real track, training the RBF neural network model by using data of a GPS during normal work,
specifically, the RBF radial basis function neural network model is established as follows: the structure of the constructed RBF neural network is shown in FIG. 2.
The RBF neural network model is output by signal source nodes (namely, ships output by inertial navigation)Location of (d) of the input layer; the number of nodes of the input layer, the hidden layer and the output layer is n, m and l respectively. Wherein an input vector of an input layer of the RBF neural network model is X = [ X ]1.x2,…,xn]TThe output vector of the output layer is Y = [ Y1.y2,…,yl]T。
Each hidden layer unit of the RBF neural network model is used for realizing a radial basis function. The value of a certain point of the radial basis function is only related to the distance between the input vector and the central point of the neuron of the hidden layer, and the value responds to the input locally. When the distance between the input and the center of the radial basis function is close, the hidden layer outputs a larger value; when the input is further from the center, the output decays exponentially. The radial basis function usually employs a gaussian function as the activation function, from which it follows:
Figure BDA0003769908930000111
in the formula: x is an input vector, oiThe central parameter of the ith function is the difference value of the central parameter and the central parameter of the ith function, the difference value is the distance from the input point to the center of each hidden layer, and i is the serial number of the function in the hidden layer; sigmaiThe variance (i.e., the path base width) of the ith function, which determines the descent rate of the function, the greater the degree of smoothing between neurons;
at this point, the output of the ith neuron of the hidden layer can be written as: wherein the neurons are configured to implement an ith activation function;
Figure BDA0003769908930000112
wherein, | | · | | is a euclidean norm.
The output layer neurons perform linear weighting on the hidden layer output, and then the output of the output layer is:
Figure BDA0003769908930000113
in the formula of omegaijThe adjustment weight value from the ith neuron of the hidden layer to the jth neuron of the output layer is shown.
And immediately storing training input information and a training target of the neural network as a training sample. Equation (11) is used as the input of the RBF neural network model, and the predicted ship speed and ship position after processing by the RBF neural network model are used as the output of the neural network.
There is no very mature theory for neural network parameter selection. The method is generally selected empirically, so that the improvement of the neural network structure and parameters still has important significance and research value. In this embodiment: optimizing the radial basis function neural network model through a particle swarm algorithm, and obtaining the optimized radial basis function neural network model so as to obtain a nonlinear relation between a ship track obtained through an inertial navigation system and a real ship track;
the method specifically comprises the following steps of optimizing RBFNN by a particle swarm algorithm: the mathematical description of the particle swarm algorithm is: in the a-dimensional search space, assume that there is a particle group e = (e) containing b particles1,e2,…,eb)TThe current position of the lambda particle is eλ＝(eλ,1,eλ,2,…，eλ,a)TThe current velocity of the particle is vλ＝(vλ,1,vλ,2,…，vλ,a)T. A fitness formula of the particle swarm is as follows:
Figure BDA0003769908930000121
in the formula:
Figure BDA0003769908930000122
the velocity of the lambda-th particle in the d-dimension at the gamma + 1-th iteration is obtained;
Figure BDA0003769908930000123
for the lambda-th particle at the gammaThe position of the d-dimension in the secondary iteration;
Figure BDA0003769908930000124
is the position of the individual extremum of the lambda particle in the d dimension at the gamma iteration;
Figure BDA0003769908930000125
setting the position of the global extreme value of the d-dimension as the particle swarm; rand () is a random number generated within the interval (0, 1), c1Determining the effect of individual experience on the particle trajectory for adjusting a non-negative acceleration constant of a maximum step length for flight in the pbest direction; c. C2Determining the effect of group experience on the particle trajectory in order to adjust a non-negative acceleration constant of the maximum step length of the flight towards the gbest direction; w is aλThe non-negative inertia factor of the lambda-th particle is obtained, and when the value is large, the global optimization capability is strong, and the local optimization capability is weak; gamma is the iteration number of the particle swarm optimization algorithm; lambda is the particle number of the particle swarm optimization algorithm;
wherein, in the present embodiment, o in the radial basis function neural network model is definediAnd omegaijRespectively as parameters in a particle swarm optimization algorithm, wherein o isiAs position of the particle swarm, ωijAs the velocity of the particle population;
determining its individual optimum (pbest) as p by calculating fitness valueλ＝(pλ,1,pλ,2,…，pλ,a)TThe global optimum (gbest) of the particle population is pg＝(pg,1,pg,2,…，pg,a)T. After the values are found, the updating formula of the self speed and the position of the population particle optimization is as follows:
Figure BDA0003769908930000126
Figure BDA0003769908930000127
wherein the content of the first and second substances,
Figure BDA0003769908930000128
the position of the lambada particle in the d dimension at the gamma iteration is determined;
the method for optimizing the radial basis function neural network model by the particle swarm optimization comprises the following steps:
s31: normalizing the sample data;
s32: initializing the radial basis function neural network model with the parameter, oi，σiForming particles, assigning random values, and respectively initializing the position and speed of the particle swarm;
s33: after the input and the output of the radial basis function neural network are obtained, calculating the fitness value of the particle swarm according to the fitness of the particle swarm so as to determine an individual extremum in the particle swarm and a global extremum of the particle swarm;
s34: calculating the adaptability value of the lambda-th particle, and storing the adaptability value of the lambda-th particle and the position of the lambda-th particle in the individual extreme value of the lambda-th particle
Figure BDA0003769908930000131
In (1), all
Figure BDA0003769908930000132
The position of the individual with the best fitness value and the fitness value in the population extreme value are stored in the population group
Figure BDA0003769908930000133
Performing the following steps;
s35: updating the position and the speed of the particle of the population to generate a new particle swarm;
s36: for single particles forming the particle swarm, the fitness value of the single particles is compared with the individual with the highest fitness value, and the individual with the highest fitness value is selected as the most updated individual extreme value;
s37: compare all current
Figure BDA0003769908930000134
And
Figure BDA0003769908930000135
to update
Figure BDA0003769908930000136
S38: judging whether an optimization target is achieved or whether the maximum training times are met, and if a termination condition is met, finishing the algorithm; otherwise, return to S43.
S4: and acquiring the position, the speed and the attitude angle of the ship obtained by the inertial navigation system when the GPS signal is interrupted according to the nonlinear relation between the ship track obtained by the inertial navigation system and the real ship track and an error state model of the inertial navigation system.
According to the method and the device, when the GPS is interrupted, the radial basis function neural network model is optimized through the particle swarm optimization, and the inertial navigation system INS is assisted to output the positioning parameters of the ship.
The overall neural network auxiliary filtering process in the S4 comprises the following steps:
the INS/GPS integrated navigation system based on PSO-RBF (particle swarm optimization radial basis function) assistance compensates the loss of GPS signals through the self-learning capability and the local approximation capability of the PSO-RBF neural network, and mainly comprises two working modes, namely PSO-RBF neural network training and PSO-RBF neural network prediction.
When the GPS receiver normally operates, the system works in a PSO-RBF neural network training mode, and training input information and a training target of the PSO-RBF neural network are immediately stored to be used as training samples. The equation (11) is used as the input of the RBF neural network model, and the predicted speed and position after being processed by the RBF neural network model are used as the output of the RBF neural network model. The output expected by the PSO-RBF neural network is the corrected position, speed and attitude information input by Kalman filtering according to the output result of the formula (12) so as to complete network training. At the moment, the absolute value of the training error is used as the individual fitness value of the particle swarm, the smaller the fitness value is, the better the individual is, the error is fed back to the RBF neural network model for internal parameter adjustment, and finally, when the error of the RBF neural network model is the minimum or the expected purpose is achievedAnd (5) when the time is calibrated, terminating the training process. In order to reduce unnecessary cross coupling during training and improve the training speed of the neural network, the embodiment adopts four parallel RBF neural network models to respectively correspond to a position component (longitude and latitude) and a speed component (north direction speed v) in the measurement informationnAnd east velocity ve). Each neural network comprises four input layer neurons (a component of motion information provided by the INS and three specific force components) and one output layer neuron (ship position and speed predicted by the RBF neural network model).
When the GPS signal is interrupted, the inertial navigation system (INS navigation) still works normally at the moment, and the GPS navigation cannot provide a measurement vector for the Kalman filter. And inputting the ship position, the ship speed and the attitude angle output by an inertial navigation system (INS navigation), and optimizing the radial basis function neural network model by using a particle swarm algorithm for prediction to obtain the result of the PSO-RBF neural network after Kalman filtering correction. And optimizing the output value of the radial basis function neural network model through a particle swarm algorithm to replace the estimated value of Kalman filtering so as to ensure that the navigation still has enough precision. The PSO-RBF neural network aided flow is shown in FIG. 3.
Simulation and analysis
In order to verify the effectiveness of the algorithm of the embodiment, the motion condition of the marine vessel is simulated by means of an MATLAB platform, and the algorithm is simulated. The ship sails at the speed of 10m/s under the condition of three-level sea, and two turns are completed in the process, and the angle is 45 degrees. The simulated movement time is 300s totally, the first 260s is an INS/GPS integrated navigation Kalman filtering result, in order to simulate the signal interruption condition of a GPS receiver, the output of the GPS receiver is disconnected from the 260s, and the measurement information is not introduced into the integrated navigation, so that the effectiveness of the PSO-RBF neural network auxiliary method is verified. Meanwhile, by means of an MATLAB neural network toolbox, the situation that Kalman filtering is assisted by a BP neural network and a traditional RBF neural network is simulated and compared. The traditional RBF neural network center adopts a K-means algorithm for selection, and the number of hidden layer neurons of the BP neural network needs to be determined by utilizing the number of the hidden layer neurons of the RBF neural network. And determining the number of hidden layer neurons after the RBF neural network is trained to be 9 according to the mean square error condition and the training time predicted by multiple experiments, so that the BP neural network also adopts 9 hidden layer neurons. The neural network presets a training target of 0.00001 and the number of iterations is 1500.
In the simulation, the gyro drift and the speed and the position of the GPS receiver are regarded as Markov processes. The main parameter cases in the experiment are shown in table 1:
TABLE 1 Experimental parameters
Figure BDA0003769908930000151
Fig. 4 and 5 are longitude error and latitude error curves of the BP neural network, the RBF neural network and the PSO-RBF neural network assisted filtering respectively. As can be seen from fig. 4, in comparison with the RBF neural network in which the error value fluctuates sharply and the BP neural network in which the error result gradually diverges, the difference between the prediction result and the true value of the PSO-RBF neural network is relatively stable and always smaller than those of the other two methods, and a relatively good filtering effect is exhibited. In fig. 5, as the prediction error of the INS navigation is gradually accumulated along with the time, the errors of the three neural network assisted algorithms are expanded to different degrees along with the increase of the time, the RBF neural network and the BP neural network have obvious divergence trends, and the PSO-RBF neural network can better inhibit the filtering divergence by correcting, so that a better filtering result is ensured, and the trajectory of the filtering result is relatively close to the real trajectory.
Fig. 6 and 7 are east-direction speed error curves and north-direction speed error curves of the BP neural network, the RBF neural network and the PSO-RBF neural network for auxiliary filtering, respectively. As can be seen from the figure: when the BP neural network is adopted, the error fluctuation is severe, the initial error value is obviously larger than that of the RBF neural network and the PSO-RBF neural network, and the gradually stabilized error is still larger than that of the RBF and the PSO-RBF. When the RBF neural network is adopted, the error is stable, but the filtering effect is not as good as that of a PSO-RBF.
To further quantify the filtering accuracy of the three algorithms, the mean error and root mean square error of the position and velocity estimates during GPS interruption are given. As can be seen from table 2 in combination with the error curve, the overall accuracy of the BP assistance algorithm is inferior to that of the RBF assistance algorithm, which indicates that the RBF neural network with local approximation is more suitable for assisting navigation filtering; and the position and speed errors corresponding to the PSO-RBF are more stable than those of the first two neural network auxiliary algorithms, and the effectiveness of the method is fully proved.
TABLE 2 mean error and root mean square error of position and velocity during GPS interrupts
Figure BDA0003769908930000161
Finally, it should be noted that: the above embodiments are only used to illustrate the technical solution of the present invention, and not to limit the same; while the invention has been described in detail and with reference to the foregoing embodiments, it will be understood by those skilled in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some or all of the technical features may be equivalently replaced; and the modifications or the substitutions do not make the essence of the corresponding technical solutions depart from the scope of the technical solutions of the embodiments of the present invention.

Claims (5)

1. An auxiliary filtering method for marine integrated navigation, which is characterized by comprising the following steps:
s1, when a GPS signal in an INS/GPS integrated navigation system is interrupted, establishing an error state model of an inertial navigation system, wherein the error state model comprises a position error state equation, a speed error state equation and an attitude error state equation;
s2: establishing a state equation of a marine INS/GPS integrated navigation system to obtain a marine INS/GPS integrated navigation system observation equation according to the state equation of the marine INS/GPS integrated navigation system;
s3: establishing a radial basis function neural network model, optimizing the radial basis function neural network module through a particle swarm algorithm, obtaining the optimized radial basis function neural network model, and obtaining a nonlinear relation between a ship track obtained through an inertial navigation system and a ship real track through the optimized radial basis function neural network model and a ship INS/GPS combined navigation system observation equation;
s4: and acquiring the ship position, the ship speed and the attitude angle obtained by the inertial navigation system when the GPS signal is interrupted according to the nonlinear relation between the ship track and the ship real track and the error state model of the inertial navigation system.
2. The auxiliary filtering method for integrated navigation in ships according to claim 1, wherein the error state model for inertial navigation in S1 is as follows:
Figure FDA0003769908920000011
in the formula:
Figure FDA0003769908920000012
error vectors of the ship in three directions of latitude, longitude and altitude are provided, wherein δ L is an error of the ship in the latitude direction, δ λ is an error of the ship in the longitude direction, and δ h is an error of the ship in the altitude direction;
Figure FDA0003769908920000013
is the speed error vector of the ship,
Figure FDA0003769908920000014
is a ship attitude error vector;
Figure FDA0003769908920000015
error vectors of a gyroscope in an inertial measurement unit in three axes;
Figure FDA0003769908920000016
error vectors of an accelerometer in the inertial measurement unit in three axes;
the position error state equation is established as follows:
Figure FDA0003769908920000021
in the formula: delta L is the geodetic latitude error of the ship; δ λ is the geodetic longitude error of the ship; δ h is the ground height error of the ship; rMIs the radius of curvature of the meridian of the earth; rNThe curvature radius of the earth prime circle is; delta vnIs the north velocity error; delta veEast-direction velocity error; delta vdIs the ground direction velocity error; h is the height of the ship; λ is the geodetic longitude of the vessel;
Figure FDA0003769908920000022
is a derivation operation;
the velocity error state equation is established as follows:
Figure FDA0003769908920000023
in the formula, L is the geodetic latitude of the ship; v. ofnIs the north velocity; v. ofeEast speed; v. ofdIs the ground speed; delta psi is a course angle error, delta theta is a pitch angle error, and delta gamma is a roll angle error; c11…C33For the attitude transfer matrix component, fdIs ground specific force; f. ofnNorth direction specific force; omegaeEast angular velocity; δ fxIs the specific force error of the accelerometer along the x-axis; δ fyThe specific force error of the accelerometer along the y-axis; δ fzIs the specific force error of the accelerometer along the z-axis;
establishing the attitude error state equation as follows:
Figure FDA0003769908920000031
in the formula: delta omegaxMeasuring angular rate error for the x-axis of the gyroscope; delta omegayIs the y-axis quantity of the gyroscopeMeasuring an angular rate error; delta omegazAngular rate error is measured for the x-axis of the gyroscope.
3. The auxiliary filtering method for integrated marine navigation system according to claim 2, wherein the step of establishing the state equation of the integrated marine INS/GPS navigation system in S2 is as follows:
establishing a state equation of the marine INS/GPS integrated navigation system can be written as:
Figure FDA0003769908920000032
in the formula: f (-) is a non-linear continuous function, w (t) is the sum of accelerometer noise and gyroscope noise; x is a navigation system state variable; t is time;
the state equation of the marine INS/GPS integrated navigation system is discretized to obtain:
xk＝F(tk-tk-1)xk-1+wk-1 (6)
in the formula: f is a state transition matrix, wk-1White gaussian noise associated with the inertial sensor; t is tkThe operation time of k moment after dispersion; x is a radical of a fluorine atomk-1Is a state variable at the moment of k-1; k is the moment after dispersion;
the discrete form of the observation equation of the marine INS/GPS integrated navigation system is established as follows:
zk＝Hkxk+vk (7)
in the formula: hkAs an observation matrix of discrete k times, VkTo measure a noise vector; z is a radical ofkThe difference between the position, the speed and the attitude angle output by the inertial navigation and the position, the speed and the attitude angle output by the GNSS navigation;
the observation matrix H is specifically represented as follows:
Figure FDA0003769908920000041
wherein, I3×3Is a third order identity matrix.
4. The auxiliary filtering method for integrated marine navigation according to claim 3, wherein in S3, the radial basis function neural network model is established as follows:
the activation function for building the radial basis function neural network model is as follows:
Figure FDA0003769908920000042
in the formula: x is an input vector, oiIs the central parameter of the ith function, i is the number of the function in the hidden layer; sigmaiIs the variance of the ith function;
at this time, the output of the ith neuron of the hidden layer is:
Figure FDA0003769908920000043
wherein, | | · | | is a european norm;
the output of the radial basis function neural network model is:
Figure FDA0003769908920000044
in the formula of omegaijThe adjustment weight value from the ith neuron of the hidden layer to the jth neuron of the output layer is shown.
5. An auxiliary filtering method used in marine integrated navigation according to claim 4, characterized in that o in the radial basis function neural network model is usediAnd ωijRespectively as the speed and position of the particle swarm in the particle swarm optimization algorithm
And in the S3, the radial basis function neural network model is optimized through a particle swarm algorithm to establish:
Figure FDA0003769908920000051
in the formula:
Figure FDA0003769908920000052
is the velocity of the lambda particle in the d dimension at the gamma +1 iteration;
Figure FDA0003769908920000053
is the position of the lambada particle in the d-dimension at the gamma iteration;
Figure FDA0003769908920000054
is the position of the individual extremum of the lambda particle in the d dimension at the gamma iteration;
Figure FDA0003769908920000055
setting the position of the global extreme value of the d-dimension as the particle swarm; rand () is a random number generated within the interval (0, 1), c1A non-negative acceleration constant that is a maximum step size for adjusting flight in the pbest direction; c. C2Determining the effect of group experience on the particle trajectory in order to adjust a non-negative acceleration constant of the maximum step length of the flight towards the gbest direction; w is aλA non-negative inertia factor for the λ -th particle; gamma is the iteration number of the particle swarm optimization algorithm; lambda is the particle number of the particle swarm optimization algorithm; d is the dimension of the search space;
the updating formula of the particle swarm optimization speed and position is as follows:
Figure FDA0003769908920000056
Figure FDA0003769908920000057
wherein the content of the first and second substances,
Figure FDA0003769908920000058
is the position of the lambda particle in the d-dimension at the gamma iteration.
CN202210898321.2A 2022-07-28 2022-07-28 Auxiliary filtering method for marine integrated navigation Pending CN115265532A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210898321.2A CN115265532A (en) 2022-07-28 2022-07-28 Auxiliary filtering method for marine integrated navigation

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210898321.2A CN115265532A (en) 2022-07-28 2022-07-28 Auxiliary filtering method for marine integrated navigation

Publications (1)

Publication Number Publication Date
CN115265532A true CN115265532A (en) 2022-11-01

Family

ID=83772564

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210898321.2A Pending CN115265532A (en) 2022-07-28 2022-07-28 Auxiliary filtering method for marine integrated navigation

Country Status (1)

Country Link
CN (1) CN115265532A (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116047567A (en) * 2023-04-03 2023-05-02 长沙金维信息技术有限公司 Deep learning assistance-based guard and inertial navigation combined positioning method and navigation method
CN117346794A (en) * 2023-12-05 2024-01-05 山东省科学院海洋仪器仪表研究所 Unmanned ship integrated navigation system and navigation method for enteromorpha tracking

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116047567A (en) * 2023-04-03 2023-05-02 长沙金维信息技术有限公司 Deep learning assistance-based guard and inertial navigation combined positioning method and navigation method
CN117346794A (en) * 2023-12-05 2024-01-05 山东省科学院海洋仪器仪表研究所 Unmanned ship integrated navigation system and navigation method for enteromorpha tracking
CN117346794B (en) * 2023-12-05 2024-02-23 山东省科学院海洋仪器仪表研究所 Unmanned ship integrated navigation system and navigation method for enteromorpha tracking

Similar Documents

Publication Publication Date Title
CN110579740B (en) Unmanned ship integrated navigation method based on adaptive federal Kalman filtering
CN109724599B (en) Wild value resistant robust Kalman filtering SINS/DVL integrated navigation method
Dai et al. An INS/GNSS integrated navigation in GNSS denied environment using recurrent neural network
CN108827310B (en) Marine star sensor auxiliary gyroscope online calibration method
CN108844536B (en) Geomagnetic navigation method based on measurement noise covariance matrix estimation
CN115265532A (en) Auxiliary filtering method for marine integrated navigation
CN110906933B (en) AUV (autonomous Underwater vehicle) auxiliary navigation method based on deep neural network
CN112798021B (en) Inertial navigation system inter-travelling initial alignment method based on laser Doppler velocimeter
CN109945895B (en) Inertial navigation initial alignment method based on fading smooth variable structure filtering
Li et al. Underwater terrain-aided navigation system based on combination matching algorithm
CN111189442B (en) CEPF-based unmanned aerial vehicle multi-source navigation information state prediction method
CN114689047B (en) Deep learning-based integrated navigation method, device, system and storage medium
CN110567455A (en) tightly-combined navigation method for quadrature updating of volume Kalman filtering
Jameian et al. A robust and fast self-alignment method for strapdown inertial navigation system in rough sea conditions
Liu et al. Strong tracking UKF-based hybrid algorithm and its application to initial alignment of rotating SINS with large misalignment angles
CN116222551A (en) Underwater navigation method and device integrating multiple data
CN114777812A (en) Method for estimating alignment and attitude of underwater integrated navigation system during traveling
CN104634348B (en) Attitude angle computational methods in integrated navigation
CN110132287A (en) A kind of satellite high-precision joint method for determining posture based on extreme learning machine network building out
Liu et al. Navigation algorithm based on PSO-BP UKF of autonomous underwater vehicle
CN110375773B (en) Attitude initialization method for MEMS inertial navigation system
CN112325878A (en) Ground carrier combined navigation method based on UKF and air unmanned aerial vehicle node assistance
Zhou et al. Geomagnetic matching algorithm based on the probabilistic neural network
CN114111840B (en) DVL error parameter online calibration method based on integrated navigation
CN113503891B (en) SINSDVL alignment correction method, system, medium and equipment

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