A multi-beam terrain matching method based on continuous measurement information

Abstract

The application relates to a multi-beam terrain matching method based on continuous measurement information and belongs to the technical field of navigation, guidance and control. The terrain matching method based on continuous measurement information can reduce the possibility of pseudo-peak and wrong-peak of a likelihood distribution. The application utilizes more measurement information and position information, can reduce the influence of measurement values with large errors on likelihood calculation, reduces the similarity of to-be-matched points, makes the to-be-matched points near real values have larger likelihoods, and makes the likelihoods of other points smaller.

Description

Technical Field

[0001] This invention relates to a multibeam terrain matching method based on continuous measurement information, belonging to the field of navigation, guidance and control technology. Background Technology

[0002] Underwater vehicles are indispensable transport equipment for marine exploration and resource development worldwide, and underwater autonomous navigation technology is one of its key technologies. Inertial navigation systems (INS) often serve as the main system of underwater autonomous navigation systems, possessing characteristics such as high stealth, complete autonomy, and no information exchange with the outside world. They provide underwater vehicles with comprehensive navigation and positioning information, including position, velocity, and attitude. However, INS errors diverge over time, requiring other navigation methods to suppress these errors. In the underwater environment, terrain matching is an effective auxiliary inertial navigation method. It matches and positions the vehicle using pre-stored terrain reference maps and real-time measured water depth information, thereby correcting the INS. The accuracy of terrain matching positioning is affected by the degree of terrain undulation, water depth measurement errors, and the initial position error of the INS. Terrain matching positioning can be divided into initial positioning and tracking positioning: the initial positioning stage provides coarse positioning information to reduce the impact of the initial position error of the INS; the tracking positioning stage utilizes a filtering tracking algorithm to obtain a real-time position estimate of the underwater vehicle in areas with large terrain undulations using real-time measured water depth data, thereby reducing the impact of water depth measurement errors.

[0003] Existing terrain matching methods rely solely on current measurement information during the tracking and localization phase, resulting in insufficient utilization of this data. Even in areas with significant terrain undulations, the similarity of multiple points in the underwater terrain can easily lead to multiple spurious peaks within the search area. Furthermore, large errors in measurements at a single moment can also cause peak misalignment within the search area. The presence of spurious peaks and peak misalignment prevents filtering tracking algorithms from converging quickly and may even cause them to diverge, thus increasing the matching error. Summary of the Invention

[0004] In view of this, the present invention provides a multibeam terrain matching method based on continuous measurement information. By utilizing continuous multibeam measurement information to evaluate the points to be matched, it can effectively reduce correlation errors and spurious peaks and misalignments in the likelihood distribution. The designed method can be used for initial and tracking positioning in terrain matching, effectively improving the convergence speed of the filtering algorithm and the accuracy of the matching positioning.

[0005] The technical solution of this invention is:

[0006] A multibeam terrain matching method based on continuous measurement information, the steps of which are as follows:

[0007] First, at the initial moment, the initial positioning result is obtained by utilizing continuous measurement information;

[0008] The initial positioning result is used as the initial information for the mass point filtering tracking algorithm. The correlation error and likelihood of each point to be matched within the search range are calculated through continuous measurement information, and the terrain matching result is obtained by mass point filtering recursion.

[0009] The continuous measurement information refers to the multibeam bathymetry data collected by the underwater vehicle within a set time range (τ = t - M + 1, ..., t ..., t + M - 1) after entering the adaptation zone, where t is the midpoint of the set time range. The midpoint t is used as the initial positioning time; that is, the midpoint t and M-1 times before and after it (a total of 2M-1 times) are selected as the time range for the required measurement information. Each time within the selected time range corresponds to a multibeam measurement sequence z. τ .

[0010] The method for determining the initial positioning result is as follows:

[0011] Step 1: Based on the search range provided by the inertial navigation system, calculate the position of each topographic reference map grid point within the search range. For the position P of the i-th grid point... t i First, it is necessary to calculate the estimated location points corresponding to M-1 times before and after time t. Q represents the number of grid points within the search range;

[0012] Step 2, estimate the point based on the location. Combined with multi-beam measurement z at each time step τ Calculate the i-th grid point P t i correlation error

[0013] Step 3, Select correlation error The grid point with the smallest error is used as the initial positioning result.

[0014] In step 1, the estimated position points corresponding to M-1 times before and after time t are calculated. The formula is:

[0015]

[0016] in, Let τ be the position of the inertial navigation system at time τ. Let be the heading angle of the inertial navigation system at the time interval between τ and τ+1. Indicates latitude.

[0017] In step 2, the correlation error The calculation formula is:

[0018]

[0019]

[0020] Among them, z τ (q) represents the q-th measurement value of the multibeam at time τ, with a total of N values. m 1; δ q This indicates the relative position of the q-th sounding point of the multibeam echo sounder to the vehicle. Indicates based on position Depth value read from topographic reference map; ω q Indicates the reliability of each multibeam measurement, |δ q The larger | is, the more ω q The smaller; ρ τ Indicates the estimated location point The credibility of it is far from being realized. The closer the location, the greater its weight.

[0021] The ρ τ The calculation formula is:

[0022]

[0023]

[0024] When τ≤t-1, The variance in both directions is:

[0025]

[0026]

[0027] When τ≥k+1 The variance in both directions is:

[0028]

[0029]

[0030] in, The variance of the inertial navigation system position error. The variance of the heading angle error; For custom parameters, satisfy

[0031] In step 3, the initial positioning result is:

[0032] The state-space equation of the particle filtering algorithm is:

[0033] x k =xk-1 +u k-1,k +w k-1

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

[0035] Where, x k Indicates the position of the underwater vehicle, u k-1,k z represents the displacement from time k-1 to time k given by the inertial navigation system. k Indicates multibeam measurement, w k-1 and v k These represent process error and measurement error, respectively.

[0036] Using the initial positioning result as the initial information for the mass point filtering tracking algorithm means: using the initial optimal positioning position... As the center of the initial set of particles at time t;

[0037] The initial set of particles is Including N point masses, the initial posterior point mass probability set is: Among them, z t This represents all measurements before time t, with the probability density value for each particle referenced to... Assign values ​​to a Gaussian distribution with a mean.

[0038] The recursive process for particle filtering includes a time update process and a measurement update process.

[0039] Let the set of particles at time k-1 (k-1≥t) be... The posterior probability set is Then the set of particles at time k is

[0040] The time update process is as follows:

[0041]

[0042]

[0043] The prior particle probability set at time k is obtained as follows

[0044] The measurement update process is as follows:

[0045]

[0046]

[0047] The posterior particle probability set at time k is obtained as follows

[0048] The method for obtaining terrain matching results through particle filtering recursion is as follows: position estimation at time k.

[0049]

[0050] During the measurement update process The calculation formula is:

[0051]

[0052]

[0053]

[0054]

[0055] in σ represents the set of measurements for a continuous multibeam array. m The standard deviation of measurement error is represented by the standard deviation of the measurement error. Indicates based on the i-th mass point The calculated estimated location point.

[0056] The location estimation point The calculation method is as follows:

[0057] For time k, the position of the i-th particle in the particle set is Select time k and the M-1 times preceding it (in total M times) (τ = k - M+1, ..., k) as the time range for which the measurement information is required;

[0058] The position points corresponding to M-1 times before time k. The calculation formula is

[0059]

[0060] in,

[0061] The ρ τ ′ is determined by the following formula:

[0062]

[0063]

[0064] The variance in both directions is:

[0065]

[0066]

[0067] in, For custom parameters, satisfy

[0068] Beneficial effects

[0069] 1. In the method of the present invention, during tracking and positioning, the likelihood calculation uses continuous measurement values ​​at multiple times, rather than just the measurement value at the current time.

[0070] 2. In the method of the present invention, an estimation method for calculating the position at other times is designed based on the position and heading information provided by the inertial navigation system.

[0071] 3. The method proposed in this invention, which utilizes continuous measurement information, can achieve different functions by focusing on measurements at different times, depending on the requirements. For example, it can use measurements before and after the current time to complete initial positioning, or use measurements before the current time to complete filtered tracking positioning.

[0072] Traditional methods for calculating likelihood rely solely on measurements taken at the current time, easily leading to a large number of matching points having the same likelihood value. This results in spurious peaks and misaligned peaks in the likelihood distribution. Spurious peaks occur when many points have high likelihoods, preventing the tracking algorithm from converging to a single true location. Misaligned peaks occur when measurement errors cause deviations between the locations corresponding to higher likelihoods and the true locations. The terrain matching method proposed in this invention, based on continuous measurement information, reduces the likelihood distribution's likelihood distortion. By utilizing more measurement and location information, this invention minimizes the impact of large measurement errors on likelihood calculation, reduces the similarity of matching points, and ensures that matching points near the true value have higher likelihoods, while other points have lower likelihoods. Attached Figure Description

[0073] Figure 1 This is a schematic diagram illustrating the use of a method based on continuous measurement information for initial positioning.

[0074] Figure 2 This is a schematic diagram for calculating the estimated location point;

[0075] Figure 3 This is a schematic diagram illustrating the use of a method based on continuous measurement information for filtering, tracking, and positioning. Detailed Implementation

[0076] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0077] Example

[0078] A multibeam terrain matching method based on continuous measurement information first obtains an initial positioning result using continuous measurement information at the initial moment; this initial positioning result is used as the initial information for the filtering tracking algorithm, and the correlation error and likelihood of each point to be matched within the search range are calculated using continuous measurement information, and the filtering algorithm is recursively used to obtain the terrain matching result.

[0079] This embodiment takes the particle filtering algorithm as an example and specifically includes the following steps:

[0080] Step 1: Initial Positioning. After the underwater vehicle enters the adaptation zone, it collects multibeam bathymetry data within a set time range (τ = t - M + 1, ..., t ..., t + M - 1), where t is the midpoint of the set time range. The midpoint t is used as the initial positioning time. That is, the midpoint t and the M-1 times before and after it (a total of 2M-1 times) are selected as the time range for which measurement information is needed. Each time within the selected time range corresponds to a multibeam measurement sequence z. τ Based on the search range provided by the inertial navigation system, calculations are performed on each topographic reference map grid point within the search range, such as... Figure 1 As shown. For the i-th grid point position P t i First, we need to calculate the position points corresponding to M-1 times before and after time t; i = 1, 2, 3, ... Q, where Q is the number of grid points in the search range;

[0081] Because the position information provided by the inertial navigation system has errors, simply translating the inertial navigation trajectory within a multi-point time range will introduce significant errors. Therefore, this invention utilizes the relationship between trajectory displacement and heading angle over a short period of time to calculate the estimated position points at multiple points. Figure 2 As shown in the figure. The dashed lines in the figure represent... arrive The direction of displacement is shown in the solid line, which represents the trajectory. If the underwater vehicle does not undergo any sharp turns or other actions that significantly change its motion (including heading and speed) during this period, the direction of displacement during this period is approximately the same as the heading at the midpoint of this period. Figure 2 This demonstrates the relationship between the displacement direction and the heading at intermediate moments when the trajectory is an arc. Clearly, the same holds true when the trajectory is a straight line. Therefore, based on the displacement output by the inertial navigation system from time τ to τ+1... Points can be Point, displacement The heading angle at the midpoint between τ and τ+1 is used for approximate calculation.

[0082] Based on the above analysis, within the time range where the measurement information is required, the calculation of the estimated position point at each time point follows a recursive relationship, obtained by using the position P of the i-th grid point.t i To calculate other estimated locations. If the navigation coordinate system is selected as the Northeast-Sky coordinate system, the calculation method is as follows:

[0083]

[0084] in, and Let represent the position of the inertial navigation system at time τ and the heading angle at the midpoint between τ and τ+1, respectively. Represents latitude. The position P of the i-th grid point can be obtained using formula (1). t i Location estimation point under the condition

[0085] Based on location estimation points Combined with multi-beam measurement z at each time step τ Calculate the i-th grid point P t i correlation error The calculation method is as follows

[0086]

[0087]

[0088] Among them, z τ (q) represents the q-th measurement value of the multibeam at time τ, with a total of N values. m 1; δ q This indicates the relative position of the q-th sounding point of the multibeam echo sounder to the vehicle. Indicates based on position Depth value read from topographic reference map; ω q Indicates the reliability of each multibeam measurement, |δ q The larger | is, the more ω q The smaller; ρ τ Indicates the estimated location point The credibility of it is far from being realized. The closer the location, the greater the weight. This invention provides ρ τ The calculation method.

[0089] Let the variance of the inertial navigation system position error over a certain period of time be... The variance of the heading angle error is Perform covariance analysis on formula (1). When τ≤t-1, continuous The variance of the position points in both directions and The variances of the positions in the two directions are recursively related, and are:

[0090]

[0091]

[0092] consider There is no approximation error, that is The above equation then transforms into:

[0093]

[0094]

[0095] Similarly, when τ≥k+1, The variance in both directions is

[0096]

[0097]

[0098] Therefore, according to the position error expression, we can obtain... The standard deviation of the position error is

[0099]

[0100] Based on the above analysis, then ρ τ Determined by the following formula

[0101]

[0102] in, For custom parameters, the following must be met:

[0103] The correlation error of each grid point can be obtained using formulas (2), (3), and (11). The grid point with the smallest error is selected as the initial position, i.e., the optimal initial positioning position at time t is...

[0104] Step 2: Perform filtering, tracking, and localization. Taking the particle filtering algorithm as an example, the state-space equation of the particle filtering algorithm is:

[0105] x k =x k-1 +u k-1,k +w k-1 (12)

[0106] z k =h(x k )+v k (13)

[0107] Where, x k Indicates the position of the underwater vehicle, u k-1,kz represents the displacement from time k-1 to time k given by the inertial navigation system. k Indicates multibeam measurement, w k-1 and v k These represent process error and measurement error, respectively. The particle filtering algorithm approximates x using a series of particles and their corresponding probability density values. k The probability distribution of these particles is called the particle set. The set of probability density values ​​can be divided into the prior particle probability set and the posterior particle probability set, depending on whether the measurement at the current moment is involved in the calculation.

[0108] First, construct an initial set of N particles. and the initial particle probability set Among them, z t This represents all measurements prior to time t. The initial optimal location will be determined. As the center of the initial particle set at time t, the probability density value corresponding to each particle is referenced to... Assigning values ​​to a Gaussian distribution with the mean, the resulting set of particle probabilities is defined as the posterior set of particle probabilities at time t.

[0109] Without loss of generality, let the set of particles at time k-1 (k-1≥t) be... The posterior probability set is Then the set of particles at time k is First, the time update process is performed: According to formula (12), the prior particle probability set at time k is... in, pass

[0110]

[0111]

[0112] To calculate.

[0113] Next, the measurement update process is performed: this process requires calculating the likelihood. To avoid spurious peaks and misaligned peaks in the likelihood distribution caused by measurements taken at a single moment, this invention utilizes continuous measurement information to calculate the likelihood, such as... Figure 3 As shown.

[0114] If the likelihood calculation for the points to be matched considers continuous water depth measurements, then in point filtering, the likelihood form becomes... in This represents a set of measurements from a continuous multibeam array. For time k, the position of the i-th particle in the particle set is... Select time k and the M-1 preceding times (a total of M times) (τ = k - M + 1, ..., k) as the time range for the measurement information to be used. Similar to the method in the initial positioning, first calculate the position points corresponding to the M-1 preceding times. The calculation method is as follows:

[0115]

[0116] in,

[0117] Based on location estimation points Combined with multi-beam measurement z at each time step τ Calculate the i-th particle correlation error and likelihood The calculation method is as follows

[0118]

[0119]

[0120]

[0121] Where, σ m This represents the standard deviation of the measurement error. Since τ≤k-1, the estimated location point... The standard deviation can be calculated using formulas (6), (7), and (10), then ρ′ τ Determined by the following formula

[0122]

[0123] in, For custom parameters, the following must be met:

[0124] Each particle can be obtained using formulas (19) and (20). likelihood Then the posterior particle probability set at time k is in, pass

[0125]

[0126]

[0127] To calculate. Therefore, the position estimate at time k. for

[0128]

[0129] Finally, return to step 2 to complete the terrain matching recursive location estimation.

[0130] This completes the terrain matching process for the underwater vehicle.

[0131] In summary, the above are merely preferred embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A multibeam terrain matching method based on continuous measurement information, characterized in that: At the initial moment, the initial positioning result is obtained by utilizing continuous measurement information; The initial positioning result is used as the initial information for the particle filtering tracking algorithm. The correlation error and likelihood of each point to be matched within the search range are calculated through continuous measurement information, and the terrain matching result is obtained by particle filtering recursion. Correlation error The calculation formula is: in, express The first time multi-beam There are 10 measured values, totaling 10 ... indivual; Indicates multi-beam The relative positions of the sounding points and the vehicle; Indicates based on position Depth values ​​read from topographic reference maps; This indicates the reliability of each multibeam measurement. The larger, The smaller; Indicates the estimated location point The credibility of it is far from being realized. The closer the location, the greater its weight.

2. The multibeam terrain matching method based on continuous measurement information according to claim 1, characterized in that: The continuous measurement information refers to the information obtained by the underwater vehicle within a set time range after it enters the adaptation zone. The multibeam bathymetry data collected internally, including t To define the midpoint of a time range, the midpoint time will be... t As the initial positioning time, i.e., selecting an intermediate time. t and before and after common Each selected time point is defined as the time range from which the measurement information is required, and each time point within the selected time range corresponds to a multibeam measurement sequence. .

3. The multibeam terrain matching method based on continuous measurement information according to claim 2, characterized in that: The method for determining the initial positioning result is as follows: Step 1: Based on the search range provided by the inertial navigation system, calculate the value for each topographic reference map grid point within the search range. i grid point position First, it is necessary to calculate the time before and after t. The estimated location point corresponding to each time moment i = 1, 2, 3, ... Q, where Q is the number of grid points within the search range; Step 2, estimate the point based on the location. Combined with multi-beam measurement at each moment Calculate the first i grid points correlation error ; Step 3, Select correlation error The grid point with the smallest error is used as the initial positioning result.

4. The multibeam terrain matching method based on continuous measurement information according to claim 3, characterized in that: In step 1, the time before and after t is calculated. The estimated location point corresponding to each time moment The formula is: in, For inertial navigation systems Location at any given moment For inertial navigation systems and The heading angle at the midpoint of the time interval, Indicates latitude.

5. The multibeam terrain matching method based on continuous measurement information according to claim 1, characterized in that: The The calculation formula is: when hour, The variance in both directions is: when hour, The variance in both directions is: in, The variance of the inertial navigation system position error. The variance of the heading angle error; For custom parameters, satisfy .

6. The multibeam terrain matching method based on continuous measurement information according to claim 3, characterized in that: In step 3, the initial positioning result is: .

7. The multibeam terrain matching method based on continuous measurement information according to claim 1, characterized in that: The state-space equation of the particle filtering algorithm is: in, Indicates the location of the underwater vehicle. This indicates that the inertial navigation system provides... k -1 hour to k Displacement offset at time 1 / 2 Indicates multibeam measurement, and These represent process error and measurement error, respectively.

8. The multibeam terrain matching method based on continuous measurement information according to claim 1, characterized in that: Using the initial positioning result as the initial information for the mass point filtering tracking algorithm means: using the initial optimal positioning position... As t The center of the initial set of particles at time 1; The initial set of particles is ,include N There are n particles, and the initial posterior probability set of the particles is... ,in, express t All measurements prior to time, the probability density value corresponding to each particle is referenced to... Assign values ​​to a Gaussian distribution with a mean.

9. The multibeam terrain matching method based on continuous measurement information according to claim 8, characterized in that: The recursive process for particle filtering includes a time update process and a measurement update process. set up The set of particles at time t is The posterior probability set is ,but k The set of particles at time point is ; ; The time update process is as follows: get k The prior particle probability set at time t is ; The measurement update process is as follows: get k The posterior particle probability set at time t is ; The method for obtaining terrain matching results through particle filtering recursion is as follows: k Position estimation at time : During the measurement update process The calculation formula is: in Represents a set of measurements from a continuous multibeam array. The standard deviation of measurement error is represented by the standard deviation of the measurement error. Indicates according to the first i A point mass The calculated estimated location point; The location estimation point The calculation method is as follows: For time k The first point set i The position of each particle is Choose the time k and its predecessor common M The time range for which the measurement information is required is defined as a specific time point. ; Before time k The position point corresponding to each moment The calculation formula is: in, ; The Determined by the following formula: The variance in both directions is: in, For custom parameters, satisfy .

Images