Submarine topography aided navigation repositioning and path replanning method, program, equipment and storage medium

Abstract

The invention belongs to the technical field of underwater navigation and path planning, and particularly relates to a submarine topography aided navigation repositioning and path replanning method, program, equipment and storage medium. According to the method, the characteristic that the navigation distance provided by the inertial navigation system in a short time is relatively accurate is introduced into the effectiveness evaluation of the navigation result of the terrain-aided navigation method, and whether repositioning is needed is judged; during repositioning, isobath points corresponding to water depth data are globally extracted, the isobath points are matched with randomly generated particles, the real position is gradually approached through step-by-step iteration, and evaluation of a repositioning result is completed by using a navigation distance provided by an inertial navigation system; and finally, substituting the distance with the target point and the terrain gradient into environment modeling, thereby improving the positioning precision of the repositioned path planning result to the target point, and improving the effectiveness of the navigation result.

Description

Technical Field

[0001] The present invention belongs to the technical field of underwater navigation and path planning, and in particular relates to a method, program, device and storage medium for seabed terrain-assisted navigation repositioning and path replanning. Background Art

[0002] In ocean exploration, autonomous underwater vehicles (AUVs) can withstand huge ocean pressure and have autonomous operation capabilities. With the improvement of AUV endurance, they can complete months of autonomous underwater navigation. AUVs equipped with a variety of ocean environment sensing devices can efficiently and accurately complete ocean environment exploration. Among the current AUV navigation methods, terrain-assisted navigation methods can provide bounded navigation errors, enabling AUVs to have long-term underwater navigation capabilities, and are currently a major research direction for AUV underwater navigation methods.

[0003] However, when AUV is performing terrain-assisted navigation, there may be continuous mismatching, causing the AUV to lose its own position and making it difficult to achieve effective underwater operations; at the same time, the AUV may be "kidnapped", causing it to lose the ability to estimate its own position. At present, there is little research on the problem of continuous matching failure or "kidnap" in submarine terrain-assisted navigation. The main research focuses on how to improve the accuracy of navigation results or how to converge navigation errors under a small range of initial errors. However, in view of the problem of unpredictable navigation error range in actual terrain-assisted navigation, it is difficult to achieve online repositioning and replanning of new paths when the search range is unknown.

[0004] The patent application with a publication date of December 5, 2023, publication number CN113532438B, and invention name "An improved ICCP terrain matching method under large initial positioning error" uses improved ICCP to achieve navigation results under large initial positioning error, but this method still requires a terrain area to be matched, and cannot achieve online repositioning when its own position area is completely unknown, resulting in the AUV being unable to quickly correct navigation errors when there is a "kidnapping" problem or its own position status is unknown.

[0005] The patent application with the publication date of October 28, 2022, publication number CN110763234B, and invention name "A method for underwater robot seabed terrain matching navigation path planning" considers terrain undulation as an influencing factor of the underwater robot state transition probability, ensuring that the terrain matching algorithm can obtain a high matching accuracy on the entire planned path. However, this method does not take into account the online replanning of the path in the case of navigation failure or "kidnapping" of the AUV in the path planning, and cannot achieve online path replanning under matching failure. Summary of the invention

[0006] The purpose of the present invention is to solve the problem of continuous mismatching or "kidnapping" of AUV during terrain-assisted navigation, and to provide a method, program, device and storage medium for repositioning and path replanning of submarine terrain-assisted navigation. The present invention first extracts the isobath points corresponding to the water depth values ​​in the entire prior chart area, and then updates the particle weights by the Euclidean distance between the particles and the isobaths, and completes the online repositioning of the AUV through progressive search; after completing the repositioning, the AUV determines its own position, and replans the path according to the distance between the AUV and the target point, and finds a suitable path to the target point.

[0007] A seabed terrain-assisted navigation repositioning method comprises the following steps:

[0008] Step 1: Initialize the position vector of each particle in the particle swarm; the position vector is a two-dimensional plane coordinate vector;

[0009] Step 2: According to the single-beam sonar and depth meter carried by the AUV, the water depth measurement value of the AUV's current location is obtained, and the isobath points corresponding to the water depth measurement value are extracted from the prior nautical chart to obtain the position vector of each isobath point;

[0010] Step 3: For each particle, calculate the shortest distance between the particle and each isobath point, and update the particle weight based on the shortest distance;

[0011] Step 4: Estimate the current position vector of the AUV based on the weight of each particle and the position vector of each particle, and estimate the heading and navigation distance of the AUV based on the position vector of the AUV at the previous moment.

[0012] Step 5: According to the inertial navigation system carried by the AUV, obtain the movement distance d of the AUV from the previous moment to the current moment 1 (t); if there are n consecutive moments, The relocation process ends; otherwise, the position vector of each particle in the particle swarm is updated according to the estimated value of the heading and navigation distance of the AUV, and the AUV moves to the next moment and returns to step 2.

[0013] Furthermore, in step 1, the position vector of each particle in the particle swarm is initialized according to the AUV position vector provided by the inertial navigation system when the repositioning is started; in order to ensure that the repositioning can be completed quickly, a larger particle initialization distribution variance is set to ensure that different particles have different motion trajectories.

[0014] Furthermore, the method for updating the weight of the particle based on the shortest distance in step 3 is specifically as follows:

[0015]

[0016] Among them, w i represents the weight of the i-th particle; σ d is the value related to the mean of the shortest distance between two depth contours in the prior chart; d imin It represents the shortest distance between the ith particle and each isobath point.

[0017] Furthermore, in step 4, the position vector of the AUV at the current moment is estimated according to the weight of each particle and the position vector of each particle, specifically:

[0018]

[0019] in, represents the estimated position vector of the AUV; P represents the total number of particles in the particle swarm; w i represents the weight of the i-th particle; X i Represents the position vector of the i-th particle.

[0020] A path replanning method based on a seabed terrain-assisted navigation repositioning method comprises the following steps:

[0021] Step 1: According to the inertial navigation system carried by the AUV, obtain the movement distance d of the AUV from the previous moment to the current moment 1 (t); According to the terrain-assisted navigation system carried by the AUV, obtain the movement distance d of the AUV from the previous moment to the current moment 2 (t); if there are n consecutive moments, |d 1 (t)-d 2 (t)| is greater than the set threshold δ, then relocation is performed; otherwise, step 2 is executed;

[0022] Step 2: Execute the optimal path planning from the current position of the AUV to the target point in the pre-established environment model;

[0023] Step 3: The AUV moves to the next moment according to the path planning. If it does not reach the target point, it returns to step 1.

[0024] Furthermore, the environmental model pre-established in step 2 is based on the terrain undulation of each area calculated according to the prior nautical chart, and the distance of each grid point from the target point is considered. The terrain undulation closer to the target point is set to have a greater impact, and the terrain undulation farther from the target point is set to have a smaller impact, so as to complete the establishment of the environmental model.

[0025] Furthermore, in the step 2, RRT* is used to perform online path planning in the pre-established environment model, which specifically includes the following steps:

[0026] Step 2.1: Based on the starting point z of the path planning init Initialize the random tree T, the random tree T is z goal To expand the search space, we generate nodes z by random sampling. rand , determine the node z in the random tree that is closest to the newly generated node near , and from node z near To z rand After extending a certain step length p, a new node z is obtained new , perform obstacle avoidance detection on the newly generated nodes, and add the nodes to the random tree after there are no obstacles;

[0027] Step 2.2: Calculate the distance z in the random tree new The nearest node is z neighbor , calculated from z init to z neighbor The cost of neighbor to z new The Euclidean distance of z neighbor Set as node z new The parent node z parent ;

[0028] Step 2.3: Calculate from z init To nearby connected node z near The cost and near to z new The Euclidean distance, while considering the node’s reward function, finds the node z with the minimum cost near As z new The parent node z parent ; Rewire according to the random tree parent node found, and calculate from z init To node z new The cost and near to z new The Euclidean distance of the node is taken into account, and the reward function of the node is considered. The node rewiring is performed to ensure that the cost of all nodes to the initial point is minimized.

[0029] Step 2.4: The random tree continues to expand until the generated z new Node distance target z goal When the distance is less than the step threshold, z new and z goal Connectivity;

[0030] Step 2.5: If the maximum number of iterations has not been reached, return to step 2.2; otherwise, output the optimal init to z goal Path.

[0031] A computer device / equipment / system comprises a memory, a processor and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the above-mentioned seabed terrain-aided navigation repositioning and path replanning method.

[0032] A computer-readable storage medium stores a computer program / instruction thereon, which, when executed by a processor, implements the steps of the above-mentioned seabed terrain-assisted navigation repositioning and path replanning method.

[0033] A computer program product includes a computer program / instruction, which, when executed by a processor, implements the steps of the above-mentioned seabed terrain-assisted navigation repositioning and path replanning method.

[0034] The beneficial effects of the present invention are:

[0035] The present invention introduces the relatively accurate characteristic of the short-term navigation distance provided by the inertial navigation system into the effectiveness evaluation of the navigation results of the terrain-assisted navigation method to determine whether repositioning is needed; in the repositioning, the isobath points corresponding to the water depth data are extracted globally, and the isobath points are used in combination with randomly generated particles, and the real position is gradually approached through step-by-step iteration, and the navigation distance provided by the inertial navigation system is used to complete the evaluation of the repositioning results; finally, the distance to the target point and the terrain slope are brought into the environmental modeling to improve the positioning accuracy of the path planning result after repositioning to reach the target point, and improve the effectiveness of the navigation results. The present invention fully considers the problems existing in AUV repositioning under the problem of continuous mismatching or "kidnapping", and proposes corresponding solutions. On the basis of completing the repositioning, local path replanning is performed to ensure the effectiveness of the navigation results when reaching the target point. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 It is a flow chart of the present invention.

[0037] Figure 2 It is a flow chart of relocation in the present invention.

[0038] Figure 3 It is a schematic diagram of online path replanning of the present invention. DETAILED DESCRIPTION

[0039] The present invention is further described below in conjunction with the accompanying drawings.

[0040] The present invention relates to an online repositioning and path replanning method for an AUV in the event of navigation failure, comprising navigation failure assessment, online repositioning, repositioning completion assessment, online path replanning and the like.

[0041] A method for seabed terrain-assisted navigation repositioning and path replanning, comprising the following steps:

[0042] Step 1: When performing terrain-assisted navigation, first determine whether the AUV has a continuous mismatch or "kidnapping" problem. Using the characteristics of the inertial navigation system's relatively accurate navigation distance information in a short period of time, compare the navigation distance provided by the inertial navigation with the movement distance of the terrain-assisted navigation matching result;

[0043] According to the inertial navigation system carried by the AUV, the movement distance d of the AUV from the previous moment to the current moment is obtained 1 (t); According to the terrain-assisted navigation system carried by the AUV, obtain the movement distance d of the AUV from the previous moment to the current moment 2 (t); if there are n consecutive moments, |d 1 (t)-d 2 (t)| is greater than the set threshold δ, it is considered that continuous mismatching or "kidnapping" problem occurs, and relocalization is required, that is, executing steps 1.1 to 1.5 to provide accurate location information for the AUV to accurately reach the target point; otherwise, execute step 2;

[0044] Step 1.1: Initialize the position vector of each particle in the particle swarm; the position vector is a two-dimensional plane coordinate vector;

[0045] In order to ensure that relocation can be completed quickly, a larger particle initialization distribution variance is set to ensure that different particles have different motion trajectories and each particle has the same weight at the initial moment;

[0046] Step 1.2: According to the single-beam sonar and depth meter carried by the AUV, obtain the water depth measurement value of the AUV's current location, extract the isobath points corresponding to the water depth measurement value in the prior nautical chart, and obtain the position vector of each isobath point;

[0047] Step 1.3: For each particle, calculate the shortest distance between the particle and each isobath point, and update the particle weight based on the shortest distance;

[0048]

[0049] Among them, w i represents the weight of the i-th particle; σ d is the value related to the mean of the shortest distance between two depth contours in the prior chart; d imin represents the shortest distance between the ith particle and each isobath point;

[0050] Step 1.4: Estimate the current position vector of the AUV based on the weight of each particle and the position vector of each particle, and estimate the heading and navigation distance of the AUV based on the position vector of the AUV at the previous moment.

[0051]

[0052] in, represents the estimated position vector of the AUV; P represents the total number of particles in the particle swarm; w i represents the weight of the i-th particle; X i represents the position vector of the i-th particle;

[0053] Step 1.5: According to the inertial navigation system carried by the AUV, obtain the movement distance d of the AUV from the previous moment to the current moment 1 (t); if there are n consecutive moments, The relocation process ends; otherwise, the position vector of each particle in the particle swarm is updated according to the estimated value of the heading and navigation distance of the AUV, and the AUV moves to the next moment and returns to step 1.2;

[0054] Step 2: Execute the optimal path planning from the current position of the AUV to the target point in the pre-established environment model;

[0055] Due to the continuous mismatching or "kidnapping" problem, the position of the AUV after repositioning deviates from the originally set trajectory. At the same time, the navigation performance of terrain-assisted navigation is affected by the terrain adaptability of the path. In order to improve the positioning effect when reaching the target point, a trajectory to the target point is planned online according to the current position, so that the AUV can reach the target point accurately.

[0056] When the AUV performs terrain-assisted navigation, the terrain undulation of the area it passes through will have a great impact on the navigation results. Therefore, the environment modeling is first performed based on the terrain undulation. The terrain characteristic parameter for calculating the terrain undulation is used as the terrain slope to provide environment modeling for the value function value of path planning.

[0057] The formula for calculating terrain relief is:

[0058]

[0059] When performing terrain-assisted navigation, if you first pass through the terrain adaptation area and then the terrain non-adaptation area to reach the destination, although you can get accurate navigation results in the adaptation area, because you pass through the non-adaptation area continuously before reaching the destination, there will be an unacceptable navigation error when you reach the destination. Therefore, when performing environmental modeling, you need to consider not only the terrain undulation, but also the distance between each grid point and the target point. Comprehensively consider the distance and terrain undulation to complete the environmental modeling;

[0060] In the pre-established environment model, RRT* is used for online path planning, which includes the following steps:

[0061] Step 2.1: Based on the starting point z of the path planning init Initialize the random tree T, the random tree T is z goal To expand the search space, we generate nodes z by random sampling. rand , determine the node z in the random tree that is closest to the newly generated node near , and from node z near To z rand After extending a certain step length p, a new node z is obtained new , perform obstacle avoidance detection on the newly generated nodes, and add the nodes to the random tree after there are no obstacles;

[0062] Step 2.2: Calculate the distance z in the random tree new The nearest node is z neighbor , calculated from z init to z neighbor The cost of neighbor to z new The Euclidean distance of z neighbor Set as node z new The parent node z parent ;

[0063] Step 2.3: Calculate from z init To nearby connected node z near The cost and near to z new The Euclidean distance, while considering the node’s reward function, finds the node z with the minimum cost near As z new The parent node z parent ; Rewire according to the random tree parent node found, and calculate from z init To node z new The cost and near to z new The Euclidean distance of the node is taken into account, and the reward function of the node is considered. The node rewiring is performed to ensure that the cost of all nodes to the initial point is minimized.

[0064] Step 2.4: The random tree continues to expand until the generated z new Node distance target z goal When the distance is less than the step threshold, z new and z goal Connectivity;

[0065] Step 2.5: If the maximum number of iterations has not been reached, return to step 2.2; otherwise, output the optimal init to z goal Path;

[0066] Step 3: The AUV moves to the next moment according to the path planning. If it does not reach the target point, it returns to step 1.

[0067] Combination Figure 1 The overall flow chart is described. First, based on the short-term navigation distance provided by the inertial navigation, it is determined whether the navigation result of the AUV is invalid. If so, the AUV global repositioning is performed, and whether the repositioning is completed is determined based on the position result obtained by the repositioning. Then, the local path is replanned based on the position obtained by the repositioning and the position of the target point to obtain a suitable path to the target point, and terrain-assisted navigation is performed along the path to reach the target point.

[0068] Combination Figure 2 The implementation method of online repositioning is described. First, random particles are initialized according to the initial position provided by the inertial navigation system. Then, isobaths are extracted from the prior nautical chart based on the water depth data obtained by the single-beam sonar and the depth meter. The generated isobaths are used to complete the particle weight update and obtain the estimated position. The obtained position is used to determine the heading and navigation distance for the next particle motion update, and the particle motion update is completed. Then, the isobath extraction and particle weight update are repeated until the repositioning is completed through evaluation.

[0069] Combination Figure 3 The implementation method of path replanning is explained. First, the terrain undulation is calculated based on the prior nautical chart, and the environment is modeled based on the terrain undulation and the distance from the target point to obtain a terrain adaptability model. Then, the online path replanning is performed through the RRT* algorithm to obtain a path to the target point, completing the path replanning.

[0070] When the AUV performs terrain-assisted navigation, due to continuous mismatching or "kidnapping" problems, the AUV cannot know its own location area. The current terrain-assisted navigation method cannot relocate when its own position is completely unknown. At the same time, the online path planning after relocation is not taken into account in the algorithm, resulting in the inability to achieve accurate terrain-assisted navigation when its own position is unknown.

[0071] The present invention introduces the relatively accurate characteristic of the short-term navigation distance provided by the inertial navigation system into the effectiveness evaluation of the navigation result of the terrain-assisted navigation method to determine whether repositioning is needed; in the repositioning, isobath points corresponding to the water depth data are extracted globally, and the isobath points are used in combination with randomly generated particles, and the real position is gradually approached through step-by-step iteration, and the navigation distance provided by the inertial navigation system is used to complete the evaluation of the repositioning result; finally, the distance to the target point and the terrain slope are introduced into the environmental modeling to improve the positioning accuracy of the path planning result after repositioning to reach the target point, thereby improving the effectiveness of the navigation result.

[0072] The present invention fully considers the problems existing in AUV repositioning under the condition of continuous mismatching or "kidnapping" and proposes corresponding solutions. On the basis of completing the repositioning, local path replanning is performed to ensure the validity of the navigation result when reaching the target point.

[0073] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A method for seabed terrain-assisted navigation repositioning, characterized in that: The following steps are involved: Step 1: Initialize the position vector of each particle in the particle swarm; the position vector is a two-dimensional plane coordinate vector; Step 2: According to the single-beam sonar and depth meter carried by the AUV, the water depth measurement value of the AUV's current location is obtained, and the isobath points corresponding to the water depth measurement value are extracted from the prior nautical chart to obtain the position vector of each isobath point; Step 3: For each particle, calculate the shortest distance between the particle and each isobath point, and update the particle weight based on the shortest distance; Step 4: Estimate the current position vector of the AUV based on the weight of each particle and the position vector of each particle, and estimate the heading and navigation distance of the AUV based on the position vector of the AUV at the previous moment. Step 5: According to the inertial navigation system carried by the AUV, obtain the movement distance d1(t) of the AUV from the previous moment to the current moment; if there are n consecutive moments, The relocation process ends; otherwise, the position vector of each particle in the particle swarm is updated according to the estimated value of the heading and navigation distance of the AUV, and the AUV moves to the next moment and returns to step 2.

2. The method for seabed terrain-assisted navigation repositioning according to claim 1, characterized in that: In the step 1, the position vector of each particle in the particle swarm is initialized according to the AUV position vector provided by the inertial navigation system when the repositioning is started; in order to ensure that the repositioning can be completed quickly, a larger particle initialization distribution variance is set to ensure that different particles have different motion trajectories.

3. The method for seabed terrain-assisted navigation repositioning according to claim 1, characterized in that: The method for updating the weight of particles based on the shortest distance in step 3 is specifically as follows: Among them, w i represents the weight of the i-th particle; σ d is the value related to the mean of the shortest distance between two depth contours in the prior chart; d imin It represents the shortest distance between the ith particle and each isobath point.

4. The method for seabed terrain-assisted navigation repositioning according to claim 1, characterized in that: In step 4, the position vector of the AUV at the current moment is estimated based on the weight of each particle and the position vector of each particle, specifically: in, represents the estimated position vector of the AUV; P represents the total number of particles in the particle swarm; w i represents the weight of the i-th particle; X i Represents the position vector of the i-th particle.

5. A path replanning method based on the seabed terrain assisted navigation repositioning method according to claim 1, characterized in that: The following steps are involved: Step 1: According to the inertial navigation system carried by the AUV, obtain the movement distance d1(t) of the AUV from the previous moment to the current moment; according to the terrain-assisted navigation system carried by the AUV, obtain the movement distance d2(t) of the AUV from the previous moment to the current moment; if |d1(t)-d2(t)| is greater than the set threshold δ for n consecutive moments, repositioning is performed; otherwise, step 2 is performed; Step 2: Execute the optimal path planning from the current position of the AUV to the target point in the pre-established environment model; Step 3: The AUV moves to the next moment according to the path planning. If it does not reach the target point, it returns to step 1.

6. The path replanning method based on seabed terrain assisted navigation repositioning according to claim 5, characterized in that; The environmental model pre-established in the step 2 is based on the terrain undulation of each area calculated according to the prior nautical chart, and the distance of each grid point from the target point is considered. The terrain undulation closer to the target point is set to have a greater impact, and the terrain undulation farther away from the target point is set to have a smaller impact, so as to complete the establishment of the environmental model.

7. The path replanning method based on seabed terrain assisted navigation repositioning according to claim 5, characterized in that; In the step 2, RRT* is used to perform online path planning in the pre-established environment model, which specifically includes the following steps: Step 2.1: Based on the starting point z of the path planning init Initialize the random tree T, the random tree T is z goal To expand the search space, we generate nodes z by random sampling. rand , determine the node z in the random tree that is closest to the newly generated node near , and from node z near To z rand After extending a certain step length p, a new node z is obtained new , perform obstacle avoidance detection on the newly generated nodes, and add the nodes to the random tree after there are no obstacles; Step 2.2: Calculate the distance z in the random tree new The nearest node is z neighbor , calculated from z init to z neighbor The cost of neighbor to z new The Euclidean distance of z neighbor Set as node z new The parent node z parent ; Step 2.3: Calculate from z init To nearby connected node z near The cost and near to z new The Euclidean distance, while considering the node’s reward function, finds the node z with the minimum cost near As z new The parent node z parent ; Rewire according to the random tree parent node found, and calculate from z init To node z new The cost and near to z new The Euclidean distance of the node is taken into account, and the reward function of the node is considered. The node rewiring is performed to ensure that the cost of all nodes to the initial point is minimized. Step 2.4: The random tree continues to expand until the generated z new Node distance target z goal When the distance is less than the step threshold, z new and z goal Connectivity; Step 2.5: If the maximum number of iterations has not been reached, return to step 2.2; otherwise, output the optimal init to z goal Path.

8. A computer device / equipment / system comprising a memory, a processor and a computer program stored in the memory, characterized in that: The processor executes the computer program to implement the steps of the method according to any one of claims 1 to 5.

9. A computer-readable storage medium having a computer program / instruction stored thereon, characterized in that: When the computer program / instructions are executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

10. A computer program product comprising a computer program / instructions, characterized in that: When the computer program / instructions are executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

Images