Underwater fusion positioning method based on Bayesian inversion

Abstract

The invention relates to the technical field of underwater positioning, in particular to an underwater fusion positioning method based on Bayesian inversion, which can effectively improve the positioning accuracy, and is characterized by comprising the following steps: obtaining depth information by using a depth meter DG, obtaining seawater temperature and seawater salinity by using a temperature-salinity-depth meter CTD, and obtaining the depth information by using the depth information; the real-time sound velocity information in the seawater is further obtained; the time delay information obtained by four transponders of a long baseline LBL is combined with the sound velocity information and the depth information obtained in the previous step to invert the three-dimensional position information of the AUV; the speed information of the AUV is measured by using the DVL; the position and speed information of the AUV in the north direction, the east direction and the sky direction is obtained through INS calculation; a difference value between position information obtained by an inertial navigation system INS and position information obtained by LBL and a difference value between speed information of the inertial navigation system INS and speed information obtained by DVL serve as observation data, Bayesian inversion is carried out, and target position coordinates are finally obtained through fusion of the observation data and model prior.

Description

Technical Field

[0001] The present invention relates to the field of underwater positioning technology, and in particular to an underwater fusion positioning method based on Bayesian inversion which can effectively improve positioning accuracy. Background technology:

[0002] Underwater positioning is a technology that provides information such as the attitude, speed, and position of an autonomous underwater vehicle (AUV). It is a prerequisite for the AUV to successfully complete various underwater tasks.

[0003] At present, the commonly used positioning methods for underwater navigation mainly include inertial navigation system (INS), hydroacoustic positioning technology, Doppler velocity log (DVL), etc., or the above methods are combined with extended Kalman filter algorithm (EKF) to realize the positioning of a single sensor node. However, due to the complex and changeable underwater environment, a single positioning technology often cannot meet the requirements of long range, multi-target and high precision. Therefore, fusion positioning combining multiple technologies will become the mainstream trend of AUV positioning in the future. Usually two or more position sensors are used to realize multi-source fusion positioning. This fusion positioning system can combine multiple sensors, such as INS, DVL, conductivity temperature depth (CTD) and magnetic compass, including time delay information, moving speed information and depth information; on this basis, the multi-source data such as speed, acceleration and depth of the underwater vehicle are fused to realize the positioning of the underwater vehicle. However, since the measurement noise generated by various measurement signals in the ocean will also change continuously, the measurement results of various measurement signals will have some uncertainties to a greater or lesser extent. Therefore, the fusion method of multi-source information is a key scientific problem that needs to be solved.

[0004] Bayesian theory is an inversion method based on statistics. It has unique advantages in solving inversion problems and can promote the integration of multiple fields. Bayesian inversion is used in geophysics, electromagnetics, fluid mechanics and many other fields. For example, the Chinese patent application number is 202210307474.5, and the patent document named "A joint Bayesian inversion method and system for reservoir lithology and physical property parameters" combines the ideas of lithology classification and physical property parameter estimation, develops a new method for joint Bayesian inversion of reservoir lithology and physical property parameters, realizes quantitative prediction of underground reservoir properties, and thus provides reliable support for fine oil and gas exploration and development; the Chinese patent application number is 202111102996.3, and the patent document named "Micro-nano probe dynamic characteristics compensation method based on Bayesian inversion modeling" realizes the dynamic characteristics compensation of the micro-nano probe system, which can quickly and effectively reduce the overshoot of the probe step output, shorten the probe response time, and improve the dynamic characteristics of the micro-nano probe system at low cost. Because the statistical model of inverse problems is very flexible, it can greatly improve all aspects of scientific and technological production and life, promote the combination of theoretical knowledge and practical applications, and has great research significance and practical value.

[0005] Bayesian inversion is based on randomness and mathematical statistics. It uses the optimal integral method to quantitatively analyze the posterior probability distribution of the model, reflecting the uncertainty of the inversion statistically. The Bayesian inversion method is used for multi-source fusion position estimation. During the inversion, some conventional methods are used to build on the existing information to make the estimated information closer to the real information; finally, the existing observation data is used to correct the obtained information to obtain the fused information. However, due to its complex and changeable marine environment, its positioning accuracy is still restricted by many factors and there is a large uncertainty. Summary of the invention:

[0006] In view of the shortcomings and deficiencies in the prior art, the present invention proposes an underwater fusion positioning method based on Bayesian inversion which is based on Bayesian inversion theory and integrates INS, DVL, long baseline line (LBL), CTD and depth gauge (DG), makes full use of ranging information and AUV multi-sensor information, effectively improves positioning accuracy, and has adaptive capability.

[0007] The present invention is obtained by the following measures:

[0008] An underwater fusion positioning method based on Bayesian inversion, characterized by comprising the following steps:

[0009] Step 1: Use the depth meter DG to obtain depth information, and use the temperature and salinity meter CTD to obtain seawater temperature and salinity, and then obtain real-time sound speed information in seawater;

[0010] Step 2: Use the time delay information obtained by the four transponders of the long baseline LBL and combine it with the sound speed information and depth information obtained in the previous step to invert the three-dimensional position information of the AUV;

[0011] Step 3: Use the Doppler velocimeter DVL to measure the speed information of the AUV; use the INS to calculate the north, east and sky position and speed information of the AUV through the measurement values ​​of the accelerometer and gyroscope;

[0012] Step 4: The difference between the position information obtained by the inertial navigation system INS and the position information obtained by the long baseline LBL and the difference between the velocity information of the inertial navigation system INS and the velocity information obtained by the Doppler velocimeter DVL are used as observation data for Bayesian inversion. After the fusion of the observation data and the model prior, the target position coordinates are finally obtained.

[0013] Step 2 of the present invention specifically includes:

[0014] Step 2-1: Since the DG measurement value is relatively accurate and the error does not accumulate, the LBL positioning method with known depth is used to obtain the forward model based on the geometric relationship: the model formula is as follows:

[0015]

[0016]

[0017] The model parameters are is the two-dimensional position information of the AUV in the carrier coordinate system, and the observation information is:

[0018]

[0019] Among them, (x i ,y i ,z i ), i = 1, 2, 3, 4 are the three-dimensional position information of the four transponders, and (x, y, z) is the three-dimensional position information of the AUV, and the depth information of the AUV z=z is obtained from the first step. DG , d i is the distance between the ith transponder and the AUV, and c is the underwater sound speed, τ i is the time difference between the AUV sending the sound wave and receiving the sound wave from the i-th transponder;

[0020] Step 2-2: Use the coordinates of the four transponders and the AUV depth information as background information to achieve the “delay-distance-coordinate” transformation, and obtain the position coordinates according to the maximum a posteriori estimation algorithm. The formula is as follows:

[0021]

[0022]

[0023] Among them, m0 takes the position coordinate of the previous moment, is the posterior covariance matrix of the model at the previous moment, m MAP To obtain the location information, the data covariance matrix C d Let C be the identity matrix. m is the covariance matrix of the model posterior;

[0024] In step 3 of the present invention, after the three-dimensional position of the AUV is obtained in step 2, the position information in the navigation coordinate system is obtained after coordinate transformation, and the formula is as follows:

[0025]

[0026]

[0027] in is the three-dimensional position of the AUV obtained in step 2, pos LBL is the three-dimensional position of the AUV after coordinate transformation, x LBL is the x-axis coordinate obtained by LBL Bayesian inversion, y LBL is the y-axis coordinate obtained by LBL Bayesian inversion, z DG is the depth information obtained by DG, L LBL , LBL 、h LBL They are After the coordinate transformation, the AUV's position in the east, north, and sky directions is: is the transformation matrix between the carrier coordinate system and the navigation coordinate system.

[0028] In step 3 of the present invention, the three-dimensional velocity of the AUV is obtained by using the two-dimensional velocity information obtained by the DVL and the z-axis velocity information obtained by differentiating the depth meter. Coordinate transformation is also required to obtain the three-dimensional velocity in the navigation coordinate system. The formula is as follows:

[0029]

[0030]

[0031] The velocity of the sound wave is c, θ is the incident angle of the sound wave, and f0 is the signal frequency emitted by the DVL. are the x-axis, y-axis, and z-axis speeds respectively, They are the north axis, east axis, and celestial axis velocities respectively.

[0032] The step 4 of the present invention specifically includes:

[0033] Step 4-1: Take the difference between the INS position and the LBL position and the difference between the INS velocity and the DVL velocity information as the EKF measurement value:

[0034] For INS, in addition to selecting attitude error, velocity error and position error as state variables, the zero bias of the gyroscope and accelerometer are also selected as the extension of the state variables, and the state vector is obtained as follows:

[0035]

[0036] Using EKF as the state update algorithm, the continuous-time system can be expressed as:

[0037]

[0038] Where Z represents the observed quantity, H is the observation matrix, Φ is the state transfer matrix, W and V are random noise;

[0039]

[0040] Among them, L INS , INS 、h INS Indicates the position of the AUV in the east, north, and sky directions measured by the INS. It indicates the speed of AUV in the east, north and sky directions measured by INS. δL, δλ and δh indicate the actual error of the position output by INS, δv N ,δv E ,δv U They represent the actual errors of INS output velocity, δL1, δλ2, and δh3 are the observation noises of the position measured by LBL in the east, north, and sky directions, δv1, δv2, and δv3 are the observation noises of the velocity measured by DVL in the east, north, and sky directions, respectively; H is the LBL position observation and DVL velocity observation matrix, X is the state matrix, and V is the random noise;

[0041] Step 4-2: Take the relationship between the state vectors before and after EKF prediction as the forward model, perform Bayesian inversion on this basis, and correct it according to the observations obtained in the previous step. The specific formula is as follows:

[0042]

[0043]

[0044]

[0045] in, is the estimated value of X at time k-1, It uses the k-1 state matrix to estimate X at time k. yes The covariance matrix of is the prior covariance matrix of the model parameters, is the prior value of the model parameter, K k is the filter gain reflecting the prediction and update weights;

[0046] Step 4-3: After the fusion of observation data and model priors, the target position coordinates are finally obtained: the specific formula is as follows:

[0047]

[0048] Where L INS , INS 、h INS are the north, east and sky position information of the AUV obtained by INS in step 3, They are the position errors in the north, east and celestial directions obtained in the previous step respectively.

[0049] The multi-source fusion positioning method of Bayesian inversion proposed in the present invention integrates INS, DVL, LBL, CTD and DG, expands the difference between the position of INS and the position of LBL and the difference between the speed of INS and the speed measurement information of DVL as measurement values, performs Bayesian inversion, and then obtains the position information. This method can effectively improve the positioning accuracy. Description of the drawings:

[0050] Attached Figure 1 It is a multi-source fusion block diagram of the present invention.

[0051] Attached Figure 2 This is the Bayesian inversion method flow used in the present invention.

[0052] Attached Figure 3 It is a comparison diagram of the predicted trajectory in the embodiment of the present invention and the actual trajectory and the trajectory predicted by other methods.

[0053] Attached Figure 4 is a curve diagram of the mean square error in the embodiment of the present invention and the mean square error of other methods. Specific implementation method:

[0054] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0055] At present, single-source positioning technology has many shortcomings, such as low positioning accuracy or cumulative errors. Therefore, the present invention uses multi-source fusion positioning technology to integrate these positioning technologies, take advantage of their strengths and make up for their weaknesses, and provide more reliable positioning information. The present invention proposes an underwater fusion positioning method based on Bayesian inversion to make up for the shortcomings of single positioning source positioning and effectively improve positioning accuracy.

[0056] The multi-source fusion technology used in the present invention is as follows Figure 1 As shown in the figure, INS, DVL, LBL, CTD and DG are integrated. It is an underwater fusion positioning algorithm that covers more positioning sources and has a wider application value. The specific steps are:

[0057] (1) Use DG to obtain depth information z = z DG , using the seawater temperature and salinity obtained by CTD, the temperature (T℃), salinity (S‰, thousandths) and depth h (m) data combined with the sound speed empirical model can obtain the real-time sound speed information in seawater;

[0058] (2) The time delay information obtained by the four transponders of LBL combined with the sound speed information and depth information obtained in the previous step can be used to invert the three-dimensional position information of the AUV; the Bayesian inversion algorithm used in the present invention is as follows: Figure 2 shown.

[0059] Since the DG measurement value is relatively accurate and the error does not accumulate, the LBL positioning method with known depth is used to obtain the forward model according to the geometric relationship. The formula is as follows:

[0060]

[0061]

[0062] The model parameters are is the two-dimensional position information of the AUV in the carrier coordinate system. The observation information is:

[0063]

[0064] Among them, (x i ,y i ,z i ), i = 1, 2, 3, 4 are the three-dimensional position information of the four transponders, and (x, y, z) is the three-dimensional position information of the AUV, and the depth information of the AUV z=z is obtained from the first step. DG .d i is the distance between the ith transponder and the AUV, and c is the underwater sound speed, τ iIt is the time difference between the AUV sending the sound wave and receiving the sound wave from the i-th transponder.

[0065] Then, the coordinates of the four transponders and the AUV depth information are used as background information to realize the transformation of “delay-distance-coordinate”. According to the maximum a posteriori estimation algorithm, the position coordinates of the target are obtained. The formula is as follows:

[0066]

[0067]

[0068] Among them, m0 takes the position coordinate of the previous moment, is the posterior covariance matrix of the model at the previous moment, m MAP To obtain the location information, the data covariance matrix C d Let C be the identity matrix. m is the covariance matrix of the model posterior.

[0069] The three-dimensional position of the AUV can be obtained through the above steps, and the position information in the navigation coordinate system can be obtained after coordinate transformation. The formula is as follows:

[0070]

[0071]

[0072] in is the 3D position of the AUV obtained in the previous step, pos LBL is the three-dimensional position of the AUV after coordinate transformation, x LBL is the x-axis coordinate obtained by LBL Bayesian inversion, y LBL is the y-axis coordinate obtained by LBL Bayesian inversion, z DG is the depth information obtained by DG, L LBL , LBL 、h LBL They are After the coordinate transformation, the AUV's position in the east, north, and sky directions is: is the transformation matrix between the carrier coordinate system and the navigation coordinate system.

[0073] (3) Use the DVL to measure the speed information of the AUV; use the INS to calculate the north, east and sky position and speed information of the AUV through the measurement values ​​of the accelerometer and gyroscope;

[0074] The three-dimensional velocity of the AUV can be obtained by the two-dimensional velocity information obtained by the DVL and the z-axis velocity information obtained by the differentiation of the depth meter. Coordinate transformation is also required to obtain the three-dimensional velocity in the navigation coordinate system. The formula is as follows:

[0075]

[0076]

[0077] The velocity of the sound wave is c, θ is the incident angle of the sound wave, and f0 is the signal frequency emitted by the DVL. are the x-axis, y-axis, and z-axis speeds respectively, They are the north axis, east axis, and celestial axis velocities respectively.

[0078] (4) The difference between the position information obtained by INS and the position information obtained by LBL and the difference between the speed information obtained by INS and the speed information obtained by DVL are used as the observation data of EKF, and then Bayesian inversion is performed to finally obtain the position information of the target. Specifically:

[0079] For INS, in addition to selecting attitude error, velocity error and position error as state variables, the zero bias of the gyroscope and accelerometer are also selected as the extension of the state variables, and the state vector is obtained as follows:

[0080]

[0081] Using EKF as the state update algorithm, the continuous-time system can be expressed as:

[0082]

[0083] Where Z represents the observation, H is the observation matrix, Φ is the state transfer matrix, and W and V are random noises.

[0084]

[0085] Among them, L INS , INS 、h INS Indicates the position of the AUV in the east, north, and sky directions measured by the INS. It indicates the speed of AUV in the east, north and sky directions measured by INS. δL, δλ and δh indicate the actual error of the position output by INS, δv N ,δv E ,δv U They represent the actual errors of INS output velocity, δL1, δλ2, and δh3 are the observation noises of the position measured by LBL in the east, north, and sky directions, and δv1, δv2, and δv3 are the observation noises of the velocity measured by DVL in the east, north, and sky directions. H is the LBL position observation and DVL velocity observation matrix, X is the state matrix, and V is the random noise.

[0086] Then the relationship between the state vectors before and after the EKF prediction is used as the forward model, and Bayesian inversion is performed on this basis, and it is corrected according to the Z obtained in the previous step. The specific formula is as follows:

[0087]

[0088]

[0089]

[0090] in, is the estimated value of X at time k-1, It uses the k-1 state matrix to estimate X at time k. yes The covariance matrix of is the prior covariance matrix of the model parameters, is the prior value of the model parameter. k is the filter gain that reflects the prediction and update weights.

[0091] After the fusion of observation data and model priors, the final position coordinates are obtained. The specific formula is as follows:

[0092]

[0093] Where L INS , INS 、h INS are the north, east and sky position information of the AUV obtained by INS in step 3, They are the position errors in the north, east and celestial directions obtained in the previous step respectively.

[0094] The performance of the technical solution of the present invention is analyzed below, specifically illustrated by simulation verification, and the simulation results are as follows:

[0095] The AUV is equipped with INS equipment, four-beam DVL equipment, LBL positioning system, CTD and DG. The INS data update time period is 0.005 seconds, the DVL data update time period is 0.1 seconds, the LBL data update time period is 0.15 seconds, and the movement time is 250 seconds. The comparison curves of the trajectories obtained by using INS alone, INS / DVL fusion positioning, INS / DVL, EKF and multi-source fusion positioning methods are shown in the figure. Figure 3 As shown, when the method of the present invention is used, the predicted curve is always relatively close to the true curve.

[0096] according to Figure 3 The following is an analysis of the relationship between error and time. Figure 4As shown in the figure, it can be seen that the method of the present invention has smaller errors than using INS alone, INS / DVL fusion positioning, INS / DVL, and using EKF to estimate the position, which shows that the performance of the fusion positioning method is better than single positioning source positioning and fusion positioning. The positioning result of the Bayesian inversion algorithm is better than that of EKF, and the fluctuation of the inversion positioning method is smaller than that of EKF positioning, and the stability of the positioning result is also better than that of EKF, which shows that the Bayesian inversion multi-source fusion method proposed in the present invention is better than the EKF algorithm. Moreover, the error curve trend of the other three algorithms is still rising, while the rising trend of the error curve of multi-source fusion positioning is not obvious. As time goes by, this difference becomes more obvious, which shows that the multi-source fusion positioning method is better at suppressing the cumulative error of inertial navigation.

[0097] The present invention adopts a multi-source fusion positioning method of Bayesian inversion, integrates INS, DVL, LBL, CTD and DG, expands the position difference between INS and LBL and the speed difference between INS and DVL as the observation information of EKF, and performs Bayesian inversion. Compared with the traditional single-source positioning method, the present invention can effectively improve the positioning accuracy.

Claims

1. An underwater fusion positioning method based on Bayesian inversion, characterized in that: The following steps are involved: Step 1: Use the depth gauge DG to obtain depth information, and use the temperature and salinity meter CTD to obtain seawater temperature and salinity, and then obtain real-time sound speed information in seawater; Step 2: Use the time delay information obtained by the four transponders of the long baseline LBL and combine it with the sound speed information and depth information obtained in the previous step to invert the three-dimensional position information of the AUV; Step 3: Use the Doppler velocimeter DVL to measure the speed information of the AUV; use the INS to calculate the north, east and sky position and speed information of the AUV through the measurement values ​​of the accelerometer and gyroscope; Step 4: The difference between the position information obtained by the inertial navigation system INS and the position information obtained by the long baseline LBL and the difference between the velocity information of the inertial navigation system INS and the velocity information obtained by the Doppler velocimeter DVL are used as observation data for Bayesian inversion. After the fusion of the observation data and the model prior, the target position coordinates are finally obtained.

2. The underwater fusion positioning method based on Bayesian inversion according to claim 1 is characterized in that: The step 2 specifically includes: Step 2-1: Since the DG measurement value is relatively accurate and the error does not accumulate, the LBL positioning method with known depth is used to obtain the forward model based on the geometric relationship: the model formula is as follows: The model parameters are is the two-dimensional position information of the AUV in the carrier coordinate system, and the observation information is: Among them, (x i ,y i ,z i ), i = 1, 2, 3, 4 are the three-dimensional position information of the four transponders, and i = 1, 2, 3, 4, (x, y, z) is the three-dimensional position information of the AUV, and the depth information of the AUV z = z obtained in the first step DG , d i is the distance between the ith transponder and the AUV, and i=1,2,3,4, c is the underwater sound speed, τ i is the time difference between the AUV sending the sound wave and receiving the sound wave from the i-th transponder; Step 2-2: Take the coordinates of the four transponders and the AUV depth information as background information to achieve the "delay-distance-coordinate" transformation, and obtain the position coordinates according to the maximum a posteriori estimation algorithm. The formula is as follows: Among them, m0 takes the position coordinate of the previous moment, is the posterior covariance matrix of the model at the previous moment, m MAP To obtain the location information, the data covariance matrix C d Let C be the identity matrix. m is the covariance matrix of the model posterior.

3. The underwater fusion positioning method based on Bayesian inversion according to claim 1 is characterized in that: Step 3: After the three-dimensional position of the AUV is obtained in step 2, the position information in the navigation coordinate system is obtained after coordinate transformation. The formula is as follows: in is the three-dimensional position of the AUV obtained in step 2, pos LBL is the three-dimensional position of the AUV after coordinate transformation, x LBL is the x-axis coordinate obtained by LBL Bayesian inversion, y LBL is the y-axis coordinate obtained by LBL Bayesian inversion, z DG is the depth information obtained by DG, L LBL , LBL 、h LBL They are After the coordinate transformation, the AUV's position in the east, north, and sky directions is: is the transformation matrix between the carrier coordinate system and the navigation coordinate system.

4. The underwater fusion positioning method based on Bayesian inversion according to claim 1 is characterized in that: In step 3, the three-dimensional velocity of the AUV is obtained by using the two-dimensional velocity information obtained by the DVL and the z-axis velocity information obtained by the differentiation of the depth meter. Coordinate transformation is also required to obtain the three-dimensional velocity in the navigation coordinate system. The formula is as follows: The velocity of the sound wave is c, θ is the incident angle of the sound wave, and f0 is the signal frequency emitted by the DVL. are the x-axis, y-axis, and z-axis speeds respectively, They are the north axis, east axis, and celestial axis velocities respectively.

5. The underwater fusion positioning method based on Bayesian inversion according to claim 1 is characterized in that: The step 4 specifically includes: Step 4-1: Take the difference between the INS position and the LBL position and the difference between the INS velocity and the DVL velocity information as the EKF measurement value: For INS, in addition to selecting attitude error, velocity error and position error as state variables, the zero bias of the gyroscope and accelerometer are also selected as the extension of the state variables, and the state vector is obtained as follows: Using EKF as the state update algorithm, the continuous-time system can be expressed as: Where Z represents the observed quantity, H is the observation matrix, Φ is the state transfer matrix, W and V are random noise; Among them, L INS , INS 、h INS Indicates the position of the AUV in the east, north, and sky directions measured by the INS. It indicates the speed of AUV in the east, north and sky directions measured by INS. δL, δλ and δh indicate the actual error of the position output by INS, δv N ,δv E ,δv U They represent the actual errors of INS output velocity, δL1, δλ2, and δh3 are the observation noises of the position measured by LBL in the east, north, and sky directions, δv1, δv2, and δv3 are the observation noises of the velocity measured by DVL in the east, north, and sky directions, respectively; H is the LBL position observation and DVL velocity observation matrix, X is the state matrix, and V is the random noise; Step 4-2: Take the relationship between the state vectors before and after EKF prediction as the forward model, perform Bayesian inversion on this basis, and correct it according to the observations obtained in the previous step. The specific formula is as follows: in, is the estimated value of X at time k-1, It uses the k-1 state matrix to estimate X at time k. yes The covariance matrix of is the prior covariance matrix of the model parameters, is the prior value of the model parameter, K k is the filter gain reflecting the prediction and update weights; Step 4-3: After the fusion of observation data and model priors, the target position coordinates are finally obtained: the specific formula is as follows: Where L INS , INS 、h INS are the north, east and sky position information of the AUV obtained by INS in step 3, They are the position errors in the north, east and celestial directions obtained in the previous step respectively.

Images