阅读说明：本技术 一种捷联惯导/多普勒组合导航系统的导航方法 (Navigation method of strapdown inertial navigation/Doppler integrated navigation system ) 是由 李万里 陈明剑 李军正 王力 赵远 张好 周舒涵 于 2021-09-24 设计创作，主要内容包括：本发明属于组合导航技术领域,具体涉及一种捷联惯导/多普勒组合导航系统的导航方法。若多普勒失效且多普勒失效时刻大于设定值,则将多普勒失效时刻和前m个时刻的模型输入、以及多普勒失效时刻和前m个时刻的多普勒输出速度输入至训练好的多普勒预测模型中,预测得到下一时刻多普勒输出速度,m＞1；并利用预测得到的多普勒输出速度进行组合导航；其中模型输入为捷联惯导/多普勒组合导航系统输出的方向余弦矩阵和速度的乘积。本发明在多普勒有效的情况下,利用组合导航系统的数据以及多普勒输出速度对多普勒预测模型进行训练,从而使得在多普勒失效后利用多普勒预测模型便可预测得到多普勒输出速度,保证导航数据不间断,短期维持组合导航系统的精度。(The invention belongs to the technical field of integrated navigation, and particularly relates to a navigation method of a strapdown inertial navigation/Doppler integrated navigation system. If the Doppler is invalid and the Doppler invalid time is larger than a set value, inputting the Doppler invalid time and the Doppler output speeds of the previous m times and the Doppler invalid time into a trained Doppler prediction model, and predicting to obtain the Doppler output speed of the next time, wherein m is larger than 1; and the combined navigation is carried out by using the Doppler output speed obtained by prediction; wherein the model input is a direction cosine matrix output by the strapdown inertial navigation/Doppler combined navigation system And velocity The product of (a). Under the condition that the Doppler is effective, the data of the integrated navigation system and the Doppler output speed are used for training the Doppler prediction model, so that the Doppler output speed can be predicted by using the Doppler prediction model after the Doppler is invalid, the navigation data is ensured to be uninterrupted, and the precision of the integrated navigation system is maintained in a short period.)

1. A navigation method of a strapdown inertial navigation/Doppler combined navigation system is characterized by comprising the following steps:

1) when the strapdown inertial navigation/Doppler combined navigation system works, judging whether Doppler is invalid or not:

2) if the Doppler is invalid and the Doppler invalid time is larger than a set value, inputting the Doppler invalid time and the Doppler output speeds of the previous m times and the Doppler invalid time into a trained Doppler prediction model, and predicting to obtain the Doppler output speed of the next time, wherein m is larger than 1; and the combined navigation is carried out by using the Doppler output speed obtained by prediction;

wherein the model input is a direction cosine matrix output by a strapdown inertial navigation/Doppler combined navigation systemAnd velocityThe product of (a); and the Doppler prediction model is obtained by training by using an input value sequence and a Doppler output speed sequence when the Doppler prediction model is in a combined navigation state.

2. The navigation method of the combined strapdown inertial navigation/doppler navigation system of claim 1, wherein in step 2), the doppler prediction model is a neural network model.

3. The integrated strapdown inertial navigation/doppler navigation system of claim 2, wherein the neural network model is a non-linear autoregressive neural network model.

4. The navigation method of the combined strapdown inertial navigation system and doppler navigation system of claim 1, wherein the error velocity equation and the attitude error equation of the inertial navigation system in the combined strapdown inertial navigation system and doppler navigation system are:

wherein, δ v is a velocity error,is the derivative of the velocity error δ v; phi is the error of the attitude,is the derivative of the attitude error phi; f. ofⁿIs a representation of specific force in a navigation coordinate system;representing the rotation angular speed of the earth in a navigation coordinate system;is the representation of the rotation angular speed of the n system relative to the e system in a navigation coordinate system;is the accelerometer zero offset, epsilon is the gyro zero offset, and

5. the navigation method of the combined strapdown inertial navigation/doppler navigation system of claim 4, wherein the selected state variables of the combined strapdown inertial navigation/doppler navigation system are:

wherein, δ v_N、δv_E、δv_DSpeed errors in the north direction, the east direction and the ground direction respectively; phi is a_N、φ_E、φ_DAttitude angle errors in the north direction, the east direction and the ground direction respectively;the zero offset of the accelerometer in the x direction, the y direction and the z direction under a carrier coordinate system respectively; epsilon_x、ε_y、ε_zThe gyroscope zero offset in the x direction, the y direction and the z direction under the carrier coordinate system is respectively.

6. The navigation method of the combined strapdown inertial navigation/doppler navigation system of claim 1, wherein the observation equation of the combined strapdown inertial navigation/doppler navigation system is:

wherein z represents an observed quantity;n is the velocity of the inertial navigation system and the Doppler output, respectively, and a conversion matrix from the Doppler carrier coordinate system d to the carrier coordinate system b is formed;a direction cosine matrix of the inertial navigation system; v. of_dThe velocity of the Doppler in a carrier coordinate system d is obtained; η is zero mean gaussian white noise; h is an observation matrix, andI_3×3is a 3 x 3 identity matrix of the cell,is composed ofIs used to form the oblique symmetric matrix.

7. The navigation method of the combined strapdown inertial navigation/doppler navigation system of claim 1, wherein in step 2), m is 3.

8. The navigation method of the strapdown inertial navigation/doppler combined navigation system according to claim 1, wherein if the determination result of step 1) is doppler failure and the doppler failure time is less than or equal to the set value, the strapdown inertial navigation/doppler combined navigation system is switched to a pure inertial navigation state and enters the combined navigation state after doppler failure is repaired.

9. The combined strapdown inertial navigation/doppler navigation system navigation method according to claim 3, wherein the non-linear autoregressive neural network model comprises an input layer, a hidden layer and an output layer, the activation function of the hidden layer is a ReLU function, and the activation function of the output layer is a linear function.

Technical Field

The invention belongs to the technical field of integrated navigation, and particularly relates to a navigation method of a strapdown inertial navigation/Doppler integrated navigation system.

Background

Currently, the means available for underwater navigation are still relatively limited. The navigation system of an underwater vehicle must have the navigation capability of long distance, long endurance and high precision. Strapdown Inertial Navigation/Doppler (SINS/DVL) integrated Navigation is one of the main ways to realize underwater autonomous Navigation at present.

Doppler is an instrument that uses an ultrasonic transducer mounted on a carrier to emit ultrasonic waves to the sea floor and measures the velocity of the carrier according to the doppler effect principle, which is shown in fig. 1. In practical application, due to factors such as complex underwater topography, fish shoal interference, over-range of the doppler velocimeter and the like, the doppler velocimeter may fail. How to maintain the navigation accuracy under the condition of Doppler failure is a problem to be studied further.

In response to this problem, a commonly adopted method is to establish a kinematic model of the underwater vehicle, provide a virtual velocity observation value by using the kinematic model under the condition of doppler failure, and perform combined navigation. In this method, it is generally assumed that the carrier is in a uniform velocity or uniform acceleration state, and there is a certain difference from the actual motion state of the carrier, so the prediction accuracy is not high.

Disclosure of Invention

The invention provides a navigation method of a strapdown inertial navigation/Doppler combined navigation system, which is used for solving the problem of low prediction precision of combined navigation in the prior art.

In order to solve the technical problems, the technical scheme and the corresponding beneficial effects of the technical scheme are as follows:

the invention provides a navigation method of a strapdown inertial navigation/Doppler combined navigation system, which comprises the following steps:

1) when the strapdown inertial navigation/Doppler combined navigation system works, judging whether Doppler is invalid or not:

2) if the Doppler is invalid and the Doppler invalid time is larger than a set value, inputting the Doppler invalid time and the Doppler output speeds of the previous m times and the Doppler invalid time into a trained Doppler prediction model, and predicting to obtain the Doppler output speed of the next time, wherein m is larger than 1; and the combined navigation is carried out by using the Doppler output speed obtained by prediction;

wherein the model input is a direction cosine matrix output by a strapdown inertial navigation/Doppler combined navigation systemAnd velocityThe product of (a); and the Doppler prediction model is obtained by training by using an input value sequence and a Doppler output speed sequence when the Doppler prediction model is in a combined navigation state.

The beneficial effects of the above technical scheme are: under the condition that the Doppler is effective, the Doppler prediction model is trained by using the data and the Doppler output speed of the integrated navigation system, so that the Doppler output speed can be predicted by using the Doppler prediction model after the Doppler is invalid, the navigation data is ensured to be uninterrupted, and the precision of the integrated navigation system is maintained in a short period. In addition, the input of the Doppler prediction model of the invention is the velocity under the carrier system obtained by using SINS/DVL combined navigation, the output is the velocity under the Doppler carrier system, the velocity and the velocity are velocity sequences, and the velocity obtained by the SINS/DVL combined navigation can reflect the change trend of the Doppler velocity to a certain extent, thereby improving the prediction precision.

Further, in step 2), the doppler prediction model is a neural network model.

Further, in order to accurately predict the doppler output velocity, the neural network model is a nonlinear autoregressive neural network model.

Further, an error velocity equation and an attitude error equation of an inertial navigation system in the strapdown inertial navigation/doppler integrated navigation system are as follows:

wherein, δ v is a velocity error,is the derivative of the velocity error δ v; phi is the error of the attitude,is the derivative of the attitude error phi; f. ofⁿIs a representation of specific force in a navigation coordinate system;representing the rotation angular speed of the earth in a navigation coordinate system;is the representation of the rotation angular speed of the n system relative to the e system in a navigation coordinate system;is the accelerometer zero offset, epsilon is the gyro zero offset, and

further, the state variables selected by the strapdown inertial navigation/doppler integrated navigation system are:

wherein, δ v_N、δv_E、δv_DSpeed errors in the north direction, the east direction and the ground direction respectively; phi is a_N、φ_E、φ_DAttitude angle errors in the north direction, the east direction and the ground direction respectively;the zero offset of the accelerometer in the x direction, the y direction and the z direction under a carrier coordinate system respectively; epsilon_x、ε_y、ε_zThe gyroscope zero offset in the x direction, the y direction and the z direction under the carrier coordinate system is respectively.

Further, the observation equation of the strapdown inertial navigation/doppler combined navigation system is as follows:

wherein z represents an observed quantity;n is the velocity of the inertial navigation system and the Doppler output, respectively, and a conversion matrix from the Doppler carrier coordinate system d to the carrier coordinate system b is formed;a direction cosine matrix of the inertial navigation system; v. of_dThe velocity of the Doppler in a carrier coordinate system d is obtained; η is zero mean gaussian white noise; h is an observation matrix, andI_3×3is a 3 x 3 identity matrix of the cell,is composed ofIs used to form the oblique symmetric matrix.

Further, in order to ensure the prediction accuracy and the calculation efficiency, in step 2), m is 3.

Further, in order to ensure the output accuracy of the integrated navigation system, if the judgment result in the step 1) is that the doppler is invalid and the doppler invalid time is less than or equal to the set value, the strapdown inertial navigation/doppler integrated navigation system is switched to a pure inertial navigation state and is switched to the integrated navigation state after the doppler invalid is repaired.

Further, the nonlinear autoregressive neural network model includes an input layer, a hidden layer, and an output layer, where an activation function of the hidden layer is a ReLU function, and an activation function of the output layer is a linear function.

Drawings

FIG. 1 is a schematic diagram of the Doppler operating principle of the present invention;

FIG. 2 is a diagram of the SINS/DVL integrated navigation architecture of the present invention;

FIG. 3 is a structural diagram of a NARX neural network model of the present invention;

fig. 4 is a flow chart of a method of the present invention.

Detailed Description

The basic concept of the invention is as follows: when Doppler is effective, a strapdown inertial navigation/Doppler combined navigation system (hereinafter referred to as an SINS/DVL combined navigation system or a combined navigation system) and Doppler output data are used for training a Doppler prediction model, the trained Doppler prediction model is accessed when Doppler fails, the trained Doppler prediction model is used for predicting Doppler output data, and the predicted data and the inertial navigation system are used for combined navigation, so that the navigation data are ensured to be uninterrupted, and the precision of combined navigation is maintained. The following describes a navigation method of a strapdown inertial navigation/doppler integrated navigation system according to the present invention in detail with reference to the accompanying drawings and embodiments.

The method comprises the following steps:

before the navigation method of the strapdown inertial navigation/Doppler integrated navigation system is implemented, an SINS/DVL integrated navigation system is introduced.

The specific structure diagram of the SINS/DVL integrated navigation system is shown in fig. 2, i.e. the integrated navigation is performed by assisting inertial navigation with the velocity information output by DVL. And converting the speed output by the DVL into a navigation coordinate system through the attitude of the inertial navigation system, taking the difference between the speed output by the DVL and the speed information output by the inertial navigation system as a measurement value, estimating the state of the integrated navigation system through Kalman filtering, and correcting the inertial navigation system.

The integrated navigation system firstly establishes an integrated navigation model of the system, namely a state equation and an observation equation, and the integrated navigation model established by the invention is as follows:

1) and (4) a state equation.

The North-East-ground (NED) geographic coordinate system is selected as the navigation system and is denoted as n. The error velocity equation and the attitude error equation of the inertial navigation system can be expressed as follows:

wherein, δ v is a velocity error,is the derivative of the velocity error δ v; phi is the error of the attitude,is the derivative of the attitude error phi; f. ofⁿNavigating for specific strengthA representation in a coordinate system;a direction cosine matrix output for the integrated navigation system;representing the rotation angular speed of the earth in a navigation coordinate system;is the representation of the rotation angular speed of the n system relative to the e system in a navigation coordinate system; accelerometer zero offsetModeling with a gyro zero bias epsilon as a constant value, then:

the state variables selected by the SINS/DVL integrated navigation system are as follows:

wherein, δ v_N、δv_E、δv_DSpeed errors in the north direction, the east direction and the ground direction respectively; phi is a_N、φ_E、φ_DAttitude angle errors in the north direction, the east direction and the ground direction respectively;the zero offset of the accelerometer in the x direction, the y direction and the z direction under a carrier coordinate system respectively; epsilon_x、ε_y、ε_zThe gyroscope zero offset in the x direction, the y direction and the z direction under the carrier coordinate system is respectively.

The state equation of the integrated navigation system can be listed according to the equations (1) and (2):

wherein F is a state transition matrix; w is white gaussian noise.

2) And (4) observing an equation.

Taking the speed difference between the inertial navigation system and the Doppler velocimeter as the observed quantity of the combined navigation, namely:

wherein z represents an observed quantity;respectively the velocity under n systems of the inertial navigation system and the Doppler output; and:

wherein the content of the first and second substances,a conversion matrix from the Doppler carrier coordinate system d to the carrier coordinate system b is formed;a direction cosine matrix of the inertial navigation system; v. of_dIs the velocity of the doppler in the carrier coordinate system d.

The observation equation for establishing the SINS/DVL integrated navigation system is as follows:

wherein eta is zero-mean Gaussian white noise; h is an observation matrix, and:

wherein, I_3×3Is a 3 × 3 identity matrix;is composed ofIs used to form the oblique symmetric matrix.

3) And (3) a neural network model assisted SINS/DVL combined navigation algorithm.

The doppler prediction model in this embodiment adopts a neural network model, and the input and output mapping relationship of the neural network model is as follows:

wherein M (-) is a nonlinear mapping function; y (k +1) is the output of the neural network model; m is the order of input delay; y (k), y (k-1) and y (k-m) are respectively the outputs of the neural network model at the time k, the time k-1 and the time k-m; x (k), x (k-1), x (k-m) are model inputs at time k, time k-1, and time k-m, respectively, and the definition model input x (k) is:

wherein the content of the first and second substances,andrespectively a direction cosine matrix and a speed output by the combined navigation system at the moment k. Order:

y(k)＝v_d(k) (11)

wherein v is_d(k) The doppler output velocity is time k.

The output y (k +1) of the neural network model is the doppler output velocity at time k + 1. That isThat is, after the training of the neural network model is completed, the data of the navigation system is combined from the time k-m to the time kAnd Doppler output velocity v_dThe doppler output velocity at time k +1 can be predicted.

Specifically, the neural network model may be a non-linear autoregressive (NARX) neural network model, and the specific structure of the neural network model is shown in fig. 3, and includes an input layer, a hidden layer, and an output layer. W is the connection weight needing adaptive adjustment in the neural network model; b is an offset; f1 is the activation function of the hidden layer, and selects the nonlinear function ReLU function; f2 is an activation function of the output layer, and a linear function is selected; the number of input variables of the input layer is 6 (here, 6 are because x and y are both 3-dimensional velocity sequences, and 6 is the number of the added variables), which respectively corresponds to x (k) and y (k); the maximum delay order of the input layer is 3; the number of hidden layer neurons is 10; the number of neurons of the output layer is 3, and the Doppler output speed at the moment of outputting k +1 correspondingly; the neural network model is optimized by adopting an Adam optimization algorithm.

After the above description, the whole integrated navigation process (i.e. the navigation method of the strapdown inertial navigation/doppler integrated navigation system of the present invention) is described with reference to fig. 4, where the order m of the input delay is 3.

Step one, the SINS/DVL integrated navigation system is powered on and started to carry out initial alignment.

Step two, after the initial alignment is completed, the SINS/DVL integrated navigation system enters an integrated navigation state, and models of the integrated navigation are shown in formulas (4) and (7).

Step three, according to the direction cosine matrix output by the SINS/DVL integrated navigation systemAnd velocityAnd equation (10) calculating the model outputGo into x and store, and output the Doppler velocity v_dRecorded as y and stored.

And step four, training the NARX neural network model by using the x sequence and the y sequence stored in the step three, wherein the NARX neural network model is as shown in figure 3, so as to obtain a trained NARX neural network model (namely a trained Doppler prediction model).

Step five, judging whether Doppler is invalid:

if the Doppler is invalid and k is more than 1000, inputting x (k), x (k-1), x (k-2), x (k-3), y (k-1), y (k-2) and y (k-3) into a trained Doppler prediction model to obtain y (k +1), namely the Doppler output speed at the next moment;

if the Doppler is invalid but k is less than or equal to 1000, the SINS/DVL combined navigation system is switched to a pure inertial navigation state, and is switched to the third step after the Doppler is invalid and repaired;

if the Doppler is not invalid, go to step three.

It should be noted that k in this step may refer to a doppler failure time, and the comparison and determination of k and 1000 is also added to the determination condition because, when k is less than or equal to 1000, it indicates that there is less data sent to the NARX neural network model for training, and then the prediction accuracy of the NARX neural network model may be low, in order to ensure the navigation accuracy, a pure inertial navigation state is switched to when k is less than or equal to 1000, and the prediction result of the NARX neural network model is only used when k is greater than 1000.

And step six, updating the y sequence by the Doppler output speed obtained by prediction in the step five, and performing SINS/DVL combined navigation by the predicted Doppler output speed.

Step seven, updating the x sequence according to the combined navigation result obtained in the step six and a formula (10); and the step five is carried out for repeated judgment.

And step eight, ending the navigation of the SINS/DVL integrated navigation system, and stopping the operation of the SINS/DVL integrated navigation system.

The method has the following effects: 1) the invention can maintain the precision of the inertial navigation/Doppler combined navigation system under the condition of Doppler failure or maintain the precision of the inertial navigation/Doppler combined navigation system in a short time; 2) the data of the combined navigation system and the Doppler data are effectively utilized to train the neural network, and the prediction precision of the Doppler velocity sequence is improved.

In this embodiment, the doppler prediction model is a nonlinear autoregressive neural network model. Other models known in the art, such as convolutional neural network models, may also be used as an alternative embodiment.