AUV adaptive calibration method based on multi-source confidence evaluation

Abstract

The application discloses a submersible combined navigation adaptive calibration method based on multi-source confidence evaluation, belongs to the technical field of underwater navigation and data processing, and is used for submersible combined navigation, and comprises the following steps: a multi-dimensional ultra-short baseline positioning system confidence evaluation model is established; a comprehensive confidence index is dynamically calculated through signal quality, geometric precision factor quality, environmental parameters and data sequence stability; the Kalman filter observation weight is adaptively adjusted based on the comprehensive confidence index; and a three-level decision mechanism is designed to replace artificial judgment. The application realizes intelligent fusion of the positioning data of the ultra-short baseline positioning system, reduces human participation, improves the autonomy of the submersible navigation and positioning, and further improves the underwater operation efficiency of the submersible.

Description

TECHNICAL FIELD

[0001] The application discloses a submersible combined navigation adaptive calibration method based on multi-source confidence evaluation and belongs to the technical field of underwater navigation and data processing. BACKGROUND

[0002] The deep-sea manned submersible has global ocean exploration capability and can carry scientists and engineering technicians to carry out close-range deep-sea exploration and sampling work. Precise navigation and positioning capability is crucial for the safety and efficiency of the submersible operation. Since electromagnetic waves cannot penetrate thick water layers, the global navigation satellite system-acoustic positioning combined with inertial navigation is the main way of underwater submersible navigation and positioning. Taking the 'Jiaolong' submersible as an example, the inertial navigation system, Doppler velocity meter, depth gauge and ultra-short baseline positioning system combined navigation and positioning mode are currently adopted.

[0003] The deep-sea environment is complex, and is affected by factors such as sound velocity profile error, multipath effect and time delay error, resulting in deviation in underwater acoustic ranging and thus reducing the positioning accuracy. The positioning error of the ultra-short baseline positioning system is about 2 ‰ of the ranging, and the operating depth of the deep-sea manned submersible is generally 1-7 km, so there is great uncertainty in positioning, which reduces the calibration accuracy of the combined navigation system. Unlike the high-reliability global navigation satellite system positioning on land, due to the large positioning error of the ultra-short baseline positioning system, when the submersible is near the bottom, the inertial navigation system + Doppler velocity meter + depth gauge can maintain short-term high-precision navigation after the submersible is given reliable initial positioning information, and the calibration accuracy of the submersible is higher without the ultra-short baseline positioning system. Since the error of the inertial navigation system accumulates with time, when the navigation trajectory deviates significantly from the positioning of the ultra-short baseline positioning system, calibration is needed, at which time the submersible operator needs to determine whether the positioning result of the ultra-short baseline positioning system is a jump point gross error. When the results of several consecutive positioning points are similar, the submersible operator considers that the positioning is reliable, and at this time, the combined navigation system is calibrated by the ultra-short baseline positioning system. However, this mode relies too much on the experience of the submersible operator and needs to determine whether the positioning result of the ultra-short baseline positioning system is reliable, which is not only laborious but also has artificial response delay, increases the operation complexity and reduces the operation efficiency, and is difficult to meet the demand for high-precision autonomous navigation and positioning. SUMMARY

[0004] The purpose of the present application is to provide a submersible combined navigation adaptive calibration method based on multi-source confidence evaluation, to solve the problem in the prior art that the deep-sea manned submersible combined navigation relies too much on the experience of the submersible operator, needs to determine the positioning result of the ultra-short baseline positioning system manually, reduces the operation efficiency and is difficult to meet the demand for high-precision autonomous navigation and positioning.

[0005] The submersible combined navigation adaptive calibration method based on multi-source confidence evaluation comprises the following steps:

[0006] S1, the submersible is a deep-sea manned submersible, the submersible multi-source sensor interface connects an inertial navigation system, a Doppler speedometer, a depth gauge, an ultra-short baseline positioning system and a temperature-salinity-depth instrument, the submersible adopts the ultra-short baseline positioning system to perform underwater acoustic positioning calibration, through real-time compensation for a positioning sound velocity profile and sound ray bending correction by establishing a sound ray tracking differential equation, corrected ultra-short baseline positioning system positioning data is obtained;

[0007] S2, set the submersible integrated navigation system, establish a comprehensive confidence index coupled by four factors based on signal quality factor, geometric precision quality factor, environmental attenuation factor and data stability factor, and perform multi-source confidence evaluation;

[0008] S3, establish an adaptive Kalman filter state model, dynamically set an observation noise covariance matrix according to the comprehensive confidence index, perform robust filter updating based on the dynamic noise covariance matrix, and construct adaptive Kalman filter fusion;

[0009] S4, based on the comprehensive confidence index, set a three-level fusion strategy and a failure protection criterion, and construct a submersible navigation and positioning decision and execution mechanism.

[0010] S1 includes S1.1, the multi-source sensor interface connects the inertial navigation system, the Doppler speedometer, the depth gauge, the ultra-short baseline positioning system and the temperature-salinity-depth instrument;

[0011] S1 includes S1.2, real-time compensation for the positioning sound velocity profile, constructing the sound velocity gradient based on the temperature-salinity-depth instrument data, dividing the water depth into layers, each layer has a thickness of , calculating the sound velocity according to the temperature and salinity:

[0012] ;

[0013] In the formula, is the sound velocity, is the sound velocity of the first layer water depth, is a variable starting from 1 for indexing, is the temperature, is the temperature of the first layer water depth, is the salinity, is the salinity of the first layer water depth, is the average depth of the first layer water depth, is the depth of the first layer water depth, is the depth of the submersible.

[0014] S1 includes, S1.3, using linear interpolation to establish continuous sound speed model :

[0015] ;

[0016] ;

[0017] S1 includes, S1.4, correcting the sound ray bending of the ultra-short baseline positioning system positioning data, solving the sound ray tracking differential equation:

[0018] ;

[0019] ;

[0020] In the formula, is the differential, is the arc length along the sound ray path, is the position vector of the target point on the sound ray, is the sound speed gradient, is the spatial coordinates of the target point; the sound ray tracking differential equation is solved by using the fourth-order Runge-Kutta method;

[0021] S1 includes, S1.5, setting the starting point , and the initial ray angle is determined by the azimuth angle of the ultra-short baseline positioning system, is the starting point coordinates, the cumulative sound ray propagation time and the ultra-short baseline positioning system measurement time satisfy:

[0022] ;

[0023] In the formula, is moment, is the absolute value symbol; output the corrected coordinates replace the original ultra-short baseline positioning system positioning data.

[0024] S2 includes, S2.1, the submersible combined navigation system includes an inertial navigation system, a Doppler velocity meter, a depth gauge and an ultra-short baseline positioning system; when the submersible is on the deck of the mother ship, the inertial navigation system is aligned by the global navigation satellite system, the inertial navigation system enters the navigation mode, after the submersible enters the water, the ultra-short baseline positioning system intervenes in the navigation mode for positioning calibration, the depth gauge intervenes in the navigation mode, the Doppler velocity meter obtains the bottom speed and intervenes in the navigation mode, and the combined navigation mode of the inertial navigation system, the Doppler velocity meter, the depth gauge and the ultra-short baseline positioning system is completed.

[0025] S2 includes, S2.2, establishing a comprehensive confidence index coupled with four factors , signal-to-noise ratio of an ultra-short baseline positioning system round-trip time sequence , roll angle pitch angle horizontal distance to the base array ultra-short baseline positioning system position sequence ultra-short baseline positioning system velocity vector and Doppler velocity meter velocity vector .

[0026] S2 comprises, S2.3, calculating a signal quality factor :

[0027] ;

[0028] wherein is a normalization of the signal-to-noise ratio of the ultra-short baseline positioning system, is a time stability, is a natural constant, is a standard deviation of the round-trip time measurement, is a standard deviation;

[0029] S2 comprises, S2.4, calculating a geometric precision quality factor :

[0030] ;

[0031] ;

[0032] wherein is a geometric precision factor, is a trace of a matrix, is an observation matrix, is a transposition symbol;

[0033] The Jacobian elements of the observation matrix are calculated as follows:

[0034] ;

[0035] ;

[0036] wherein is a partial derivative, is a geometric distance from the user to the base array, is a coordinate position of the base array, is a coordinate position of the user;

[0037] S2 comprises, S2.5, calculating an environmental attenuation factor :

[0038] ;

[0039] wherein, is the exponential attenuation effect of depth on signal strength, is the attitude compensation, is the linear attenuation effect of range on signal strength, is the maximum value;

[0040] S2 includes, S2.6, calculating a data stability factor :

[0041] ;

[0042] ;

[0043] wherein, is the standard deviation of the USBL position sequence, is the USBL position sequence of mean values of directional coordinates, is the USBL position sequence of mean values of directional coordinates, is the Euclidean distance of the USBL velocity vector and the Doppler velocity log velocity vector, is the Euclidean distance, the velocity consistency term uses the Euclidean distance:

[0044] ;

[0045] wherein, are the components in directions, respectively, are the components in directions, respectively; S2 includes, S2.7, performing a multi-source confidence assessment, calculating a composite confidence index :

[0046]

[0047] ;

[0048] wherein, is the adaptive adjustment weight, , using a normalized output;

[0049] a constraint condition is set for : when , it indicates that the submersible is located at the edge of the array, ;

[0050] a constraint condition is set for​​ Set constraints: when meters, the exponential decay effect of depth on signal strength accelerates; when or , ; when meters, zero out;

[0051] Set constraints: on and set thresholds, above and thresholds, .

[0052] S3 includes, S3.1, establishing an adaptive Kalman filter state model:

[0053] ;

[0054] ;

[0055] ;

[0056] where, is the state vector at time , is the state vector, is the error, is the attitude angle, is the velocity, is the position, is the is the state transition matrix from to , is the state transition matrix, is the process noise, is the observation vector at time , is the observation vector, is the observation noise, is the matrix, is the identity matrix. S3 includes, S3.2, dynamically setting the observation noise covariance matrix based on

[0057] :

[0058] ;

[0059] ;

[0060] ;

[0061] ​​​ ;

[0062] wherein is the observation noise covariance matrix, is the observation noise covariance matrix at time is the nominal noise covariance matrix, is an adaptive scaling factor, is a diagonal matrix; when fusing into the state estimate is stopped;

[0063] S3 comprises, S3.3, performing a filter update when :

[0064] ;

[0065] ;

[0066] wherein is the Kalman gain, is the a posteriori estimation error covariance matrix, is the a priori state estimate obtained by model prediction at time ;

[0067] S3 comprises, S3.4, a robust processing mechanism in the filter update, calculating the observation residual:

[0068] ;

[0069] wherein is the observation residual, is the observation residual at time applies a Huber weight function :

[0070] ;

[0071] wherein is the corresponding weight component, is the standard deviation of the observation residual;

[0072] S3 comprises, S3.5, modifying the Kalman gain:

[0073] ;

[0074] ;

[0075] wherein is the modified Kalman gain, is a priori estimation error covariance matrix, is a diagonal weight matrix; based on Realize adaptive calibration of integrated navigation.

[0076] S4 includes, S4.1, set three-level fusion strategy, segment confidence index interval is:

[0077] ;

[0078] is high confidence, full weight fusion of ultra-short baseline positioning system positioning data, so that ;

[0079] is medium confidence, reduced weight fusion of ultra-short baseline positioning system positioning data, so that ;

[0080] is rejected to fuse ultra-short baseline positioning system positioning data, only use inertial navigation system, Doppler velocity meter and depth gauge integrated navigation.

[0081] S4 includes, S4.2, set failure protection mechanism, if more than 10 times in succession , the following operations are performed:

[0082] S4.2.1, start zero speed update, detect the submarine is stationary, when m / s and m / s, the forced velocity observation is zero;

[0083] S4.2.2, acoustic relocation, send a positioning request to the surface mother ship through underwater acoustic communication, receive the ultra-short baseline positioning system beacon signal;

[0084] S4.2.3, mode switching, if not restored within 20 minutes, switch to inertial navigation mode and alarm.

[0085] Compared with the prior art, the present application has the following beneficial effects: the present application realizes intelligent fusion of ultra-short baseline positioning system positioning data, reduces human participation, improves the autonomy of submarine navigation and positioning, and further improves the efficiency of submarine underwater operation. BRIEF DESCRIPTION OF DRAWINGS

[0086] Figure 1 is a flowchart of the present application;

[0087] Figure 2 is an ultra-short baseline positioning system positioning data correction flowchart;

[0088] Figure 3 is a multi-source confidence assessment flowchart;

[0089] Figure 4 is a fusion flowchart of adaptive Kalman filter;

[0090] Figure 5 is a flowchart of constructing a submersible navigation positioning decision and execution mechanism;

[0091] Figure 6 is a submersible horizontal trajectory comparison chart;

[0092] Figure 7 is a depth change comparison chart;

[0093] Figure 8 is a position error comparison chart;

[0094] Figure 9 is a multi-source confidence assessment chart;

[0095] Figure 10 is a fusion strategy decision chart. DETAILED DESCRIPTION

[0096] In order to make the purpose, technical solutions and advantages of the present application clearer, the technical solutions in the present application will be clearly and completely described below. Obviously, the described embodiments are part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor fall within the scope of protection of the present application.

[0097] The submersible combined navigation adaptive calibration method based on multi-source confidence assessment comprises:

[0098] S1, the submersible is a deep-sea manned submersible, and a multi-source sensor interface of the submersible is connected with an inertial navigation system, a Doppler velocity meter, a depth gauge, an ultra-short baseline positioning system and a temperature-salinity-depth instrument. The submersible adopts the ultra-short baseline positioning system to perform underwater acoustic positioning calibration, corrects the acoustic line bending through real-time compensation on the positioning sound velocity profile and establishment of the acoustic line tracking differential equation, and obtains the corrected positioning data of the ultra-short baseline positioning system;

[0099] S2, a submersible combined navigation system is set, a comprehensive confidence index of four-factor coupling is established based on a signal quality factor, a geometric precision quality factor, an environmental attenuation factor and a data stability factor, and multi-source confidence assessment is performed;

[0100] S3, an adaptive Kalman filter state model is established, an observation noise covariance matrix is dynamically set according to the comprehensive confidence index, robust filter updating is performed based on the dynamic noise covariance matrix, and adaptive Kalman filter fusion is constructed;

[0101] S4, set up three-level fusion strategy and failure protection criterion based on comprehensive confidence index, and construct submersible navigation and positioning decision and execution mechanism.

[0102] S1 includes S1.1, connecting inertial navigation system, Doppler speedometer, depth gauge, ultra-short baseline positioning system and temperature-salinity depth gauge through multi-source sensor interface;

[0103] S1 includes S1.2, real-time compensation for positioning sound velocity profile, constructing sound velocity gradient based on temperature-salinity depth gauge data, dividing water depth into layers, each layer thickness is , calculating sound velocity according to temperature and salinity:

[0104] ;

[0105] In the formula, is sound velocity, is the sound velocity of the first layer water depth, is a variable starting from 1 for indexing, is temperature, is the temperature of the first layer water depth, is salinity, is the salinity of the first layer water depth, is the average depth of the first layer water depth, is the depth of the first layer water depth, is the depth of the submersible.

[0106] S1 includes S1.3, establishing continuous sound velocity model using linear interpolation:

[0107] ;

[0108] ;

[0109] S1 includes S1.4, correcting sound ray bending of ultra-short baseline positioning system positioning data, solving sound ray tracking differential equation:

[0110] ;

[0111] ;

[0112] In the formula, is differential, is the arc length along the sound ray path, is the position vector of the target point on the sound ray, is sound velocity gradient, The spatial coordinates of the target point; the sound ray tracking differential equation is solved by using the fourth-order Runge-Kutta method;

[0113] S1 includes S1.5, setting the starting point , and the initial ray angle is determined by the azimuth angle of the ultra-short baseline positioning system, is the starting point coordinates, and the cumulative sound ray propagation time and the ultra-short baseline positioning system measurement time satisfies:

[0114] ;

[0115] In the formula, is moment, is the absolute value symbol; output the corrected coordinates Replace the original ultra-short baseline positioning system positioning data.

[0116] S2 includes S2.1, the submersible combined navigation system includes an inertial navigation system, a Doppler velocity meter, a depth gauge, and an ultra-short baseline positioning system; when the submersible is on the deck of the mother ship, the inertial navigation system is aligned by the global navigation satellite system, the inertial navigation system enters the navigation mode, after the submersible enters the water, the ultra-short baseline positioning system intervenes in the navigation mode for positioning calibration, the depth gauge intervenes in the navigation mode, and the Doppler velocity meter intervenes in the navigation mode after obtaining the bottom speed, completing the combined navigation mode of the inertial navigation system, the Doppler velocity meter, the depth gauge, and the ultra-short baseline positioning system.

[0117] S2 includes S2.2, establishing a four-factor coupled comprehensive confidence index , , collecting the signal-to-noise ratio of the ultra-short baseline positioning system , the round-trip time sequence , , the roll angle , the pitch angle , the horizontal distance from the base array , the ultra-short baseline positioning system position sequence , the ultra-short baseline positioning system velocity vector , and the Doppler velocity meter velocity vector .

[0118] S2 includes S2.3, calculating the signal quality factor :

[0119] ;

[0120] In the formula, is the normalized signal-to-noise ratio of the ultra-short baseline positioning system, is the time stability, is a natural constant, is the standard deviation of the round trip time measurements, is the standard deviation;

[0121] S2 comprises, S2.4, calculating a geometric accuracy quality factor :

[0122] ;

[0123] ;

[0124] wherein, is the geometric accuracy factor, is the trace of a matrix, is the observation matrix, is the transpose symbol;

[0125] The Jacobian elements of the observation matrix are calculated as:

[0126] ;

[0127] ;

[0128] wherein, is the partial derivative, is the geometric distance from the user to the base station, is the coordinate position of the base station, is the coordinate position of the user;

[0129] S2 comprises, S2.5, calculating an environmental attenuation factor :

[0130] ;

[0131] wherein, is the exponential attenuation effect of depth on signal strength, is the attitude compensation, is the linear attenuation effect of distance on signal strength, is the maximum value;

[0132] S2 comprises, S2.6, calculating a data stability factor :

[0133] ;

[0134] ;

[0135] wherein, is the standard deviation of the ultra-short baseline positioning system position sequence, Position sequence for ultra-short baseline positioning system average of directional coordinates, Position sequence for ultra-short baseline positioning system average of directional coordinates, Euclidean distance of velocity vector for ultra-short baseline positioning system and velocity vector for Doppler velocity log, Euclidean distance, velocity consistency term adopts Euclidean distance:

[0136] ;

[0137] wherein, is component in the direction of, is component in the direction of, ;

[0138] S2 includes, S2.7, performing multi-source confidence evaluation, calculating comprehensive confidence index :

[0139] ;

[0140] wherein, is adaptive adjustment weight, , adopting normalized output;

[0141] constraint condition is set for When , it indicates that the submersible is located at the edge of the base array, ;

[0142] constraint condition is set for When meters, the exponential attenuation effect of depth on signal strength is accelerated; when or , ; when meters, zero;

[0143] constraint condition is set for Threshold values are set for and When and threshold values are exceeded, .

[0144] S3 includes, S3.1, establishing an adaptive Kalman filter state model:

[0145] ;

[0146] ​ ;

[0147] ;

[0148] wherein, is the state vector at time, is the state vector, is the error, is the attitude angle, is the velocity, is the position, is the state transition matrix from time to time, is the state transition matrix, is the process noise, is the observation vector at time, is the observation vector, is the observation noise, is the matrix, is the identity matrix.

[0149] S3 comprises, S3.2, setting the observation noise covariance matrix dynamically according to

[0150] ;

[0151] ;

[0152] ;

[0153] ; wherein,

[0154] is the observation noise covariance matrix, is the observation noise covariance matrix at time, is the nominal noise covariance matrix, is the adaptive scaling factor, is a diagonal matrix; when stop fusing into the state estimate; S3 comprises, S3.3, performing filter update when

[0155]

[0156] ;

[0157] ; ​​

[0158] wherein, is the Kalman gain, is the posterior estimation error covariance matrix, is the prior state estimation value predicted by the model at time ;

[0159] S3 comprises, S3.4, a robust processing mechanism in filter update, calculating the observation residual:

[0160] ;

[0161] wherein, is the observation residual, is the observation residual at time, Huber weight function is imposed on residual component :

[0162] ;

[0163] wherein, is the corresponding weight component, is the standard deviation of the observation residual;

[0164] S3 comprises, S3.5, modifying the Kalman gain:

[0165] ;

[0166] ;

[0167] wherein, is the modified Kalman gain, is the prior estimation error covariance matrix, is the diagonal weight matrix; based on to realize adaptive calibration of integrated navigation.

[0168] S4 comprises, S4.1, setting a three-level fusion strategy, dividing the confidence index interval as:

[0169] ;

[0170] is high confidence, the ultra-short baseline positioning system positioning data is fully weighted fusion, so that ;

[0171] is medium confidence, the ultra-short baseline positioning system positioning data is reduced weight fusion, so that ;

[0172] ​When the speed is greater than 0.5 m / s and less than 2 m / s, the fusion of the positioning data of the ultra-short baseline positioning system is rejected, and only the combination of the inertial navigation system, the Doppler velocity meter and the depth gauge is used for navigation.

[0173] S4 includes S4.2, setting a failure protection mechanism, if more than 10 times in succession , the following operations are performed:

[0174] S4.2.1, starting zero speed update, detecting that the submersible is stationary, when more than 0.5 m / s and less than 2 m / s, the speed observation is forced to zero; more than 0.5 m / s and less than 2 m / s, the speed observation is forced to zero;

[0175] S4.2.2, acoustic relocation, sending a positioning request to the surface mother ship through underwater acoustic communication and receiving the ultra-short baseline positioning system beacon signal;

[0176] S4.2.3, mode switching, if the speed is not restored within 20 minutes , switching to the inertial navigation mode and alarming.

[0177] The present application is further described below in conjunction with the accompanying drawings, as shown in the figure, the overall process of the present application is as follows: Figure 1

[0178] Correcting the positioning data of the ultra-short baseline positioning system, as shown in the figure, including sound velocity profile compensation and sound ray tracing, the sound velocity profile compensation includes setting layers and establishing a continuous sound velocity model using linear interpolation, and the sound ray tracing includes setting a sound ray tracing differential equation, solving the sound ray tracing differential equation using the fourth-order Runge-Kutta method and setting boundary conditions. Figure 2

[0179] Based on the signal quality factor, the geometric precision quality factor, the environmental attenuation factor and the data stability factor, a comprehensive confidence index coupled by four factors is established and multi-source confidence evaluation is performed, first, relevant data is acquired, including collecting the signal-to-noise ratio of the ultra-short baseline positioning system signal, the round-trip time sequence, the submersible depth, the roll angle, the pitch angle, the horizontal distance from the base array, the ultra-short baseline positioning system position sequence, the ultra-short baseline positioning system speed vector and the Doppler velocity meter speed vector, as shown in the figure. Figure 3 ​​As shown, the signal quality factor is calculated based on signal-to-noise ratio normalization and time stability, the geometric accuracy quality factor is calculated based on geometric accuracy factor, the environmental attenuation factor is calculated based on depth attenuation, attitude compensation and distance attenuation, the data stability factor is calculated based on position dispersion and speed consistency, the comprehensive confidence index is established by combining the four factors, and multi-source confidence evaluation is carried out; the round trip time sequence is the total time spent by the acoustic signal from the transmitter, received by the target transponder and immediately replied, and then received by the receiver near the original transmitter; the base array is a hydroacoustic beacon array arranged at a known accurate position on the seabed or water surface, and the base array of the present application is located at the position of the mother ship; the position sequence of the ultra-short baseline positioning system is the set of horizontal positions of the five positioning points of the ultra-short baseline positioning system in the sliding window; the sliding window is a technique for operating on a string or array of a certain size, which is implemented on data structures such as queues, stacks or lists, and the start and end positions of the window are adjusted to adapt to different problem requirements; the time stability is the fluctuation or dispersion degree of the measured value of the round trip time of the ultra-short baseline positioning system within a period of time; the Jacobian element in the geometric accuracy quality factor needs to be calculated, and the Jacobian element is used to describe how a multivariate system responds to a small input change; and When the threshold value is m or m / s, the positioning data of the ultra-short baseline positioning system is triggered to reduce the weight.

[0180] An adaptive Kalman filter fusion is constructed, as shown in Figure 4 , which includes establishing an adaptive Kalman filter state model, dynamically setting and adjusting the observation noise covariance matrix according to the comprehensive confidence index, and finally performing robust filter updating. In the robust processing mechanism, the Huber weight function needs to be applied to the residual component; the Huber weight function is a dynamic weight allocation strategy, which is used to balance the least square method and absolute loss in the iterative optimization algorithm.

[0181] Based on the comprehensive confidence index, a three-level fusion strategy and failure protection criterion are set, and a submersible navigation positioning decision and execution mechanism is constructed, as shown in Figure 5 , the three-level fusion strategy of full weight fusion, reduced weight fusion and rejection fusion is set based on the comprehensive confidence index, and the failure protection criterion is that first, continuous low confidence detection is performed, and when the of the positioning data of the ultra-short baseline positioning system is more than 10 times in a row, zero speed update, acoustic relocation and mode switching operations are performed according to the situation.

[0182] The update frequency of the embodiment of the present application is , , 0.5 seconds / time, and the result comparison is as follows:

[0183] As shown in Figure 6 , from the submarine horizontal trajectory contrast, it can be seen that the fused optimized trajectory is basically consistent with the true trajectory, which is better than the inertial navigation system derived trajectory result, which contains its own error accumulation. In the figure, INS is the inertial navigation system, USBL is the ultra-short baseline positioning system, X position is coordinate value in X direction, Y position is coordinate value in Y direction.

[0184] As shown in Figure 7 , from the depth change contrast result, it can be seen that the fused depth, true depth and depth gauge measured depth are basically coincident, which is significantly better than the inertial navigation system depth information.

[0185] As shown in Figure 8 , from the position error contrast result, it can be seen that the inertial navigation system (INS) error accumulates more and more with time, the ultra-short baseline positioning system (USBL) error also increases with the increasing range, which inevitably affects the combination accuracy, while the fused error has a trend of increasing, but due to the weight reduction fusion, the error is controlled within an acceptable range.

[0186] As shown in Figure 9 and Figure 10 , the proposed multi-source confidence evaluation is well reflected in the comprehensive confidence index, which is used for fusion strategy decision, and reflects the three-level fusion strategies of full weight fusion, weight reduction fusion and rejection fusion.

[0187] As can be seen from the performance index comparison of each time period, the root mean square (RMS) error after fusion is significantly better than the root mean square error of the inertial navigation system (INS) and the ultra-short baseline positioning system (USBL), and the overall improvement amplitude is 98.7%, as shown in Table 1:

[0188] Table 1, performance index comparison

[0189] .

[0190] In summary, the submarine combined navigation adaptive calibration method based on multi-source confidence evaluation significantly improves the accuracy and reliability of the submarine combined navigation, and realizes adaptive and automatic combined processing, reduces the complexity of system operation, and is conducive to improving the operation efficiency.

[0191] The above examples are only used for illustrating the technical solutions of the present application, and are not intended to limit the present application. Although the present application has been described in detail with reference to the foregoing examples, it should be understood by those skilled in the art that the technical solutions recorded in the foregoing examples can be modified, or some or all of the technical features can be replaced by equivalent replacements, and 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 application.

Claims

1. A method for adaptive calibration of an underwater vehicle integrated navigation based on multi-source confidence assessment, characterized in that, The method comprises the following steps: S1, the submersible is a deep-sea manned submersible, the submersible multi-source sensor interface is connected with an inertial navigation system, a Doppler speedometer, a depth gauge, an ultra-short baseline positioning system and a temperature-salinity-depth instrument, the submersible adopts the ultra-short baseline positioning system to perform underwater acoustic positioning calibration, the positioning data of the corrected ultra-short baseline positioning system is obtained by real-time compensation on the positioning sound velocity profile and sound ray bending correction on the basis of the establishment of a sound ray tracking differential equation; S2, a submersible integrated navigation system is set, a four-factor coupled comprehensive confidence index is established based on a signal quality factor, a geometric precision quality factor, an environmental attenuation factor and a data stability factor, and multi-source confidence evaluation is performed; S3, an adaptive Kalman filter state model is established, the observation noise covariance matrix is dynamically set according to the comprehensive confidence index, robust filtering update is performed based on the dynamic noise covariance matrix, and adaptive Kalman filter fusion is constructed; S4, a three-level fusion strategy and a failure protection criterion are set based on the comprehensive confidence index, and a submersible navigation positioning decision and execution mechanism is constructed.

2. The multi-source confidence assessment based AUV integrated navigation adaptive calibration method according to claim 1, characterized in that, S1 includes S1.1, the multi-source sensor interface is connected with the inertial navigation system, the Doppler speedometer, the depth gauge, the ultra-short baseline positioning system and the temperature-salinity-depth instrument; S1 includes, S1.2, real-time compensation for the positioning sound velocity profile, constructing the sound velocity gradient based on the data of the thermosalinograph, dividing the water depth into layers, the thickness of each layer is , calculating the sound velocity according to the temperature and salinity: ； where, is the speed of sound, is the speed of sound for the is the speed of sound for the water depth of the is a variable that increments from a starting value of 1 for indexing, is the temperature, is the temperature for the is the temperature for the water depth of the is the salinity, is the salinity for the is the salinity for the water depth of the is the average depth for the is the depth for the is the depth for the is the depth for the is the depth of the submersible.

3. The multi-source confidence assessment based AUV integrated navigation adaptive calibration method according to claim 2, characterized in that, S1 comprises, S1.3, establishing a continuous sound speed model using linear interpolation : ； ； S1 includes S1.4, the sound ray bending of the ultra-short baseline positioning system positioning data is corrected, and the sound ray tracking differential equation is solved: ； ； wherein is the differential, is the arc length along the ray path, is the position vector of the target point on the ray, is the sound speed gradient, is the spatial coordinate of the target point; the ray tracing differential equation is solved using the fourth order Runge-Kutta method; S1 comprises, S1.5, setting the origin and the initial ray angle is determined from the ultra-short baseline positioning system azimuth angle, is the origin coordinate, the cumulative sound ray propagation time and the ultra-short baseline positioning system measurement time satisfies: ； In the formula, is Time, is the absolute value symbol; output corrected coordinates Replace the original ultra-short baseline positioning system positioning data.

4. The multi-source confidence assessment based AUV integrated navigation adaptive calibration method according to claim 3, characterized in that, S2 includes S2.1, the submersible integrated navigation system comprises the inertial navigation system, the Doppler speedometer, the depth gauge and the ultra-short baseline positioning system; when the submersible is on the deck of the mother ship, the inertial navigation system is aligned through the global navigation satellite system, the inertial navigation system enters the navigation mode, after the submersible enters the water, the ultra-short baseline positioning system intervenes in the navigation mode for positioning calibration, the depth gauge intervenes in the navigation mode, the Doppler speedometer intervenes in the navigation mode after obtaining the bottom speed, and the integrated navigation mode of the inertial navigation system, the Doppler speedometer, the depth gauge and the ultra-short baseline positioning system is completed.

5. The multi-source confidence assessment based AUV integrated navigation adaptive calibration method according to claim 4, characterized in that, S2 comprises, S2.2, establishing a four-factor coupled composite confidence index , , collecting ultra-short baseline positioning system signal signal-to-noise ratio , round-trip time sequence , , roll angle , pitch angle , horizontal distance from base array , ultra-short baseline positioning system position sequence , ultra-short baseline positioning system velocity vector and Doppler velocity meter velocity vector .

6. The multi-source confidence assessment based AUV integrated navigation adaptive calibration method according to claim 5, characterized in that, S2 comprises, S2.3, computing a signal quality factor : ； wherein is the ultra-short baseline positioning system signal-to-noise ratio normalization, is the time stability, is the natural constant, is the standard deviation of the round-trip time measurements, is the standard deviation; S2 comprises, S2.4, calculating a geometric accuracy quality factor : ； ； wherein is a geometric dilution of precision, is a trace of a matrix, is an observation matrix, is a transpose symbol; Observation matrix The formula for calculating the Jacobian elements of ； ； wherein is the partial derivative, is the geometric distance from the user to the array, is the coordinate position of the array, is the coordinate position of the user; S2 comprises, S2.5, calculating an environmental attenuation factor : ； wherein is an exponential attenuation effect of depth on signal strength, is a pose compensation, is a linear attenuation effect of distance on signal strength, is a maximum value; S2 comprises, S2.6, calculating a data stability factor : ； ； wherein the standard deviation of the ultra-short baseline positioning system position sequence, the ultra-short baseline positioning system position sequence the average of the directional coordinates, the ultra-short baseline positioning system position sequence the average of the directional coordinates, the Euclidean distance of the ultra-short baseline positioning system velocity vector and the Doppler velocity log velocity vector, the Euclidean distance, the velocity consistency term adopts the Euclidean distance: ； wherein is respectively in the direction of the component in the direction of is respectively in the direction of the component in the direction of S2 comprises, S2.7, performing multi-source confidence assessment, calculating a composite confidence index : ； In the formula, to adaptively adjust the weights, , and adopt normalized output; To Set constraints: when the vehicle is at the edge of the array, ; To set constraints: when depth is greater than 1.5 meters, the exponential decay effect of depth on signal strength accelerates; when or depth is greater than 2.5 meters, the signal strength is zeroed out; when depth is greater than 3.5 meters, the signal strength is zeroed out; Set constraints: on Set constraints: on and Set thresholds, above and When thresholds are exceeded, .

7. The multi-source confidence assessment based AUV integrated navigation adaptive calibration method according to claim 6, characterized in that, S3 includes S3.1, an adaptive Kalman filter state model is established: ； ； ； In the formula, for Time-state vector For state vectors, For error, For attitude angle, For speed, For location, for Time's up The state transition matrix at time t, Here is the state transition matrix. For process noise, for Observation vector at time, For the observation vector, To observe the noise, for matrix, It is an identity matrix.

8. The multi-source confidence assessment based AUV integrated navigation adaptive calibration method according to claim 7, characterized in that, S3 comprises, S3.2, determining, for each of the plurality of sensors, a respective sensor-specific observation noise covariance matrix based on the respective sensor-specific observation noise covariance matrix of the respective sensor, and dynamically setting the observation noise covariance matrix ； ； ； ； wherein is the observation noise covariance matrix, is is the observation noise covariance matrix at time is the nominal noise covariance matrix, is an adaptive scaling factor, is a diagonal matrix; when stop fusing into the state estimate; S3 comprises, S3.3, performing filter update when filter update: ； ； wherein is the Kalman gain, is the posteriori estimation error covariance matrix, is the a priori state estimate obtained by model prediction at time instant is the a posteriori state estimate obtained by model prediction at time instant S3 includes S3.4, a robust processing mechanism in filtering update, and the observation residual is calculated: ； wherein is the observation residual, is the observation residual at time t, the residual component is subjected to a Huber weight function : ； wherein is corresponding weight components, is the standard deviation of the observation residuals; S3 includes S3.5, the Kalman gain is corrected: ； ； wherein is the modified Kalman gain, is the a priori estimation error covariance matrix, is a diagonal weight matrix; based on Adaptive calibration of integrated navigation is implemented.

9. The multi-source confidence assessment based vehicle integrated navigation adaptive calibration method according to claim 8, wherein, S4 includes S4.1, a three-level fusion strategy is set, and the confidence index interval is divided as follows: ； When the confidence level is high, the positioning data of the ultra-short baseline positioning system is fully fused with the weight ; When the confidence level is medium, the positioning data of the ultra-short baseline positioning system is fused with reduced weight ; At this time, the fusion of ultra-short baseline positioning system positioning data is rejected, and only the combination of inertial navigation system, Doppler velocity meter and depth gauge navigation is used.

10. The multi-source confidence assessment based AUV integrated navigation adaptive calibration method according to claim 9, characterized in that, S4 comprises, S4.2, setting a fail-safe mechanism, if 10 times or more in succession the following operation is performed: S4.2.1, start zero velocity update, detect submersible stationary, when m / s and m / s, force velocity observation to zero. S4.2.2, acoustic relocation, the positioning request is sent to the surface mother ship through underwater acoustic communication, and the ultra-short baseline positioning system beacon signal is received; S4.2.3, Mode switch, if not recovered within 20 minutes Switch to inertial navigation mode and alert.

Images