Adaptability analysis method based on terrain matching capability quantification

Abstract

The invention provides an adaptability analysis method based on terrain matching capability quantification. The method comprises the following steps: firstly, analyzing the composition of terrain matching errors, and dividing the terrain matching errors into local errors and similar errors; secondly, giving an expression of a local error through theoretical derivation, and expressing a similar error in a probability form; and finally, combining the local error and the similar error, and giving a scaled suitability index capable of reflecting the matching error. The adaptability index can consider the influence of a sensor, a terrain reference map model and terrain similarity on matching, and has clear physical significance.

Description

Technical Field

[0001] The present invention belongs to the technical field of navigation, guidance and control, and particularly relates to an adaptability analysis method based on quantification of terrain matching ability. Background Art

[0002] An underwater vehicle is an indispensable carrier operation equipment for ocean exploration and ocean resource development in various countries around the world. Underwater autonomous navigation technology is one of its key technologies. An inertial navigation system is often used as the main system of an underwater autonomous navigation system, which has the characteristics of high concealment, complete autonomy, and no information exchange with the outside world, and can provide comprehensive navigation and positioning information such as the position, speed, and attitude of an underwater vehicle. However, the errors of the inertial navigation system diverge with time, and other navigation methods are needed to suppress the errors. In the underwater environment, terrain matching is an effective auxiliary inertial navigation method, which performs matching positioning through a pre-stored terrain reference map and real-time measured water depth information, and then corrects the inertial navigation system. Among them, terrain adaptability analysis is one of the core technologies, which provides a basis for whether matching positioning can be carried out in the navigation area, is the basis for subsequent selection of the adaptation area and trajectory planning, and has an important impact on the accuracy of terrain matching positioning.

[0003] However, the matching positioning accuracy is affected by the accuracy of the sensors or terrain reference map models used, including real-time measurement errors, terrain reference map errors (mapping errors and interpolation errors generated when reading the map), position errors between each point in the used inertial navigation sequence, etc. When performing matching in the same area, different accuracy sensors and models will cause differences in the matching positioning results. Most current studies do not start from the perspective of actual terrain matching requirements, do not consider the influence of sensor accuracy and terrain reference map models on matching, nor consider the influence of terrain similarity that seriously affects the matching positioning accuracy, resulting in the separation of adaptability analysis from terrain matching; and when using the adaptability results to select the adaptation area in current studies, the physical meaning of the thresholds used is not clear enough, and it is difficult to divide the area according to the positioning accuracy requirements of actual tasks. Therefore, adaptability analysis needs to start from the perspective of actual matching positioning, comprehensively consider the above error sources, and be able to quantify the matching positioning ability of the area under different influencing factor conditions. Summary of the Invention

[0004] To solve the above problems, the present invention provides an adaptability analysis method based on quantification of terrain matching ability, which considers the influencing factors of terrain matching, can be closely combined with terrain matching, and the obtained adaptability results are more in line with the actual matching positioning results and have clear physical meanings.

[0005] An adaptability analysis method based on quantification of terrain matching ability includes the following steps:

[0006] Step 1: Assume that the true position of the vehicle at time k is pk , different Δp are sampled from the probability density function f(Δp k corresponding to the matching position error Δp k ) through the Monte Carlo method from the true position p k ; k ;

[0007] Step 2: Take the root mean square of the different Δp k sampled as the adaptation error R(p k ), where the larger the adaptation error R(p k ), the greater the positioning error of the vehicle at the current true position p k , and the lower the adaptability to the current true position p k ; the smaller the adaptation error R(p k ), the smaller the positioning error of the vehicle at the current true position p k , and the higher the adaptability to the current true position p k .

[0008] Furthermore, the probability density function f(Δp k ) is as follows:

[0009]

[0010] where N represents the total number of local matching regions divided within the search range set with the true position p k as the center, j represents the serial number of the local matching region, and j = 1, 2,..., N, represents the primary probability density function of the matching position error Δp k , and there is:

[0011]

[0012] where, represents the probability that when the true position is p k , the center point of the j-th local matching region is finally determined to be the optimal matching point most similar to the true position p k , is the normalized probability; represents the distance vector between k p two points; represents the mean value of the local error when the difference cost function between the inertial navigation sequence of the vehicle in the j-th local matching region and the true position sequence Represents the primary probability density function The mean value of; Represents the inertial navigation sequence of the vehicle in the j-th local matching area And the true position sequence The local error when the difference cost function between them is minimized The covariance matrix of; Represents the primary probability density function The covariance matrix of; Represents the effective range of the local error And Vr represents the size of the local matching area, and Vr satisfies Where, Vr(1) and Vr(1) represent the first element and the second element of Vr respectively, Respectively represent The first element and the second element of; Represents the primary probability density function The effective range in the j-th local matching area.

[0013] Furthermore, the calculation formula of the primary probability density function Is as follows:

[0014]

[0015] Where, if Δp k Within The demonstration function Otherwise 0; Represents Δp k Satisfies the normal distribution with a mean of The covariance matrix is The denominator Represents the normalization constant, ensuring that the integral of the primary probability density function over Is 1;

[0016] The mean value And the covariance matrix Are calculated as follows:

[0017]

[0018] Where, Represents the combined error e that combines the terrain reference map error, depth measurement error, and inertial navigation position error at the i-th moment i The mean value of, Represents the combined error e i The variance of; ▽ represents the gradient calculation; T represents the transpose; Represents translating the true position sequence to the center point of the j-th local matching area The i-th true position point of the new sequence after the point; k - l to k + l are the moments corresponding to the true position sequence; the depth difference of the true position at the i-th moment Indicates the true position point after translation on the terrain reference map The depth at Indicates the true position point p before translation on the terrain reference map i The depth at; the sum of the depth gradients of the j-th local matching region Indicates depth The gradient of

[0019] Furthermore, the method of dividing the local matching region within the search range set with the true position p k as the center is as follows:

[0020] Take the center point of each local matching region as the point to be matched, then the point to be matched According to the set resolution Res p , in the form of a grid, set a point to be matched every Res in the XY direction p apart.

[0021] Furthermore, according to the different matching algorithms adopted by the vehicle, the resolution Res p can be set to different sizes; among them, if the points searched by the matching algorithm are distributed according to a fixed resolution, the distribution of the points to be matched is set according to the same resolution; if the points searched by the matching algorithm are randomly distributed, the resolution Res p has a maximum value as follows:

[0022]

[0023] where, ΔH represents the maximum value of the depth value difference between all adjacent 2 local matching regions in the terrain reference map where the vehicle is currently located; R represents the minimum value of the standard deviation of the measurement error within the search range set with the true position p k as the center; Res represents the resolution of the terrain reference map.

[0024] Furthermore, the calculation method of the probability is as follows:

[0025]

[0026] where, ω τ is the inertial navigation position error at the i-th moment The corresponding probability density coefficients when taking five classical values respectively, and the five classical values are respectively is the mean of the normal distribution that the inertial navigation position error conforms to, is the standard deviation of the normal distribution that the inertial navigation position error conforms to; The time sequence number i = k - l, …, k, …, k + l, where k - l and k + l are respectively l time instants before and after the k-th time instant;

[0027] denotes the inertial navigation position error When the value of is determined to be five classical values, due to random error and terrain similarity, the center point of the j-th local matching area is finally determined to be k the probability of the optimal matching point that is most similar to the true position p; denotes the inertial navigation position error When the value of is determined to be five classical values, due to depth measurement error and inertial navigation position error, the probability that the terrain near is judged to be the true terrain; denotes the inertial navigation position error When the value of is determined to be five classical values, due to terrain similarity, is determined to be k the probability of the candidate matching point that is similar to the true position p; denotes the parameter model composed of terrain reference map error, depth measurement error, and inertial navigation position error, denotes the terrain reference map in grid form.

[0028] Furthermore, the calculation method of the probability is as follows:

[0029]

[0030] where η is the normalization coefficient, X and Y respectively represent the variables of the conditional probability, denotes the combined error e that synthesizes the terrain reference map error, depth measurement error, and inertial navigation position error at the i-th time instant i of the mean value, denotes the combined error e i of the variance; denotes the position on the terrain reference map at the depth of, denotes the inertial navigation sequence obtained by the vehicle at the i-th time instant; denotes the depth at the position p i on the terrain reference map, p i denotes the true position where the vehicle is located at the i-th time instant, i = k - l, …, k, …, k + l;

[0031] The probability The calculation method is as follows:

[0032]

[0033] Among them, represents the number of position points with the same depth value as the true position p k in the j-th local matching region corresponding to p k , NUM{·} represents the number of elements in the obtained set, represents the inertial navigation data sequence at the j-th local matching region on the terrain reference map at the theoretical depth, represents the inertial navigation data except for the j-th local matching region, represents the inertial navigation data sequence except for the j-th local matching region on the terrain reference map at the theoretical depth, the local matching region serial number ε = 1,…, N, ∈ j is the threshold corresponding to the j-th local matching region.

[0034] Furthermore, when sampling different Δp k from the probability density function f(Δp k ) through the Monte Carlo method, the sampled Δp k satisfies the following constraint conditions:

[0035] |Δp k (1)| ≤ Sr(1), |Δp k (2)| ≤ Sr(2)

[0036] Among them, Δp k (1) is the first element in Δp k , Δp k (2) is the second element in Δp k , Sr(1) is half of the length of the search range set with the true position p k as the center, and Sr(2) is half of the width of the search range set with the true position p k as the center.

[0037] Furthermore, the calculation method of the adaptation error R(p k ) is as follows:

[0038]

[0039] Among them, represents calculating the expectation.

[0040] Further, steps 1 and 2 are performed for each point of the vehicle on the terrain reference map to obtain the positioning error of the vehicle on the entire terrain reference map and the adaptability to the entire terrain reference map.

[0041] Beneficial effects:

[0042] 1. The present invention provides an adaptability analysis method based on terrain matching ability quantification, which decomposes the matching error into two parts: local error and similarity error. Among them, the local error comes from sensor error and terrain reference map error, and the similarity error comes from terrain similarity. The present invention conducts theoretical analysis on them respectively and combines to obtain the theoretical expression of the final matching error. This theoretical expression considers the factors affecting terrain matching and is highly consistent with the real terrain matching result, providing a solid theoretical basis for the quantitative analysis of terrain matching error.

[0043] 2. The present invention provides an adaptability analysis method based on terrain matching ability quantification. The proposed adaptability error has a clear physical meaning, and its essence is the statistical representation of terrain matching error in a local area. In the subsequent selection process of the adaptability area, the adaptability error threshold can be flexibly divided according to the specific requirements of positioning accuracy for the actual task, so as to achieve the scientific selection of the adaptability area. Description of the drawings

[0044] Figure 1 It is an illustration of the terrain matching principle, related parameters and variables provided by the present invention. Detailed implementation manners

[0045] In order to enable those skilled in the art to better understand the solution of the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application.

[0046] An adaptability analysis method based on terrain matching ability quantification includes the following steps:

[0047] Step 1: Assume that the true position of the vehicle at time k is p k , and different Δp k corresponding to the matching position error are sampled from the probability density function f(Δp k ) of the true position p k through the Monte Carlo method; k

[0048] Step 2: Take the root mean square of the different sampled Δp k as the adaptability error R(p k ). Among them, the larger the adaptability error R(p k ), the greater the positioning error of the vehicle at the current true position p k , and the less adaptable it is to the current true position pk The lower the adaptability; the smaller the adaptation error R(p k ), the smaller the positioning error of the vehicle at the current true position p k , and the higher the adaptability to the current true position p k .

[0049] Specifically, the probability density function f(Δp k ) is as follows:

[0050]

[0051] where N represents the total number of local matching regions divided within the search range set with the true position p k as the center, j represents the serial number of the local matching region, and j = 1, 2,..., N, represents the primary probability density function of the matching position error Δp k , and there is:

[0052]

[0053] where, represents the probability that when the true position is p k , due to random errors and terrain similarity, the center point of the j-th local matching region is finally determined to be the optimal matching point most similar to the true position p k , is the normalized probability; represents p k the distance vector between two points; represents the mean value of the local error when the difference cost function between the inertial navigation sequence of the vehicle in the j-th local matching region and the true position sequence is minimized; represents the mean value of the primary probability density function ; represents the covariance matrix of the local error when the difference cost function between the inertial navigation sequence of the vehicle in the j-th local matching region and the true position sequence is minimized; represents the covariance matrix of the primary probability density function ; represents the effective range of the local error , and Vr represents the size of the local matching region, and Vr satisfies Among them, Vr(1) and Vr(2) respectively represent the first element and the second element of Vr. respectively represent the first element and the second element of; represents the primary probability density function in the j-th local matching region.

[0054] Furthermore, the primary probability density function is calculated as follows:

[0055]

[0056] Among them, if Δp k is within the demonstration function otherwise it is 0; represents that Δp k satisfies a normal distribution with a mean of and a covariance matrix of ; the denominator represents the normalization constant, ensuring that the integral of the primary probability density function over is 1;

[0057] The mean and the covariance matrix are calculated as follows:

[0058]

[0059] Among them, represents the combined error e that combines the terrain reference map error, depth measurement error, and inertial navigation position error at the i-th moment i of the mean, represents the variance of the combined error e i ; ▽ represents the gradient calculation; T represents the transpose; represents the i-th true position point of the new sequence after translating the true position sequence to the point according to the center point of the j-th local matching region; k-l to k+l are the corresponding moments of the true position sequence; the depth difference of the true position at the i-th moment represents the depth at the translated true position point on the terrain reference map, represents the depth at the true position point p i before translation on the terrain reference map; the depth gradient sum of the j-th local matching region and represents the gradient of the depth ;

[0060] Furthermore, the probability The calculation method is as follows:

[0061]

[0062] where ω τ is the inertial navigation position error at the i-th moment are the probability density coefficients corresponding to when the values of take five classical values respectively, and the five classical values are is the mean of the normal distribution that the inertial navigation position error obeys, is the standard deviation of the normal distribution that the inertial navigation position error

[0063] represents the inertial navigation position error When the value of is determined to be five classical values, due to random error and terrain similarity, the center point k of the j-th local matching area is finally determined to be the probability of the optimal matching point most similar to the true position p represents the inertial navigation position error When the value of is determined to be five classical values, due to depth measurement error and inertial navigation position error, the probability that the terrain near is judged to be the true terrain; represents the inertial navigation position error k When the value of is determined to be five classical values, due to terrain similarity, is determined to be the probability of the candidate matching point similar to the true position p

[0064] Furthermore, the probability is calculated as:

[0065]

[0066] where η is the normalization coefficient, X and Y respectively represent the variables of conditional probability, represents the combined error e i that comprehensively combines the terrain reference map error, depth measurement error, and inertial navigation position error at the i-th moment is the mean of the combined error e i is the variance of the combined error e; Indicates the position on the terrain reference map at the depth of, represents the inertial navigation sequence obtained by the vehicle at the i-th moment; represents the position p on the terrain reference map i at the depth of, p i represents the true position of the vehicle at the i-th moment, i = k - l, …, k, …, k + l;

[0067] Furthermore, the probability is calculated as follows:

[0068]

[0069] wherein, represents the number of position points with the same depth value as the true position p k in the j-th local matching region corresponding to the true position p k on the terrain reference map, NUM{·} represents the number of elements in the obtained set, represents the inertial navigation data sequence at the in the j-th local matching region on the terrain reference map, represents the inertial navigation data except for the j-th local matching region, represents the inertial navigation data sequence except for the j-th local matching region on the terrain reference map at the theoretical depth of, the local matching region serial number ε = 1, …, N, ∈ j is the threshold corresponding to the j-th local matching region.

[0070] It should be noted that if you want to obtain the positioning error of the vehicle on the entire terrain reference map and its adaptability to the entire terrain reference map, then the vehicle performs the above steps 1 and 2 at each point on the terrain reference map. Specifically as follows:

[0071] S1: For each point in the terrain reference map, with this point as the center, a rectangular search area is formed with the search side length as the semi-side length, and this search range is divided into an average of N local matching regions;

[0072] S2: Calculate the probability density function of the matching error when performing terrain matching within each local matching region, and this matching error is denoted as the local error;

[0073] S3: Calculate the similarity error of each local region, that is, the probability of the appearance of the optimal matching point within this region and the matching error caused by the optimal matching point when performing terrain matching globally;

[0074] S4: Sum up the matching errors and similarity errors within all local matching regions to obtain the probability density function of the matching errors within the entire search range, and perform quantization.

[0075] The following details the acquisition method and basic principle of the adaptation error that can quantify the matching ability.

[0076] Before proceeding with the detailed description, define the parameters and variables involved, as Figure 1 shown.

[0077] Assume that the position of the current grid point to be studied is p k , and the corresponding time is k (that is, assume that the grid point position where the vehicle is located at time k is p k ). The present invention aims to use the terrain measurement data and inertial navigation data from times k - l to k + l, a total of 2l + 1 times, to calculate the local error and similarity error. Among them, p k is generally a two-dimensional vector, often the longitude and latitude in the geographical system or the XY values in the projection system. As Figure 1 shown, the gray dashed line represents the terrain reference map in grid form, denoted as with a resolution size of Res; define the true position sequence of the vehicle centered on p k as as shown by the black solid line in Figure 1 ; the actually obtained measured depth value sequence is and the inertial navigation position sequence corresponding to the black solid line is as shown by the green solid line in Figure 1 .

[0078] The search range is formed by expanding a length of Sr on both sides centered on p k , denoted as The size of the search range is related to the specific matching algorithm or requirements (for example, in the Terrain Contour Matching (TERCOM) algorithm, the search range is determined by the circular error probability of inertial navigation, and in the filtering algorithm, it is determined by the prior covariance).

[0079] Assume that the search range consists of N local matching regions, and the center point of the jth local matching region is which is referred to as the point to be matched in this article, and the set is denoted as The length of the expansion on both sides of the point is Vr, and this local matching region is denoted as

[0080] Assume that the true depth at the position p in the true position sequence of the vehicle i is h(p i ), and the depth read from the terrain reference map is Definition Let it be the terrain reference map error, and assume it satisfies the normal distribution, that is Let (in the true position sequence ) the position p i The measured depth at is z i , define δz i = z i - h(p i ) as the measurement error, and assume it satisfies Definition Let it be the inertial navigation position error between the i-th moment and the k-th moment, and assume it satisfies Record all the above errors as Denote the parameter model of all sensors, that is

[0081] Step 1: Division of the local area within the search range.

[0082] In this step, the local area within the search range will be divided according to a certain resolution, that is, determine the points to be matched and the corresponding local areas

[0083] Since the terrain reference map is stored in the form of grid points, therefore, the points to be matched can be set in the form of a grid according to a certain resolution Res p , that is, in the XY direction, set a point to be matched every other Res p . According to the search methods of different matching algorithms, the resolution Res p can be set to different sizes. If the points searched by the matching algorithm are distributed according to a fixed resolution, such as the TERCOM algorithm or the particle filtering algorithm, then the distribution of the points to be matched can be set according to the same resolution; if the points searched by the matching algorithm are randomly distributed, such as the particle filtering algorithm, in order to ensure that all the set points to be matched can fully reflect the characteristics of terrain changes, there is a maximum value for the resolution Res p , to ensure that terrain changes will not be submerged by measurement noise, that is

[0084]

[0085] where, ΔH represents the maximum value of the depth value difference between all adjacent 2 local matching areas in the terrain reference map where the vehicle is currently located; R represents the minimum value of the standard deviation of the measurement error within the search range set with the true position p k as the center; Res represents the resolution of the terrain reference map. For the second case, this embodiment recommends In summary, as can be seen Figure 1 shown, the width Vr of the local matching area = Resp / 2.

[0086] Step 2: Calculation of local error.

[0087] As Figure 1 shown, first calculate the degree of deviation between the matching position and the true position obtained during matching in the j-th local area due to measurement error, inertial navigation error, topographic reference map error, etc. Denote as an arbitrary point within the local area. In actual matching, the true position sequence cannot be obtained. Therefore, the inertial navigation sequence and the position relationship between them are usually used to determine the best matching point. As

[0088] shown by the green dashed line in Figure 1 , transform the inertial navigation sequence of the green solid line to ensure coincides with . The transformation includes translation, rotation, etc., and is related to the specific matching algorithm strategy. If the transformation is a simple translation, the translated inertial navigation sequence (green dashed line) is where

[0089]

[0090] The inertial navigation data can be the longitude and latitude in the geographic coordinate system or the XY values in the projection system;

[0091] To further analyze the local area, the true position sequence is also subjected to the same transformation to ensure that p k coincides with . Then the translated true position sequence (black dashed line) is where

[0092]

[0093] Define the cost function of the difference between the translated inertial navigation sequence (green dashed line) and the true position sequence (black solid line) as

[0094]

[0095] z i is the measured depth at position p i , is the read from the topographic reference map;

[0096] At , make reach the minimum value corresponding to The point is the optimal matching position point within the area at time k. inside.

[0097] Let represent the error between the position change given by inertial navigation between time i and time k and the true position change. According to Equation (3), Define the local error as Then Equation (4) becomes

[0098]

[0099] Since this step mainly calculates the deviation within a local range, it can be considered that the terrain in the terrain reference map changes linearly at this time. Then there is

[0100]

[0101] Substituting into (4) gives

[0102]

[0103] Among them, Let represent the combined error after integrating measurement error, inertial navigation position error, and terrain reference map error. is the terrain reference map error;

[0104] Since it is necessary to calculate when it is the smallest corresponding to That is Therefore, take the derivative of (7) and set it to 0,

[0105]

[0106] According to Finally, the local error is obtained as

[0107]

[0108] Since is the sum of each part of the error and satisfies the Gaussian distribution, its mean is

[0109]

[0110] The variance is

[0111]

[0112] Therefore, the local error is also a random variable. For the convenience of description, let

[0113]

[0114] Then the local error Satisfy the mean of

[0115]

[0116] Covariance matrix is

[0117] a Gaussian distribution, and denote this probability density function as

[0118] Since the assumption of local terrain linearization is used in the derivation of Equation (6), the results of Equations (13) and (14) can only express the matching error within the local matching area due to all sensor parameters and the terrain reference map model When Δp k is relatively large, the assumption of local terrain linearization may not hold, and the results do not have reference significance at this time. Therefore, it is necessary to truncate the above Gaussian distribution.

[0119] Combined with the distribution range of the local matching area and it can be known that the effective range of satisfies “(1)” represents the first element. Then finally ( when it is the smallest, the corresponding (the point on the green dotted line), that is, ) the probability density function is as follows

[0120]

[0121] where, if is within , otherwise it is 0; the denominator is the normalization constant, ensuring that the integral of the probability density function over is 1. Obviously, when Vr = +∞,

[0122] To sum up, for the convenience of expression, the above analysis process is denoted as

[0123]

[0124] As mentioned above, the local error can reflect the statistical characteristics of the terrain matching positioning error in a certain area under specific sensors and terrain reference map models. If that is, when the local area is near the true position, it can reflect the upper limit of the terrain matching positioning accuracy, and the statistical characteristics of this area can provide guidance for the selection of sensors and terrain reference map models.

[0125] Step 3: Calculation of similarity error.

[0126] Due to the existence of inevitable measurement errors, topographic reference map errors, and inertial navigation position errors, every point within the search range may be regarded as the optimal point. Therefore, it is necessary to calculate the probability that each point to be matched within the search range is regarded as the optimal matching point due to terrain similarity and random errors. Since the influence of the random error characteristics and terrain similarity on terrain matching is independent, let X be the position of the optimal matching point and Y be the true position. Then the position The probability of being the optimal matching point can be calculated according to the following formula

[0127]

[0128] where is the probability that when the true position is p k it is finally judged as the optimal matching point due to random errors and terrain similarity; represents the probability that the terrain near is judged as the true terrain due to measurement errors and model errors; represents the probability that the point is selected as the matching point due to terrain similarity. Note that represents the probability of terrain similarity, rather than a specific position point.

[0129] First, calculate Translate the inertial navigation sequence to the point to ensure it coincides with it. Similar to Equation (2), the translated sequence is where then there is

[0130]

[0131] where and are shown in Equations (10) and (11) respectively, and η is the normalization coefficient.

[0132] Next, calculate Based on the following assumption: If there are K position points within the search range with the same terrain value d, then when the measured value is d, each point has the same probability of being judged as the optimal matching point. Therefore, the terrain near the point ​It is necessary to compare with all points within the search range. Due to the existence of reading errors in the topographic reference map, there will always be differences in topographic values between two location points. Therefore, this paper believes that when the difference is less than a certain threshold, the topographies of the two points are approximately considered similar. To reduce the computational burden, The calculation method is as follows

[0133]

[0134] where NUM represents the number of set elements; ∈ j is the threshold, which is set in the present invention as

[0135] However, the inertial navigation errors need to be determined for the calculations of equations (18) and (20). Therefore, in this part, according to the characteristics of inertial navigation errors, 5 special error points are selected for approximate calculation. According to Define as the standard deviation of the inertial navigation position error, then can take for calculation. Therefore, equations (18) and (20) can obtain 5 results, denoted as and Then equation (17) can also obtain 5 results, denoted as The final similarity probability can be obtained from

[0136]

[0137] for calculation, where ω τ is the coefficient after normalizing the probability density of 5 special inertial navigation position errors. If there is a clear divergence characteristic of the inertial navigation position error, the special error points can be set according to the actual characteristics.

[0138] To sum up, for each point to be matched and the sequence within the search range, its similarity error is defined as

[0139]

[0140] where, as shown in equation (21), represents the probability that is regarded as the optimal matching point due to topographic similarity and random errors; is p k the distance vector between two points, indicating the matching error caused when is regarded as the optimal matching point.

[0141] Step 4: Calculation of the adaptation error of the quantifiable matching ability

[0142] According to the above analysis, pk When the point is the true position, after synthesizing the local error and the similarity error, the final matching position error Δp k has a probability density function of

[0143]

[0144] where

[0145]

[0146] Equation (23) means that within the search range, each point to be matched has a probability of being regarded as the optimal matching point. Within its corresponding local matching region the matching error satisfies a truncated Gaussian distribution f trunc is in the form of (15). Summing up the probability distributions of all points to be matched can obtain the probability distribution of the global matching error; Equation (29) calculates the local error characteristics caused by sensor and model errors at and its vicinity, represents the effective range when calculating the local error of this point; Equation (28) calculates the probability that is regarded as the optimal matching point and the resulting matching error; Equations (25), (26) and (27) represent transforming the local error characteristics into global error characteristics, that is

[0147] To give a scalar adaptability index, therefore this paper defines the root mean square (RMS) of the point matching error Δp k as the adaptation error, that is k The larger the adaptation error R(p

[0148]

[0149] ), the lower the adaptability of the p k point is considered; the smaller the adaptation error R(p k ), the higher the adaptability of the p k point is considered. k

[0150] Since only the points within the search range can be possibly judged as the optimal matching points, there are the following restrictive conditions

[0151] |Δp k (1)| ≤ Sr(1), |Δp k (2)| ≤ Sr(2) (31)

[0152] Sr represents the set search range size, and this value is related to the specific matching algorithm. Since Δp​k is only a two-dimensional random variable, so the fitness R(p k ) can be easily calculated from the distribution (23) through Monte Carlo sampling. Further, the adaptation error obtained in the present invention is essentially an estimated matching error, so different thresholds can be set according to actual needs to achieve the selection of adaptation regions at different levels.

[0153] In summary, the present invention first analyzes the composition of terrain matching errors, divides them into local errors and similarity errors. Secondly, the expression of local errors is given through theoretical derivation, and the similarity errors are expressed in the form of probability. Finally, the local errors and similarity errors are combined, and a scalar fitness index that can reflect the matching error is given. This fitness index can consider the effects of sensors, terrain reference map models, and terrain similarity on matching, and has a clear physical meaning. It can be seen that the fitness analysis method based on the quantification of terrain matching ability proposed by the present invention is closely combined with the influencing factors of terrain matching, combines the local errors and similarity errors during terrain matching, and can quantify the matching and positioning ability of regions under different precision sensors and terrain reference map models; compared with the traditional fitness analysis method, the method of the present invention has a more clear physical meaning and more comprehensive consideration of influencing factors.

[0154] Of course, the present invention can also have many other embodiments. Without departing from the spirit and essence of the present invention, those skilled in the art can certainly make various corresponding changes and deformations according to the present invention, but these corresponding changes and deformations should all fall within the protection scope of the appended claims of the present invention.

Claims

1. An adaptability analysis method based on terrain matching ability quantification, characterized in that including the following steps: Step 1: Assume that the actual position of the spacecraft at time k is p k , using the Monte Carlo method from the true position p k The corresponding matching position error Δp k The probability density function f(Δp k ) to sample different Δp k ; Step 2: Take the root mean square of the different Δp obtained by sampling k as the adaptation error R(p k ). Among them, the larger the adaptation error R(p k ), the greater the positioning error of the vehicle at the current true position p k , and the lower the adaptability to the current true position p k ; the smaller the adaptation error R(p k ), the smaller the positioning error of the vehicle at the current true position p k , and the higher the adaptability to the current true position p k .

2. The adaptability analysis method based on terrain matching ability quantification according to claim 1, wherein, The probability density function f(Δp k ) is as follows: where N represents the total number of local matching regions divided within a search range set with the true position p k as the center, j represents the serial number of the local matching region, and j = 1, 2,..., N, denotes the primary probability density function of the matching position error Δp k and there is: Among them, represents the probability that when the true position is p k , due to random errors and terrain similarity, the center point of the j-th local matching region is finally determined to be the optimal matching point most similar to the true position p k , and is the normalized probability; represents p k the distance vector between two points; represents the mean value of the local error when the difference cost function between the inertial navigation sequence of the vehicle in the j-th local matching region and the true position sequence is minimized; represents the mean value of the primary probability density function ; represents the covariance matrix of the local error when the difference cost function between the inertial navigation sequence of the vehicle in the j-th local matching region and the true position sequence is minimized; represents the covariance matrix of the primary probability density function ; represents the effective range of the local error , and Vr represents the size of the local matching region, and Vr satisfies where Vr(1) and Vr(1) respectively represent the first element and the second element of Vr, respectively represent the first element and the second element of represents the effective range of the primary probability density function in the j-th local matching region.

3. The adaptability analysis method based on terrain matching ability quantification according to claim 2, wherein Primary probability density function The calculation formula is as follows: where, if Δp k within the exemplary function otherwise 0; denotes that Δp k satisfies a normal distribution with a mean of and a covariance matrix of ; the denominator denotes the normalization constant, ensuring that the integral of the primary probability density function over is 1; Mean value and covariance matrix are calculated as follows: Among them, represents the combined error e that synthesizes the terrain reference map error, depth measurement error, and inertial navigation position error at the i-th moment i of the mean value, represents the variance of the combined error e i ; represents gradient calculation; T represents transpose; represents the i-th true position point of the new sequence after translating the true position sequence to the point according to the center point of the j-th local matching area; k - l to k + l are the corresponding moments of the true position sequence; the depth difference of the true position at the i-th moment represents the depth at the translated true position point on the terrain reference map, represents the depth at the true position point p i on the terrain reference map before translation; the sum of the depth gradients of the j-th local matching area represents the gradient of the depth .

4. The adaptability analysis method based on terrain matching ability quantification according to claim 2, characterized in that In the search range set with the real position p k The method of dividing the local matching area within the set search range centered on Take the center points of each local matching area as the points to be matched, then the points to be matched are set at intervals of Res p in the XY direction in the form of a grid, with one point to be matched p set at each interval of Res 5. The adaptability analysis method based on terrain matching ability quantification according to claim 4, characterized in that According to different matching algorithms adopted by the vehicle, the resolution Res p can be set to different sizes; among them, if the points searched by the matching algorithm are distributed according to a fixed resolution, the distribution of the points to be matched is set according to the same resolution; if the points searched by the matching algorithm are randomly distributed, the resolution Res p has a maximum value as follows: Among them, ΔH represents the maximum value of the depth value difference between all adjacent two local matching regions in the terrain reference map where the vehicle is currently located; R represents the minimum value of the standard deviation of the measurement error within the search range set with the true position p k as the center; Res represents the resolution of the terrain reference map.

6. The adaptability analysis method based on terrain matching ability quantification according to claim 1, characterized in that Probability is calculated as follows: where, ω τ is the inertial navigation position error at the i-th moment are the probability density coefficients corresponding to when the values of take five classical values respectively, and the five classical values are is the mean of the normal distribution that the inertial navigation position error obeys, is the standard deviation of the normal distribution that the inertial navigation position error obeys; the moment serial number i = k - l, …, k, …, k + l, and k - l and k + l are respectively l moments before and after the k-th moment; Indicates the inertial navigation position error When the value of is determined to be five classical values, due to random error and terrain similarity, the center point of the j-th local matching area Is finally determined to be the optimal matching point most similar to the true position p k Probability; Indicates the inertial navigation position error When the value of is determined to be five classical values, due to depth measurement error and inertial navigation position error Probability that the terrain near is judged to be the true terrain; Indicates the inertial navigation position error When the value of is determined to be five classical values, due to terrain similarity Is determined to be a candidate matching point similar to the true position p k Probability; Indicates a parameter model composed of terrain reference map error, depth measurement error, and inertial navigation position error Indicates the terrain reference map in grid form 7. The adaptability analysis method based on terrain matching ability quantification according to claim 6, characterized in that Probability is calculated as follows: where η is the normalization coefficient, and X and Y respectively represent the variables of the conditional probability represents the combined error e that synthesizes the terrain reference map error, depth measurement error, and inertial navigation position error at the i-th moment i the mean value of represents the combined error e i the variance of; represents the position on the terrain reference map at the depth of, represents the inertial navigation sequence obtained by the vehicle at the i-th moment; represents the position p on the terrain reference map i at the depth of, p i represents the true position of the vehicle at the i-th moment, i = k - l, …, k, …, k + l; Probability The calculation method is as follows: Among them, represents the true position p k in the j-th local matching region corresponding to the true position p k The number of position points with the same depth value as p, NUM{·} represents obtaining the number of elements in the set, represents the inertial navigation data sequence in the j-th local matching region on the terrain reference map at the theoretical depth, represents the inertial navigation data except for the j-th local matching region, represents the inertial navigation data sequence on the terrain reference map except for the j-th local matching region at the theoretical depth, local matching region serial number ε = 1,…,N, ∈ j is the threshold corresponding to the j-th local matching region.

8. The adaptability analysis method based on terrain matching ability quantification according to claim 1, characterized in that Sampling different Δp from the probability density function f(Δp k ) by the Monte Carlo method k , the sampled Δp k satisfies the following limiting conditions: |Δp k (1)|≤ Sr(1), |Δp k (2)|≤ Sr(2) where, Δp k (1) is the first element of Δp k , and Δp k (2) is the second element of Δp k . Sr(1) is half of the length of the search range set with the true position p k as the center, and Sr(2) is half of the width of the search range set with the true position p k as the center.

9. The adaptability analysis method based on terrain matching ability quantification according to claim 1, characterized in that Adaptation error R(p k ) is calculated as follows: Among them, represents calculating the expectation.

10. The adaptability analysis method based on terrain matching ability quantification according to claim 1, wherein Performing Step 1 and Step 2 on each point of the aircraft on the terrain reference map to obtain the positioning error of the aircraft on the entire terrain reference map and the adaptability to the entire terrain reference map.

Images