A method for restraining positioning error divergence of SINS / DVL integrated navigation system when USBL measurement fails

Abstract

The present application belongs to the technical field of integrated navigation, and studies the suppression of positioning errors of an integrated navigation system under the condition that USBL measurement fails under the condition that SINS and DVL work normally. The purpose of the present application is achieved by the following technical method, comprising the following steps: analyzing the relationship between the vehicle system speed and the USBL relative position measurement; under the condition that the initial USBL relative position measurement result is given, the subsequent USBL measurement result is recursively calculated based on the vehicle system speed integration; a back propagation (BP) neural network is introduced to train the navigation data online; during the USBL measurement failure period, the trained BP neural network model is used to predict the recursive error; the proposed method is simulated and verified. In order to suppress the divergence of the positioning error of the integrated navigation system during the USBL measurement time, a USBL measurement repair method based on the BP neural network is proposed to provide additional position reference information when the USBL fails.

Description

Technical Field

[0001] This invention belongs to the field of integrated navigation technology. Under the condition that SINS and DVL are working normally, this invention studies the suppression of positioning error of integrated navigation system under the condition of USBL measurement failure. Background Technology

[0002] DVL (Dual Volume Level) is characterized by high update frequency, good real-time performance, and wide operating range. SINS / DVL is one of the commonly used underwater navigation methods, but its positioning error can slowly diverge. USBL (Underground Vision Level) can provide position information, and when combined with SINS, it can obtain accurate positioning results. However, USBL's operating range is limited by the transponder, and its update frequency is low, resulting in discontinuous navigation information. Different navigation sensors have different advantages and disadvantages. Utilizing navigation information from SINS, DVL, and USBL to construct a SINS / DVL / USBL integrated navigation system can leverage the strengths of each sensor and compensate for their weaknesses. Maintaining the positioning accuracy of the SINS / DVL / USBL integrated navigation system mainly depends on the measurement information from USBL. When the underwater vehicle moves out of the USBL's operating range or when the acoustic signal is severely attenuated or interfered with by the marine environment, USBL may experience continuous and prolonged measurement failures. Although the SINS / DVL integrated navigation system can maintain attitude and velocity accuracy for an extended period even when USBL measurements fail, the lack of USBL measurement information causes the positioning error to gradually diverge, hindering the safe and efficient execution of underwater vehicles' missions. Therefore, research is needed to suppress the positioning error of the integrated navigation system under USBL measurement failure conditions, assuming both SINS and DVL are functioning normally. Summary of the Invention

[0003] In view of the above-mentioned prior art, the technical problem to be solved by the present invention is to provide a method for suppressing the divergence of positioning error in SINS / DVL integrated navigation system when USBL measurement fails.

[0004] The objective of this invention is achieved through the following technical methods, including the following steps:

[0005] Step 1: Analyze the relationship between the system velocity and the relative position measurement of USBL;

[0006] Step 2: Given the initial USBL relative position measurement results, recursively extrapolate subsequent USBL measurement results based on the carrier system velocity integral;

[0007] Step 3: Introduce a back propagation (BP) neural network to train the navigation data online;

[0008] Step 4: During the USBL measurement failure period, the trained BP neural network model is used to predict the recursive error;

[0009] Step 5: Perform simulation and experimental verification of the proposed method.

[0010] Beneficial effects:

[0011] I. In order to suppress the divergence of positioning error of integrated navigation system during USBL measurement validity period, a USBL measurement repair method based on BP neural network is proposed by utilizing the relationship between the vehicle system velocity and USBL measurement results, so as to provide additional position reference information when USBL fails.

[0012] Second, the proposed method effectively suppresses positioning error divergence and can achieve higher positioning accuracy than the simple SINS / DVL integrated navigation system. Attached Figure Description

[0013] Figure 1 To simulate the trajectory and changes in motion parameters;

[0014] Figure 2 The attitude error curves of the two algorithms are shown in the simulation.

[0015] Figure 3 To verify the position error curves of the two algorithms under simulation;

[0016] Figure 4 The actual USBL recursion error and the USBL recursion error predicted by the BP neural network are used.

[0017] Figure 5 The experimental trajectory on the lake surface;

[0018] Figure 6 To experimentally verify the attitude error curves of the two algorithms;

[0019] Figure 7 To experimentally verify the position error curves of the two algorithms. Detailed Implementation

[0020] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0021] In the description of this invention, it should be understood that the terms "upper," "middle," "outer," "inner," etc., which indicate orientation or positional relationship, are only for the convenience of describing this invention and simplifying the description, and do not indicate or imply that the components or elements referred to must have a specific orientation, or be constructed and operated in a specific orientation, and therefore should not be construed as limiting this invention.

[0022] Step 1: Analyze the relationship between the system velocity and the relative position measurement of USBL;

[0023] The attitude matrix in the traditional SINS error model is defined in the space of a third-order special orthogonal group matrix (SO(3)), while the velocity and position vectors are defined in three-dimensional Euclidean space. The multiplicative closure of the matrix space and the additive closure of the vector space mean that the errors of each navigation parameter are also defined in the same space. This means that the attitude error in the special orthogonal group matrix space is not considered when defining the velocity and position errors, resulting in inaccurate error definitions. To solve this problem, in recent years, Lie group theory has been introduced into integrated navigation systems.

[0024] By defining the attitude representing rotation and the velocity and position representing translation in the same Lie group matrix space, the spatial inconsistency problem is effectively improved, thereby enhancing the performance of the navigation system.

[0025] Constructing elements in a special Euclidean group using posture, velocity, and position:

[0026]

[0027] Where χ is called the group state, and the actual pose matrix of SINS is... SINS' true speed is The actual location of SINS is Special Euclidean groups satisfy the relevant properties of Lie groups and are therefore a type of Lie group; the relevant theories of Lie groups are fully applicable to them.

[0028] The state of the Lie group constructed from the navigation parameters solved by SINS is defined as follows:

[0029]

[0030] The attitude matrix calculated by SINS is: The speed of SINS solution is The location of SINS solution is

[0031] Since there is a clear difference between left and right matrix multiplication, the error η of the Lie group state can be divided into two categories: left-invariant Lie group error and right-invariant Lie group error. Each category of Lie group error can be further divided into two types.

[0032] Left-invariant Lie group error

[0033]

[0034] Right-invariant Lie group error

[0035]

[0036] Left-invariant Lie group error refers to the error when the Lie group state χ and Simultaneously, multiplying by the same matrix on the left results in the Lie group error remaining unchanged; the right-invariant Lie group error refers to the error that remains unchanged when the group state χ and χ are multiplied by the same matrix. At the same time, the Lie group error remains unchanged after multiplying to the right by the same matrix.

[0037] Previous studies have found that the SINS error model defined by the right-invariant Lie group is more suitable for constructing mathematical models of SINS / DVL and SINS / USBL integrated navigation systems, and that compact combinations have more advantages than loose combinations. Therefore, this invention focuses on the research of SINS / DVL / USBL compact integrated navigation systems defined by the right-invariant Lie group error.

[0038] Error from the first type of right-invariant Lie group The definition yields:

[0039]

[0040] Using the relationship between Lie groups and Lie algebras, we can obtain the error vector in the Lie algebra space as follows:

[0041]

[0042] in, Represents the misalignment angle vector. dp represents the velocity error vector in the Lie algebra space under the right-invariant Lie group error definition. r Let represent the position error vector in the Lie algebra space under the right-invariant Lie group error definition. for The magnitude of is , and 'a' is the unit rotation vector.

[0043] As can be seen from (6), due to Therefore, the influence of the misalignment angle is also considered in the velocity and position error vectors of the right-invariant Lie group error. The constructed navigation error is more accurate.

[0044] Assuming the misalignment angle is small, formula (6) can be simplified to

[0045]

[0046] Regarding dv in the above formula r and dp rDifferentiating both sides simultaneously yields

[0047]

[0048] The velocity error differential equation under the definition of the reference frame misalignment angle in the traditional SINS error model is as follows:

[0049]

[0050] The differential equation of the attitude matrix and the definition of the reference frame misalignment angle (φ) in the traditional SINS error model are used to solve the problem. ee′ Substituting the velocity error differential equation (as defined) And ignoring higher-order small quantities, we obtain

[0051]

[0052] Substituting the differential equation of the attitude matrix and the differential equation of the position error in the traditional SINS error model into... And ignoring higher-order small quantities, we obtain

[0053]

[0054] Formulas (10) and (11) are... The velocity error differential equation and position error differential equation are defined under the following conditions. Combining these with the inertial device error, a right-invariant Lie group can be obtained. Define the SINS error state equation.

[0055]

[0056] Wherein, the state transition matrix F r Noise allocation matrix G r The specific form of the noise vector W is as follows:

[0057]

[0058] The velocity measurement model in the DVL beam direction is as follows:

[0059]

[0060] in, This represents the installation deviation matrix, where l is the lever arm between SINS and DVL, and k dvl This represents the calibration coefficient error of DVL. This represents the velocity vectors in the four beam directions measured by DVL, including measurement noise.

[0061] The difference between the velocity solution value of SINS projected onto the four beam directions of DVL and the measured velocity in the four beam directions of DVL is used as the filter measurement.

[0062]

[0063] in, This represents the coordinate transformation matrix calculated by SINS. This indicates the speed of SINS solution. This indicates the location of the SINS solution.

[0064] According to the reference frame misalignment angle φ ee′ By defining and ignoring higher-order minor quantities, the error terms in formula (17) are expanded as follows:

[0065]

[0066] According to the definition of each error vector in the first type of right-invariant Lie group error, the above equation can be written as:

[0067]

[0068] Simplifying the above equations, we obtain the measurement equations for the SINS / DVL compact integrated navigation system under the right-invariant error definition.

[0069] z DVL紧 =H DVL紧,r dx r +υ DVL紧 (20)

[0070]

[0071] Among them, υ DVL紧 This indicates measurement noise.

[0072] For the SINS / DVL compact integrated navigation system under the right-invariant error definition, depth information from the pressure sensor is also used to suppress the divergence of the astronomical position error. The final model of the SINS / DVL compact integrated navigation system under the right-invariant error definition is as follows:

[0073]

[0074] When the measurement information of DVL is updated but the measurement information of USBL is not updated, the SINS / DVL / USBL tightly integrated navigation system model is the same as that in formula (23).

[0075] When the DVL measurement information is not updated, but the USBL measurement information is updated, the SINS / DVL / USBL tightly coupled navigation system model is:

[0076]

[0077] When the measurement information from DVL and USBL is updated simultaneously, the SINS / DVL / USBL tightly coupled navigation system model is:

[0078]

[0079] After installation deviation matrix compensation, the relative position vector calculated by USBL is a physical quantity in the carrier coordinate system. The combination of SINS and DVL can output a relatively accurate carrier system velocity. Therefore, the relationship between the relative position vector measured by USBL and the carrier system velocity is as follows:

[0080]

[0081] In the formula, s and t represent the data update times of two consecutive USBLs, t > s, and ts is the measurement update interval of USBL. and These represent the relative position vectors solved by USBL at time s and time t, respectively. This represents the system velocity calculated by SINS with DVL assistance. δp b This indicates the error in the integral of the carrier system velocity, which is mainly caused by the carrier system velocity error. This represents the installation deviation matrix between SINS and USBL.

[0082] Step 2: Given the initial USBL relative position measurement results, recursively extrapolate the subsequent USBL measurement results based on the carrier system velocity integral;

[0083] According to formula (26), the relative position vector measured by USBL at time t is It can be rewritten as.

[0084]

[0085] Due to error δp b It is unknown, only the velocity of the system at time s and time t is used. and the measurement results of USBL at time s Recursively calculate the relative position vector measured by USBL at time t. There will be a significant error. If δp can be monitored during navigation... b By making predictions, the measurement results of USBL can be accurately recursively extrapolated during the USBL measurement failure period using formula (27), thereby suppressing the divergence of the positioning error of the integrated navigation system.

[0086] Step 3: Introduce a back propagation (BP) neural network to train the navigation data online;

[0087] Due to δp bThe main influences are the velocity error of the carrier system and the angular motion of the carrier, which follow predictable patterns. Therefore, a neural network is considered to analyze δp at different times during the navigation process. b Fitting was performed on δp during USBL failure. b To improve the accuracy of the calculation in formula (27), a prediction is made. The BP neural network is a multi-layer feedforward artificial neural network with error correction backpropagation. It has a strong nonlinear mapping ability for input and output information and good generalization ability. The BP neural network is particularly suitable for fitting and function approximation tasks, and its simple structure and low computational cost make it widely used in various data prediction problems. Therefore, it is considered to construct a BP neural network model during normal operation of USBL, and train the BP neural network model using sensor measurement data to enable it to have the ability to predict δp. b It has good predictive ability. During USBL measurement failure, a trained BP neural network model is used to predict δp. b Prediction is performed, and the relative position information measured by USBL is recursively calculated using formula (27) to suppress the divergence rate of positioning error in the integrated navigation system.

[0088] The BP neural network model training was completed under the condition that SINS, DVL, and USBL were all functioning normally. Under these conditions, the SINS / DVL / USBL integrated navigation algorithm and the SINS / DVL integrated navigation algorithm were run in parallel without interference. Normal navigation output adopted the result of the SINS / DVL / USBL integrated navigation algorithm; the SINS / DVL integrated navigation algorithm was run to simulate the navigation system output under USBL measurement failure conditions for training the BP neural network model.

[0089] Due to δp b The magnitude of this value is mainly related to the carrier system velocity and the carrier's angular motion. Therefore, the training input for the BP neural network model is the integral of the carrier system velocity between two consecutive USBL update times. Transformation matrix of carrier coordinate system The corresponding Euler angle θ and the training output δp at the last USBL update time. b Among them, the speed of the carrier system The output adopts the SINS / DVL integrated navigation algorithm.

[0090] The training output δp of the BP neural network model b The following formula is used for calculation.

[0091]

[0092] In the formula, and This represents the vehicle system velocity and the vehicle coordinate system transformation matrix output by the SINS / DVL integrated navigation system. This represents the projection of the relative position vector of the USBL acoustic array relative to the transponder, calculated by the output of the SINS / DVL / USBL integrated navigation system, onto the carrier coordinate system. The reason for not directly using the USBL measurement output to calculate formula (28) is that the output of the integrated navigation system with a robust filter is smoother and more stable, which is more conducive to ensuring the calculation accuracy of the training output.

[0093] It is expressed as follows

[0094]

[0095] In the formula, Indicates the location of the transponder. and This represents the attitude matrix and position vector output by the SINS / DVL / USBL integrated navigation system. usbl This refers to the lever arm of SINS and USBL.

[0096] The entire BP neural network model training process is as follows:

[0097] (1) Initialize the parameters of the BP neural network model and set the number of training samples N.

[0098] (2) Run the SINS / DVL / USBL integrated navigation algorithm and the SINS / DVL integrated navigation algorithm in parallel. When the USBL measurement data is updated, use the output of the SINS / DVL integrated navigation algorithm to calculate the integral of the vehicle system velocity from the last USBL measurement update time to the current time. Transformation matrix of carrier coordinate system The corresponding Euler angle θ(t) and the training output δp at the last USBL update time. b (tT usbl Construct the training set input samples T usbl The update period for USBL measurement is determined. The training set output sample Y(t) = δp is constructed using formulas (28) and (29). b (t).

[0099] (3) If the training set is not full, add the input and output samples from step (2) to the training set. If the training set is full, delete the oldest sample in the training set and add the input and output samples from step (2) to the training set.

[0100] (4) When USBL measurement fails, the BP neural network model is trained using the existing training set.

[0101] Step 4: During the USBL measurement failure period, the trained BP neural network model is used to predict the recursive error;

[0102] Assume that the USBL measurement data is updated normally at time t, and the USBL measurement becomes invalid after time t. Set a time interval T. u Every T u The relative position vector measurement results of USBL are recursively calculated over time.

[0103] First, the output of the SINS / DVL integrated navigation system is used to calculate the time from t to t+T. u The integral of the velocity of the carrier system projected onto the carrier coordinate system at time t Transformation matrix of carrier coordinate system The corresponding Euler angle θ(t+T) u ).

[0104] Secondly, with θ(t+T u The predicted recursive error at time t) Using a BP neural network as input, the recursion error is predicted. Based on formula (27), we obtain t+T u Recursive results of USBL relative position measurement at time 1

[0105]

[0106] Finally, the SINS / USBL loosely coupled navigation system is constructed using the above USBL measurement recursive results to suppress the divergence of SINS / DVL coupled navigation positioning error.

[0107] Subsequent times every T u The process repeats based on the previous USBL recursive result until the USBL measurement result is updated normally. Furthermore, considering that the error of the USBL recursive result will gradually increase over time, a maximum continuous working time for the positioning error suppression method should be set in actual use. This should be limited when the USBL measurement failure time exceeds T. max At this time, the results of USBL will no longer be recursively predicted to prevent negative impact on the positioning accuracy of the SINS / DVL integrated navigation system.

[0108] Step 5: Perform simulation and experimental verification of the proposed method.

[0109] To verify the effectiveness of the proposed method, the following two algorithms are compared:

[0110] Algorithm 1: Perform SINS / DVL / USBL combined navigation when USBL is not invalid; perform SINS / DVL combined navigation when USBL is invalid.

[0111] Algorithm 2: When USBL is not in failure, perform SINS / DVL / USBL combined navigation; when USBL fails, use the positioning error suppression method introduced to perform navigation calculation.

[0112] Simulation verification:

[0113] A Multiple Adaptive Statistical Similarity Measure Kalman Filter (MASSMKF) based on covariance decomposition and statistical similarity measurement is used as the filter for the integrated navigation system. The measurement noise of the four beam directions of DVL is randomly generated from the following distributions.

[0114]

[0115] Formula (31) indicates that there is a 90% probability that the distribution follows the normal distribution N(0, (0.1m / s) 2 The measurement noise is generated with a 10% probability according to a normal distribution N(0, (2m / s)) with a larger variance. 2 ) Generate measurement noise.

[0116] The azimuth and slant range measurement noise of USBL are randomly generated from the following distributions.

[0117]

[0118] The meaning of formula (32) is similar to that of formula (31). The numerical simulation sensor parameters are shown in Table 1:

[0119] Table 1 Simulation parameter settings

[0120]

[0121]

[0122] The USBL measurement noise is set to zero-mean Gaussian noise, and the filter parameters are set according to the sensor measurement accuracy. The simulated trajectory is as follows: Figure 1 As shown.

[0123] The initial horizontal attitude error was randomly selected within the range of [0°, 10°], and the initial heading error was randomly selected within the range of [0°, 120°]. Fifty Monte Carlo simulations were performed, and the mean and root mean square errors were statistically analyzed.

[0124] 10,000 seconds ago, the SINS, DVL and USBL measurement results were updated normally, with only occasional outliers. After 10,000 seconds, the USBL measurement was disabled.

[0125] The training set sample size is set to 200, the RBF intermediate layer is set to 5 layers, and the time interval T is set to 1. u The simulation results were obtained by setting the time to 10 seconds.

[0126] Figure 2 The figure shows the attitude error curves of the two algorithms. As can be seen from the figure, since the navigation system is always in integrated navigation mode, the attitude errors of both algorithms converge accurately. Furthermore, the attitude error curves of the two algorithms are very similar during the USBL failure period after 10000 seconds. Table 2 shows the RMS attitude error of the two algorithms after 10000 seconds. The table shows that the difference in RMS attitude error between Algorithm 1 and Algorithm 2 is within 0.0001 degrees, indicating that the proposed positioning error suppression method does not adversely affect attitude accuracy.

[0127] Figure 3 The figure shows the position error curves for the two algorithms. It can be seen from the figure that when USBL fails, the positioning error of Algorithm 1 increases significantly. Although the positioning error of Algorithm 2 also increases, it is still significantly smaller than that of Algorithm 1. Table 3 shows the RMS position errors of Algorithm 1 and Algorithm 2 after 10000s. The table shows that the RMS horizontal position error of Algorithm 2 is 6.3954m, which is 43.03% smaller than that of Algorithm 1. Therefore, Algorithm 2 can effectively suppress the increase in positioning error during USBL failure.

[0128] Figure 4 The actual USBL recursion error and the USBL recursion error predicted by the BP neural network were statistically analyzed. As can be seen from the figure, the recursion error predicted by the BP neural network and the actual recursion error have similar trends and their values ​​are relatively close. This is the reason why Algorithm 2 can achieve better positioning accuracy.

[0129] Simulation results show that the proposed method can maintain a higher positioning accuracy than the SINS / DVL integrated navigation system for a longer period of USBL failure, effectively suppressing the divergence of positioning error.

[0130] Table 210000s later, attitude error RMS

[0131]

[0132] Table 3. Position Error RMS after 10000s

[0133]

[0134] Experimental verification:

[0135] The proposed positioning error suppression algorithm was validated using lake test data. The experimental trajectory is as follows: Figure 5 As shown.

[0136] The main experimental equipment included fiber optic SINS, French fiber optic strapdown inertial navigation system (PHINS), DVL, USBL, and GPS. The PHINS / GPS combined system was used as a navigation reference to evaluate the accuracy of the SINS / USBL tightly coupled navigation system.

[0137] Table 4 Parameters of Experimental Equipment on Lake Surface

[0138]

[0139] Due to the favorable experimental environment on the lake, few outliers were observed in the acoustic sensors. To verify the effectiveness of the proposed robust filter, additional outliers were added to the DVL and USBL measurement data with a 10% probability. Specifically, the four beam outliers of the DVL measurement data followed a Gaussian distribution N(0, (1m / s)). 2 Randomly generated from the Gaussian distribution N(0, (2°), the USBL azimuth measurement outliers are derived from the Gaussian distribution N(0, (2°)). 2 Randomly generated from the data, the USBL slope range measurement outliers are derived from a Gaussian distribution N(0, (200m)). 2 The values ​​are randomly generated in the range, and the generation of different measurement field values ​​is independent of each other.

[0140] Using MASSMKF as the filter for the integrated navigation system, a BP neural network was trained using navigation data from 1000s to 1600s, and USBL measurement was interrupted after 1600s. The results of Algorithm 1 and Algorithm 2 are as follows.

[0141] Figure 6 The attitude error curves for Algorithm 1 and Algorithm 2 are similar to the simulation results. Even in the real environment, Algorithm 1 and Algorithm 2 have almost the same attitude accuracy. Figure 7 The figure shows the position error curves for Algorithm 1 and Algorithm 2. The results also indicate that Algorithm 2 can more effectively suppress the divergence of position error. Table 6 shows the RMS position error of Algorithm 1 and Algorithm 2 after 1600s. The results in the table show that the RMS of the horizontal position error of Algorithm 2 is 30.17% smaller than that of Algorithm 1, and the effect of suppressing the divergence of positioning error is obvious.

[0142] The simulation and experimental results show that the proposed positioning error suppression strategy under USBL failure is effective and can maintain the positioning accuracy of the navigation system for a relatively long period of USBL interruption.

[0143] Table 6. Position Error RMS after 1600s

[0144]

[0145] In the description of this specification, references to terms such as "an embodiment," "example," "specific example," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the invention. In this specification, illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples.

[0146] The preferred embodiments of the present invention disclosed above are merely illustrative of the invention. These preferred embodiments do not exhaustively describe all details, nor do they limit the invention to the specific implementations described. Clearly, many modifications and variations can be made based on the content of this specification. This specification selects and specifically describes these embodiments to better explain the principles and practical applications of the invention, thereby enabling those skilled in the art to better understand and utilize the invention. The invention is limited only by the claims and their full scope and equivalents.

Claims

1. A method for suppressing the divergence of positioning errors of a SINS / DVL integrated navigation system when USBL measurement fails, characterized in that, Comprising the following steps: Step one: analyze the relationship between the carrier system velocity and the USBL relative position measurement; Step two: based on the initial USBL relative position measurement, recursively integrate the subsequent USBL measurement results under the given condition; Step three: introduce the BP neural network to train the navigation data online; Step 3.1: initialize the BP neural network model parameters and set the number of training set samples N; Step 3.2: Run the SINS / DVL / USBL integrated navigation algorithm and the SINS / DVL integrated navigation algorithm in parallel, and when the USBL measurement data is updated, use the output of the SINS / DVL integrated navigation algorithm to calculate the body velocity integral from the last time of USBL measurement update to the current time , the carrier coordinate system transformation matrix corresponding Euler angles , and the training output at the last USBL update time , is the USBL measurement update period; Constructing training set input samples ; For the training output of the BP neural network model is calculated using the following formula: wherein, and denotes the body velocity and body coordinate system transformation matrix output by the SINS / DVL integrated navigation system; denotes the projection of the relative position vector of the USBL acoustic array with respect to the transponder calculated by the SINS / DVL / USBL integrated navigation system output in the body coordinate system, , denotes the position of the transponder, and denotes the attitude matrix and position vector output by the SINS / DVL / USBL integrated navigation system, denotes the SINS to USBL lever arm; Constructing training set output samples ; Step 3.3: if the training set samples are not full, add the input and output samples in step 2 to the training set; If the training set samples are full, delete the earliest sample in the training set and add the input and output samples in step 2 to the training set; Step 3.4: when the USBL measurement fails, train the BP neural network model using the existing training set; Step four: during the USBL measurement failure, use the trained BP neural network model to predict the recursive error; Assume that the USBL measurement data is normally updated at time t, and the USBL measurement fails after time t; set a time interval , every time to recursively measure the relative position vector of the USBL; First, the integral of the vehicle velocity in the vehicle coordinate system from time t to time is calculated using the output of the SINS / DVL integrated navigation system ; the Euler angles corresponding to the vehicle coordinate system transformation matrix at time t are calculated ; Secondly, with , and the predicted recursive error value at time t Using a BP neural network as input, the recursion error is predicted. ;Calculation obtained Recursive results of USBL relative position measurement at time: Finally, use the recursive results of the USBL relative position measurement to construct a SINS / USBL loose combination navigation system to suppress the divergence of the SINS / DVL combination navigation positioning error; Every other Time repeats the last procedure based on the last USBL recursive result until the USBL measurement result is normally updated; The longest continuous working time of the positioning error suppression method is set, when the USBL measurement invalid time is greater than the result of the USBL is no longer recursively predicted, thereby preventing negative effects on the positioning accuracy of the SINS / DVL integrated navigation system.

Images