A seabed terrain aided navigation repositioning and path replanning method, program, device and storage medium

Abstract

The present application belongs to the technical field of underwater navigation and path planning, and particularly relates to a seabed terrain assisted navigation repositioning and path re-planning method, program, device and storage medium. The present application introduces the characteristic of relatively accurate navigation distance provided by the inertial navigation system in a short term into the navigation result effectiveness evaluation of the terrain assisted navigation method, judges 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 cooperation with the randomly generated particles to gradually approach the real position 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 from the target point and the terrain slope are brought into the environment modeling to improve the positioning accuracy of the path planning result after repositioning to the target point, and improve the effectiveness of the navigation result.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of underwater navigation and path planning, and particularly relates to a seabed terrain assisted navigation repositioning and path re-planning method, program, device and storage medium. BACKGROUND

[0002] In marine exploration, an autonomous underwater vehicle (AUV) can withstand huge ocean pressure and has autonomous operation capability. With the improvement of the endurance of the AUV, the AUV can complete underwater autonomous navigation for months. The AUV carrying various marine environment sensing devices can efficiently and accurately complete marine environment exploration. In the current AUV navigation method, the terrain assisted navigation method can provide a bounded navigation error, so that the AUV has long-term underwater navigation capability, and is a main research direction of the current AUV underwater navigation method.

[0003] However, in the terrain assisted navigation of the AUV, there may be continuous mismatching, resulting in loss of the position of the AUV, and it is difficult to realize effective underwater operation. Meanwhile, the AUV also loses the ability to estimate its own position. At present, there is little research on the problem of continuous matching failure of seabed terrain assisted navigation. The current research mainly focuses on how to improve the accuracy of the navigation result or how to converge the navigation error under a small initial error, but it is difficult to realize online repositioning and re-planning of a new path under the condition that the search range is unknown, in view of the problem that the navigation error range cannot be predicted in actual terrain assisted navigation.

[0004] The patent application with the publication date of December 5, 2023, the publication number of CN113532438B and the invention name of "an improved ICCP terrain matching method under large initial positioning error", uses an improved ICCP to realize the navigation result under large initial positioning error, but this method still needs a terrain area to be matched, and cannot realize online repositioning under the condition that the own position area is completely unknown, resulting in that the AUV cannot realize rapid correction of the navigation error under the condition that the own position state is unknown.

[0005] The patent application with the publication date of October 28, 2022, the publication number of CN110763234B and the invention name of "an underwater robot seabed terrain matching navigation path planning method", considers the terrain undulation as an influence factor of the state transition probability of the underwater robot, and ensures that the terrain matching algorithm can obtain high matching accuracy on the whole path planned. However, this method does not consider the online path re-planning under navigation failure or the path of the AUV, and cannot realize online path re-planning under matching failure. SUMMARY

[0006] The application aims to solve the problem of continuous mismatching when AUV performs terrain aided navigation, and provides a seabed terrain aided navigation relocation and path re-planning method, program, device and storage medium. The application first extracts the isobath points corresponding to the water depth value in the entire prior map area, then updates the particle weight by the Euclidean distance of the particle distance isobath, and makes the AUV complete online relocation by progressive search; after completing the relocation, the position of the AUV is determined, the path is re-planned according to the distance of the AUV from the target point, and a suitable path to the target point is found.

[0007] A seabed terrain aided navigation relocation method, comprising 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 gauge carried by the AUV, obtain the water depth measurement value of the position of the AUV at the current time, extract the isobath points corresponding to the water depth measurement value in the prior map, and obtain the position vector of each isobath point;

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

[0011] Step 4: estimate the position vector of the AUV at the current time according to the weight of each particle and the position vector of each particle, and estimate the heading and sailing distance of the AUV according to the position vector of the AUV at the last time

[0012] Step 5: according to the inertial navigation system carried by the AUV, obtain the motion distance d1(t) of the AUV from the last time to the current time; if the AUV has been continuously moving for n times, then the relocation process is ended; otherwise, the position vector of each particle in the particle swarm is updated according to the heading and sailing distance estimate value of the AUV, and the AUV moves to the next time and returns to step 2.

[0013] Further, 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 relocation starts; in order to ensure that the relocation can be quickly completed, a larger particle initialization distribution variance is set to ensure that different particles have different motion trajectories.

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

[0015]

[0016] wherein w i represents the weight of the i-th particle; σ d is a value related to the mean of the shortest distance between two isobaths in the prior map; d imin represents the shortest distance of the i-th particle to each isobath point.

[0017] Further, the step 4 estimates the position vector of the AUV at the current time according to the weight of each particle and the position vector of each particle, specifically:

[0018]

[0019] wherein, represents the estimated value of the 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 topography aided navigation relocation method, comprising the following steps:

[0021] Step 1: According to the inertial navigation system carried by the AUV, the motion distance d1(t) of the AUV from the last time to the current time is obtained; according to the terrain aided navigation system carried by the AUV, the motion distance d2(t) of the AUV from the last time to the current time is obtained; if |d1(t)-d2(t)| is greater than the set threshold δ for consecutive n times, the relocation is performed; otherwise, step 2 is performed.

[0022] Step 2: The optimal path planning from the current time position of the AUV to the target point is performed in the pre-established environment model.

[0023] Step 3: The AUV moves to the next time according to the path planning, and if the target point is not reached, returns to step 1.

[0024] Further, the pre-established environment model in step 2 is calculated according to the terrain undulation of each region in the prior map, and the distance of each grid point from the target point is considered, and the terrain undulation closer to the target point is set to have greater influence, and the terrain undulation farther from the target point is set to have smaller influence, to complete the establishment of the environment model.

[0025] Further, in the pre-established environment model in step 2, RRT* is used for online path planning, specifically comprising the following steps:

[0026] Step 2.1: According to the starting point z init , a random tree T is initialized, and the random tree T is expanded in the search space with z goal as the target, and a node z is generated 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 The new node z is obtained by extending the step by a certain step size p. new For newly generated nodes, obstacle avoidance detection is performed, and if there are no obstacles, the nodes are added to the random tree;

[0027] Step 2.2: Calculate the distance z in the random tree new The nearest node is z neighbor Calculate from z init To z neighbor The cost, and from z neighbor To z new The Euclidean distance, and z neighbor Let's set it as node z new parent node z parent ;

[0028] Step 2.3: Calculate from z init To the nearest connected node z near The cost and z near To z new Using the Euclidean distance and considering the node's reward function, find the node z with the minimum cost. near As z new parent node z parent Rewire the tree based on the found random tree parent node, and calculate the value from z. init to node z new The cost and z near To z new The Euclidean distance is calculated, and the reward function of each node is considered. By rewiring the nodes, the cost to reach the initial point for all nodes is minimized.

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

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

[0031] A computer device / equipment / system includes 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-described seabed topography-assisted navigation repositioning and path replanning method.

[0032] A computer readable storage medium, having stored thereon computer programs / instructions, which, when executed by a processor, implement the steps of the seabed terrain aided navigation relocation and path replanning method.

[0033] A computer program product, comprising computer programs / instructions, which, when executed by a processor, implement the steps of the seabed terrain aided navigation relocation and path replanning method.

[0034] The present application has the following beneficial effects:

[0035] The present application introduces the short-term navigation distance provided by the inertial navigation system into the terrain aided navigation method navigation result effectiveness evaluation, judges whether relocation is needed, extracts the corresponding contour line points of the water depth data in the global in the relocation, and uses the contour line points to gradually approach the real position through the step-by-step iteration of the randomly generated particles, and uses the navigation distance provided by the inertial navigation system to complete the relocation result evaluation, finally, the distance from the target point and the terrain slope are brought into the environment modeling, the positioning accuracy of the path planning result after relocation to the target point is improved, and the effectiveness of the navigation result is improved. The present application fully considers the problems existing in the AUV relocation under the continuous mismatching problem, and proposes the corresponding solving method, on the basis of completing the relocation, performs the local path replanning, and ensures the effectiveness of the navigation result when reaching the target point. BRIEF DESCRIPTION OF DRAWINGS

[0036] Figure 1 is the flow chart of the present application.

[0037] Figure 2 is the flow chart of the relocation in the present application.

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

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

[0040] The present application relates to an online relocation and path replanning method under the AUV navigation failure condition, comprising navigation failure condition evaluation, online relocation, relocation completion evaluation, online path replanning and the like.

[0041] A seabed terrain aided navigation relocation and path replanning method, comprising the following steps:

[0042] Step one: when the terrain aided navigation is performed, firstly, it is judged whether the AUV has the continuous mismatching problem, the short-time navigation distance information characteristic of the inertial navigation system is used, the navigation distance provided by the inertial navigation is compared with the motion distance of the terrain aided navigation matching result;

[0043] According to the inertial navigation system carried by the AUV, a motion distance d1(t) of the AUV from a previous time to a current time is obtained; according to a terrain auxiliary navigation system carried by the AUV, a motion distance d2(t) of the AUV from the previous time to the current time is obtained; if |d1(t)-d2(t)| is greater than a set threshold δ for consecutive n times, it is considered that a continuous mismatch problem occurs, and repositioning needs to be performed, that is, steps 1.1 to 1.5 are performed, to provide accurate position information for the AUV to accurately reach the target point; otherwise, step two is performed;

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

[0045] To ensure that the repositioning can be quickly completed, 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 time;

[0046] Step 1.2: according to the single-beam sonar and the depth gauge carried by the AUV, a water depth measurement value of the position of the AUV at the current time is obtained, isobath points corresponding to the water depth measurement value are extracted in the prior sea chart, and position vectors of the isobath points are obtained;

[0047] Step 1.3: for each particle, the shortest distance of the particle from each isobath point is calculated, and the weight of the particle is updated based on the shortest distance;

[0048]

[0049] wherein w i represents the weight of the i-th particle; σ d is a value related to the average of the shortest distances between two isobaths in the prior sea chart; d imin represents the shortest distance of the i-th particle from each isobath point;

[0050] Step 1.4: according to the weight of each particle and the position vector of each particle, the position vector of the AUV at the current time is estimated, and the heading and sailing distance of the AUV are estimated according to the position vector of the AUV at the previous time

[0051]

[0052] wherein represents the position vector estimation value 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, the motion distance d1(t) of the AUV from the last time to the current time is obtained; if the continuous n times, the repositioning process is ended; otherwise, according to the heading and the navigation distance estimate of the AUV, the position vector of each particle in the particle swarm is updated, and the AUV moves to the next time and returns to step 1.2;

[0054] Step 2: Perform 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 mismatch problem, the position of the AUV after repositioning deviates from the original set trajectory, and the navigation performance of the terrain-aided navigation is affected by the terrain adaptability of the path. In order to improve the positioning effect when reaching the target point, an online trajectory is planned to reach the target point according to the current position, so that the AUV can accurately reach the target point;

[0056] When the AUV performs terrain-aided navigation, the terrain relief of the area passed will have a great influence on the navigation result, so first of all, the environment is modeled according to the terrain relief, and the terrain feature parameter of calculating the terrain relief, i.e. the terrain slope, is used to provide environment modeling for the value function value of path planning;

[0057] The formula for calculating the terrain relief is:

[0058]

[0059] When performing terrain-aided navigation, if the terrain non-adaptation area is passed after the terrain adaptation area to reach the terminal point, although accurate navigation results can be obtained in the adaptation area, but due to the continuous passage of the non-adaptation area before reaching the terminal point, it leads to difficult to receive navigation errors when reaching the terminal point, therefore, when performing environment modeling, not only the terrain relief needs to be considered, but also the distance of each grid point from the target point needs to be considered, and the distance and terrain relief are considered comprehensively to complete the environment modeling;

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

[0061] Step 2.1: According to the starting point z init of path planning, initialize a random tree T, and the random tree T is expanded in the search space with z goal as the target, generate a node z rand by random sampling, determine the node z near closest to the newly generated node in the random tree, and extend a certain step p from the node z near to z rand to obtain a new node z newThe newly generated node is detected for obstacle avoidance, and after no obstacle is detected, the node is added to the random tree;

[0062] Step 2.2: Calculate the distance z new from the nearest node z neighbor , calculate the cost from z init to z neighbor , and the Euclidean distance from z neighbor to z new , and set z neighbor as the parent node z new of the node z parent ;

[0063] Step 2.3: Calculate the cost from z init to the nearby connected node z near and the Euclidean distance from z near to z new , and find the node z near with the minimum cost as the parent node z new of z parent , according to the reward function of the node; according to the found random tree parent node, rewire the node, calculate the cost from z init to the node z new and the Euclidean distance from z near to z new , and consider the reward function of the node to ensure that all nodes have the minimum cost to the initial point through node rewiring;

[0064] Step 2.4: Continue to expand the random tree until the distance between the generated z new node and the target z goal is less than the step threshold, and then connect z new and z goal ;

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

[0066] Step 3: The AUV moves according to the path planning to the next moment, and if the target point is not reached, returns to step 1.

[0067] In combination Figure 1The whole flow chart is explained as follows: firstly, whether the AUV has a navigation result failure is judged according to the navigation distance provided by the inertial navigation system in a short term, if yes, the AUV global relocation is executed, and whether the relocation is completed is judged according to the position result obtained by the relocation, then the local path re-planning is carried out according to the position obtained by the relocation and the position of the target point, a suitable path reaching the target point is obtained, the terrain assisted navigation is carried out along the path, and the target point is reached.

[0068] In combination Figure 2 The implementation mode of the online relocation is explained as follows: firstly, the initial position provided by the inertial navigation system is used for random particle initialization, then the isobath extraction is carried out in the prior sea chart according to the water depth data obtained by the single-beam sonar and the depth gauge, the particle weight update is completed by using the generated isobath, the estimated position is obtained, the heading and the navigation distance of the next particle motion update are determined by using the obtained position, the particle motion update is completed, then the isobath extraction and the particle weight update are repeated until the relocation is completed through the evaluation.

[0069] In combination Figure 3 The implementation mode of the path re-planning is explained as follows: firstly, the terrain undulation is calculated according to the prior sea chart, the environment modeling is carried out according to the terrain undulation and the distance from the target point, and the terrain adaptability model is obtained. Then the online path re-planning is carried out through the RRT* algorithm, a path reaching the target point is obtained, and the path re-planning is completed.

[0070] When the AUV carries out the terrain assisted navigation, due to the continuous mismatching problem, the AUV cannot know the position area where the AUV is located, and the current terrain assisted navigation method cannot carry out the relocation under the condition that the position of the AUV is completely unknown, and the online path planning after the relocation is not considered in the algorithm, so that the accurate terrain assisted navigation cannot be realized under the condition that the position of the AUV is unknown.

[0071] The present application introduces the navigation distance provided by the inertial navigation system in a short term into the navigation result effectiveness evaluation of the terrain assisted navigation method, judges whether the relocation is needed, extracts the isobath points corresponding to the water depth data in the global in the relocation, and gradually approaches the real position through the step-by-step iteration by using the isobath points and the randomly generated particles, and the relocation result evaluation is completed by using the navigation distance provided by the inertial navigation system, finally the distance from the target point and the terrain slope are brought into the environment modeling, the positioning accuracy of the path planning result reaching the target point after the relocation is improved, and the effectiveness of the navigation result is improved.

[0072] The present application fully considers the problems existing in the AUV relocation under the continuous mismatching problem, and proposes the corresponding solving method, on the basis of completing the relocation, the local path re-planning is carried out, and the effectiveness of the navigation result when reaching the target point is ensured.

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

Claims

1. A seabed terrain aided navigation relocation method, characterized by, The method comprises the following steps: Step 1: initializing a position vector of each particle in the particle swarm; the position vector is a two-dimensional plane coordinate vector; Step 2: obtaining a water depth measurement of a position of the AUV at the current time according to a single-beam sonar and a depth gauge carried by the AUV, extracting an isobath point corresponding to the water depth measurement in a prior sea chart, and obtaining a position vector of each isobath point; Step 3: for each particle, calculating a shortest distance of the particle from each isobath point, and updating a weight of the particle based on the shortest distance; where w i represents the weight of the i-th particle; σ d is a value related to the mean of the shortest distances between two isobaths in the prior map; d imin represents the shortest distance of the i-th particle to each isobath point; Step 4: Estimate the position vector of the AUV at the current time according to the weight of each particle and the position vector of each particle, and estimate the heading and sailing distance of the AUV according to the position vector of the AUV at the last time Step 5: According to the inertial navigation system carried by the AUV, the motion distance d1(t) of the AUV from the last time to the current time is obtained; if the n continuous times, the relocation process is ended; otherwise, according to the heading and the navigation distance estimation value of the AUV, the position vector of each particle in the particle swarm is updated, and the AUV moves to the next time and returns to step 2.

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

3. The seabed terrain aided navigation relocation method of claim 1, wherein: In the step 4, the position vector of the AUV at the current time is estimated according to the weight of each particle and the position vector of each particle, and specifically: wherein, represents the position vector estimate of the AUV; P represents the total number of particles in the swarm; w i represents the weight of the i-th particle; X i represents the position vector of the i-th particle.

4. A path replanning method based on the seabed terrain aided navigation relocation method of claim 1, characterized in that, The method comprises the following steps: Step one: obtaining a motion distance d1(t) of the AUV from a last time to a current time according to an inertial navigation system carried by the AUV; obtaining a motion distance d2(t) of the AUV from the last time to the current time according to a terrain auxiliary navigation system carried by the AUV; if |d1(t)-d2(t)| is greater than a set threshold δ for consecutive n times, repositioning is performed; otherwise, step two is performed; Step two: performing optimal path planning of a current position of the AUV to a target point in a pre-established environment model; Step three: the AUV moves to a next time according to the path planning, and if the target point is not reached, returns to step one.

5. The seabed terrain aided navigation repositioning based path re-planning method according to claim 4, characterized in that; In the step two, the pre-established environment model is obtained by calculating a terrain undulation of each region according to the prior sea chart, and considering a distance of each grid point from the target point, and setting that a terrain undulation closer to the target point has a greater influence and a terrain undulation farther from the target point has a smaller influence to complete establishment of the environment model.

6. The seabed terrain aided navigation repositioning based path re-planning method according to claim 4, characterized in that; In the step two, RRT* is used for online path planning in the pre-established environment model, and specifically comprises the following steps: Step 2.1: Based on the starting point z of the path planning init Initialize a random tree T, with random tree T starting from z goal To expand the search space for the target, nodes z are generated through 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 The new node z is obtained by extending the step by a certain step size p. new For newly generated nodes, obstacle avoidance detection is performed, and if there are no obstacles, the nodes are added to the random tree; Step 2.2: Compute distance z in random tree new The last node is z neighbor , compute the cost from z init to z neighbor , and the Euclidean distance from z neighbor to z new , and set z neighbor as the parent of node z new z parent ; Step 2.3: Calculate from z init To the nearest connected node z near The cost and z near To z new Using the Euclidean distance and considering the node's reward function, find the node z with the minimum cost. near As z new parent node z parent Rewire the tree based on the found random tree parent node, and calculate the value from z. init to node z new The cost and z near To z new The Euclidean distance is calculated, and the reward function of each node is considered. By rewiring the nodes, the cost to reach the initial point for all nodes is minimized. Step 2.4: The random tree continues to expand until the generated z... new Node distance from target z goal When the distance is less than the step size threshold, z new and z goal Connected; Step 2.5: If the maximum number of iterations is not reached, go to Step 2.2; otherwise, output the optimal path from z init to z goal .

7. A computer device comprising a memory, a processor, and a computer program stored on the memory, wherein: The processor executes the computer program to realize the steps of the method in any one of claims 1 to 6.

8. A computer readable storage medium having stored thereon a computer program, characterized in that: The computer program is executed by the processor to realize the steps of the method in any one of claims 1 to 6.

9. A computer program product comprising computer instructions, characterized in that: The computer instructions are executed by the processor to realize the steps of the method in any one of claims 1 to 6.

Images