A method, program, device and storage medium for matching seabed topography and gravity and magnetic data based on Bayesian filtering

Abstract

The present application belongs to the technical field of underwater multi-sensor fusion matching positioning, and particularly relates to a seabed terrain and gravity-magnetic fusion matching method based on Bayesian filtering, a program, an equipment and a storage medium. The present application converts the selection of corresponding points of seabed terrain into a graph isomorphism problem, improving the accuracy and robustness of the selection of corresponding points. The present application uses an isogram nearest point iteration-based seabed gravity-magnetic information corresponding point selection algorithm, improving the accuracy of the selection of corresponding points. The present application uses a filter-based point set registration algorithm, converting the solving of rotation and translation parameters into a filtering problem, making the rotation and translation parameters explore in a dynamic space, using a nonlinear Bayesian filter to optimize the problem, making the algorithm converge to a global optimal solution. The present application uses Bayesian filtering to fuse the gravity-magnetic matching result, the terrain matching result and the inertial navigation result, obtaining an optimal estimation of the pose.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of underwater multi-sensor fusion matching positioning, and particularly relates to a seabed terrain and gravity-magnetic fusion matching method based on Bayesian filtering, a program, an equipment and a storage medium. BACKGROUND

[0002] With the development of marine geophysical equipment and underwater robot technology, AUV marine geophysical positioning technology has been widely applied. Commonly used are marine gravity assisted positioning, marine geomagnetic assisted positioning and seabed terrain assisted positioning. The marine gravity and geomagnetic assisted positioning has the problems of high likelihood of gravity and geomagnetic chart data and excessive isogrades, which leads to mismatching. The seabed terrain assisted positioning has the problems of great dependence of initial corresponding point selection in point cloud matching and easy convergence to local minimum in registration process, which finally leads to positioning failure. In addition, single reliance on gravity, geomagnetism and terrain leads to problems such as inaccurate positioning, poor positioning stability and small application range due to unobvious changes in prior map information. Moreover, the existing assisted positioning method cannot give the positioning error statistical characteristics of various assisted positioning, and it is difficult to realize optimal pose estimation.

[0003] Therefore, it is urgent to propose a seabed terrain and gravity-magnetic fusion matching method capable of realizing optimal pose estimation to solve the above problems. SUMMARY

[0004] The application aims to solve the problems of insufficient positioning robustness, limited application scenarios and poor positioning accuracy of the existing seabed gravity, geomagnetism and terrain assisted positioning, and provides a seabed terrain and gravity-magnetic fusion matching method based on Bayesian filtering.

[0005] A seabed terrain and gravity-magnetic fusion matching method based on Bayesian filtering comprises the following steps:

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

[0007] Step 2: the seabed terrain point cloud X and the reference point cloud Y are represented by graphs respectively, and the corresponding adjacency matrices A x and A y are obtained; the permutation matrix B of A x and A y is solved by using graph isomorphism and the Monte Carlo framework; 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 terrain point cloud X one by one;

[0008] Step 3: For the measuring point n in the measuring point sequence of gravity anomaly and magnetic field intensity x , find the points equal to the measuring value of the measuring point on the reference gravity anomaly map and the reference magnetic field intensity map respectively to form two contour lines, and save the intersection point 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 problem of X and Y, n and n' as filter models respectively, and solve it by using Bayesian filtering to obtain the rotation and translation change parameters and error covariance matrix corresponding to the filter model;

[0010] Step 5: Use the rotation and translation change parameters of X and Y to correct the seafloor topography point cloud X to obtain the seafloor topography matching result; use the rotation and translation change parameters of n and n' to correct the measuring 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 seafloor topography matching result and the matching result of gravity anomaly and magnetic field intensity according to the corresponding error covariance matrix, the pose measurement information of the underwater vehicle is obtained; the difference between the pose measurement information of the underwater vehicle and the pose of the underwater vehicle output by the inertial navigation system is taken as the observation information, and the pose error of the inertial navigation system at each time is taken as the state, which is modeled as a filter model, and the extended Kalman filter is used to solve it to obtain the optimal estimation of the pose error of the inertial navigation system; the pose error is fed back to the inertial navigation system for closed-loop correction to obtain the optimal estimation of the pose of the underwater vehicle.

[0012] Further, the method of representing the seafloor topography point cloud X and the reference point cloud Y by a graph in step 2 is that 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, the edge of the graph is the function of the distance between two points in the point cloud, and the edge exists between two points if the distance between the two points is less than a set threshold, and the weight of the edge is the square of the distance between the two points.

[0013] Further, the method of solving the permutation matrix B of A x and A y by using graph isomorphism in step 2 is:

[0014] The constraint of the graph isomorphism problem is:

[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] wherein the graph representation of the seafloor topography point cloud X corresponds to an adjacency matrix A x wherein the number of vertices is m, the graph representation of the reference point cloud Y corresponds to an adjacency matrix A y wherein the number of vertices is n; B∈R n×m is a matrix with non-negative elements, and the sum of the elements of each row and the sum of the elements of each column are both 1; W∈R n×m is a random matrix with independent Gaussian distribution elements; (i,j)∈S represents that if the point x i in X and the point y i in Y are not corresponding points, the pruning strategy is determined as follows:

[0021] (1) If the edge number difference between the point x i and the point y i is greater than 4, the point x i and the point y i are not corresponding points;

[0022] (2) If the weight difference between the point x i and the point y i is greater than , the point x i and the point y i are not corresponding points; c is a constant,

[0023] Further, the registration problem of X and Y, n and n' in step 4 is modeled as a filtering model, which is specifically:

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

[0025]

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

[0027] Further, the step 4 employs the Bayesian filter to solve specifically as follows:

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

[0029] Step 4.2: state update;

[0030]

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

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

[0033]

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

[0035]

[0036] wherein ξ 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 observation

[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 , 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] Further, in the step 6, the seabed terrain matching result and the matching result of the gravity anomaly and the magnetic field strength are weighted according to the corresponding error covariance matrix to obtain the pose measurement information of the underwater vehicle Specifically,

[0051]

[0052] wherein P1 and P2 are the error covariance matrices of X and Y, n and n' obtained in step 4; X1, Y1, θ1 and X2, Y2, θ2 are the seabed terrain matching result and the matching result of the gravity anomaly and the magnetic field strength obtained in step 5.

[0053] Further, in the step 6, the difference between the pose measurement information of the underwater vehicle and the underwater vehicle pose output by the inertial navigation system is taken as observation information, and the pose error of the inertial navigation system at each time is taken as state, which is modeled as a filtering model, specifically:

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

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

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

[0057] A computer device / apparatus / system, comprising a memory, a processor and a computer program stored on the memory, the processor executing the computer program to implement the steps of the above-mentioned fusion matching method of seafloor terrain and gravity and geomagnetic based on Bayesian filtering.

[0058] A computer readable storage medium having stored thereon a computer program / instructions, which, when executed by a processor, implement the steps of the above-mentioned fusion matching method of seafloor terrain and gravity and geomagnetic based on Bayesian filtering.

[0059] A computer program product comprising a computer program / instructions, which, when executed by a processor, implement the steps of the above-mentioned fusion matching method of seafloor terrain and gravity and geomagnetic based on Bayesian filtering.

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

[0061] The present application converts the selection of corresponding points of seafloor terrain into a graph isomorphism problem, so that the points in the point cloud of seafloor terrain are no longer isolated points, but elements that have a relationship with other points, improving the accuracy and robustness of the selection of corresponding points; the present application uses a contour nearest point iteration-based selection algorithm for corresponding points of seafloor gravity and geomagnetic information, improving the accuracy of the selection of corresponding points and reducing the computational complexity of the selection of corresponding points, avoiding the problem of mismatching produced by the contour nearest point iteration algorithm due to too many contour points and too high likelihood of the chart data; the present application uses a filtering-based point set registration algorithm to convert the solving of rotation and translation parameters into a filtering problem, so that the rotation and translation parameters are explored in a dynamic space, this method is applied to terrain point cloud matching and gravity and geomagnetic point set registration, effectively avoiding the problem of convergence to a local minimum of the matching result, and through the optimization of the problem by using nonlinear Bayesian filtering, the algorithm converges to a global optimal solution. At the same time, the statistical characteristics of the registration error are obtained; the present application uses Bayesian filtering to fuse the matching results of gravity and geomagnetic, the matching results of terrain, and the inertial navigation results to obtain the optimal estimation of the pose. Compared with single seafloor terrain, gravity and geomagnetic aided positioning, the present application has stronger applicability, better positioning accuracy and stronger robustness, and still has good matching positioning ability when the information richness of one or two of terrain, gravity and geomagnetic is insufficient. BRIEF DESCRIPTION OF DRAWINGS

[0062] Figure 1 is a schematic diagram of the selection of corresponding points of seafloor terrain point cloud.

[0063] Figure 2 is a schematic diagram of corresponding point selection of seabed gravity and magnetic information.

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

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

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

[0067] In conjunction with Figures 1 to 3 , the present application is to solve the problems of insufficient positioning robustness, limited application scenarios, poor positioning accuracy and other problems existing in the existing seabed gravity, geomagnetic and topographic auxiliary positioning, and proposes a fusion matching method for seabed topography and seabed gravity and magnetic based on Bayesian filtering, including:

[0068] I. In the process of underwater vehicle navigation, collect inertial navigation data, multi-beam sonar sounding data, gravity anomaly data and magnetic field intensity data, process sensor data, obtain seabed topographic point cloud sub-map and gravity and magnetic measurement point sequence, and extract the reference point cloud map, reference gravity anomaly map and reference magnetic field intensity map of the region from the map library;

[0069] Processing sensor data specifically includes: processing multi-beam sonar echo intensity information, motion compensation and suppressing measurement noise, using voxel filtering method for downsampling processing of topographic point cloud data, seabed gravity and magnetic detection information is along the measurement point sequence of underwater vehicle, through establishing carrier magnetic field mathematical model to carry out computer compensation for magnetic measurement data, and correcting gravity meter lag effect and zero drift.

[0070] II. Corresponding point selection between seabed topographic point clouds is carried out in the way of graph isomorphism, and the point correspondence relationship is obtained;

[0071] The point cloud corresponding point selection algorithm based on graph isomorphism is specifically: the average distance between the points in the multi-beam sonar collected point cloud X={x1, x2,..., x m} and the points in the reference point cloud sub-map Y={y1, y2,..., y3} is calculated, wherein x i ∈R 3 , y i ∈R 3is a three-dimensional point of three-dimensional vector, two point clouds are represented by graphs respectively, the vertices of the graph are the positions of the points in the respective point cloud, the edges of the graph are the functions of the distances between two points, there is an edge between two points if the distance between the two points is less than a set distance, 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), 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 , where is the weight between vertex i and vertex j. Thus, the corresponding point selection problem is converted into a graph isomorphism problem, since the actually detected point cloud is only partially overlapped, there is no strict permutation matrix, and the graph isomorphism problem is appropriately improved and relaxed constraints, 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] where A x ,A y are the adjacency matrices of two point sets X and Y, the number of vertices of which are m and n respectively, B∈R n×m is a matrix with non-negative elements, the sum of the elements of each row and the sum of the elements of each column of the matrix are 1, W∈R n×m is a random matrix with independent Gaussian distribution elements. The constraint B ij = 0, (i,j)∈S is obtained by pruning strategy, if the point x i in the point set X and the point y i in the point set Y are not corresponding points, P ij = 0, the pruning strategy is as follows:

[0078] 1. If the number of edges of the two points is more than 4, the two points are not corresponding points.

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

[0080] After pruning, the permutation matrix B is solved by the Monte Carlo framework, so that the adjacency matrices of the two point clouds are equal, and the point correspondence is obtained, for example, B13 = 1, means the third point in X and the first point in Y correspond, adjust Y according to B, for B 13 = 1, change the index of the first point of Y from 1 to 3, repeat the above operation, sequentially adjust all points in Y, finally get Y which is one-to-one corresponding with the points in X.

[0081] Thirdly, the corresponding point selection of the seabed gravity and magnetic point set is carried out by using the isogram nearest point iteration method to obtain the point correspondence relationship.

[0082] The corresponding point selection algorithm of the seabed gravity and magnetic point set by iteration isogram is as follows: for all measurement points n1, n2, n3,..., n L , n x = [x x , y x , z x ] is the three-dimensional position of the xth measurement point, the measured values of gravity anomaly and magnetic field intensity G1, G2, G3,..., G L , M1, M2, M3,..., M L obtained in step one. For any measurement point n x , the gravity anomaly value and the magnetic field intensity value thereof are measured by a sensor, and a point equal to the measurement value is found on the gravity and geomagnetic reference map to form two isograms, and the intersection point of the two isograms is saved as the corresponding point n x of the indicated track point. The above operation is performed on all measurement points to finally obtain the corresponding point set "n

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

[0084] Fourthly, the point set registration problem is modeled as a filtering model, and the problem is optimized by using Bayesian filtering to obtain the transformation relationship between the point sets and the error statistical characteristics.

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

[0086]

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

[0088] v(k-1) is set to a very large number, so that the system constantly explores in the dynamic space, and as the iteration proceeds, the noise variance is reduced at , so that x k The exploration space is constantly reduced, 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] Five, according to the positioning correction results obtained by matching the seafloor topography and the seafloor gravity and magnetic matching, and the respective error statistical characteristics, the pose information is fused by using Bayesian filtering to obtain the inertial navigation error and its statistical characteristics, which are compensated to the inertial navigation to obtain the optimal estimation of the pose and the error covariance matrix.

[0090] First, the seafloor topography point cloud X is corrected by using the rotation and translation change parameters of X and Y to obtain the seafloor topography matching result; the measured point sequence n of the gravity anomaly and the magnetic field intensity is corrected by using the rotation and translation change parameters of n and n' to obtain the matching result of the gravity anomaly and the magnetic field intensity;

[0091] Then, the seafloor topography matching result, the matching result of the gravity anomaly and the magnetic field intensity are weighted according to the corresponding error covariance matrix to obtain the underwater vehicle pose 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; X1, Y1, θ1 and X2, Y2, θ2 are the seafloor topography matching result and the matching result of the gravity anomaly and the magnetic field intensity obtained in step 5.

[0094] Finally, the difference between the underwater vehicle pose measurement information and the underwater vehicle pose output by the inertial navigation system is taken as the observation information, and the pose error of the inertial navigation system at each time is taken as the state, which is modeled as a filtering model, and the extended Kalman filter is used to solve it. Obtain the optimal estimation of the pose error of the inertial navigation system; the pose error is fed back to the inertial navigation system for closed-loop correction to obtain the optimal estimation of the underwater vehicle pose.

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

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

[0097] wherein is the pose error of the inertial navigation system, respectively the position error along the X, 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 pose of the matched positioning and the pose output by the inertial navigation system; h(x k ) = x k ; is the pose error of the matched positioning.

[0098] Embodiment 1:

[0099] Firstly, the seabed topographic base map, seabed gravity anomaly base map and seabed magnetic field intensity base map of the region are obtained, a magnetic field mathematical model of the underwater vehicle is established, inertial navigation data is obtained during the navigation of the underwater vehicle, multi-beam sonar depth data, gravity anomaly data and magnetic field intensity data are collected, multi-beam sonar echo intensity information is processed, motion compensation and measurement noise suppression are performed, and the processed point cloud data is divided into point cloud sub-maps according to the richness of geometric features and the length of the scanning area.

[0100] The magnetic survey data is calculated by establishing a carrier magnetic field mathematical model, the gravity information is corrected for gravimeter lag effect and zero drift, and the gravity and magnetic survey points are segmented to obtain a plurality of gravity and magnetic survey point sequences with a limit of 100 meters.

[0101] Taking the center position of the current point cloud sub-map as the origin and 100 meters as the radius, all the reference sub-maps within this range are collected, the overlap ratio of the reference sub-maps with the current point cloud sub-map is calculated, and the reference sub-map with the largest overlap ratio is selected as the reference sub-map to be registered. The obtained topographic point cloud sub-map X and the selected reference sub-map 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 functions of the distance between two points. If the distance between two points is less than , then there is an edge between the 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), 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 , wherein is the weight between vertex i and vertex j. The corresponding point selection problem is converted 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 an element-wise non-negative matrix, S is a set of index pairs, since the collected point cloud may have outliers or structural missing, the two point clouds will not be completely overlapped, first, the two graphs are pruned, if the point x i in the point set X and the point y i in the point set Y are not corresponding points, then B ij = 0, specifically: if the number of edges of the two points is more than 4, then the two points are not corresponding points, if the weight difference of the two points is greater than a certain value, then 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, the permutation matrix B is obtained, and the point correspondence relationship of the two pruned point clouds is obtained, according to B, Y is adjusted, for example, B 13 = 1, change the index of the first point of Y from 1 to 3, and so on, sequentially adjust all the points in Y, the final corresponding relationship is shown in Figure 1 .

[0108] The iterative nearest contour method is used for seabed gravity and magnetic point set corresponding point selection, as shown in Figure 2 INS provides a set of track indicators n1, n2, n3,..., n L , n x = [x x , y x , z x ] is the three-dimensional position of the xth measurement point, the gravity and magnetic sensors provide the measured values G1, G2, G3,..., G L , M1, M2, M3,..., M L of gravity anomaly and magnetic field strength corresponding to the real position. For any point n x on the track, the gravity anomaly value G x and the magnetic field strength value M x measured by the sensor are found on the gravity and magnetic reference map within a range of 200 meters around the inertial navigation indicated position, forming two contours, and the intersection of the two contours is the corresponding point n' xThe above operation is performed on all INS indicated track points to finally obtain corresponding point sets 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, as shown in Figure 2 , and specifically:

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

[0111]

[0112] where the corresponding points are obtained by the above steps, u is the source point set, which remains unchanged in the iteration process, corresponding to the point set X or the measurement point sequence n1, n2, n3,..., n L , y k =Y is the target point set, corresponding to the point set Y or n'1, n'2, n'3,..., n' L of the above steps. x k is the kth step state, v(k-1) is a Gaussian white noise, v(k-1) ∈ N(x k-1 ; 0, Q K-1 ), Q k is set to a large number at the beginning, so that the system continuously explores in the dynamic space, e(k) is the kth step observation Gaussian white noise, e(k) ∈ N(y k ; 0, R k ), where R k is the covariance matrix thereof, and as the iteration proceeds,

[0113]

[0114] When , the noise variance is reduced, and Q k is updated so that the exploration space becomes smaller and gradually converges to the global optimal solution. Since the model has strong nonlinearity, the Bayesian filtering is used for solving, and the flow is as shown in Figure 3 . The specific steps are as follows:

[0115] After the source point set u and the target point set Y are selected by the corresponding points, the state vector and the covariance matrix thereof are initialized as x=x0, P=P0. The process model variance Q k =Q0, the observation noise variance matrix R k =R0, Q0 takes a large value, and in the following steps, Q k is selected according to whether the system converges or not.

[0116] Step 4.1 : Initialization Let k = 1, initialize x 0|0 , P 0|0 , Q0and R0.

[0117] Step 4.2: State update;

[0118]

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

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

[0121]

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

[0123]

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

[0125]

[0126] Step 4.6: Estimate predicted observation

[0127]

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

[0129]

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

[0131]

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

[0133]

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

[0135]

[0136] Step 4.11: If 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 and the error covariance matrix P k|k of the source point set and the target point set are obtained.

[0138] For the detected terrain information, the rotation and translation change parameters of the source point cloud and the target point cloud are obtained by performing the seabed terrain corresponding point selection based on graph isomorphism and the point set registration based on Bayesian filtering, and the terrain matching positioning result is obtained by correcting the source point cloud pose using the change parameters; for the detected gravity and magnetic information, the rotation and translation change parameters of the source point set and the target point set are obtained by performing the corresponding point selection based on the nearest point iteration of the contour line and the point set registration based on Bayesian filtering, and the gravity and magnetic matching positioning result is obtained by correcting the source point set pose using the change parameters. The position correction frequencies obtained by the terrain point cloud matching and the gravity and magnetic point cloud matching are different, wherein the frequency of the terrain matching is higher than that of the gravity and magnetic matching, and only the terrain matching positioning information is used when there is no gravity and magnetic matching positioning information, and the terrain matching positioning information and the gravity and magnetic matching positioning information are weighted and fused when there is gravity and magnetic matching positioning information. The inertial navigation pose error [δ x , δ y , δ θz ] at each time is taken as the state, the gravity and magnetic matching and the terrain matching positioning information are weighted according to their respective error statistical characteristics to obtain the pose information, and the specific process is as follows:

[0139]

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

[0141] The difference between the weighted pose information and the inertial navigation pose is taken as the observation information, and the Bayesian filter is used to model the problem, and the filter modeling is as follows:

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

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

[0144] wherein is an inertial navigation pose error, respectively, X, Y axis position error and Z axis angle error, f(x k ) is an error analysis equation of inertial navigation, w k-1 is state noise, is the difference between the matching positioning pose and the inertial navigation pose, h(x k ) = x k , is the pose error of the matching positioning.

[0145] The optimal estimation of the inertial navigation pose error is obtained by using the extended Kalman filter, the error is fed back to the inertial navigation, the closed loop correction is carried out on the inertial navigation, the optimal estimation of the pose is obtained, and the whole fusion positioning process is as shown in Figure 4 .

[0146] Application scenario: the application can be applied to underwater positioning of underwater robots, multi-beam sonar depth data, gravity anomaly data and magnetic field intensity data are obtained through a multi-beam sonar, a gravimeter and a magnetometer, the data are matched with a reference map, positioning results and error statistical characteristics are obtained, then the positioning results are fused through a data fusion algorithm based on Bayesian filtering to obtain the optimal estimation of the position. Whether there is only one of the multi-beam sonar depth data, the gravity anomaly data and the magnetic field intensity data or there are multiple data, the application can realize more accurate positioning than the prior art.

[0147] The above only describes the preferred embodiments of the application and is not used to limit the application, for those skilled in the art, the application can have various changes and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the application shall be included in the protection scope of the application.

Claims

1. A method for matching fusion of seafloor topography and gravity and magnetic based on Bayesian filtering, characterized in that, The method comprises the following steps: Step 1: collecting inertial navigation system data, multi-beam echo sounder sounding data, gravity anomaly data and magnetic field intensity data during the underwater vehicle sailing in the target area; processing the collected data to obtain seabed topography point cloud X, gravity anomaly and magnetic field intensity measurement point sequence n; extracting reference point cloud Y, reference gravity anomaly map and reference magnetic field intensity map of the target area from the map library; Step 2: the seabed terrain point cloud X and the reference point cloud Y are represented by graphs respectively, and the corresponding adjacency matrix A is obtained x and A y ; in the graph isomorphism mode, the permutation matrix B of A x and A y is solved through the Monte Carlo framework; 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 terrain point cloud X one by one; Step 3: for the measuring point n in the measuring point sequence n of the gravity anomaly and the magnetic field intensity x , respectively find the points equal to the measuring value of the measuring point on the reference gravity anomaly map and the reference magnetic field intensity map to form two contour lines, and save the intersection point of the two contour lines as the corresponding point n' of the measuring point x , and finally obtain the corresponding point set n' of all measuring points; Step 4: modeling the registration problems of X and Y, and n and n' as filter models respectively, and solving them by using Bayesian filtering to obtain the rotation and translation change parameters and error covariance matrices corresponding to the filter models; Step 5: correcting the seabed topography point cloud X by using the rotation and translation change parameters of X and Y to obtain the seabed topography matching result; and correcting the gravity anomaly and magnetic field intensity measurement point sequence n by using the rotation and translation change parameters of n and n' to obtain the matching result of the gravity anomaly and magnetic field intensity; Step 6: obtaining the underwater vehicle pose measurement information by weighting the seabed topography matching result and the matching result of the gravity anomaly and magnetic field intensity according to the corresponding error covariance matrices; taking the difference between the underwater vehicle pose measurement information and the underwater vehicle pose output by the inertial navigation system as observation information, and taking the inertial navigation system pose error at each time as state to model a filter model, and solving the filter model by using extended Kalman filtering to obtain the optimal estimation of the inertial navigation system pose error; feeding back the pose error to the inertial navigation system for closed-loop correction to obtain the optimal estimation of the underwater vehicle pose.

2. The method of claim 1, wherein the method is characterized by: The method of representing the seabed topography point cloud X and the reference point cloud Y by 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 edges; the vertex of the graph is the position of the point in the point cloud, the edge of the graph is the function of the distance between two points in the point cloud, and the edge exists between two points if the distance between the two points is less than a set threshold, and the weight of the edge is the square of the distance between the two points.

3. The method of claim 1, wherein the method is characterized by: The step 2 uses graph isomorphism, and A is solved by Monte Carlo framework x The method for obtaining the permutation matrix B of A y is specifically as follows: The constraint of the graph isomorphism problem is: min Tr(W T B) BA x = A y B B1 = 1, B T 1 = 1 B ij ≥0 B ij = 0, (i,j) e S Wherein, the graph representation of the seabed topography point cloud X corresponds to the adjacency matrix A x The number of vertices in the graph representation of the reference point cloud Y corresponds to the adjacency matrix A y The number of vertices is n; B is in R n×m It is a matrix with non-negative elements, and the sum of the elements of each row and the sum of the elements of each column are 1; W is in R n×m It is a random matrix with independent Gaussian distribution elements; (i,j) is in S, which means that if the point x i in X is not the corresponding point y i in Y, it is determined by the pruning strategy, specifically: (1) If the edge number difference between point x i and point y i is greater than 4, then point x i and point y i are not corresponding points; (2) if the weight difference between point x i and point y i is greater than then point x i and point y i are not corresponding points; c is a constant, 4. The method of claim 1, wherein: The modeling of the registration problems of X and Y, and n and n' as filter models in step 4 is specifically as follows: x k = x k-1 + v(k-1) where u is the set of source points, y k is the set of target points; if it is the registration problem of X and Y, then u = X, y k = Y; if it is the registration problem of n and n', then u = n, y k = n'; x k is the state of the kth iteration, v(k-1) is a Gaussian white noise, v(k-1) e N(x k-1 ; 0, Q K-1 ); e(k) is a Gaussian white noise of the kth iteration observation, e(k) e N(y k ; 0, R k ), 5. The method of claim 4, wherein: The solving by using Bayesian filtering in step 4 is specifically as follows: Step 4.1: Initialize k = 1, initialize x 0|0 , P 0|0 , Q0 and R0; Step 4.2: state updating; P k|k-1 = P k-1|k-1 + Q k-1 Step 4.3: P k|k-1 decomposed into S k|k-1 and Step 4.4: Estimate point X i,k|k-1 ; wherein ξ i is the volume point, Step 4.5: Estimate the propagation point Y i,k|k-1 ; Step 4.6: Estimate 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-translation variation parameters and the error covariance matrix P kk ; Step 4.11: If then let Q k = 0.9Q k-1 , k = k + 1, go to Step 4.2; otherwise, output the rotation-translation variation parameters and the error covariance matrix P kk .

6. The method of claim 1, wherein: The step 6 matches the seabed terrain, the gravity anomaly and the magnetic field strength according to the corresponding error covariance matrix, and obtains the pose measurement information of the underwater vehicle Specifically: Wherein, P1 and P2 are the error covariance matrices of X and Y, and n and n' obtained in step 4; X1, Y1, θ1 and X2, Y2, θ2 are the seabed topography matching result and the matching result of the gravity anomaly and magnetic field intensity obtained in step 5.

7. The method of claim 6, wherein the method is characterized by: The modeling of the filter model in step 6 by taking the difference between the underwater vehicle pose measurement information and the underwater vehicle pose output by the inertial navigation system as observation information, and taking the inertial navigation system pose error at each time as state is specifically as follows: x k = f(x k-1 ) + w k-1 z k = h(x k ) + v k wherein is the position error of the inertial navigation system, respectively the position error along the X, 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 position measurement information of the underwater vehicle and the position of the underwater vehicle output by the inertial navigation system; h(x k ) = x k ; is the position error of the matched positioning.

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

9. A computer readable storage medium having stored thereon computer programs / instructions, characterized in that: The computer program / instructions are executed by the processor to implement the steps of the method of any one of claims 1 to 7.

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

Images