Gravity matching method based on characteristics of gravity measurement data

Abstract

The present invention discloses a gravity matching method based on the characteristics of gravity measurement data. The present invention takes into account the variation characteristics of gravity measurement data, analyzes the extreme value measurement points, and combines with the gravity background map to obtain a relatively accurate position estimation point that is not affected by the initial position error of the inertial navigation. Then, taking this position estimation point as the initial position point of the filtering gravity matching algorithm for gravity matching, it can effectively reduce the influence of the matching initial error caused by the inertial navigation system and the real-time gravity measurement data error, and improve the gravity matching positioning accuracy. In addition, the present invention also introduces the concept of direction measurement according to the relationship between the underwater vehicle's heading angle and the navigation displacement direction within the matching period in the filtering gravity matching algorithm, which can effectively solve the problem of slow convergence speed in the filtering gravity matching algorithm. At the same time, the algorithm also has a certain ability to resist outliers.

Description

Technical Field

[0001] The present invention relates to the technical field of navigation, guidance and control, and particularly relates to a gravity matching method based on gravity measurement data characteristics. Background Art

[0002] Underwater vehicles are indispensable carrier operation equipment for ocean exploration and ocean resource development in various countries around the world. The key technology is underwater autonomous navigation technology. Inertial navigation can provide comprehensive navigation and positioning information such as position, speed, and attitude for underwater vehicles due to its characteristics of high concealment, complete autonomy, and no information exchange with the outside world, and is widely used in the field of underwater navigation. In the case of long-term underwater navigation, the errors of the inertial navigation system diverge with time, and other navigation methods are needed to compensate for the errors. The characteristics of the ocean gravity field are stable, and gravity-aided inertial navigation has become a research hotspot for underwater autonomous navigation. The gravity matching algorithm is the core of gravity-aided inertial navigation, which is mainly affected by two aspects: the inertial navigation error at the start of matching and the real-time gravity measurement error. The inertial navigation error at the start of matching affects the convergence of the gravity matching algorithm, and is sensitive to the real-time gravity measurement error when dealing with isolines. Traditional filtering-based gravity matching algorithms (i.e., using filtering algorithms for gravity matching, such as SITAN, PF, PMF, etc.) do not involve the processing of isolines, and only use the magnitude of the real-time gravity measurement value as the measurement value, which can effectively reduce the influence of gravity measurement errors. However, the initial value of the filter comes from the navigation information output by the inertial navigation system, and the errors existing in this part make the matching algorithm converge slowly, resulting in low matching efficiency. In addition, traditional filtering-based gravity matching algorithms only use the magnitude of the real-time gravity measurement value, and do not consider the change characteristics of the gravity measurement value. The only available gravity measurement data characteristics are not fully utilized, making the efficiency of traditional filtering-based gravity matching algorithms not high. Summary of the Invention

[0003] In view of this, the present invention provides a gravity matching method based on gravity measurement data characteristics. By introducing gravity measurement data characteristics into the filtering model, it can effectively reduce the influence of the matching initial error caused by the inertial navigation system and the real-time gravity measurement data error, and improve the gravity matching positioning accuracy.

[0004] The gravity matching method based on gravity measurement data characteristics of the present invention includes:

[0005] Step 1, plan the trajectory of the underwater vehicle in the gravity adaptation area; wherein, the planned trajectory needs to meet: passing through points with obvious gravity change characteristics in the adaptation area; the planned trajectory is tangent to an isoline in the adaptation area, and the curvature of the isoline at the tangent point cannot be too small, that is, the difference in the direction angles of two adjacent points near the tangent point on the isoline needs to be greater than the inertial navigation course angle error;

[0006] Step 2: The underwater vehicle sails along the planned track, measures the gravity anomaly value in real time, and obtains the extreme value points of the local gravity anomaly and their corresponding moments.

[0007] Step 3: Estimation of the position of the underwater vehicle at the extreme moment:

[0008] S31: According to the gravity anomaly value corresponding to the extreme value point, find the corresponding contour line in the gravity background map.

[0009] S32: Calculate the tangents of each discrete point on the contour line described in S31.

[0010] S33: Use the heading angle of the inertial navigation system at the extreme moment as the extreme tangent angle, calculate the tangent k corresponding to the extreme tangent angle. When k is between the tangents of two consecutive discrete points on the contour line, obtain the estimated position point by the weighted average method.

[0011] S34: Take the estimated position point closest to the inertial navigation position point as the position at the current extreme moment.

[0012] Step 4: Set the position at the extreme moment obtained in Step 3 as the initial position point of the filtering type gravity matching algorithm, and perform gravity matching.

[0013] Preferably, in Step 1, the A* algorithm or the genetic algorithm is used for track planning.

[0014] Preferably, in Step 2, the measured gravity anomaly value is fitted by a local function, and then the first derivative is set to zero to obtain the extreme value point.

[0015] Preferably, in Step 2, a sliding window with a length of N is designed, where N is an odd number; calculate the standard deviation of N consecutive gravity anomaly measurement values within the sliding window; slide the sliding window, and the data at the center of the sliding window corresponding to the minimum value in the standard deviation is used as the extreme value point.

[0016] Preferably, N is 7 or 15.

[0017] Preferably, in S32, arbitrarily select three consecutive discrete points a(x1, y1), b(x2, y2) and c(x3, y3) on the contour line, calculate the radius r of the circle formed by these three points and the center O(x c , y c ), then the tangent of point b is

[0018]

[0019] Preferably, in Step 4, the heading angle of the inertial navigation system is constructed as a direction measurement, which is used as measurement information together with the gravity anomaly measurement.

[0020] Preferably, the particle filter algorithm is adopted for gravity matching.

[0021] Preferably, in the particle filter algorithm, particles are weighted by a Gaussian function.

[0022] Beneficial effects:

[0023] (1) Considering the variation characteristics of gravity measurement data, the present invention analyzes extreme measurement points, combines with the gravity background map to obtain a relatively accurate position estimation point that is not affected by the initial position error of the inertial navigation, and then uses this position estimation point as the initial position point of the filtering gravity matching algorithm for gravity matching, which can effectively reduce the influence of the matching initial error and real-time gravity measurement data error caused by the inertial navigation system, and improve the gravity matching positioning accuracy.

[0024] (2) In the filtering gravity matching algorithm, according to the relationship between the underwater vehicle's heading angle and the navigation displacement direction within the matching period, the concept of direction measurement is introduced, which can effectively solve the problem of slow convergence speed in the filtering gravity matching algorithm. In addition, by adopting the particle filter algorithm, particles can be weighted, such as by a Gaussian function, so that the algorithm of the present invention also has a certain ability to resist outliers. Description of the Drawings

[0025] Figure 1 It is the application process of underwater vehicle gravity matching when the filtering algorithm is selected as the particle filter algorithm proposed by the present invention;

[0026] Figure 2 It is a demonstration diagram of extreme measurement points and extreme moments;

[0027] Figure 3 It is the schematic diagram of the principle of finding extreme points by the window method;

[0028] Figure 4 It is the position estimation process of the underwater vehicle at the extreme moment;

[0029] Figure 5 It is the principle of constructing direction measurement. Detailed Embodiments

[0030] The following combines the drawings and gives examples to describe the present invention in detail.

[0031] The present invention provides a gravity matching method based on the characteristics of gravity measurement data. By studying the characteristics of underwater vehicle gravity measurement data near extreme points, combining the positional relationship between the navigation route and the contour lines of the gravity background map, the position of the vehicle at the extreme point moment is calculated. On this basis, direction measurement is established using the vehicle's heading angle, and position estimation is performed throughout the process through a filtering gravity matching algorithm.

[0032] In this embodiment, taking the particle filter algorithm as an example, the matching process is as follows: Figure 1 as shown below, and specifically includes the following steps:

[0033] Step 1: Plan the navigation route of the underwater vehicle within the gravity adaptation area through the trajectory planning algorithm.

[0034] Before the underwater vehicle enters the adaptation area, the trajectory within the adaptation area should be planned through the trajectory planning algorithm (such as A* algorithm, genetic algorithm, etc.). The planned trajectory needs to meet the following requirements: ① Pass through the points with obvious gravity change characteristics within the adaptation area; ② The planned trajectory is tangent to an isoline within the adaptation area, and the curvature of the isoline near the tangent point cannot be too small, that is, the difference in the direction angles of two adjacent points of the isoline near the tangent point needs to be greater than the inertial navigation course angle error. The purpose of the trajectory planning algorithm is to make the gravity measurement data sequence present extreme points. By studying the extreme points of the real-time measured gravity values, a foundation is laid for subsequent position estimation.

[0035] Step 2: After the underwater vehicle enters the adaptation area, it navigates according to the planned trajectory, and records the gravity anomaly measurement data in real time to obtain the extreme points of the local gravity anomaly and their corresponding moments, that is, the extreme moments, as shown below. The extreme points include maximum points and minimum points. Figure 2 as shown below. The extreme points include maximum points and minimum points.

[0036] When there is no error in gravity measurement, the extreme points can be obtained by locally fitting the function and then setting the first derivative to zero. However, in reality, errors are inevitably present, and the change degree of the gravity anomaly measurement values near the extreme moments is slower compared to other time points. Therefore, the present invention designs a sliding window with a length of N (N is an odd number, and N generally takes [7, 15]). Within a period of time when extreme points appear, the extreme points are obtained by judging the change degree of the gravity anomaly measurement values within the sliding window, as shown below. At time k, for the previous consecutive N gravity anomaly measurement values g Figure 3 ~g k-N+1 ~g k , calculate the standard deviation of these data.

[0037]

[0038] In the area where extreme points may appear, slide and calculate the standard deviation of the data within each window, and find the sliding window corresponding to the minimum value among these standard deviations. Take the data at the center of this sliding window as the extreme measurement point, and the moment corresponding to this extreme measurement point is the extreme moment.

[0039] Step 3: Estimate the position of the underwater vehicle at the extreme moment.

[0040] The gravity anomaly values in the adaptation area are generally distributed in the form of isolines. The underwater vehicle uses a gravimeter to measure the real-time gravity anomaly value, which is equivalent to the underwater vehicle being on the isoline corresponding to the measured value at this moment. For the occurrence of extreme measurement points, the reason is that the path of the underwater vehicle is tangent to the corresponding isoline. When two curves are tangent, a tangential angle is required to determine the tangent point. The present invention uses the heading angle output by the inertial navigation system to determine the tangential angle. Generally speaking, the output frequency of the inertial navigation system is higher than the frequency of gravity measurement. Therefore, when the extreme measurement point and the extreme moment are determined, the heading angle output by the inertial navigation system at this moment can be used as the tangential angle.

[0041] Therefore, the present invention first finds the corresponding isoline in the gravity background map through an isoline search algorithm such as the interpolation method according to the gravity anomaly value corresponding to the extreme measurement point, and calculates the tangent of the points on the isoline. The isoline is generally represented by numerous discrete points, and each point has a corresponding tangent. The present invention locally approximates the isoline as an arc to calculate the tangent of each point. Taking three consecutive discrete points a(x1, y1), b(x2, y2), and c(x3, y3) as an example, calculate the radius r of the circle formed by these three points and the center O(x c ,y c ). Then the tangent of point b is

[0042]

[0043] By performing the above operations on all points on the isoline, the tangents of all points on the isoline can be obtained. Since the isoline is approximately continuous, the change in the obtained tangents is also approximately continuous. Let the heading angle of the inertial navigation system at the extreme moment The corresponding tangent is When k is between the tangents of two consecutive points on the isoline, the estimated position point is obtained by the weighted average method. For example, when k m <k<k n At this time, k m The corresponding point is m(x m ,y m ), k n The corresponding point is n(x n ,y n ), then the possible position estimation point can be obtained through equations (4) and (5) as Where

[0044]

[0045] There is

[0046] Obviously, there may be more than one obtained position estimation point, so it is necessary to determine according to the inertial navigation trajectory and the navigation time. Generally, an isoline in the gravity background map has a wide distribution range, and the obtained several position estimation points are far apart. When the navigation time is not long, or the inertial navigation system has been reset in position before, at this time, it can be judged through the inertial navigation trajectory points, and the position estimation point closest to the inertial navigation position point is selected as the position at the current extreme moment. As Figure 4 shown

[0047] Step 4: Set the position at the current extreme moment obtained in Step 3 as the initial position point of the filtering type gravity matching algorithm, and perform gravity matching.

[0048] In this embodiment, the filtering type algorithm is illustrated by taking the particle filtering algorithm as an example. When the position at the extreme moment is estimated in Step 3, it can be used as the initial value of the particle filtering algorithm. Starting from the extreme moment, after a certain delay, the position of the underwater vehicle is estimated in real time. In addition, the particle filtering algorithm can also estimate the past track.

[0049] Specifically, in the gravity-aided inertial navigation system, in order to reduce the computational burden, the particle filtering algorithm usually uses a position model, and its filtering model is

[0050] x k = x k-1 + u k-1,k + w k-1 (6)

[0051] y k = h(x k ) + v k (7)

[0052] where, x k represents the position of the underwater vehicle at time k (defined as a two-dimensional variable in the present invention, [latitude, longitude]), u k-1,k represents the displacement given by the inertial navigation system from time k-1 to time k, w k-1 represents the displacement error of this part, y k represents the measured local gravity anomaly value, h(·) represents the process of reading the gravity anomaly value from the gravity background map according to the position, v k represents the measurement error. According to the principle of particle filtering, when weighting the particles, since the same gravity anomaly value corresponds to an isoline in the gravity background map, therefore, the particles distributed near the isoline have larger weights, and there will be more particles with similar weights, and it is difficult to converge to the correct position after resampling.

[0053] Obviously, only one gravity anomaly measurement information is not enough. Since the available external measurement information in the gravity-aided inertial navigation system is only from the gravity meter, the present invention will use the heading angle information output by the inertial navigation system to construct the measurement information about the direction. Assume that the time interval between two adjacent gravity matches is T. As Figure 5 shown, the thick line represents the trajectory of the underwater vehicle from point A to point B during the T time, and the dashed line represents the displacement during this period. If the underwater vehicle does not perform behaviors such as sharp turning during this period that cause obvious changes in the motion state (including heading and speed), then the direction of the displacement during this period is approximately the same as the heading at the middle moment of this period. Figure 5 Figure 4 shows the relationship between the displacement direction and the heading at the middle moment when the track is an arc. Obviously, when the track is a straight line, the above also holds.

[0054] The heading angle error output by the inertial navigation system is in an oscillating form and does not diverge with time. The heading angle at the middle moment of this period can be used as the direction of the displacement during this period. Therefore, the filtering type of gravity matching algorithm starts from the extreme moment and estimates the position of the underwater vehicle in real time after a short delay. In addition, the past tracks can also be smoothed. Therefore, the present invention constructs the heading angle at the middle moment as the direction measurement, and together with the gravity anomaly measurement, it is used as the measurement information of the particle filter algorithm to comprehensively calculate the posterior weight of the particles. Then the measurement equation of the filter becomes

[0055] z k =η(x k ) + ε k (8)

[0056] where z k =[y k r k T , η(x k ) = [h(x k ) q(x k )] T , ε k = [v k ξ k )] T , r k represents the heading angle direction of the inertial navigation output during the middle period from k - 1 to k, that is, the direction measurement, q(·) represents the direction angle calculated according to the current position x k and the previous moment position x k-1 , and ξ k represents this part of the error.

[0057] 1) Initial particle generation

[0058] For i = 1, 2,..., N, ​

[0059]

[0060] Normalization:

[0061]

[0062] 2) For the k-th moment (k≥1), assume there is a posterior at the (k - 1)-th moment and the estimation result at the (k - 1)-th moment Then there is the following

[0063]

[0064] Normalization:

[0065]

[0066] Resampling to obtain new and Then the estimation result at the k-th moment is

[0067]

[0068] In Equation (9), represents the i-th particle initially generated, represents its corresponding weight. p0(x0) is set as a Gaussian density function, whose mean is the initial position estimate and the covariance is the diagonal element of the initial covariance matrix P0. The meaning of Equation (9) is to sample N particles from p0(x0);

[0069] In Equation (10),

[0070]

[0071] where y0 represents the gravity anomaly measurement value at the initial moment of filtering, h(·) represents the process of reading the gravity anomaly value from the gravity background map according to the position, and σ y represents the standard deviation of gravity measurement, which is generally unknown. Therefore, the size can be set here according to the degree of dependence on the gravity measurement value. When attaching more importance to the gravity anomaly measurement value, σ y can take a smaller value, and vice versa.

[0072] In Equation (12),

[0073]

[0074] where Q k-1 represents the process noise covariance matrix at the (k - 1)-th moment, and this term is also unknown. In this paper, it needs to be comprehensively evaluated according to the matching period, inertial navigation system parameters, and the operating state of the vehicle;

[0075] In formula (13),

[0076]

[0077] wherein, similar to the meaning of σ y σ y,k and σ r,k respectively represent the importance attached to the gravity anomaly measurement and the direction measurement at time k. If the x-axis of the gravity background map coordinate system is latitude and the y-axis is longitude, then here the calculation method is:

[0078]

[0079] In the formula, represents the first element of and represents the posterior position estimate calculated at time k-1. represents reading the gravity anomaly value at the position from the gravity background map.

[0080] Thus, the gravity matching process of the underwater vehicle is completed.

[0081] In summary, the above is only a preferred embodiment of the present invention and is not intended to limit the protection scope of the present invention. Any modifications, equivalent replacements, improvements, 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 the characteristics of gravity measurement data, characterized in that, Including: Step 1: Plan the trajectory of the underwater vehicle within the gravity adaptation area. Among them, the planned trajectory needs to meet the following requirements: pass through the points with obvious gravity change characteristics within the adaptation area; The planned trajectory is tangent to an isoline within the adaptation area, and the difference in the direction angles of two adjacent points near the tangent point on this isoline is greater than the inertial navigation course angle error; Step 2: The underwater vehicle sails according to the planned trajectory, and measures the gravity anomaly value in real time to obtain the extreme value point of the local gravity anomaly value and its corresponding moment; Step 3: Estimation of the position of the underwater vehicle at the extreme moment: S31: According to the gravity anomaly value corresponding to the extreme value point, find the corresponding isoline in the gravity background map; S32: Calculate the tangents of each discrete point on the isoline described in S31; S33: Take the course angle of the inertial navigation system at the extreme moment as the extreme tangent angle, calculate the tangent corresponding to the extreme tangent angle, and when the tangent corresponding to the extreme tangent angle is between the tangents of two consecutive discrete points on the isoline, obtain the estimated position point by the weighted average method; S34: Take the estimated position point closest to the inertial navigation position point as the position at the current extreme moment; Step 4: Set the position at the extreme moment obtained in Step 3 as the initial position point of the filtering type gravity matching algorithm for gravity matching.

2. The gravity matching method based on the characteristics of gravity measurement data according to claim 1, characterized in that In the said Step 1, the A* algorithm or the genetic algorithm is used for trajectory planning.

3. The gravity matching method based on the characteristics of gravity measurement data according to claim 1, characterized in that, In the said Step 2, the measured gravity anomaly value is fitted by a local function, and then the first derivative is set to zero to obtain the extreme value point.

4. The gravity matching method based on the characteristics of gravity measurement data according to claim 1, characterized in that, In the said Step 2, a sliding window with a length of N is designed, where N is an odd number; calculate the standard deviation of N consecutive gravity anomaly measurement values within the sliding window; slide the sliding window, and the data at the center of the sliding window corresponding to the minimum value in the standard deviation is used as the extreme value point.

5. The gravity matching method based on the characteristics of gravity measurement data according to claim 4, wherein The said N takes an odd number in [7, 15].

6. The gravity matching method based on the characteristics of gravity measurement data according to claim 1, wherein In S32, arbitrarily select three consecutive discrete points a(x1, y1), b(x2, y2), and c(x3, y3) on the isoline, and calculate the radius r and the center O(x c , y c ) of the circle formed by these three points. Then, the tangential direction kb of point b is 7. The gravity matching method based on the characteristics of gravity measurement data according to claim 1, wherein In the said Step 4, the course angle of the inertial navigation system is constructed as a direction measurement and used as measurement information together with the gravity anomaly measurement.

8. The gravity matching method based on the characteristics of gravity measurement data according to claim 1 or 7, characterized in that, The particle filter algorithm is used for gravity matching.

9. The gravity matching method based on the characteristics of gravity measurement data according to claim 8, wherein, In the said particle filter algorithm, the particles are weighted by a Gaussian function.

Images