Bayesian filtering-based submarine topography and gravity and magnetism fusion matching method, program, equipment and storage medium

Through Bayesian filtering technology, the robustness and accuracy of seabed positioning are solved, and stronger applicability and positioning accuracy are achieved, and the limitations of a single auxiliary positioning method are avoided.

CN118999547AActive Publication Date: 2024-11-22HARBIN ENG UNIV
View PDF 4 Cites 0 Cited by

Patent Information

Application Number
CN202411264086.9
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-09-10
Publication Date
2024-11-22
Estimated Expiration
2044-09-10

AI Technical Summary

Technical Problem

The existing subsea gravity, geomagnetic and terrain-assisted positioning technologies have problems such as insufficient positioning robustness, limited applicable scenarios, and poor positioning accuracy. Especially when relying solely on gravity, geomagnetic or terrain-assisted positioning, the changes in prior map information are not obvious. Causes mispositioning and poor positioning stability.

Method used

The fusion matching method of seabed terrain and heavy magnetism based on Bayesian filtering is used to adjust the reference point cloud through graph isomorphism to correspond one by one with the seabed terrain point cloud, and the rotation translation change parameters and Solving the error covariance matrix, combined with the inertial navigation system, closed-loop correction is performed to achieve optimal estimation of position.

Benefits of technology

It improves the accuracy and robustness of the selection of corresponding points, avoids mismatch problems, ensures positioning accuracy and stability, enhances applicability, and maintains good matching capabilities when information richness is insufficient.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118999547A_ABST
    Figure CN118999547A_ABST
Patent Text Reader

Abstract

The invention belongs to the technical field of underwater multi-sensor fusion matching positioning, and particularly relates to a submarine topography and gravity and magnetism fusion matching method based on Bayesian filtering, a program, equipment and a storage medium. According to the method, submarine topography corresponding point selection is converted into a graph isomorphism problem, so that the accuracy and robustness of corresponding point selection are improved; a seabed gravity and magnetic information corresponding point selection algorithm based on isoline nearest point iteration is adopted, so that the accuracy of corresponding point selection is improved; a point set registration algorithm based on filtering is adopted, solution of rotation and translation parameters is converted into a filtering problem, the rotation and translation parameters are explored in a dynamic space, nonlinear Bayesian filtering is adopted to optimize the problem, and the algorithm is converged to a globally optimal solution; and performing data fusion on the gravity and magnetic matching result, the terrain matching result and the inertial navigation result by adopting Bayesian filtering to obtain optimal estimation of the pose.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention belongs to the technical field of underwater multi-sensor fusion matching and positioning, and specifically relates to a method, program, device and storage medium for fusion matching of seabed topography and gravity and magnetism based on Bayesian filtering. Background Art

[0002] With the development of marine geophysical equipment and underwater robot technology, AUV marine geophysical positioning technology has been widely used. Common ones include marine gravity-assisted positioning, marine geomagnetic-assisted positioning, and seabed terrain-assisted positioning. Among them, marine gravity and geomagnetic-assisted positioning have the problem of mismatching caused by high likelihood of gravity and magnetic chart data and too many equivalent points. Seabed terrain-assisted positioning has the problem that point cloud matching relies heavily on the selection of initial corresponding points and the registration process easily converges to the local minimum, which ultimately leads to positioning failure. In addition, relying solely on gravity, geomagnetism, and terrain will lead to mispositioning, poor positioning stability, and a small scope of application because the prior map information does not change significantly. Moreover, the existing auxiliary positioning methods cannot give accurate positioning error statistical characteristics of multiple auxiliary positioning, making it difficult to achieve optimal pose estimation.

[0003] Therefore, it is urgent to propose a fusion matching method of seabed topography, gravity and magnetism that can achieve optimal posture estimation to solve the above problems. Summary of the invention

[0004] The purpose of the present invention is to solve the problems of insufficient positioning robustness, limited applicable scenarios, poor positioning accuracy, etc. in the existing seabed gravity, geomagnetism and terrain-assisted positioning, and to provide a fusion matching method of seabed topography and gravity and magnetism based on Bayesian filtering.

[0005] A fusion matching method of seabed topography and gravity and magnetism based on Bayesian filtering includes the following steps:

[0006] Step 1: The underwater vehicle collects inertial navigation system data, multi-beam sonar sounding data, gravity anomaly data and magnetic field intensity data while navigating in the target area; processes the collected data to obtain the seabed topography point cloud X, the measurement point sequence n of gravity anomaly and magnetic field intensity; extracts the benchmark point cloud Y, benchmark gravity anomaly map and benchmark magnetic field intensity map of the target area from the map library;

[0007] Step 2: Represent the seabed topography point cloud X and the reference point cloud Y with graphs, and obtain the corresponding adjacency matrix A x and A y ; Using graph isomorphism, solve A through the Monte Carlo framework x With A y The permutation matrix B is obtained; the reference point cloud Y is adjusted according to the permutation matrix B so that the reference point cloud Y corresponds to the points in the seabed topography point cloud X one by one;

[0008] Step 3: For the measurement point n in the measurement point sequence n of gravity anomaly and magnetic field intensity x , respectively find points on the benchmark gravity anomaly map and the benchmark magnetic field intensity map that are equal to the measured value of the measuring point to form two contour lines, and save the intersection of the two contour lines as the corresponding point n′ of the point x , and finally obtain the corresponding point set n′ of all measuring points;

[0009] Step 4: Model the registration problems of X and Y, n and n′ as filter models respectively, solve them by Bayesian filtering, and obtain the rotation and translation change parameters and error covariance matrix corresponding to the filter models;

[0010] Step 5: Use the rotation and translation change parameters of X and Y to correct the seabed topography point cloud X to obtain the seabed topography matching result; use the rotation and translation change parameters of n and n′ to correct the measurement point sequence n of gravity anomaly and magnetic field intensity to obtain the matching result of gravity anomaly and magnetic field intensity;

[0011] Step 6: After weighting the seabed terrain matching results, gravity anomaly and magnetic field strength matching results according to the corresponding error covariance matrix, the position and attitude measurement information of the underwater vehicle is obtained; the difference between the position and attitude measurement information of the underwater vehicle and the position and attitude of the underwater vehicle output by the inertial navigation system is used as the observation information, and the position and attitude error of the inertial navigation system at each moment is used as the state, which is modeled as a filtering model and solved using the extended Kalman filter to obtain the optimal estimate of the position and attitude error of the inertial navigation system; the position and attitude error is fed back to the inertial navigation system for closed-loop correction to obtain the optimal estimate of the position and attitude of the underwater vehicle.

[0012] Furthermore, the method of representing the seabed topography point cloud X and the reference point cloud Y with graphs in step 2 is specifically as follows: the point cloud is represented by a graph G=(V, E, W), V is the vertex set of the graph, E is the edge set of the graph, and W is the weight set of the edge; the vertex of the graph is the position of the point in the point cloud, and the edge of the graph is a function of the distance between two points in the point cloud. If the distance between the two points is less than a set threshold, there is an edge between the two points, and the weight of the edge is the square of the distance between the two points.

[0013] Furthermore, in step 2, graph isomorphism is used to solve A through the Monte Carlo framework. x With A y The method of permuting the matrix B is as follows:

[0014] The constraints of the graph isomorphism problem are:

[0015] min Tr(W T B)

[0016] BA x =A y B

[0017] B1=1,B T 1=1

[0018] B ij ≥0

[0019] B ij =0, (i, j)∈S

[0020] Among them, the graph of the seabed topography point cloud X represents the corresponding adjacency matrix A x The number of vertices in is m, and the graph of the reference point cloud Y represents the corresponding adjacency matrix A y The number of vertices in is n; B∈R n×m is a matrix whose elements are all non-negative, and the sum of the elements in each row and the sum of the elements in each column of the matrix is ​​1; W∈R n×m is a random matrix with independent Gaussian distribution elements; (i, j)∈S means that if the point x in X i and point y in Y i It is not a corresponding point, and is determined by the pruning strategy, specifically:

[0021] (1) If point x i With point y i The difference in the number of sides of is greater than 4, then the point x i With point y i It is not a corresponding point;

[0022] (2) If point x i With point y i The weight difference is greater than Then point x i With point y i is not a corresponding point; c is a constant,

[0023] Furthermore, in step 4, the registration problems of X and Y, and n and n′ are respectively modeled as filtering models, specifically:

[0024] x k =x k-1 +v(k-1)

[0025]

[0026] Among them, u is the source point set, y k is the target point set; if it is a registration problem between X and Y, then u=X,y k =Y; if it is a registration problem of n and n′, then u = n, y k =n′; x k is the state of the kth iteration, v(k-1)∈Ν(x k-1; 0,Q K-1 ), e(k) is the Gaussian white noise observed at the kth iteration, e(k)∈N(y k ; 0,R k ),

[0027] Furthermore, the Bayesian filter is used to solve the problem in step 4 as follows:

[0028] Step 4.1: Initialize k = 1 and initialize x 0|0 , P 0|0 , Q0 and R0;

[0029] Step 4.2: Status update;

[0030]

[0031] P k|k-1 =P k-1|k-1 +Q k-1

[0032] Step 4.3: P k|k-1 Decompose into S k|k-1 and

[0033]

[0034] Step 4.4: Estimate point X i,k|k-1 ;

[0035]

[0036] Among them, ξ i is the volume point,

[0037] Step 4.5: Estimate the propagation point Y i,k|k-1 ;

[0038]

[0039] Step 4.6: Estimate the predicted observations

[0040]

[0041] Step 4.7: Estimate the observation variance matrix P yy,k|k-1 ;

[0042]

[0043] Step 4.8: Estimate the cross-variance matrix P xy,k|k-1 ;

[0044]

[0045] Step 4.9: Estimate the Bayesian gain K k ;

[0046]

[0047] Step 4.10: Estimate the rotation and translation change parameters and the error covariance matrix P k|k ;

[0048]

[0049] Step 4.11: If Then let Q k =0.9Q k-1 , k=k+1, return to step 4.2; otherwise, output the rotation and translation change parameters and the error covariance matrix P k|k .

[0050] Furthermore, in step 6, the seabed terrain matching result, the gravity anomaly and the magnetic field strength matching result are weighted according to the corresponding error covariance matrix to obtain the position and posture measurement information of the underwater vehicle. Specifically:

[0051]

[0052] Among them, P1 and P2 are the error covariance matrices of X and Y, n and n′ obtained in step 4 respectively; X1, Y1, θ1 and X2, Y2, θ2 are the seabed topography matching results and the matching results of gravity anomaly and magnetic field intensity obtained in step 5 respectively.

[0053] Furthermore, in step 6, the difference between the underwater vehicle's posture measurement information and the underwater vehicle's posture output by the inertial navigation system is used as observation information, and the posture error of the inertial navigation system at each moment is used as the state, and the model is modeled as a filtering model specifically as follows:

[0054] x k =f(x k-1 )+w k-1

[0055] z k =h(x k )+v k

[0056] in is the position error of the inertial navigation system, which is the position error along the X and Y axes and the angle error along the Z axis; f(x k ) is the error analysis equation of the inertial navigation system; w k-1is the state noise; is the difference between the underwater vehicle's attitude measurement information and the underwater vehicle's attitude output by the inertial navigation system; h(x k )=x k ; is the pose error of matching positioning.

[0057] A computer device / equipment / system includes a memory, a processor and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the above-mentioned Bayesian filtering-based seabed topography and gravity and magnetic fusion matching method.

[0058] A computer-readable storage medium stores a computer program / instruction thereon, which, when executed by a processor, implements the steps of the above-mentioned method for fusion matching of seabed topography and gravity and magnetism based on Bayesian filtering.

[0059] A computer program product includes a computer program / instruction, which, when executed by a processor, implements the steps of the above-mentioned method for fusion matching of seabed topography and gravity and magnetism based on Bayesian filtering.

[0060] The beneficial effects of the present invention are:

[0061] The present invention converts the seabed terrain corresponding point selection into a graph isomorphism problem, so that the points in the seabed terrain point cloud are no longer isolated points, but elements that have a relationship with other points, thereby improving the accuracy and robustness of the corresponding point selection; the seabed gravity and magnetic information corresponding point selection algorithm based on the iteration of the nearest point of the contour line is adopted, which improves the accuracy of the corresponding point selection, reduces the complexity of the corresponding point selection calculation, and avoids the mismatching problem caused by the contour line nearest point iteration algorithm due to the likelihood of the chart data and too many equal points; the present invention uses a point set registration algorithm based on filtering to convert the solution of the rotation and translation parameters into a filtering problem, so that the rotation and translation parameters are explored in the dynamic space. This method is applied to terrain point cloud matching and gravity and magnetic point set registration, effectively avoiding the problem of the matching result converging to the local minimum, and optimizing the problem by using nonlinear Bayesian filtering, so that the algorithm converges to the global optimal solution. At the same time, the statistical characteristics of the registration error are obtained; the present invention uses Bayesian filtering to fuse the gravity and magnetic matching results, terrain matching results, and inertial navigation results to obtain the optimal estimate of the position and posture. Compared with single seabed topography, gravity and geomagnetic assisted positioning has stronger applicability, better positioning accuracy and stronger robustness, and can still have good matching positioning capabilities when the information richness of one or both of the terrain, gravity and geomagnetism is insufficient. BRIEF DESCRIPTION OF THE DRAWINGS

[0062] Figure 1 It is a schematic diagram of the corresponding point selection of the seabed topography point cloud.

[0063] Figure 2 This is a schematic diagram of the selection of corresponding points for seabed gravity and magnetic information.

[0064] Figure 3 It is a schematic diagram of the point set registration algorithm.

[0065] Figure 4 It is a schematic diagram of the overall framework of the present invention. DETAILED DESCRIPTION

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

[0067] Combination Figures 1 to 3 In order to solve the problems of insufficient positioning robustness, limited applicable scenarios, poor positioning accuracy, etc. in the existing seabed gravity, geomagnetism and terrain-assisted positioning, the present invention proposes a fusion matching method of seabed topography and seabed gravity and magnetism based on Bayesian filtering, including:

[0068] 1. During the navigation of the underwater vehicle, inertial navigation data, multi-beam sonar bathymetric data, gravity anomaly data, and magnetic field strength data are collected, and the sensor data are processed to obtain the seabed topography point cloud sub-map and gravity and magnetic measurement point sequences. At the same time, the benchmark point cloud map, benchmark gravity anomaly map, and benchmark magnetic field strength map of the area are extracted from the map library;

[0069] Processing sensor data includes: processing multi-beam sonar echo intensity information, motion compensation and suppression of measurement noise, downsampling of terrain point cloud data using voxel filtering, seabed gravity and magnetic detection information as a sequence of measurement points along the underwater vehicle, computer compensation of magnetic measurement data by establishing a mathematical model of the carrier magnetic field, and gravity information correction for gravimeter lag effect and zero drift.

[0070] Second, the corresponding points between the seabed topography point clouds are selected by using graph isomorphism to obtain the point correspondence relationship;

[0071] The point cloud corresponding point selection algorithm based on graph isomorphism is as follows: calculate the multi-beam sonar acquisition point cloud X = {x1, x2, ..., x m} and the average distance between the points in the reference point cloud submap Y = {y1, y2, ..., y3}, where x i ∈R 3 ,y i ∈R 3is a three-dimensional point of a three-dimensional vector. The two point clouds are represented by graphs respectively. The vertices of the graphs are the positions of the points in their respective point clouds. The edges of the graphs are functions of the distance between the two points. If the distance between the two points is less than the set distance, there is an edge between the two points. The weight of the edge is the square of the distance between the two points. The point set is represented by a graph G = (V, E, W), where V is the vertex of the graph, E is the edge of the graph, and W is the weight of the edge. Taking the point set Y as an example, its adjacency matrix A y ∈R m×m ,in is the weight between vertex i and vertex j. Therefore, the corresponding point selection problem is transformed into a graph isomorphism problem. Since the point clouds actually detected are only partially overlapped and there is no strict permutation matrix, the graph isomorphism problem is appropriately improved and the constraints are relaxed, as described in the following formula:

[0072] min Tr(W T B)

[0073] BA x =A y B

[0074] B1=1,B T 1=1

[0075] B ij ≥0

[0076] B ij =0, (i, j)∈S

[0077] Among them, A x ,A y is the adjacency matrix of two point sets X and Y, whose number of vertices is m and n respectively, B∈R n×m is a matrix whose elements are all non-negative, the sum of each row and column of which is 1, W∈R n×m is a random matrix whose elements are independent Gaussian distributions. Constraint B ij = 0, (i, j)∈S is obtained through pruning strategy, if the point x in the point set X i and point y in point set Y i Not a corresponding point, P ij =0, the pruning strategy is as follows:

[0078] 1. If the difference in the number of sides between two points is greater than 4, then the two points are not corresponding points.

[0079] 2. If the weight difference between two points is greater than These two points are not corresponding points, c is a constant,

[0080] After pruning, the permutation matrix B is solved through the Monte Carlo framework to make the adjacency matrices of the two point clouds equal and obtain the point correspondence, such as B13 =1, indicating that the third point in X corresponds to the first point in Y. Adjust Y according to B. 13 =1, change the index of the first point of Y from 1 to 3, repeat the above operation, adjust all the points in Y in sequence, and finally obtain a one-to-one correspondence between the points in Y and X.

[0081] 3. Use the method of iterative iteration of the nearest point of the contour line to select the corresponding points of the seabed gravity and magnetic point set to obtain the point correspondence;

[0082] The corresponding point selection algorithm of the seabed gravity and magnetic point set of iterative contour lines is as follows: for all measuring points n1, n2, n3, ..., n L , n x =[x x ,y x ,z x ] is the three-dimensional position of the xth measuring point, and the measured values ​​of gravity anomaly and magnetic field intensity G1, G2, G3, ..., G L 、M1,M2,M3,...,M L For any measuring point n x The gravity anomaly value and magnetic field strength value are measured by the sensor, and the point equal to the measured value is found on the gravity and geomagnetic reference map to form two contour lines, and the intersection of the two contour lines is saved as the corresponding point n of the indicated track point x , perform the above operations on all measuring points and finally get the corresponding point set"

[0083] n1,n2,n3,...,n L .

[0084] Fourth, the point set registration problem is modeled as a filtering model, and the Bayesian filter is used to optimize the problem to obtain the transformation relationship between point sets and the error statistical characteristics;

[0085] x k =x k-1 +v(k-1)

[0086]

[0087] Among them, u is the source point set, y k is the target point set; if it is a registration problem between X and Y, then u=X,y k =Y; if it is a registration problem of n and n′, then u = n, y k =n′; x k is the state of the kth iteration, v(k-1)∈Ν(x k-1 ; 0,Q K-1), e(k) is the Gaussian white noise observed at the kth iteration, e(k)∈N(y k ; 0,R k ),

[0088] v(k-1) is set to a large number as the initial value, so that the system continues to explore in the dynamic space. As the iteration proceeds, When the variance of the noise is reduced, x k The exploration space keeps getting smaller and gradually converges to the global optimal solution, and finally the rotation and translation relationship between the source point set and the target point set and the matching error covariance matrix are obtained.

[0089] 5. Based on the seabed terrain matching and seabed gravity and magnetic matching, the positioning correction results and their respective error statistical characteristics are obtained. Bayesian filtering is used to fuse the position and attitude information to obtain the inertial navigation error and its statistical characteristics, which are compensated to the inertial navigation to obtain the optimal estimate of the position and attitude and the covariance matrix of the error.

[0090] Firstly, the seafloor topography point cloud X is corrected using the rotation and translation variation parameters of X and Y to obtain the seafloor topography matching result; the measurement point sequence n of gravity anomaly and magnetic field intensity is corrected using the rotation and translation variation parameters of n and n′ to obtain the matching result of gravity anomaly and magnetic field intensity;

[0091] After that, the seabed topography matching results, gravity anomaly and magnetic field intensity matching results are weighted according to the corresponding error covariance matrix to obtain the underwater vehicle's position and posture measurement information. Specifically:

[0092]

[0093] Among them, P1 and P2 are the error covariance matrices of X and Y, n and n′ obtained in step 4 respectively; X1, Y1, θ1 and X2, Y2, θ2 are the seabed topography matching results and the matching results of gravity anomaly and magnetic field intensity obtained in step 5 respectively.

[0094] Finally, the difference between the underwater vehicle's posture measurement information and the underwater vehicle's posture output by the inertial navigation system is taken as observation information, and the posture error of the inertial navigation system at each moment is taken as the state. It is modeled as a filtering model and solved using the extended Kalman filter to obtain the optimal estimate of the posture error of the inertial navigation system; the posture error is fed back to the inertial navigation system for closed-loop correction to obtain the optimal estimate of the underwater vehicle's posture.

[0095] x k =f(x k-1 )+w k-1

[0096] z k =h(xk )+v k

[0097] in is the position error of the inertial navigation system, which is the position error along the X and Y axes and the angle error along the Z axis; f(x k ) is the error analysis equation of the inertial navigation system; w k-1 is the state noise; The difference between the position and posture of the matching positioning and the position and posture output by the inertial navigation system; h(x k )=x k ; is the pose error of matching positioning.

[0098] Embodiment 1:

[0099] First, the seabed topography benchmark map, seabed gravity anomaly benchmark map, and seabed magnetic field intensity benchmark map of the area are obtained, and a magnetic field mathematical model is built for the underwater vehicle. Inertial navigation data is acquired during the navigation of the underwater vehicle, and multi-beam sonar bathymetry data, gravity anomaly data, and magnetic field intensity data are collected. The multi-beam sonar echo intensity information, motion compensation, and measurement noise suppression are processed. The terrain point cloud data is downsampled using the voxel filtering method. The processed point cloud data is divided into point cloud sub-maps according to the richness of geometric features and the length of the scanned area.

[0100] By establishing a mathematical model of the carrier magnetic field, the magnetic survey data is compensated by computer, and the gravity information is corrected for the hysteresis effect of the gravimeter and the zero drift. The gravity and magnetic measurement points are divided into several gravity and magnetic measurement point sequences within a limit of 100 meters.

[0101] Taking the center of the current point cloud submap as the origin and 100 meters as the radius, collect all the reference submaps within this range, calculate the overlap ratio with the current point cloud submap, and use the one with the largest overlap ratio as the reference submap to be registered. The obtained terrain point cloud submap X and the selected reference submap Y are expressed in the form of a graph. The vertices of the graph are the positions of the points in the point cloud, and the edges of the graph are the functions of the distance between the two points. The distance between the two points is less than There is an edge between two points, and the weight of the edge is the square of the distance between the two points. The point set is represented by a graph G = (V, E, W), where V is the vertex of the graph, E is the edge of the graph, and W is the weight of the edge. Taking the point set Y as an example, its adjacency matrix A y ∈R m×m ,in is the weight between vertex i and vertex j. The corresponding point selection problem is transformed into a graph isomorphism problem.

[0102] min Tr(W T B)

[0103] BAx =A y B

[0104] B1=1,B T 1=1

[0105] B ij ≥0

[0106] B ij =0, (i, j)∈S

[0107] Where B∈R n×m is a matrix whose elements are all non-negative, S is a set of subscript pairs. Since the collected point clouds may have external points or structural defects, the two point clouds will not completely overlap. First, the two graphs must be pruned. If the point x in the point set X i and point y in point set Y i is not a corresponding point, then B ij =0, specifically: if the difference in the number of edges of two points is greater than 4, then the two points are not corresponding points. If the weight difference of two points is greater than a certain value, the two points are not corresponding points. After pruning, since there is no analytical solution, the Monte Carlo method is used to solve the graph isomorphism problem, so that the two graphs are equal, and the permutation matrix B is obtained, thereby obtaining the point correspondence relationship between the two pruned point clouds, and adjusting Y according to B, for example, B 13 = 1, change the index of the first point of Y from 1 to 3, and so on, adjust the order of all the points in Y, and finally get the corresponding relationship as follows Figure 1 shown.

[0108] The iterative nearest contour method is used to select the corresponding points of the seabed gravity and magnetic point set, such as Figure 2 As shown, INS provides a set of indicated tracks n1, n2, n3, ..., n L , n x =[x x ,y x ,z x ] is the three-dimensional position of the xth measuring point. The gravity and geomagnetic sensors provide the measured values ​​of gravity anomaly and magnetic field strength corresponding to the real position G1, G2, G3, ..., G L 、M1,M2,M3,...,M L For any point n on the track x The gravity anomaly value G is measured by the sensor x and magnetic field strength value M x , find a point equal to the measured value on the gravity and geomagnetic reference map within 200 meters around the inertial navigation indication position to form two contour lines. The intersection of these two contour lines is the corresponding point n' of the indicated track point x, perform the above operations on all INS indicated track points and finally obtain the corresponding point set n'1,n'2,n'3,...,n' L .

[0109] After obtaining the point correspondence, point set registration is performed and the point cloud registration problem is modeled as a filtering model, such as Figure 2 As shown, specifically:

[0110] x k =x k-1 +v(k-1)

[0111]

[0112] The corresponding points are obtained by the above steps, u is the source point set, and u remains unchanged during the iteration process, corresponding to the point set X or the measurement point sequence n1, n2, n3, ..., n in the above steps. L ,y k =Y is the target point set, corresponding to the point set Y or n'1, n'2, n'3, ..., n' in the above step L .x k is the k-th step state, v(k-1)∈Ν(x k-1 ; 0,Q K-1 ), Q k The initial value is set to a large number, so that the system continues to explore in the dynamic space. e(k) is the Gaussian white noise observed in the kth step, e(k)∈N(y k ; 0,R k ), where R k is its covariance matrix. As the iteration proceeds,

[0113]

[0114] exist When the variance of the noise is reduced, Q is updated k The exploration space becomes smaller and smaller, and gradually converges to the global optimal solution. Since the model has strong nonlinearity, Bayesian filtering is used to solve it. The process is as follows: Figure 3 The specific steps are as follows:

[0115] The source point set u and the target point set Y are selected through the corresponding points, and the state vector and its covariance matrix x = x0, P = P0 are initialized. The variance Q of the initialization process model k =Q0, observation noise variance matrix R k =R0, Q0 takes the larger value, in the next step, Q k Choose whether to reduce it based on whether the system converges.

[0116] Step 4.1: Initialize k = 1 and initialize x 0|0 , P 0|0 , Q0 and R0;

[0117] Step 4.2: Status update;

[0118]

[0119] P k|k-1 =P k-1|k-1 +Q k-1

[0120] Step 4.3: P k|k-1 Decompose into S k|k-1 and

[0121]

[0122] Step 4.4: Estimate point X i,k|k-1 ;

[0123]

[0124] Step 4.5: Estimate the propagation point Y i,k|k-1 ;

[0125]

[0126] Step 4.6: Estimate the predicted observations

[0127]

[0128] Step 4.7: Estimate the observation variance matrix P yy,k|k-1 ;

[0129]

[0130] Step 4.8: Estimate the cross-variance matrix P xy,k|k-1 ;

[0131]

[0132] Step 4.9: Estimate the Bayesian gain K k ;

[0133]

[0134] Step 4.10: Estimate the rotation and translation change parameters and the error covariance matrix P k|k ;

[0135]

[0136] Step 4.11: If Then let Q k =0.9Q k-1 , k=k+1, return to step 4.2; otherwise, output the rotation and translation change parameters and the error covariance matrix P k|k .

[0137] where ξ i is the volume point, After convergence, the rotation and translation change parameters of the source point set and the target point set are obtained. And the error covariance matrix P k|k .

[0138] For the detected terrain information, the corresponding point selection based on graph isomorphism and the point set registration based on Bayesian filtering are performed to obtain the rotation and translation change parameters of the source point cloud and the target point cloud. The change parameters are used to correct the source point cloud pose to obtain the terrain matching positioning result; for the detected gravity and magnetic information, the corresponding point selection based on the nearest point iteration of the contour line and the point set registration based on Bayesian filtering are performed to obtain the rotation and translation change parameters of the source point set and the target point set. The change parameters are used to correct the source point set pose to obtain the gravity and magnetic matching positioning result. The position correction frequencies obtained by terrain point cloud matching and gravity and magnetic point cloud matching are different, among which the frequency of terrain matching is higher than that of gravity and magnetic matching. When there is no gravity and magnetic matching positioning information, only the terrain matching positioning information is used. When there is gravity and magnetic matching positioning information, the terrain matching positioning information and the gravity and magnetic matching positioning information are weighted fused. Take the inertial guidance posture error [δ x ,δ y ,δ θz ] as the state, the gravity and magnetic matching and terrain matching positioning information are weighted according to their respective error statistical characteristics to obtain the posture information, specifically:

[0139]

[0140] x m ,y m , are the positions around the X-axis and Y-axis and the attitude around the Z-axis of the gravity and magnetic matching and terrain matching positioning information weighted according to their respective error statistical characteristics. P1 and P2 are the error covariance matrices of the terrain matching and gravity and magnetic matching positioning results, respectively. X1, Y1, θ1 and X2, Y2, θ2 are the terrain matching positioning information and gravity and magnetic matching positioning information, respectively.

[0141] The difference between the weighted pose information and the inertial pose is As observation information, a Bayesian filter is used to model the problem. The filter is modeled as:

[0142] x k=f(x k-1 )+w k-1

[0143] z k =h(x k )+v k

[0144] in is the inertial navigation posture error, which is the position error along the X and Y axes and the angle error along the Z axis, f(x k ) is the error analysis equation of inertial navigation, w k-1 is the state noise, To match the difference between the positioning posture and the inertial navigation posture, h(x k )=x k , is the pose error of matching positioning.

[0145] The extended Kalman filter is used to solve the optimal estimate of the inertial navigation position and attitude error, and the error is fed back to the inertial navigation, and the inertial navigation is corrected in a closed loop to obtain the optimal estimate of the position and attitude. The entire fusion positioning process is as follows: Figure 4 shown.

[0146] Application scenario: The present invention can be applied to underwater positioning of underwater robots. Multi-beam sonar, gravimeter, and magnetometer are used to obtain multi-beam sonar depth data, gravity anomaly data, and magnetic field strength data. These data are matched with the reference map to obtain positioning results and error statistics. Then, the positioning results are fused through a data fusion algorithm based on Bayesian filtering to obtain the optimal estimate of the position. Whether there is only one of the multi-beam sonar depth data, gravity anomaly data, and magnetic field strength data or multiple data, the present invention can achieve more accurate positioning than existing methods.

[0147] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A fusion matching method of seabed topography and gravity and magnetic fields based on Bayesian filtering, characterized in that: The following steps are involved: Step 1: The underwater vehicle collects inertial navigation system data, multi-beam sonar sounding data, gravity anomaly data and magnetic field intensity data while navigating in the target area; processes the collected data to obtain the seabed topography point cloud X, the measurement point sequence n of gravity anomaly and magnetic field intensity; extracts the benchmark point cloud Y, benchmark gravity anomaly map and benchmark magnetic field intensity map of the target area from the map library; Step 2: Represent the seabed topography point cloud X and the reference point cloud Y with graphs, and obtain the corresponding adjacency matrix A x and A y ; Using graph isomorphism, solve A through the Monte Carlo framework x With A y The permutation matrix B is obtained; the reference point cloud Y is adjusted according to the permutation matrix B so that the reference point cloud Y corresponds to the points in the seabed topography point cloud X one by one; Step 3: For the measurement point n in the measurement point sequence n of gravity anomaly and magnetic field intensity x , respectively find points on the benchmark gravity anomaly map and the benchmark magnetic field intensity map that are equal to the measured value of the measuring point to form two contour lines, and save the intersection of the two contour lines as the corresponding point n′ of the point x , and finally obtain the corresponding point set n′ of all measuring points; Step 4: Model the registration problems of X and Y, n and n′ as filter models respectively, solve them by Bayesian filtering, and obtain the rotation and translation change parameters and error covariance matrix corresponding to the filter models; Step 5: Use the rotation and translation change parameters of X and Y to correct the seabed topography point cloud X to obtain the seabed topography matching result; use the rotation and translation change parameters of n and n′ to correct the measurement point sequence n of gravity anomaly and magnetic field intensity to obtain the matching result of gravity anomaly and magnetic field intensity; Step 6: After weighting the seabed terrain matching results, gravity anomaly and magnetic field strength matching results according to the corresponding error covariance matrix, the position and attitude measurement information of the underwater vehicle is obtained; the difference between the position and attitude measurement information of the underwater vehicle and the position and attitude of the underwater vehicle output by the inertial navigation system is used as the observation information, and the position and attitude error of the inertial navigation system at each moment is used as the state, which is modeled as a filtering model and solved using the extended Kalman filter to obtain the optimal estimate of the position and attitude error of the inertial navigation system; the position and attitude error is fed back to the inertial navigation system for closed-loop correction to obtain the optimal estimate of the position and attitude of the underwater vehicle.

2. The method for fusion matching of seabed topography and gravity and magnetism based on Bayesian filtering according to claim 1 is characterized by: The method of representing the seabed topography point cloud X and the reference point cloud Y with graphs in step 2 is specifically as follows: The point cloud is represented by a graph G = (V, E, W), where V is the vertex set of the graph, E is the edge set of the graph, and W is the edge weight set; the vertices of the graph are the positions of the points in the point cloud, and the edges of the graph are the function of the distance between two points in the point cloud. If the distance between two points is less than the set threshold, there is an edge between the two points, and the weight of the edge is the square of the distance between the two points.

3. The method for fusion matching of seabed topography and gravity and magnetism based on Bayesian filtering according to claim 1 is characterized in that: In step 2, graph isomorphism is used to solve A through the Monte Carlo framework. x With A y The method of permutation matrix B is as follows: The constraints of the graph isomorphism problem are: min Tr(W T B) NOT x = Yes y B B1=1,B T 1=1 B ij ≥0 B ij =0,(i,j)∈S Among them, the graph of the seabed topography point cloud X represents the corresponding adjacency matrix A x The number of vertices in is m, and the graph of the reference point cloud Y represents the corresponding adjacency matrix A y The number of vertices in is n; B∈R n×m is a matrix whose elements are all non-negative, and the sum of the elements in each row and the sum of the elements in each column of the matrix is ​​1; W∈R n×m is a random matrix with independent Gaussian distribution elements; (i, j)∈S means that if the point x in X i and point y in Y i It is not a corresponding point, and is determined by the pruning strategy, specifically: (1) If point x i With point y i The difference in the number of sides of is greater than 4, then the point x i With point y i It is not a corresponding point; (2) If point x i With point y i The weight difference is greater than Then point x i With point y i is not a corresponding point; c is a constant, 4. The method for fusion matching of seabed topography and gravity and magnetism based on Bayesian filtering according to claim 1 is characterized in that: In step 4, the registration problem of X and Y, and n and n′ are respectively modeled as filtering models, specifically: x k =x k-1 +v(k-1) Among them, u is the source point set, y k is the target point set; if it is a registration problem between X and Y, then u=X,y k =Y; if it is a registration problem of n and n′, then u = n, y k =n′; x k is the state of the kth iteration, v(k-1)∈Ν(x k-1 ; 0,Q K-1 ), e(k) is the Gaussian white noise observed at the kth iteration, e(k)∈N(y k ; 0,R k ), 5. The method for fusion matching of seabed topography and gravity and magnetism based on Bayesian filtering according to claim 4 is characterized in that: The Bayesian filter is used to solve the problem in step 4 as follows: Step 4.1: Initialize k = 1 and initialize x 0|0 , P 0|0 , Q0 and R0; Step 4.2: Status update; P k|k-1 =P k-1|k-1 +Q k-1 Step 4.3: P k|k-1 Decompose into S k|k-1 and Step 4.4: Estimate point X i,k|k-1 ; Among them, ξ i is the volume point, Step 4.5: Estimate the propagation point Y i,k|k-1 ; Step 4.6: Estimate the predicted observations Step 4.7: Estimate the observation variance matrix P yy,k|k-1 ; Step 4.8: Estimate the cross-variance matrix P xy,k|k-1 ; Step 4.9: Estimate the Bayesian gain K k ; Step 4.10: Estimate the rotation and translation change parameters and the error covariance matrix P kk ; Step 4.11: If Then let Q k =0.9Q k-1 , k=k+1, return to step 4.2; otherwise, output the rotation and translation change parameters and the error covariance matrix P kk .

6. The method for fusion matching of seabed topography and gravity and magnetism based on Bayesian filtering according to claim 1 is characterized by: In step 6, the seabed terrain matching results, the gravity anomaly and the magnetic field strength matching results are weighted according to the corresponding error covariance matrix to obtain the position and posture measurement information of the underwater vehicle. Specifically: Among them, P1 and P2 are the error covariance matrices of X and Y, n and n′ obtained in step 4 respectively; X1, Y1, θ1 and X2, Y2, θ2 are the seabed topography matching results and the matching results of gravity anomaly and magnetic field intensity obtained in step 5 respectively.

7. The method for fusion matching of seabed topography and gravity and magnetism based on Bayesian filtering according to claim 6 is characterized by: In step 6, the difference between the underwater vehicle's posture measurement information and the underwater vehicle's posture output by the inertial navigation system is used as observation information, and the posture error of the inertial navigation system at each moment is used as the state, and the model is modeled as a filtering model, specifically: x k =f(x k-1 )+w k-1 z k =h(x k )+v k in is the position error of the inertial navigation system, which is the position error along the X and Y axes and the angle error along the Z axis; f(x k ) is the error analysis equation of the inertial navigation system; w k-1 is the state noise; is the difference between the underwater vehicle's attitude measurement information and the underwater vehicle's attitude output by the inertial navigation system; h(x k )=x k ; is the pose error of matching positioning.

8. A computer device / equipment / system comprising a memory, a processor and a computer program stored in the memory, characterized in that: The processor executes the computer program to implement the steps of the method according to any one of claims 1 to 7.

9. A computer-readable storage medium having a computer program / instruction stored thereon, characterized in that: When the computer program / instructions are executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

10. A computer program product comprising a computer program / instructions, characterized in that: When the computer program / instructions are executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

Citation Information

Patent Citations

  • Bayesian estimation-based particle filter gravity-assisted inertial navigation matching method

    CN105157704A

  • Particle filter-based gravity sampling vector matching positioning method

    CN105180938A

  • Underwater terrain matching and positioning method based on Gaussian process regression learning

    CN111220146A

  • Underwater navigation method and device integrating various data

    CN116222551A