A gravity matching method based on a computational geometry triangle matching algorithm

Abstract

The present invention discloses a gravity matching method based on a computational geometry triangle matching algorithm. The present invention converts the gravity field background map into projection coordinates, performs triangular mesh subdivision, and uses the gravity anomaly value information as the triangle elevation information. By using the local information of the gravity field background map after triangulation and the real-time information of the inertial navigation indicated track, the three-dimensional information of the gravity field is comprehensively considered from a geometric level, effectively improving the accuracy of gravity matching positioning. The matching triangles obtained by the present invention are matched from the real-time track triangles, and the degree of error divergence is much smaller than that of the traditional gravity matching algorithm. Moreover, the algorithm has less operation time each time, low computational complexity, strong real-time performance, and is suitable for long-term high-precision autonomous matching positioning of underwater vehicles.

Description

Technical Field

[0001] The present invention relates to the technical field of navigation, guidance and control, and particularly to a gravity matching method based on a computational geometry triangle matching algorithm. Background Art

[0002] Inertial navigation systems are widely used in various land, sea, air and space carriers because they can provide real-time navigation and positioning information such as position, speed and attitude for the carrier. However, the navigation errors of inertial navigation systems will accumulate over time, and the long-term navigation accuracy will deteriorate. Therefore, it is necessary to use other external information to assist in navigation. The gravity field is an inherent physical field of the earth. Measuring instruments such as gravimeters can obtain the gravity anomaly information at the location in real time. Since the gravity anomaly information has good spatio-temporal position characteristics and does not damage the stealth of underwater vehicles, using a gravity-aided inertial navigation system can safely and effectively improve the navigation and positioning accuracy of the inertial navigation system.

[0003] The gravity matching positioning algorithm is the core of the gravity-aided inertial navigation system. Traditional gravity matching algorithms can be divided into sequential matching algorithms and single-point matching algorithms according to the sampling method. Sequential matching algorithms need to obtain a large number of sampling points before matching, so the real-time performance of the algorithm is poor, and there will be a certain delay in the finally obtained matching position. Single-point matching requires a high-precision initial position. If the linearization method is not properly selected or the linearization error is large, the situation of filter divergence will occur. Traditional gravity matching algorithms do not fully exploit the grid information and geometric topology information of the gravity field background map, and cannot fully consider the three-dimensional characteristics of the gravity field, which will lead to large errors in matching and positioning. Summary of the Invention

[0004] In view of this, the present invention provides a gravity matching method based on a computational geometry triangle matching algorithm, which can effectively utilize the geometric topology information of the gravity field and the inertial navigation indication trajectory, and obtain the trajectory information closest to the real track through the triangle normal vector matching, thereby improving the accuracy and real-time performance of gravity matching navigation.

[0005] The gravity matching method based on the computational geometry triangle matching algorithm of the present invention includes the following steps:

[0006] Step 1, preprocess the grid point information of the gravity field background map and the real-time track longitude and latitude and gravity anomaly value of the underwater vehicle: convert the longitude and latitude coordinates into projection coordinates, and convert the gravity anomaly value into elevation information;

[0007] Step 2, perform triangular mesh division on the preprocessed gravity field background map and the inertial navigation track;

[0008] Step 3: Taking the local extreme value of gravity anomaly in the inertial navigation track as the division basis, and taking each gravity anomaly peak-valley or valley-peak segment as a matching unit;

[0009] Step 4: For each matching unit, search for the gravity field triangle whose normal vector is closest to the normal vector of the track triangle and the angle between the normal vector and the horizontal plane is closest to the angle between the normal vector of the track triangle and the horizontal plane; this gravity field triangle is the gravity field matching triangle that satisfies the graphic matching;

[0010] Step 5: According to the gravity field matching triangle obtained in Step 4, use the matching point acquisition algorithm based on the gravity anomaly ratio to obtain the gravity matching points corresponding to the track points; the connection of all gravity matching points is the final matching track.

[0011] Preferably, the following formula is used to convert the gravity anomaly change value Δg and the elevation information Z:

[0012]

[0013] Among them, Z(h) is the elevation information converted from the gravity anomaly value at an altitude of h; g0 is the reference gravity anomaly value on the geoid; g(h) is the gravity anomaly value at a height of h, and Δg is the gravity anomaly change value.

[0014] Preferably, in Step 2, the Delaunay triangulation algorithm is used for triangulation.

[0015] Preferably, in Step 4, the search range is: a circular area with a radius of R around the inertial navigation track triangle.

[0016] Preferably, the radius R of the circular area is:

[0017] R = R0 + nr

[0018] Among them, R0 represents the radius of the circular area during the initial matching, which is generally related to the resolution of the gravity field background map; n represents the number of matches, and r represents the change rate of the radius of the circular area, which is generally related to the measurement frequency of the gravimeter.

[0019] Preferably, in Step 4, the triangle normal vector matching criterion is:

[0020]

[0021] Among them, v i represents the normal vector of the triangle in the inertial navigation track unit, and v ij represents the normal vector of the triangle in the gravity field background map, and dis(v i , v ij ) represents the normal vector v i and vij The distance between

[0022] Preferably, in step 5, the track matching points are obtained in the following manner:

[0023] Take out the gravity anomaly value information of the three vertices of the inertial navigation track triangle ABC and the gravity field matching triangle abc, and arrange them in ascending order; assume the sequence is: Z(A), Z(B), Z(C), and Z(a), Z(b), Z(c); calculate the differences between Z(A) and Z(B), Z(C), and obtain the ratio by which Z(B) divides Z(A)Z(C), denoted as k, that is

[0024]

[0025] Divide Z(a)Z(c) according to the ratio k to obtain the fixed feature point thereon, denoted as P, that is

[0026] Z(P) = k * ΔZ(ac) + Z(a)

[0027] Judge whether the value is on the ab side or the bc side according to the magnitudes of the gravity anomaly values at point P and point b. If Z(c) > Z(b) > Z(P) > Z(a), then it is on the ab side; if Z(c) > Z(P) > Z(b) > Z(a), then it is on the bc side; assume it is on the bc side, then set the corresponding point as the active feature point, denoted as point Q, and arrange them in ascending order of gravity anomaly value as b, Q, c. Determine the position of point Q according to the proportion of the gravity anomaly value of point Q on bc; connect point P and point b, point Q and point a respectively, and the intersection of the two line segments is the matching point H.

[0028] Beneficial effects:

[0029] (1) In the present invention, the gravity field background map is converted into projection coordinates, and triangular grid meshing is performed, and the gravity anomaly value information is used as the triangular elevation information. By using the local information of the gravity field background map after triangular meshing and the real-time information of the inertial navigation indicated track, the three-dimensional information of the gravity field is comprehensively considered from a geometric level, effectively improving the accuracy of gravity matching positioning. The matching triangle obtained by the present invention is matched from the real-time track triangle, and the error divergence degree is much smaller than that of the traditional gravity matching algorithm. Moreover, the time for each operation of the algorithm is short, the computational complexity is low, and the real-time performance is strong, which is suitable for high-precision autonomous matching positioning of underwater submersibles during long-term navigation.

[0030] (2) Based on the spatial correction formula of gravity anomaly value The gravity anomaly value can be directly converted into elevation information, which is convenient and fast.

[0031] (3) The Delaunay triangulation algorithm adopted by the present invention is used for triangulation, which can show the geometric features of the gravity field point set provided in the gravity field background map, accurately describe the information of the gravity field, and the algorithm has high feasibility and small complexity.

[0032] (4) The present invention determines the search range when matching the gravity triangle and the track triangle based on the resolution of the gravity field background map and the measurement frequency of the gravimeter, improving the search efficiency and accuracy.

[0033] (5) The present invention uses the gravity anomaly values of the three vertices of the track triangle and their proportional relationship to determine the matching points actually corresponding to the track points in the gravity matching triangle, with simple calculation and high matching accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] Figure 1 is a flowchart of the gravity matching method based on the computational geometry triangle matching algorithm according to an embodiment of the present invention;

[0035] Figure 2 is a schematic diagram of the matching point acquisition algorithm based on the gravity anomaly ratio according to an embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0036] The following is a detailed description of the present invention with reference to the accompanying drawings and embodiments.

[0037] The present invention provides a gravity matching method based on the computational geometry triangle matching algorithm. First, the gravity background map is converted to the projection coordinate system; then, through the triangulation algorithm, the gravity field background map and the inertial navigation indicated track are triangulated, and the matching unit is divided according to the peak-valley / valley-peak of the gravity anomaly value. In each unit, according to the principle that the normal vectors of the triangles are parallel and the distance is short, the optimal matching triangle is selected, and the optimal matching track is obtained, effectively improving the accuracy of gravity matching positioning.

[0038] The specific matching process is as Figure 1 shown, including the following steps:

[0039] Step 1, perform data preprocessing on the raster point information of the pre-stored gravity field background map and the longitude, latitude, and gravity anomaly value information of the indicated track measured in real time by the underwater vehicle through the gravimeter: use the Mercator transformation to transform the longitude and latitude coordinates into projection coordinates; according to the spatial correction formula of the gravity anomaly value, convert the gravity anomaly value of the gravity field sampling point into elevation information.

[0040] Specifically, taking the equator as the standard parallel, the prime meridian as the central meridian, the intersection of the equator and the prime meridian as the coordinate origin, and taking eastward and northward as the positive directions, and westward and southward as the negative directions. The functional relationship for converting from the geodetic coordinates (B, L) to the projection coordinates (X, Y) is:

[0041]

[0042] Among them, a is the length of the semi-major axis of the ellipsoid, taking 6378136.49 m; b is the length of the semi-minor axis of the ellipsoid, taking 6356755.00 m; e is the first eccentricity, taking 0.0818; e' is the second eccentricity, taking 0.0821; N is the radius of curvature of the prime vertical, and the calculation formula is B is the standard latitude; B0 is the origin latitude, set to 0°; L is the standard longitude, L0 is the origin longitude, set to 0°; the subscript N represents north, and the subscript E represents east.

[0043] For the spatial correction of gravity anomalies, the influence of the change in the measurement point position on the gravity anomaly value is mainly considered. According to the Taylor expansion, the spatial correction can be regarded as: extending the reference gravity value anomaly g0 on the geoid along the elevation. When the height is h, the gravity anomaly value is g(h), then the spatial calibration is the difference between the gravity anomaly value at height h and the original height. Taking the elevation information h as the increment of the Earth's radius R, we can get:

[0044]

[0045] Among them, the error term is That is the height correction value.

[0046] If the Earth is regarded as an ellipsoid with a flattening of f, the normal gravity anomaly value g0 at a point on the geoid in a state consistent with the calm sea level and the normal

[0047] gravity anomaly value g(h) at a point with an altitude of h at this point satisfy the following equation:

[0048]

[0049] Among them, g e is the gravity value at the Earth's equator, R is the radius of the Earth, q is the ratio of the inertial centrifugal force to the gravitational force at the equator, is the geographical latitude.

[0050] The Earth parameters adopt g e = 9780300, R = 6371.2 km, f = 1 / 298.3, q = 0.003467.

[0051] The approximate spatial correction formula is obtained as:

[0052] Δg = g(h) - g0 = 0.3086h (4)

[0053] From this, it can be obtained that every 1 m of elevation displacement generates a gravity anomaly change of approximately 0.3086 mGal, that is:

[0054]

[0055] The normal gravity anomaly value g0 on the general permanent geoid is 0. Thus, through formulas (1) and (5), the longitude and latitude information and the gravity anomaly value information can be preprocessed and converted into coordinates X, Y, and Z in meters.

[0056] Step 2: Use the Delaunay triangulation algorithm to divide the preprocessed gravity field background map and the inertial navigation trajectory into triangular meshes.

[0057] Step 3: Locate the positions of the local maximum and minimum values of the gravity anomaly in the inertial navigation track, and take each segment of the gravity anomaly peak-valley or valley-peak as a matching unit.

[0058] Step 4: Take the nth matching unit of the inertial navigation track. From the projected coordinates of the triangles in the matching unit, obtain the gravity field triangular region within a certain error range, and get the included angle between the normal vectors of all the triangles within the range and the horizontal plane, as well as the distance between the plane where the track triangle is located and the plane where the gravity field triangle is located. Traverse all the gravity field triangles within the range, and find the triangle whose included angle with the horizontal direction of the track is closest to being equal and whose normal vector distance is the smallest, which is the matching triangle for graphic matching. The triangle normal vector matching criterion is:

[0059]

[0060] where, v i represents the normal vector of the triangle in the inertial navigation track unit, v ij represents the normal vector of the triangle in the gravity field background map, and dis(v i , v ij ) represents the distance between the normal vectors v i and v ij .

[0061] Step 5: According to the gravity field matching triangle obtained in Step 4, use the matching point acquisition algorithm based on the gravity anomaly ratio. The schematic diagram is as shown in the appendix Figure 2 as shown. Let triangle ABC represent the triangle of the inertial navigation track, and triangle abc represent the obtained gravity field matching triangle. Take out the gravity anomaly value information of the three vertices of triangle ABC and arrange them in ascending order. Let A, B, and C represent the gravity anomaly value information of the corresponding vertices respectively. After arranging them in order, the sequence A, B, C is obtained, where A is the minimum value and C is the maximum value. By calculating the difference between AB and AC, obtain the ratio k of the total length AC divided by B, that is

[0062] Extract the gravity anomaly value information of the three vertices of the gravity field matching triangle abc, and arrange them in ascending order. Let a, b, and c represent the gravity anomaly value information of the corresponding vertices respectively. After arranging them in order, we get the sequence a, b, c, where a is the minimum value and c is the maximum value. Divide ac according to the previously obtained ratio k to obtain the fixed feature point on it, denoted as P, that is, Z(P) = k * ΔZ(ac) + Z(a). Determine which part of the three sides this value is located in according to the gravity anomaly value at point P: If Z(c) > Z(b) > Z(P) > Z(a), then it is located on the ab side; if Z(c) > Z(P) > Z(b) > Z(a), then it is located on the bc side. Assume it is located on bc, and set the corresponding point as the active feature point, denoted as point Q. Arrange them in ascending order of gravity anomaly value as b, Q, c, and determine the position of point Q according to the proportion of the gravity anomaly value of point Q on bc. Connect point P and point Q to their corresponding triangle vertices respectively, and the intersection point of the two line segments is the matching point H. Connect all the matching track points to obtain the final matching track.

[0063] Therefore, the gravity matching method based on the computational geometry triangle matching algorithm carried out according to the above steps obtains the matching track according to the real-time track triangle matching. The error divergence degree is much smaller than that of the traditional gravity matching algorithm, and it has good robustness and is suitable for long-term high-precision autonomous matching positioning of underwater vehicles.

[0064] In summary, the above is only a preferred embodiment of the present invention and is not used to limit the protection scope of the present invention. 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 gravity matching method based on a computational geometry triangle matching algorithm, characterized in that, It includes the following steps: Step 1, preprocess the grid point information of the gravity field background map, the real-time track longitude and latitude of the underwater vehicle, and the gravity anomaly value: convert the longitude and latitude coordinates into projected coordinates, and convert the gravity anomaly value into elevation information; Step 2, perform triangular mesh division on the preprocessed gravity field background map and the inertial navigation track; Step 3, taking the local extreme value of gravity anomaly in the inertial navigation track as the division basis, and taking each section of gravity anomaly peak-valley or valley-peak as a matching unit; Step 4, for each matching unit, search for the gravity field triangle whose normal vector is closest to the normal vector of the track triangle and the angle between the normal vector and the horizontal plane is closest to the angle between the normal vector of the track triangle and the horizontal plane; this gravity field triangle is the gravity field matching triangle that satisfies the graphic matching; Step 5, according to the gravity field matching triangle obtained in Step 4, adopt a matching point acquisition algorithm based on the gravity anomaly ratio to obtain the gravity matching point corresponding to the track point; the connection line of all gravity matching points is the final matching track.

2. The gravity matching method based on the computational geometry triangle matching algorithm according to claim 1, wherein, Use the following formula to convert the gravity anomaly change value Δg and the elevation information Z: Among them, Z(h) is the elevation information converted from the gravity anomaly value at an altitude of h; g0 is the reference gravity anomaly value on the geoid; g(h) is the gravity anomaly value at a height of h, and Δg is the gravity anomaly change value.

3. The gravity matching method based on the computational geometry triangle matching algorithm according to claim 1 or 2, characterized in that In the said Step 2, the Delaunay triangulation algorithm is used for triangulation.

4. The gravity matching method based on the computational geometry triangle matching algorithm according to claim 1, wherein In the said Step 4, the search range is: a circular area with a radius of R around the inertial navigation track triangle.

5. The gravity matching method based on the computational geometry triangle matching algorithm according to claim 4, characterized in that The radius R of the said circular area is: R = R0 + nr Among them, R0 represents the radius of the circular area at the initial match, which is generally related to the resolution of the gravity field background map; n represents the number of matches, and r represents the change rate of the radius of the circular area, which is generally related to the measurement frequency of the gravimeter.

6. The gravity matching method based on the computational geometry triangle matching algorithm according to claim 1 or 4 or 5, characterized in that In the said Step 4, the triangle normal vector matching criterion is: Among them, v i represents the triangular normal vector in the inertial navigation trajectory unit, and v ij represents the triangular normal vector of the gravity field background map. dis(v i , v ij ) represents the distance between the normal vectors v i and v ij .

7. The gravity matching method based on the computational geometry triangle matching algorithm according to claim 1 or 4 or 5, characterized in that In the said Step 5, the track matching points are obtained in the following way: Take out the gravity anomaly value information of the three vertices of the inertial navigation track triangle ABC and the gravity field matching triangle abc, and arrange them in ascending order; assume the sequence is: Z(A), Z(B), Z(C), and Z(a), Z(b), Z(c); calculate the difference between Z(A) and Z(B), Z(C) to obtain the ratio of Z(B) dividing Z(A)Z(C), denoted as k, that is Divide Z(a)Z(c) according to the ratio k to obtain the fixed feature point on it, denoted as P, that is Z(P) = k * ΔZ(ac) + Z(a) Judge whether the value is on the ab side or the bc side based on the magnitudes of the gravity anomaly values at point P and point b. If Z(c) > Z(b) > Z(P) > Z(a), then it is on the ab side; if Z(c) > Z(P) > Z(b) > Z(a), then it is on the bc side. Assume it is on bc, then set the corresponding point as the active feature point, denoted as point Q. Arrange them in ascending order of gravity anomaly values as b, Q, c. Determine the position of point Q according to the proportion of the gravity anomaly value of point Q on bc. Connect point P and point b, and point Q and point a respectively. The intersection point of the two line segments is the matching point H.

Images