An auxiliary filtering method for marine integrated navigation

Abstract

The present invention discloses an auxiliary filtering method for marine integrated navigation, comprising: S1: establishing an error state model of an inertial navigation system when the GPS signal in an INS / GPS integrated navigation system is interrupted; S2: establishing a state equation for the marine INS / GPS integrated navigation system; S3: establishing a radial basis function neural network model and optimizing the radial basis function neural network module using a particle swarm algorithm to obtain a nonlinear relationship between a ship's trajectory obtained by the inertial navigation system and the ship's actual trajectory; and S4: obtaining the ship's position, speed, and attitude angle obtained by the inertial navigation system when the GPS signal is interrupted. The present invention predicts the ship's position, speed, and attitude angle output by the INS / GPS integrated navigation system when the GPS signal is interrupted, trains stability, and can effectively improve navigation solution accuracy.

Description

Technical Field

[0001] The present invention relates to the field of navigation technology, and in particular to an auxiliary filtering method used in ship integrated navigation. Background Art

[0002] In marine integrated navigation, the combination of inertial navigation (INS) and global positioning system (GPS) is the most widely used, offering advantages such as high accuracy, low cost, and all-weather operation. However, in severe weather conditions at sea, the GPS receiver's accuracy can drop dramatically due to electromagnetic interference, shielding of the receiving antenna, and other factors, leading to filter divergence and severe degradation of the integrated navigation performance of the pure inertial navigation mode. Summary of the Invention

[0003] The present invention provides an auxiliary filtering method for ship integrated navigation to overcome the above technical problems.

[0004] In order to achieve the above object, the technical solution of the present invention is:

[0005] An auxiliary filtering method for marine integrated navigation comprises the following steps:

[0006] S1. When the GPS signal in the INS / GPS integrated navigation system is interrupted, an error state model of the inertial navigation system is established, wherein the error state model includes a position error state equation, a velocity error state equation, and an attitude error state equation;

[0007] S2: establishing a state equation of the ship INS / GPS integrated navigation system, so as to obtain an observation equation of the ship INS / GPS integrated navigation system according to the state equation of the ship INS / GPS integrated navigation system;

[0008] S3: establishing a radial basis function neural network model, and optimizing the radial basis function neural network module by using a particle swarm algorithm to obtain an optimized radial basis function neural network model, so as to obtain a nonlinear relationship between the ship trajectory obtained by the inertial navigation system and the actual trajectory of the ship by using the optimized radial basis function neural network model and the observation equation of the ship INS / GPS integrated navigation system;

[0009] S4: According to the nonlinear relationship between the ship trajectory and the actual ship trajectory and the error state model of the inertial navigation system, the ship position, ship speed and attitude angle obtained by the inertial navigation system when the GPS signal is interrupted are obtained.

[0010] Furthermore, the error state model of inertial navigation established in S1 is as follows:

[0011]

[0012] Where: is the error vector of the ship in the latitude, longitude and altitude directions, where δL is the error of the ship in the latitude direction, δλ is the error of the ship in the longitude direction, and δh is the error of the ship in the altitude direction; is the ship's speed error vector, is the ship attitude error vector; is the error vector of the gyroscope in the inertial measurement unit on three axes; is the error vector of the accelerometer in the inertial measurement unit in three axes;

[0013] The position error state equation is established as follows:

[0014]

[0015] Where: δ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; R M is the radius of curvature of the Earth's meridian; R N is the radius of curvature of the Earth's angular circle; δv n is the north velocity error; δv e is the eastward velocity error; δv d is the ground velocity error; h is the height of the ship; λ is the geodetic longitude of the ship; To find the derivative operation;

[0016] The velocity error state equation is established as follows:

[0017]

[0018] Where L is the geodetic latitude of the ship; v n is the north velocity; v e is the eastward velocity; v d is the ground velocity; δψ is the heading angle error, δθ is the pitch angle error, and δγ is the roll angle error; C 11 …C 33 is the attitude transfer matrix component, f d is the ground force; f n Northward force; ω e Eastward angular velocity; δf x is the specific force error of the accelerometer along the x-axis; δf y is the specific force error of the accelerometer along the y-axis; δf z is the specific force error of the accelerometer distributed along the z-axis;

[0019] The attitude error state equation is established as follows:

[0020]

[0021] Where: δω x is the x-axis angular rate error of the gyroscope; δω y is the y-axis angular rate error of the gyroscope; δω z is the gyroscope's x-axis measured angular rate error.

[0022] Furthermore, the steps of establishing the state equation of the marine INS / GPS integrated navigation system in S2 are as follows:

[0023] The state equation of the ship's INS / GPS integrated navigation system can be written as:

[0024]

[0025] Where: f(·) is a nonlinear continuous function, w(t) is the sum of accelerometer noise and gyroscope noise; x is the navigation system state variable; t is time;

[0026] Discretizing the state equation of the ship's INS / GPS integrated navigation system, we can obtain:

[0027] x k =F(t k -t k-1 )x k-1 +w k-1 (6)

[0028] Where: F is the state transfer matrix, w k-1 is the Gaussian white noise associated with the inertial sensor; t k is the running time at the discretized k moment; x k-1 is the state variable at time k-1; k is the discrete moment;

[0029] The discrete form of the observation equation of the ship INS / GPS integrated navigation system is established as follows:

[0030] z k =H k x k +v k (7)

[0031] Where: H k is the observation matrix at the discretized k-th moment, V k is the measurement noise vector; z k It is the difference between the position, velocity, attitude angle output by inertial navigation and the position, velocity, attitude angle output by GNSS navigation;

[0032] The observation matrix H is specifically expressed as follows:

[0033]

[0034] Among them, I 3×3 is the third-order identity matrix.

[0035] Furthermore, in S3, the radial basis neural network model is established as follows:

[0036] The activation function for establishing the radial basis neural network model is as follows:

[0037]

[0038] Where: X is the input vector, o i is the central parameter of the i-th function, i is the number of the function in the hidden layer; σ i is the variance of the i-th function;

[0039] At this time, the output of the i-th neuron in the hidden layer is:

[0040]

[0041] Among them, ||·|| is the Euclidean norm;

[0042] The output of the radial basis neural network model is:

[0043]

[0044] Where ω ij is the adjustment weight between the i-th neuron in the hidden layer and the j-th neuron in the output layer.

[0045] Furthermore, the o in the radial basis neural network model i and ω ij As the velocity and position of the particle swarm in the particle swarm optimization algorithm, respectively,

[0046] In S3, the radial basis neural network model is optimized by particle swarm algorithm to establish:

[0047]

[0048] Where: is the velocity of the λth particle in the dth dimension at the γ+1th iteration; is the position of the λth particle in the dth dimension at the γth iteration; is the position of the individual extreme value of the λth particle in the dth dimension at the γth iteration; is the position of the global extreme value of the particle swarm in the dth dimension; rand() is a random number generated in the interval (0,1), c1 is a non-negative acceleration constant that adjusts the maximum step length of flight towards pbest; c2 is a non-negative acceleration constant that adjusts the maximum step length of flight towards gbest, which determines the effect of group experience on particle trajectory; wλ is the non-negative inertia factor of the λth particle; γ is the iteration number of the particle swarm optimization algorithm; λ is the particle number of the particle swarm optimization algorithm; d is the dimension of the search space;

[0049] The update formula of particle swarm optimization's own speed and position is as follows:

[0050]

[0051]

[0052] in, is the position of the λth particle in the dth dimension at the γth iteration.

[0053] Beneficial effects: The auxiliary filtering method for ship integrated navigation of the present invention establishes an error state model of the inertial navigation system when the GPS signal in the INS / GPS integrated navigation system is interrupted, and uses a radial basis function neural network optimized by a particle swarm algorithm to calculate the nonlinear relationship between the ship trajectory obtained by the inertial navigation system and the actual trajectory of the ship. When the GPS signal is interrupted and the navigation is lost, the ship position, ship speed and attitude angle output by the INS / GPS integrated navigation are predicted, and the training stability is achieved, thereby effectively improving the navigation solution accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following is a brief introduction to the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative labor.

[0055] Figure 1 This is a flow chart of the ship integrated navigation auxiliary filtering method of the present invention;

[0056] Figure 2 RBF neural network topology diagram in an embodiment of the present invention;

[0057] Figure 3 is a flowchart of a neural network assisted filtering in an embodiment of the present invention;

[0058] Figure 4 A comparison diagram of longitude error curves of BP neural network, RBF neural network, and PSO-RBF neural network assisted filtering in an embodiment of the present invention;

[0059] Figure 5 This is a comparison chart of latitude error curves of BP neural network, RBF neural network, and PSO-RBF neural network assisted filtering in an embodiment of the present invention;

[0060] Figure 6 This is a comparison diagram of eastward velocity error curves of BP neural network, RBF neural network, and PSO-RBF neural network assisted filtering in an embodiment of the present invention;

[0061] Figure 7 This is a comparison chart of northbound velocity error curves of BP neural network, RBF neural network, and PSO-RBF neural network assisted filtering in an embodiment of the present invention. DETAILED DESCRIPTION

[0062] To make the objectives, technical solutions, and advantages of the embodiments of the present invention more clear, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. All other embodiments obtained by ordinary technicians in this field based on the embodiments of the present invention without making any creative efforts shall fall within the scope of protection of the present invention.

[0063] This embodiment provides an auxiliary filtering method for marine integrated navigation. Figure 1 As shown, the following steps are included:

[0064] S1. When the GPS signal in the INS / GPS integrated navigation system is interrupted, an error state model of the inertial navigation system is established, wherein the error state model includes a position error state equation, a velocity error state equation, and an attitude error state equation;

[0065] The error state model of the inertial navigation (INS) established in S1 is as follows:

[0066] In this embodiment, since the tightly coupled integrated navigation structure that uses pseudorange and pseudorange rate as the measurement vector for the ship's INS / GPS integrated navigation cannot suppress filter divergence when the GPS satellite has no output at all, and has the disadvantages of large computational complexity and complex design, a loosely coupled closed-loop structure that is easier to implement is selected in this embodiment. The integrated navigation coordinate system uses the local geographic coordinate system (NED), the carrier coordinate system is the "front-right-down" coordinate system of the ship's forward direction, and the difference between the positions output by the INS and GPS is taken as the observation quantity. The geographic coordinate system (NED) is used as the navigation coordinate system, and the position error state equation, velocity error state equation, and attitude error state equation of the inertial navigation system are established;

[0067] Use error state variables instead of the usual state vector:

[0068]

[0069] Where: is the error vector of the ship in the latitude, longitude and altitude directions, where δL is the error of the ship in the latitude direction, δλ is the error of the ship in the longitude direction, and δh is the error of the ship in the altitude direction; is the speed error vector of the ship, whose components are the errors in the three directions of the local horizontal coordinate system; is the ship attitude error vector; is the error vector of the gyroscope in the inertial measurement unit on three axes; is the error vector of the accelerometer in the inertial measurement unit in three axes;

[0070] The position error state equation is established as follows:

[0071]

[0072] Where: δ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; R M is the radius of curvature of the Earth's meridian; R N is the radius of curvature of the Earth's angular circle; δv n is the north velocity error; δv e is the eastward velocity error; δv d is the ground velocity error; h is the height of the ship; λ is the geodetic longitude of the ship; To find the derivative operation;

[0073] The velocity error state equation is established as follows:

[0074]

[0075] Where L is the geodetic latitude of the ship; v n is the north velocity; v e is the eastward velocity; v d is the ground velocity; δψ is the heading angle error, δθ is the pitch angle error, and δγ is the roll angle error; C 11 …C 33 is the attitude transfer matrix component, f d is the ground force; f n Northward force; ω e Eastward angular velocity; δf x is the specific force error of the accelerometer along the x-axis; δf y is the specific force error of the accelerometer along the y-axis; δf z is the specific force error of the accelerometer along the z-axis, where the specific force error model is assumed to be a first-order Markov process;

[0076] The attitude error state equation is established as follows:

[0077]

[0078] Where: δω x is the x-axis angular rate error of the gyroscope; δω y is the y-axis angular rate error of the gyroscope; δω z is the x-axis angular rate error of the gyroscope; the gyroscope angular rate error model is assumed to be a first-order Markov process;

[0079] S2: taking the difference between the position output by the GPS and the position output by the INS before the GPS signal is interrupted as the observation quantity, selecting an appropriate feedback structure and inertial navigation parameter correction method according to needs, and using the Kalman filter to fuse the navigation information of the INS and GPS, that is, the ship speed, ship position and attitude angle output by the INS and GPS, so as to establish the state equation of the marine INS / GPS integrated navigation system, so as to obtain the observation equation of the marine INS / GPS integrated navigation system according to the state equation of the marine INS / GPS integrated navigation system;

[0080] The steps of establishing the state equation of the marine INS / GPS integrated navigation system in S2 are as follows:

[0081] The state equation of the ship's INS / GPS integrated navigation system can be written as:

[0082]

[0083] Where: f(·) is a nonlinear continuous function, w(t) is the sum of accelerometer noise and gyroscope noise; x is the navigation system state variable; t is time;

[0084] Discretizing the state equation of the ship's INS / GPS integrated navigation system, we can obtain:

[0085] x k =F(t k -t k-1 )x k-1 +w k-1 (6)

[0086] Where: F is the state transfer matrix, w k-1 is the Gaussian white noise associated with the inertial sensor; t k is the running time at the discretized k moment; x k-1 is the state variable at time k-1; k is the discrete moment;

[0087] Specifically, this embodiment uses the position of GPS navigation and the acceleration and angular velocity of the gyroscope obtained by the accelerometer in the INS as corresponding measurement vectors.

[0088] Since the Kalman filter is linear, it needs to be discretized when used. Therefore, the discrete form of the observation equation of the marine INS / GPS integrated navigation system is established as follows:

[0089] z k =H k x k +v k (7)

[0090] Where: H k is the observation matrix at the discretized k-th moment, V k is the measurement noise vector. k It is the difference between the position, velocity, attitude angle output by inertial navigation and the position, velocity, attitude angle output by GNSS navigation;

[0091] H is the observation matrix, which is expressed as follows:

[0092]

[0093] Among them, I 3×3 is the third-order identity matrix.

[0094] S3: Establishing a radial basis function neural network model and optimizing the radial basis function neural network module through a particle swarm algorithm to obtain an optimized radial basis function neural network model, so as to obtain a nonlinear relationship between the ship trajectory obtained by the inertial navigation system and the actual trajectory of the ship through the optimized radial basis function neural network model and the observation equation of the ship INS / GPS integrated navigation system, and correct the navigation result of the inertial navigation;

[0095] In S3, a RBF neural network model (radial basis function neural network model) is established to construct a nonlinear relationship between the ship trajectory obtained by the inertial navigation system and the actual ship trajectory, and the RBF neural network model is trained using data from the normal operation of the GPS.

[0096] Specifically, the RBF radial basis neural network model is established as follows: The structure of the constructed RBF neural network is as follows Figure 2 shown.

[0097] The RBF neural network model consists of an input layer consisting of signal source nodes (i.e., the position of the ship output by the inertial navigation); a hidden layer and an output layer whose number of hidden layer units can be determined as needed. The number of nodes in the input layer, hidden layer, and output layer are n, m, and l respectively. The input vector of the input layer of the RBF neural network model is X = [x1.x2,…,x n ] T , the output vector of the output layer is Y=[y1.y2,…,y l ] T .

[0098] Each hidden layer unit in the RBF neural network model is used to implement a radial basis function. The value of a point in the radial basis function is only related to the distance between the input vector and the center point of the hidden layer neuron, and it responds locally to the input. When the input is close to the center of the radial basis function, the hidden layer output is large; when the input is far from the center, the output decays exponentially. The radial basis function usually uses a Gaussian function as the activation function, which can be obtained:

[0099]

[0100] Where: X is the input vector, o i is the central parameter of the i-th function, the difference between the two is the distance between the input point and the center of each hidden layer, and i is the number of the function in the hidden layer; σ i is the variance of the i-th function (i.e., the width of the radius basis), which determines the rate of decrease of the function. The larger it is, the better the smoothness between neurons.

[0101] At this point, the output of the i-th neuron in the hidden layer can be written as: where the neuron is used to implement the i-th activation function;

[0102]

[0103] where ||·|| is the Euclidean norm.

[0104] The output layer neurons linearly weight the hidden layer output, and the output of the output layer is:

[0105]

[0106] Where ω ij is the adjustment weight between the i-th neuron in the hidden layer and the j-th neuron in the output layer.

[0107] The training input information and training targets of the neural network are saved immediately as training samples. Formula (11) is used as the input of the RBF neural network model, and the ship's speed and position predicted by the RBF neural network model are used as the output of the neural network.

[0108] Since there is no mature theory for neural network parameter selection, it is usually selected based on experience. Therefore, improving the neural network structure and parameters is still of great significance and research value. In this embodiment: the radial basis function neural network model is optimized using a particle swarm algorithm to obtain an optimized radial basis function neural network model to obtain a nonlinear relationship between the ship's trajectory obtained by the inertial navigation system and the ship's actual trajectory;

[0109] The specific steps of optimizing RBFNN with particle swarm algorithm include: The mathematical description of particle swarm algorithm is: In the a-dimensional search space, assume that there is a particle swarm containing b particles e=(e1,e2,…,e b ) T , the current position of the λth particle is e λ =(e λ,1 ,e λ,2 ,…,e λ,a ) T , the current velocity of the particle is v λ =(v λ,1 ,v λ,2 ,…,v λ,a ) T The fitness formula of particle swarm is:

[0110]

[0111] Where: is the velocity of the λth particle in the dth dimension at the γ+1th iteration; is the position of the λth particle in the dth dimension at the γth iteration; is the position of the individual extreme value of the λth particle in the dth dimension at the γth iteration; is the position of the global extreme value of the particle swarm in the dth dimension; rand() is a random number generated in the interval (0,1); c1 is a non-negative acceleration constant that adjusts the maximum step length of flight toward pbest, which determines the effect of individual experience on the particle trajectory; c2 is a non-negative acceleration constant that adjusts the maximum step length of flight toward gbest, which determines the effect of group experience on the particle trajectory; w λ is the non-negative inertia factor of the λth particle. When the value is large, the global optimization ability is strong and the local optimization ability is weak; γ is the iteration number of the particle swarm optimization algorithm; λ is the particle number of the particle swarm optimization algorithm;

[0112] In this embodiment, the o in the radial basis neural network model is i and ω ij As parameters in the particle swarm optimization algorithm, respectively, where o i As the position of the particle group, ω ij as the velocity of the particle group;

[0113] By calculating the fitness value, the individual optimal value (pbest) is determined as p λ =(p λ,1 ,p λ,2 ,…,p λ,a ) T , the global optimal value (gbest) of the particle swarm is p g =(p g,1 ,pg,2 ,…,p g,a ) T After finding the above values, the update formula for optimizing the speed and position of the swarm particles is as follows:

[0114]

[0115]

[0116] in, is the position of the λth particle in the dth dimension at the γth iteration;

[0117] The implementation steps of optimizing the radial basis neural network model by particle swarm optimization are as follows:

[0118] S31: normalize the sample data;

[0119] S32: Initialize the radial basis neural network model by parameters, o i , σ i Compose particles and assign random values ​​to initialize the position and velocity of the particle swarm;

[0120] S33: After obtaining the input and output of the radial basis function neural network, the fitness value of the particle swarm is calculated according to the fitness of the particle swarm to determine the individual extreme value in the particle swarm and the global extreme value of the particle swarm;

[0121] S34: Calculate the fitness value of the λth particle, and store the fitness value of the λth particle and the position of the λth particle in the individual extreme value of the λth particle In, all The location and fitness value of the best individual in the fitness value are stored in the particle swarm extreme value middle;

[0122] S35: Update the position and velocity of the swarm particles to generate a new swarm of particles;

[0123] S36: For each particle that makes up the particle swarm, its fitness value 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;

[0124] S37: Compare all current as well as To update

[0125] S38: Determine whether the optimization goal has been achieved or whether the maximum number of training times has been met. If the termination condition has been met, the algorithm ends; otherwise, return to S43.

[0126] S4: According to the nonlinear relationship between the ship trajectory obtained by the inertial navigation system and the actual trajectory of the ship and the error state model of the inertial navigation system, the ship position, ship speed and attitude angle obtained by the inertial navigation system when the GPS signal is interrupted are obtained.

[0127] This embodiment can solve the problem of GPS interruption by optimizing the radial basis function neural network model through the particle swarm algorithm to assist the inertial navigation system INS in outputting the positioning parameters of the ship.

[0128] The overall neural network assisted filtering process in S4 is:

[0129] The INS / GPS integrated navigation system assisted by PSO-RBF (Particle Swarm Optimization Radial Basis Function Neural Network) compensates for the lack of GPS signals through the self-learning ability and local approximation ability of the PSO-RBF neural network. It mainly has two working modes: PSO-RBF neural network training and PSO-RBF neural network prediction.

[0130] When the GPS receiver is operating normally, the system works in the PSO-RBF neural network training mode, and the training input information and training targets of the PSO-RBF neural network are saved in real time as training samples. Formula (11) is used as the input of the RBF neural network model, and the speed and position predicted after processing by the RBF neural network model will be used as the output of the RBF neural network model. The output expected by the PSO-RBF neural network is the position, speed and attitude information after the Kalman filter input correction with the output of formula (12) to complete the network training. At this time, the absolute value of the training error is used as the individual fitness value of the particle swarm. The smaller the fitness value, the better the individual. It is fed back to the RBF neural network model for internal parameter adjustment. Finally, when the RBF neural network model error is minimized or the expected target is achieved, the training process is terminated. In order to reduce unnecessary cross-coupling in training and improve the training speed of the neural network, this embodiment uses four parallel RBF neural network models corresponding to the position component (latitude and longitude) and velocity component (northbound velocity v in the measurement information) respectively. n and eastward velocity v e Each neural network consists of four input layer neurons (a motion information component and three specific force components provided by an INS) and one output layer neuron (the ship position and speed predicted by the RBF neural network model).

[0131] When the GPS signal is interrupted, the inertial navigation system (INS navigation) is still working normally, and the GPS navigation cannot provide the measurement vector to the Kalman filter. The ship position, ship speed and attitude angle output by the inertial navigation system (INS navigation) are input, and the radial basis function neural network model is optimized by the particle swarm algorithm for prediction, and the result after the PSO-RBF neural network corrects the Kalman filter can be obtained. The output value of the radial basis function neural network model optimized by the particle swarm algorithm is used to replace the estimated value of the Kalman filter to ensure that the navigation still has sufficient accuracy. The PSO-RBF neural network auxiliary process is as follows: Figure 3 shown.

[0132] Simulation and Analysis

[0133] To verify the effectiveness of the algorithm in this embodiment, the algorithm was simulated using the MATLAB platform to simulate the motion of a ship at sea. The ship sailed at a speed of 10 m / s in sea state level 3 and completed two turns, each with an angle of 45°. The simulation lasted for 300 seconds, with the first 260 seconds representing the Kalman filter results for the INS / GPS integrated navigation. To simulate a GPS receiver signal interruption, the GPS receiver output was disconnected from the 260th second onwards, preventing its measurement information from being incorporated into the integrated navigation. This validated the effectiveness of the PSO-RBF neural network-assisted method. Simultaneously, using the MATLAB Neural Network Toolbox, Kalman filtering was simulated using a BP neural network and a traditional RBF neural network as support, and the results were compared. The centers of the traditional RBF neural network were selected using the K-means algorithm, while the number of hidden layer neurons in the BP neural network was determined using the number of RBF hidden layer neurons. Based on the mean square error (MSE) and training time predicted from multiple experiments, the number of hidden layer neurons in the RBF neural network after training was determined to be 9, so the BP neural network also used 9 hidden layer neurons. The preset training target of the neural network is 0.00001 and the number of iterations is 1500.

[0134] In this simulation, the gyro drift, GPS receiver speed and position are all considered as Markov processes. The main parameters in the experiment are shown in Table 1:

[0135] Table 1 Experimental parameters

[0136]

[0137] Figure 4 、 Figure 5 They are the longitude error and latitude error curves of BP neural network, RBF neural network and PSO-RBF neural network assisted filtering respectively. Figure 4It can be seen that compared with the RBF neural network whose error value fluctuates violently and the BP neural network whose error results gradually diverge, the gap between the prediction results of the PSO-RBF neural network and the true value is relatively stable and has always been smaller than the other two methods, showing a relatively good filtering effect. Figure 5 In the present study, since the prediction error of INS navigation will gradually accumulate over time, the errors of the three neural network assisted algorithms will expand to varying degrees over time. The RBF neural network and BP neural network show a more obvious divergent trend, while the correction of the PSO-RBF neural network can better suppress the filter divergence. In this case, a better filtering result is guaranteed, and its trajectory is relatively close to the true trajectory.

[0138] Figure 6 、 Figure 7 The following are the eastward velocity error and northward velocity error curves for filtering using a BP neural network, an RBF neural network, and a PSO-RBF neural network. The figure shows that the error fluctuates more dramatically when using a BP neural network, with initial errors significantly larger than those of the RBF and PSO-RBF neural networks. After stabilization, the error remains larger than those of the RBF and PSO-RBF neural networks. The error is more stable when using an RBF neural network, but the filtering effect is not as good as that of the PSO-RBF neural network.

[0139] To further quantify the filtering accuracy of the three algorithms, the mean error and root mean square error of position and velocity estimation during GPS outages are presented. Table 2, combined with the error curves, shows that the BP-assisted algorithm is inferior to the RBF-assisted algorithm in overall accuracy, indicating that the RBF neural network, with its local approximation properties, is more suitable for assisted navigation filtering. The position and velocity errors corresponding to the PSO-RBF algorithm are more stable than those of the first two neural network-assisted algorithms, fully demonstrating the effectiveness of this method.

[0140] Table 2 Mean error and root mean square error of position and velocity during GPS outage

[0141]

[0142] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the above embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. An auxiliary filtering method for marine integrated navigation, characterized in that: The steps include: S1. When the GPS signal in the INS / GPS integrated navigation system is interrupted, an error state model of the inertial navigation system is established, wherein the error state model includes a position error state equation, a velocity error state equation, and an attitude error state equation; The error state model of inertial navigation established in S1 is as follows: Where: is the error vector of the ship in the latitude, longitude and altitude directions, where δL is the error of the ship in the latitude direction, δλ is the error of the ship in the longitude direction, and δh is the error of the ship in the altitude direction; is the ship's speed error vector, is the ship attitude error vector; is the error vector of the gyroscope in the inertial measurement unit on three axes; is the error vector of the accelerometer in the inertial measurement unit in three axes; The position error state equation is established as follows: Where: δ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; R M is the radius of curvature of the Earth's meridian; R N is the radius of curvature of the Earth's angular circle; δv n is the north velocity error; δv e is the eastward velocity error; δv d is the ground velocity error; h is the height of the ship; λ is the geodetic longitude of the ship; To find the derivative operation; The velocity error state equation is established as follows: Where L is the geodetic latitude of the ship; v n is the north velocity; v e is the eastward velocity; v d is the ground velocity; δψ is the heading angle error, δθ is the pitch angle error, and δγ is the roll angle error; C 11 …C 33 is the attitude transfer matrix component, f d is the ground force; f n Northward force; ω e Eastward angular velocity; δf x is the specific force error of the accelerometer along the x-axis; δf y is the specific force error of the accelerometer along the y-axis; δf z is the specific force error of the accelerometer distributed along the z-axis; The attitude error state equation is established as follows: Where: δω x is the x-axis angular rate error of the gyroscope; δω y is the y-axis angular rate error of the gyroscope; δω z is the x-axis measured angular rate error of the gyroscope; S2: establishing a state equation of the ship INS / GPS integrated navigation system, so as to obtain an observation equation of the ship INS / GPS integrated navigation system according to the state equation of the ship INS / GPS integrated navigation system; S3: establishing a radial basis function neural network model, and optimizing the radial basis function neural network module by using a particle swarm algorithm to obtain an optimized radial basis function neural network model, so as to obtain a nonlinear relationship between the ship trajectory obtained by the inertial navigation system and the actual trajectory of the ship by using the optimized radial basis function neural network model and the observation equation of the ship INS / GPS integrated navigation system; S4: According to the nonlinear relationship between the ship trajectory and the actual ship trajectory and the error state model of the inertial navigation system, the ship position, ship speed and attitude angle obtained by the inertial navigation system when the GPS signal is interrupted are obtained.

2. The auxiliary filtering method for marine integrated navigation according to claim 1, characterized in that: The steps of establishing the state equation of the marine INS / GPS integrated navigation system in S2 are as follows: The state equation of the ship's INS / GPS integrated navigation system can be written as: Where: f(·) is a nonlinear continuous function, w(t) is the sum of accelerometer noise and gyroscope noise; x is the navigation system state variable; t is time; Discretizing the state equation of the ship's INS / GPS integrated navigation system, we can obtain: x k =F(t k -t k-1 )x k-1 +w k-1 (6) Where: F is the state transfer matrix, w k-1 is the Gaussian white noise associated with the inertial sensor; t k is the running time at the discretized k moment; x k-1 is the state variable at time k-1; k is the discrete moment; The discrete form of the observation equation of the ship INS / GPS integrated navigation system is established as follows: z k =H k x k +v k (7) Where: H k is the observation matrix at the discretized k-th moment, V k is the measurement noise vector; z k It is the difference between the position, velocity, attitude angle output by inertial navigation and the position, velocity, attitude angle output by GNSS navigation; The observation matrix H is specifically expressed as follows: Among them, I 3×3 is the third-order identity matrix.

3. The auxiliary filtering method for marine integrated navigation according to claim 2, characterized in that: In S3, the radial basis neural network model is established as follows: The activation function for establishing the radial basis neural network model is as follows: Where: X is the input vector, o i is the central parameter of the i-th function, i is the number of the function in the hidden layer; σ i is the variance of the i-th function; At this time, the output of the i-th neuron in the hidden layer is: Among them, ||·|| is the Euclidean norm; The output of the radial basis neural network model is: Where ω ij is the adjustment weight between the i-th neuron in the hidden layer and the j-th neuron in the output layer.

4. The auxiliary filtering method for marine integrated navigation according to claim 3, characterized in that: The o in the radial basis neural network model i and ω ij As the velocity and position of the particle swarm in the particle swarm optimization algorithm, respectively, In S3, the radial basis neural network model is optimized by particle swarm algorithm to establish: Where: is the velocity of the λth particle in the dth dimension at the γ+1th iteration; is the position of the λth particle in the dth dimension at the γth iteration; is the position of the individual extreme value of the λth particle in the dth dimension at the γth iteration; is the position of the global extreme value of the particle swarm in the dth dimension; rand() is a random number generated in the interval (0,1), c1 is a non-negative acceleration constant that adjusts the maximum step length of flight towards pbest; c2 is a non-negative acceleration constant that adjusts the maximum step length of flight towards gbest, which determines the effect of group experience on particle trajectory; w λ is the non-negative inertia factor of the λth particle; γ is the iteration number of the particle swarm optimization algorithm; λ is the particle number of the particle swarm optimization algorithm; d is the dimension of the search space; The update formula of particle swarm optimization's own speed and position is as follows: in, is the position of the λth particle in the dth dimension at the γth iteration.

Images