AUV terrain aided navigation method based on window sliding

Abstract

The application considers the inertial navigation precision heading and navigation distance information in a short time into the terrain aided navigation method by fully considering the navigation error characteristics of the inertial navigation system, and improves the robustness of the matching algorithm. In the initial search stage, the initial search range is set according to the position information obtained by the inertial navigation, the path is initialized according to the water depth measurement data, the path point is screened according to the water depth measurement data, the heading and navigation distance information provided by the inertial navigation, and whether to perform window sliding is judged by comparing the obtained path with the inertial navigation navigation distance, so that the segmented matching positioning is realized. In each segment, the final path is determined according to the value function, and the matching positioning of the whole path is completed.

Description

TECHNICAL FIELD

[0001] The application belongs to the field of seabed terrain auxiliary navigation and positioning, and particularly relates to an AUV terrain auxiliary navigation method based on window sliding. BACKGROUND

[0002] With the rapid development of AUV technology, AUV has the ability of autonomous operation, and gradually begins to undertake the tasks of seabed environment detection and scientific investigation, which requires AUV to have the ability of underwater covert navigation, and puts forward higher requirements for the navigation system of AUV. Conventional underwater navigation methods such as inertial navigation and acoustic navigation are not suitable for long-time underwater covert operation due to the cumulative error with time and the need for prior installation of underwater beacons.

[0003] The terrain measurement sensor used in the terrain auxiliary navigation method is mostly a multi-beam sonar. A multi-beam sonar can measure a large amount of seabed terrain data at a time, has high measurement accuracy and good stability, and can obtain a relatively accurate navigation result. However, the energy supply of AUV itself is limited, and it cannot support the long-time operation of the multi-beam sonar, which limits the underwater endurance of AUV and makes it impossible to perform underwater long-distance navigation. In order to maintain long-time underwater navigation and positioning, the multi-beam sonar is replaced by a single-beam sonar, which can greatly extend the underwater endurance of AUV. However, the single-beam sonar can only measure limited terrain information at a time, and has high requirements for the number of terrain features and its own measurement accuracy. In the process of long-distance terrain auxiliary navigation of AUV, it is difficult to obtain a good navigation result in the area with poor terrain features.

[0004] Although the inertial navigation system has cumulative error with time and cannot be used for long-distance underwater navigation of AUV, the heading and distance measurement of the inertial navigation system in a short time is extremely accurate, and can provide extremely accurate heading and navigation distance information in a short time. SUMMARY

[0005] The application aims to solve the problem of low robustness of single-beam sonar terrain auxiliary navigation, and provides an AUV terrain auxiliary navigation method based on window sliding, which fully considers the error characteristics of the inertial navigation system and improves the robustness of the single-beam terrain auxiliary navigation method.

[0006] An AUV terrain auxiliary navigation method based on window sliding comprises the following steps:

[0007] Step 1: calculating terrain confidence;

[0008] Step 2: performing initial path search according to step 1;

[0009] Step 3: entering the matching positioning loop according to step 2;

[0010] Step 4: calculate the difference between the distance provided by the inertial navigation system and the distance provided by the inertial navigation system;

[0011] Step 5: according to step 4, determine whether to perform window sliding, if window sliding is performed, steps 2 to 4 are executed in a loop until the end point is reached, and the matching positioning is completed.

[0012] The present application can also include:

[0013] 1. Step 1 is as follows:

[0014] Step 1.1: generate a contour map according to the prior map, and divide the contour map into N contour sub-maps according to the grid division standard;

[0015] Step 1.2: environment modeling is performed according to the density of the contour lines in each contour sub-map, and the number of contour lines n in each sub-map region is calculated. c The terrain confidence of each sub-map is calculated by the number of contour lines.

[0016] 2. Step 2 is as follows:

[0017] Step 2.1: perform terrain aided navigation algorithm initialization, set the initial search range radius as R according to the position information provided by the inertial navigation system, and ensure that the real position of the AUV is within the search range in the case of large positioning error;

[0018] Step 2.2: two consecutive single-beam sonar measurements are performed within the initial search range to obtain water depth values z1 and z2, and two contour line data L1 and L2 within the search range are obtained, and the intersection points p1 and p2 of the two contour lines and the grid lines within the search range are calculated, and the initial range search is performed with p1 as the initial point.

[0019] 3. Step 3 is as follows:

[0020] Step 3.1: take p1 as the starting point, calculate whether there is an intersection point between it and the contour line L2 along the navigation θ1 of the inertial navigation system, if there is an intersection point, the initial point is retained, otherwise it is deleted, wherein the calculation method of whether there is an intersection point is that the points on the contour line 1 are connected to the two points on the contour line 2, and the slope of each line is calculated; by judging the relationship between the three slopes, it is determined whether there is an intersection point, and the judgment formula is:

[0021]

[0022] In the formula, θ1 is the heading information provided by the inertial navigation system, p 1,i is the coordinate of the i-th initial point in p1, p 2,j and p 2,j+1 are the coordinates of the j-th and j+1-th points in p2;

[0023] Step 3.2: Calculate the coordinates of the intersection point of the isobath p1 along the inertial navigation route and p2 The distance of p1 from isobath L2 is calculated as L range The calculated distance is compared with the distance L ins provided by the inertial navigation, and the distance difference is compared with the set error threshold δ. The initial point p1 smaller than the error threshold is retained, and the rest is discarded;

[0024] Step 3.3: The initial points obtained in step six are too many, resulting in a decrease in operation speed. The initial points are equally divided in the initial search range, and the initial path point with the smallest error in each segment is retained, and the rest is discarded;

[0025] Step 3.4: The path points obtained above are all on the grid lines in the initial search range, which may be different from the true position. Therefore, the initial nodes are encrypted in a certain range to avoid unnecessary errors caused by the initial points being on the grid lines. The obtained path points are judged for intersection and distance calculation, and effective initial point screening is performed through the error threshold to complete the path initialization;

[0026] Step 3.5: Start single-beam sonar measurement to obtain water depth data z * , and obtain the position information (x * , y * ) at this time. According to the isobath density, set the isobath confidence of this area, and set the water depth measurement confidence interval according to the isobath confidence. When the isobath is dense, set a small confidence interval, and when the isobath is sparse, set a larger confidence interval. Obtain the water depth data in the confidence interval;

[0027] According to the measured water depth data and the water depth data in the confidence interval, the corresponding isobath data is obtained, and the isobath is discretized into isobath points. Through steps 3.1 and 3.2, it is calculated whether the path point on the previous isobath along the heading provided by the inertial navigation has an intersection with the isobath. If there is an intersection, the distance is calculated. The calculated distance is compared with the navigation distance provided by the inertial navigation, and the path smaller than the error threshold is retained;

[0028] Step 3.6: As time increases, the number of initial path points obtained in step 3.5 gradually decreases until the distance difference between all isobaths meets the error threshold δ. At this time, the navigation method has converged, and the terrain aided navigation method ends;

[0029] Step 3.7: Calculate the final path of this area, if the initial path point is greater than 1, the final path number is W, determine the final path initial point according to the value function, and determine the whole path information. The value function of the path is related to the whole path information, and the size is the reciprocal of the path length error sum, and the calculation formula is:

[0030]

[0031] In the formula is the distance measurement value of the jth final path inertial navigation, is the path distance between the two isobaths of the jth final path, and j is the jth path in the final reserved path;

[0032] Step 3.8: Determine the weight of each path according to the value function of each path, and the weight calculation formula is as follows:

[0033]

[0034] Step 3.9: Weight normalization, the formula is as follows:

[0035]

[0036] Step 3.10: Obtain the position information of the path end point according to the weight formula, and the calculation formula is as follows:

[0037]

[0038] In the formula x j and y j are the horizontal coordinates of the end point of the jth final path.

[0039] 4, Step 4, with the increase of time, the number of initial path points obtained in step 3.5 is gradually reduced, until the distance difference between all isobaths meets the error threshold δ, at this time the navigation method has been converged, and the terrain aided navigation method in this stage is ended.

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

[0041] Compared with the prior art, the beneficial effects of the present application are: in the terrain aided positioning using single-beam sonar, due to the limited information obtained and the existence of terrain feature poor area, the positioning robustness is lost, and only the positioning characteristics of the inertial navigation system are considered, and the high-precision measurement information of the inertial navigation in a short time is not considered in the matching method, so that high-precision positioning results cannot be achieved.

[0042] The application improves the robustness of the matching algorithm by fully considering the navigation error characteristics of the inertial navigation system, and considering the inertial navigation accurate heading and navigation distance information in a short time into the terrain aided navigation method. In the initial search stage, the initial search range is set according to the position information obtained by the inertial navigation, the path is initialized according to the water depth measurement data, the path points are screened according to the water depth measurement data, the heading and navigation distance information provided by the inertial navigation, and whether to perform window sliding is determined by comparing the obtained path with the inertial navigation distance, so as to realize segmented matching positioning. In each segment, the final path is determined according to the value function, and the matching positioning of the whole path is completed. BRIEF DESCRIPTION OF DRAWINGS

[0043] Figure 1 is a flowchart of the application;

[0044] Figure 2 is a flowchart of the matching method of the application;

[0045] Figure 3 is a window sliding flowchart of the application. DETAILED DESCRIPTION

[0046] The application will be further described below in combination with the drawings.

[0047] Example 1:

[0048] The application relates to an AUV terrain aided navigation method based on window sliding, which comprises initial path search, terrain confidence calculation, matching positioning cycle and window sliding positioning. The terrain confidence is calculated according to the density of the contour line, in the terrain matching, the initial path search is firstly performed, the appropriate initial path point is found, the confidence interval is set according to the terrain confidence, the path point is screened by using the measurement information provided by the inertial navigation, whether to perform window sliding matching is determined according to the distance error, the final path information is calculated according to the value function, and the end point is reached.

[0049] The content of the application is realized by comprising the following steps:

[0050] 1. Firstly, the terrain matching confidence is calculated, the contour line map is generated according to the prior chart data, the contour line map is divided into grids according to the grid division standard, and N contour line sub-maps are generated;

[0051] 2. Environment modeling is performed, according to the contour line matching characteristics, the contour line matching result is related to the density of the contour line, the number of contour lines in each sub-map is calculated as n c , the terrain confidence of each sub-map is calculated by the number of contour lines in the sub-map;

[0052] 3. Terrain aided navigation positioning, first, the algorithm initialization, according to the initial position information provided by the inertial navigation, set the initial search range radius R, wherein the search radius and the error of inertial navigation, to ensure that in the case of large positioning error, the real position of AUV within the search range, improve the robustness of positioning;

[0053] 4. In the initial search range, the initial path point is determined, and the water depth values z1 and z2 are obtained by two consecutive single-beam sonar measurements in the search range. According to the contour map, the contour data corresponding to the water depth data in the search range is L1 and L2. The intersection of the two contour lines and the grid lines in the search range is p1 and p2, and the initial path search is performed with p1 as the initial path point;

[0054] 5. According to the inertial navigation system, the heading θ1 and the navigation distance L ins information, starting from the initial point p1, along the heading provided by the inertial navigation, to determine whether there is an intersection with the contour line L2 in the search range, if there is an intersection, the initial point is retained, otherwise deleted, wherein the calculation method of whether there is an intersection is that, from the initial path point p1 on the contour line L1 to any two consecutive points on the contour line L2, the slope of each line is calculated. According to the slope of the line and the heading information provided by the inertial navigation, the relationship between the three slopes is determined to determine whether there is an intersection, and the judgment formula is:

[0055]

[0056] where θ1 is the heading information provided by the inertial navigation, p 1,i is the coordinate of the i-th initial point in p1, p 2,j and p 2,j+1 are the coordinates of the j-th and j+1-th points in p2.

[0057] 6. Determine whether the initial path point exists with the contour line L2 in the search range by calculation, and calculate the intersection coordinates of p1 along the heading of the inertial navigation and the straight line formed by the two consecutive points in p2 as The distance L range between p1 and ins is calculated, and the calculated distance is compared with the distance L ins provided by the inertial navigation. According to the error characteristics of the inertial navigation, the error threshold δ is set. The difference between the calculated distance and the inertial navigation distance is compared with the error threshold. The initial path point is retained if it is less than the error threshold, and the rest of the initial path point is deleted;

[0058] 7、The above-mentioned initial path point is more, which may result in the reduction of algorithm operation efficiency, and therefore the path point needs to be screened. Firstly, the isobath path point pi in the search range is equally divided by M, and the two isobath connecting lines in each segment are reserved with the minimum distance error provided by the inertial navigation. On the basis of ensuring the positioning accuracy, the calculation burden is reduced, and the remaining points are discarded;

[0059] 8、The above-mentioned initial path point coordinates are all in the grid lines, while the real path point information may not be on the grid lines, resulting in the loss of robustness of positioning. Therefore, the isobath encryption is needed. The path encryption is carried out near the above-mentioned initial path point. The encryption quantity is ensured to be not greater than one fifth of the priori map resolution between any two continuous initial path points, so as to avoid the decline of the positioning result accuracy due to the real position of the AUV not being on the grid line, and the obtained path point is subjected to intersection judgment and distance calculation, and the effective initial point screening is carried out through the error threshold, so as to complete the path initialization;

[0060] 9、The path search cycle stage is started, the water depth data of the position of the AUV is measured by the single-beam sonar as z * , the position information at this place is obtained from the inertial navigation data as (x * , y * ), the terrain confidence at this position is obtained according to the calculated terrain confidence combined with the position given by the inertial navigation, the confidence is combined with the measured water depth, and the measurement confidence interval is set. When the terrain confidence is low, a larger confidence interval needs to be set to improve the robustness of the algorithm. When the confidence is high, a smaller confidence interval needs to be set to improve the calculation efficiency of the algorithm;

[0061] According to the measured water depth data and the water depth data of the confidence interval, the corresponding isobath data is obtained, the isobath is discretized into isobath points, and the path point on the previous isobath is calculated along the heading of the inertial navigation whether there is an intersection with the isobath through 5 and 6. If there is an intersection, the distance between the isobaths is calculated, and the calculated distance is compared with the navigation distance provided by the inertial navigation, and compared with the error threshold to determine whether the initial path point is reserved;

[0062] 10、With the increase of time, the number of initial path points obtained by 10 gradually decreases, until the distance difference between all isobaths is obtained. The distance difference satisfies the error threshold δ, which indicates that the navigation method has been converged at this time. The terrain aided navigation method in this stage is ended, and the window sliding matching positioning is carried out;

[0063] 11、Calculate the final path of the area, if the initial path point is greater than 1, the final path number is W, determine the final path initial point according to the value function, and determine the whole path information. The value function of the path is related to the whole path information, and its size is the reciprocal of the sum of the path length error and the calculation formula is:

[0064]

[0065] In the formula is the distance measurement value of the jth final path inertial navigation, is the path distance between the two isobaths of the jth final path, and j is the jth path in the final reserved path;

[0066] 12、Determine the weight of each path according to the value function of each path, and the weight calculation formula is as follows:

[0067]

[0068] 13、Weight normalization, formula as follows:

[0069]

[0070] 14、According to the weight formula, the position information of the path end point is obtained, and the calculation formula is as follows:

[0071]

[0072] In the formula x j and y j are the horizontal coordinates of the end point on the jth final path.

[0073] 15、Window sliding matching is performed, and steps 3 to 14 are executed in a loop until the end point is reached.

[0074] Example 2:

[0075] Figure 1 is the flowchart of the present application, and the specific implementation process is as follows:

[0076] (1) Calculate the terrain confidence;

[0077] (2) Perform initial path search;

[0078] (3) Enter the matching positioning loop;

[0079] (4) Calculate the difference from the distance provided by inertial navigation;

[0080] (5) Determine whether to perform window sliding and complete matching positioning.

[0081] Figure 2 is the flowchart of the matching method of the present application

[0082] (1) First, determine the search range of the algorithm according to the position information provided by the inertial navigation;

[0083] (2) Perform initial path point search in the initial search range;

[0084] (3) Perform initial point screening according to the heading and distance information provided by the inertial navigation;

[0085] (4) Segment the search range and select the optimal point in each region for reservation;

[0086] (5) Encrypt the reserved path points to improve the robustness of the algorithm;

[0087] (6) Measure the water depth using a single-beam sonar and generate corresponding contour line data;

[0088] (7) Set the measurement confidence interval according to the terrain confidence;

[0089] (8) Compare the distance between the contours with the inertial navigation distance to screen the path points;

[0090] (9) Complete the matching positioning.

[0091] Figure 3 is the window sliding flowchart of the present application

[0092] (1) First, determine the initial path;

[0093] (2) Enter the path search loop stage;

[0094] (3) Perform matching positioning;

[0095] (4) Compare with the distance information provided by the inertial navigation;

[0096] (5) Determine whether all path information meets the error threshold requirement;

[0097] (6) Calculate the position information of the AUV

[0098] (7) Perform window sliding until the end point is reached.

[0099] The above only describes the preferred embodiments of the present application and is not intended to limit the present application. For those skilled in the art, the present application can have various modifications and changes. Any modification, equivalent replacement, improvement, etc. made within the spirit and principles of the present application shall be included in the protection scope of the present application.

Claims

1. A window-sliding-based AUV terrain-aided navigation method, characterized in that: The method comprises the following steps: Step 1: Calculate the terrain confidence; Step 2: According to step 1, perform initial path search; Step 2.1: Perform terrain aided navigation algorithm initialization, set the initial search range radius R according to the position information provided by the inertial navigation system, and ensure that the AUV's true position is within the search range in the case of large positioning error; Step 2.2: Perform two consecutive single-beam sonar measurements within the initial search range to obtain water depth values z1 and z2, obtain two search range isobath data L1 and L2, calculate the intersection points p1 and p2 of the two isobaths and the grid lines within the search range, and take p1 as the initial point to perform initial range search; Step 3: According to step 2, enter the matching positioning loop; Step 3.1: Take p1 as the starting point, calculate whether it has an intersection point with the isobath L2 along the navigation θ1 of the inertial navigation system, if there is an intersection point, the initial point is retained, otherwise it is deleted, wherein the calculation method of whether there is an intersection point is that the points on the isobath 1 are connected to the two consecutive points in the isobath 2, and the slopes of each line are calculated; by judging the relationship between the three slopes, it is determined whether there is an intersection point, and the judgment formula is: where θ1is the heading information provided by the inertial navigation, p 1,i′ is the coordinate of the i'th initial point in p1 2,j′ and p 2,j′+1 is the coordinate of the j'th and j'+1'th point in p2; Step 3.2: Calculate the coordinates of the intersection point of the contour line p1 along the inertial navigation and p2 as The distance of p1 from the contour line L2 is calculated as L range The calculated distance is compared with the distance L ins provided by the inertial navigation, and the distance difference is compared with the set error threshold δ. The initial point p1 with a distance difference less than the error threshold is retained, and the rest are discarded. Step 3.3: The initial points obtained in step 6 are too many, which leads to a decrease in operation speed, so the initial points are equally divided in the initial search range, and the initial path point with the smallest error in each segment is retained, and the remaining points are discarded; Step 3.4: The path points obtained above are all on the grid lines within the initial search range, which may be different from the true position, so the initial nodes are encrypted within a certain range to avoid unnecessary errors caused by the initial points being on the grid lines, and the obtained path points are subjected to intersection point judgment and distance calculation, and effective initial point screening is performed through an error threshold to complete path initialization; Step 3.5: Start single-beam sonar measurement to obtain water depth data z * , and obtain the position information at this time as (x * , y * ), set the confidence of the contour line of this area according to the density of the contour line, and set the confidence interval of the water depth measurement according to the confidence of the contour line, when the contour line is dense, set a small confidence interval, when the contour line is sparse, set a larger confidence interval, and obtain the water depth data of the confidence interval; According to the measured water depth data and the water depth data in the confidence interval, the corresponding isobath data is obtained, and the isobath is discretized into isobath points, through steps 3.1 and 3.2, it is calculated whether the path point on the previous isobath has an intersection point with the isobath along the heading provided by the inertial navigation system, if there is an intersection point, the distance is calculated, and the calculated distance is compared with the navigation distance provided by the inertial navigation system, and the path with an error threshold is retained; Step 3.6: With the increase of time, the number of initial path points obtained in step 3.5 gradually decreases, until the distance difference between all isobaths satisfies the error threshold δ, at this time the navigation method has converged, and the terrain aided navigation method in this stage ends; Step 3.7: Calculate the final path of this area, if there is more than one initial path point, the final path number is W, determine the final path initial point according to the value function, determine the whole path information, the value function of the path is related to the whole path information, and the size is the reciprocal of the path length error sum, the calculation formula is: In the formula is the distance measurement value of the jth final path inertial navigation, is the path distance between the two isobaths of the jth final path, and j is the jth path in the final reserved path. Step 3.8: According to the value function of each path, the weight of each path is determined, and the weight calculation formula is as follows: Step 3.9: Weight normalization, formula as follows: Step 3.10: According to the weight formula, the position information of the path end point is obtained, and the calculation formula is as follows: wherein x j and y j are the horizontal coordinates of the end point on the jth final path, respectively. Step 4: Calculate the difference between the distance provided by the inertial navigation and the distance provided by the terrain aided navigation; Step 5: According to step 4, determine whether to perform window sliding, if so, perform steps 2 to 4 in a loop until the end point is reached, and the matching positioning is completed.

2. The window-sliding based AUV terrain-aided navigation method according to claim 1, wherein: Step 1 is as follows: Step 1.1: Generate a contour map according to the prior chart, and divide the contour map into N' contour sub-maps according to the grid division standard; Step 1.2: Environment modeling according to the isobath density in each isobath submap, calculate the number of isobaths n in each submap area c Calculate the terrain confidence of each submap by the number of isobaths.

3. The window-sliding based AUV terrain-aided navigation method according to claim 1, wherein: Step 4: As time increases, the number of initial path points obtained in step 3.5 gradually decreases until the distance difference between all contours meets the error threshold δ, at which time the navigation method has converged, and the terrain aided navigation method ends.

Images