CN114088098A - Auxiliary navigation path planning method for polar region underwater vehicle database - Google Patents
Auxiliary navigation path planning method for polar region underwater vehicle database Download PDFInfo
- Publication number
- CN114088098A CN114088098A CN202111357833.XA CN202111357833A CN114088098A CN 114088098 A CN114088098 A CN 114088098A CN 202111357833 A CN202111357833 A CN 202111357833A CN 114088098 A CN114088098 A CN 114088098A
- Authority
- CN
- China
- Prior art keywords
- value
- point
- polar region
- navigation
- gravity
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 34
- 230000005484 gravity Effects 0.000 claims abstract description 51
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 46
- 238000009825 accumulation Methods 0.000 claims abstract description 4
- 230000000694 effects Effects 0.000 claims abstract description 4
- 238000004364 calculation method Methods 0.000 claims description 14
- 238000011156 evaluation Methods 0.000 claims description 3
- 230000006978 adaptation Effects 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 238000010845 search algorithm Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/203—Specially adapted for sailing ships
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3863—Structures of map data
- G01C21/387—Organisation of map data, e.g. version management or database structures
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Databases & Information Systems (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention provides a method for planning an auxiliary navigation path of a polar region underwater vehicle database, which comprises the following steps: acquiring a polar region gravity value and depth data, and performing accurate interpolation on a part of sparse intervals; gridding the image, constructing a grid map, and drawing gravity and depth isolines; superposing and fusing the gravity and depth grid graphs, and calculating the matchable degree of each grid window with the specified size; dividing all grid points into five types, and dividing the five types of grid points into regions according to properties; setting a matching priority; establishing a directed graph model, and adding weights to all directed edges; determining a navigation route of the underwater vehicle in a certain polar region at a certain time; according to the path planned by the path planning algorithm, dangerous navigation environments such as an underwater iceberg are avoided, the database navigation is used for assisting inertial navigation through a matching algorithm in the navigation process, and the long-time use error accumulation effect of an inertial navigation system is inhibited, so that the underwater vehicle in the polar region can safely, quickly and accurately reach a target point.
Description
Technical Field
The invention relates to an auxiliary navigation path planning method for a polar region underwater vehicle database, and belongs to the field of navigation and path planning of polar region underwater vehicles.
Background
The two poles are drivers of global climate change, and the two pole areas contain abundant resources, so that the two pole areas have important positions in the fields of future sea wars and non-wars. With the development of science and technology and economy, two-pole scientific research tasks are imperative, and the accurate positioning, navigation and path planning of the underwater vehicle in the polar region are important.
The existing polar region navigation means mainly comprise satellite navigation and inertial navigation, but because the satellites above the polar region are less in distribution, and meanwhile, magnetic storms and other phenomena frequently occur, satellite signals are easily influenced, the inertial navigation technology is the most important method for polar region navigation at present, but the platform type or strapdown inertial navigation system has the problem of difficulty in accurate positioning. Therefore, the geophysical information, the path planning algorithm and the inertial navigation technology are combined, the geophysical information database is used for assisting inertial navigation, and the method has a wide application prospect.
Path planning is widely used in research of mobile robots, and refers to that the robot searches an optimal path from an initial position to a target position according to a performance index set at the beginning. The polar region database auxiliary navigation mainly assists the inertial navigation technology through polar region gravity value and depth data, but in the polar region, gravity and depth data are not completely knowable, and do not completely have gradients, these gravity depth unknown points and gradient not big points are not good adaptation areas, how to pass through appropriate path planning algorithm, on the basis of guaranteeing the design performance index, properly pass through these not good adaptation areas, guarantee accurate positioning when accelerating navigation speed, have important meaning to the quick safe hidden execution polar region task of the underwater vehicle of the polar region.
Patent CC112799414A discloses a slack trajectory planning method, which divides global planning into a plurality of sub-flight segments according to the length of a design path, and completes local flight path planning by using a self-adaptive differential evolution particle swarm optimization algorithm.
Patent CN112967450A discloses an algorithm based on a-x to realize effective planning of the path of the transportation device, and further realize determination of the path of the transportation device to complete path selection.
An improved A algorithm is used in an 'underwater gravity matching navigation path planning based on the improved A algorithm' of European and Yangmda in 12 months in 2020 to realize a simulation experiment of the underwater gravity matching navigation path planning. The method can be used for carrying out gravity matching navigation path planning in general areas, but the algorithm path planning has no generalization capability in terms of articles and simulation results, and only places with gravity data can be selected, and the method cannot achieve quick and effective results in polar region environments.
The path planning algorithm mainly comprises a graph search method, an artificial potential field method, an RRT algorithm, a BUG algorithm, an incremental heuristic algorithm and the like. The invention relates to path planning by using Dijkstra algorithm in a graph search algorithm. The global path planning algorithm is used for firstly finding a path with better navigation capacity, and the local path planning algorithm is used for searching a path with better navigation capacity again when the earth geographic information data measured by the underwater vehicle has larger error with an original database or encounters random drift underwater icebergs.
In summary, when the current path planning algorithm is combined with the assisted navigation of the earth geographic information database, the generalization capability is very low, and the algorithm can only be applied under the conditions of sufficient data and obvious data change. For some important strategic areas of polar regions, the conditions of insufficient geographic data of the earth and a large number of dangerous line environments such as underwater icebergs exist, and the current method cannot be well applied to the polar regions. The method is based on Dijkstra algorithm, combines gravity and depth data, improves the generalization capability of path planning, can better plan the hidden, safe, accurate and quick feasible path of the polar region underwater vehicle, and realizes the application of the path planning on the polar region underwater vehicle.
Disclosure of Invention
The invention aims to solve the problems that an inertial navigation system is difficult to accurately position and is difficult to quickly and safely carry out tasks in a concealed manner in an extreme environment, and provides a database aided navigation path planning method, so that an extreme underwater vehicle can sail in a database aided navigation matching area as much as possible according to specified performance indexes in the sailing process, the accuracy of the inertial navigation system in the extreme positioning is improved, the extreme underwater vehicle can safely and accurately sail in certain strategic areas in a concealed manner, and related tasks are carried out.
The purpose of the invention is realized as follows: the method comprises the following steps:
the method comprises the following steps: acquiring a polar region gravity value and depth data, and performing accurate interpolation on a part of sparse intervals;
step two: gridding the image, constructing a grid map, and drawing gravity and depth isolines;
step three: superposing and fusing the gravity and depth grid graphs, and calculating the matchable degree of each grid window with the specified size;
step four: dividing all grid points into five types, and dividing the five types of grid points into regions according to properties;
step five: setting a matching priority, wherein the priority is in sequence of first class, second class, third class, fourth class and fifth class, and the fifth class point is strictly forbidden to be planned as a route region class;
step six: establishing a directed graph model, and adding weights to all directed edges;
step seven: planning a path according to the current position and the target position of the underwater vehicle in the polar region, and determining the navigation route of the underwater vehicle in the polar region at a certain time;
step eight: according to the path planned by the path planning algorithm, dangerous navigation environments such as an underwater iceberg are avoided, the database navigation is used for assisting inertial navigation through a matching algorithm in the navigation process, and the long-time use error accumulation effect of an inertial navigation system is inhibited, so that the underwater vehicle in the polar region can safely, quickly and accurately reach a target point.
The invention also includes such structural features:
1. step three, the specific calculation of the matchable degree of each grid window with the specified size is as follows:
σ＝(Rlong+Rlat)/2
in the formula, RlongAnd RlatRespectively calculating the matchable degrees of the gravity value and the depth value in a selected window in the longitude direction and the latitude direction, wherein sigma is the average value of the matchable degrees of the gravity value and the depth value in the selected window in the longitude direction and the latitude direction, reflects the variation amplitude of data in the window, and can be used as a factor for evaluating whether the position window can be a matchable position; m and n represent the size of the selected window grid map, g (i, j) is the gravity value of the point (i, j), and h (i, j) is the depth value of the point (i, j); if the data gradient between adjacent positions (i, j) and (i, j +1) or (i, j) and (i +1, j) in the window grid is larger, namely the data change of the adjacent positions of the window is more obvious, the larger the sigma value of the window is, namely the more suitable the window is for matching the auxiliary inertial navigation system.
2. Step four, the classification is as follows: the first type is a point with both gravity value and depth value, the second type is a point with only gravity value, the third type is a point with only depth value, the fourth type is a point without both gravity value and depth value, and the fifth type is a point with dangerous route environment such as underwater icebergs; the matchable degree of the fourth type of grid points is set to be one fifth of the sum of the matchable degrees of the second type of grid points and the third type of grid points, and the matchable degree of the fifth type of grid points in the environment with dangerous air routes such as underwater icebergs is set to be 0.
3. Step four, the classification is as follows: the first type is a point with both gravity value and depth value, the second type is a point with only gravity value, the third type is a point with only depth value, the fourth type is a point without both gravity value and depth value, and the fifth type is a point with dangerous route environment such as underwater icebergs; the matchable degree of the fourth type of grid points is set to be one fifth of the sum of the matchable degrees of the second type of grid points and the third type of grid points, and the matchable degree of the fifth type of grid points in the environment with dangerous air routes such as underwater icebergs is set to be 0.
4. And fifthly, controlling the priority through a weight k, and calculating the matching degree of the grid window to participate in the calculation of the weight k.
5. In the seventh step, the path planning algorithm uses Dijkstra algorithm, which specifically comprises the following steps:
firstly, creating three sets, namely an open set, a close set and a distance set S, wherein the open set stores nodes of which the shortest paths are not determined, all the nodes are stored in the close set during initialization, the nodes of which the shortest paths are determined are stored in the close set, the nodes are empty during initialization, the S set stores the shortest distances among all the nodes, the first value is set to be 0 during initialization, and the rest are set to be infinite;
taking out the starting point from the open set, adding the starting point into the close set, calculating the distance from each node in the open set to the starting point, and setting the starting point as a father node of each node in the open set;
selecting a node of a path with the minimum calculated value of the starting point, taking the node out of the open set, and adding the node into the close set;
updating the distance set S, recording the minimum path calculation value into the distance set S, and setting the shortest path values of other starting points to be infinite;
starting from the node in the selected close set, calculating the distance from other points in the open set to the point, and setting the point as a parent node of the rest nodes in the open set;
selecting the node with the minimum calculated value of the point, taking the node out of the open set, and adding the node into the close set;
updating the distance set S, comparing the minimum path calculation value with the value in the original distance set S, if a smaller value exists, updating and storing the minimum value into the distance set, and if the smaller value exists, keeping the minimum value;
and maintaining circulation until the target point is taken out from the open set and added into the close set, finishing the iteration of the algorithm, sequentially searching the father node of the target point from the target point until the starting point, connecting the father node and the starting point into a line to obtain the shortest path from the starting point to the target end point, and calculating the relative value of the relative node in the distance set S to obtain the shortest path value.
6. The database matching algorithm in the step eight uses an improved ICCP algorithm, and specifically comprises the following steps:
resolving pose information through IMU data carried by an underwater vehicle in a polar region, measuring gravity and depth values of the same timestamp position at the same time, taking the course as constraint, and rapidly determining a matching point by calculating a distance evaluation value function delta;
δ＝10+50*k
wherein: l isIMUResolving distance, L, for IMU data in short timecalIn order to calculate the distance between two points of the gravity and the depth of the timestamp on the corresponding contour line, k is the weight value in claim 4, and delta is the tolerance distance threshold value with the value range of 10 m-60 m.
Compared with the prior art, the invention has the beneficial effects that: the invention converts the geographical map of the polar region into the grid map, divides grid points of the grid map into 5 types according to properties, evaluates the matching degree of the 5 types of points, calculates corresponding calculation weight by combining the matching degree value, establishes a directed graph model, reasonably utilizes Dijkstra algorithm to plan a path, improves the generalization capability of path planning, plans a place with more geographical information textures of the underwater vehicle of the polar region on the basis of ensuring that the underwater vehicle of the polar region does not touch dangerous points such as an underwater iceberg and the like, and avoids the condition that the vehicle completely avoids places without gravity and depth data. The improved ICCP algorithm is used, the real-time matching speed is accelerated, the inertial navigation information can be corrected in real time, and the inertial navigation information can safely, quickly and effectively reach a designated target point, so that the polar region underwater vehicle can smoothly carry out polar region tasks.
Drawings
FIG. 1 is a flow chart of the present invention;
fig. 2 is a flow chart of the path planning algorithm of the present invention.
Detailed Description
The present invention will be described in further detail with reference to the accompanying drawings and specific embodiments. It is to be understood that the described embodiments are only some of the embodiments of the invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
With reference to fig. 1 and 2, the method divides the gravity and the depth of the polar region into a plurality of regions capable of being matched according to the data texture degree on the basis of obtaining the gravity and the depth of the polar region, establishes a directed graph model, and utilizes a Dijkstra algorithm to realize global path planning, so that the problem that an aircraft in the polar region cannot assist in correcting inertial navigation errors when the aircraft runs to a place without the gravity or depth data and avoids dangerous regions such as an underwater iceberg and the like is avoided, and further, the real-time matching speed is accelerated through an improved ICCP algorithm, and the aircraft can reach a specified position in a safe, accurate and rapid manner to perform a polar region task.
The method for planning the auxiliary navigation path of the polar region aircraft database specifically comprises the following steps:
the method comprises the following steps: measuring polar region gravity and depth data, and performing accurate interpolation on a part of sparse regions;
step two: gridding the image, constructing a grid map, and drawing gravity and depth isolines;
step three: superposing and fusing the gravity and the depth grid graph, and calculating the matchable degree of each grid window with the specified size;
because local data of the polar region is sparse and the gradient is not obvious, the method is carried out by comparing the matching degree of each selected window, namely a block matching method.
σ＝(Rlong+Rlat)/2
In the formula, RlongAnd RlatCalculating the matchable degree of the gravity value and the depth value in a selected window along the longitude direction and the latitude direction respectively, wherein sigma is the average value of the matchable degree of the gravity value and the depth value in the selected window along the longitude direction and the latitude direction, reflecting the variation amplitude of data in the window, and can be used as a factor for evaluating whether the position window can be a matchable position. m and n represent the size of the selected window grid map, g (i, j) is the gravity value of the point (i, j), and h (i, j) is the depth value of the point (i, j). If the data gradient between adjacent positions (i, j) and (i, j +1) or (i, j) and (i +1, j) in the window grid is larger, namely the data change of the adjacent positions of the window is more obvious, the larger the sigma value of the window is, namely the more suitable the window is for matching the auxiliary inertial navigation system.
Step four: dividing all grid points into five types, wherein the first type is a point with both gravity value and depth value, the second type is a point with only gravity value, the third type is a point with only depth value, the fourth type is a point without both gravity value and depth value, the fifth type is a point with dangerous route environment such as underwater iceberg, and the like, and dividing the five types of grid points into areas according to properties. The matchable degree of the fourth type of grid points is set to be one fifth of the sum of the matchable degrees of the second type of grid points and the third type of grid points, and the matchable degree of the fifth type of grid points in the dangerous airline environment such as the underwater iceberg is set to be 0;
step five: setting a matching priority, wherein the priority is in sequence of first class, second class, third class, fourth class and fifth class, a fifth class point is strictly forbidden to be planned as a airline zone class, the priority is controlled by a weight, and the matchable degree calculated in the step three participates in the weight calculation;
step six: establishing a directed graph model, and adding weights to all directed edges;
step seven: planning a path according to the current position and the target position of the underwater vehicle in the polar region, and determining the navigation route of the underwater vehicle in the polar region at a certain time;
step eight: according to the path planned by the path planning algorithm, dangerous navigation environments such as an underwater iceberg are avoided, the database navigation is used for assisting inertial navigation through a matching algorithm in the navigation process, and the long-time use error accumulation effect of an inertial navigation system is inhibited, so that the underwater vehicle in the polar region can safely, quickly and accurately reach a target point.
Example two: on the basis of the first embodiment, the path planning method in the step seven uses a Dijkstra algorithm, which specifically comprises the following steps:
first three sets are created, an open set, a close set and a distance set S. The open set stores nodes of which the shortest path is not determined, and all the nodes are initialized. The close set stores the nodes with the determined shortest path and is empty during initialization. And S sets store the shortest distance between all nodes, and when the S sets are initialized, the first value is set to be 0, and the rest are set to be infinite.
And taking out the starting point from the open set, adding the starting point into the close set, calculating the distance from each node in the open set to the starting point, and setting the starting point as a parent node of each node in the open set.
And selecting the node of the path with the minimum calculated value of the starting point, taking the node out of the open set, and adding the node into the close set.
The distance set S is updated. The minimum path calculation value is recorded in the distance set S, and the shortest path values with the starting point are set to infinity.
And starting from the node in the close set selected in the last step, calculating the distance from other points in the open set to the point, and setting the point as a parent node of the rest nodes in the open set.
And selecting the node with the minimum calculated value from the point, taking the node out of the open set, and adding the node into the close set.
The distance set S is updated. And comparing the minimum path calculation value with the value in the original distance set S, if the minimum path calculation value has a smaller value, updating and storing the minimum value into the distance set, and if the minimum path calculation value has a larger value, keeping the minimum path calculation value.
And maintaining circulation until the target point is taken out from the open set and added into the close set, finishing the iteration of the algorithm, sequentially searching the father node of the target point from the target point until the starting point, connecting the father node and the starting point into a line to obtain the shortest path from the starting point to the target end point, and calculating the relative value of the relative node in the distance set S to obtain the shortest path value.
Example two: on the basis of the first embodiment and the second embodiment, the database matching algorithm in the step eight uses an improved ICCP algorithm, and the ICCP algorithm needs to obtain rotation and translation transformation T of each iteration, so that the distance squared value function M between the transformed current iteration point set X and the closest point set Y of the X on the contour line C is minimum. The ICCP algorithm is improved, firstly, pose information is calculated through IMU data carried by an underwater vehicle in a polar region, and meanwhile, gravity and depth of the same timestamp position are measured. And (4) taking the heading as a constraint, and quickly determining a matching point by calculating a distance evaluation value function delta.
δ＝10+50*k
Wherein L isIMUResolving distance, L, for IMU data in short timecalIn order to calculate the distance between two points of the gravity and the depth of the timestamp on the corresponding contour line, k is the weight value in claim 4, and delta is the tolerance distance threshold value with the value range of 10 m-60 m.
Claims (6)
1. A method for auxiliary navigation path planning of a polar region underwater vehicle database is characterized by comprising the following steps:
the method comprises the following steps: acquiring a polar region gravity value and depth data, and performing accurate interpolation on a part of sparse intervals;
step two: gridding the image, constructing a grid map, and drawing gravity and depth isolines;
step three: superposing and fusing the gravity and depth grid graphs, and calculating the matchable degree of each grid window with the specified size;
step four: dividing all grid points into five types, and dividing the five types of grid points into regions according to properties;
step five: setting a matching priority, wherein the priority is in sequence of first class, second class, third class, fourth class and fifth class, and the fifth class point is strictly forbidden to be planned as a route region class;
step six: establishing a directed graph model, and adding weights to all directed edges;
step seven: planning a path according to the current position and the target position of the underwater vehicle in the polar region, and determining the navigation route of the underwater vehicle in the polar region at a certain time;
step eight: according to the path planned by the path planning algorithm, dangerous navigation environments such as an underwater iceberg are avoided, the database navigation is used for assisting inertial navigation through a matching algorithm in the navigation process, and the long-time use error accumulation effect of an inertial navigation system is inhibited, so that the underwater vehicle in the polar region can safely, quickly and accurately reach a target point.
2. The method for polar region underwater vehicle database aided navigation path planning as claimed in claim 1, wherein the step three of calculating the degree of matchability of each grid window with a specified size specifically comprises:
σ＝(Rlong+Rlat)/2
in the formula, RlongAnd RlatRespectively calculating the matchable degrees of the gravity value and the depth value in a selected window in the longitude direction and the latitude direction, wherein sigma is the average value of the matchable degrees of the gravity value and the depth value in the selected window in the longitude direction and the latitude direction, reflects the variation amplitude of data in the window, and can be used as a factor for evaluating whether the position window can be a matchable position; m and n representSelecting the size of a window grid map, wherein g (i, j) is the gravity value of a point (i, j), and h (i, j) is the depth value of the point (i, j); if the data gradient between adjacent positions (i, j) and (i, j +1) or (i, j) and (i +1, j) in the window grid is larger, namely the data change of the adjacent positions of the window is more obvious, the larger the sigma value of the window is, namely the more suitable the window is for matching the auxiliary inertial navigation system.
3. The method for polar region underwater vehicle database assisted navigation path planning of claim 1, wherein the classification of step four is: the first type is a point with both gravity value and depth value, the second type is a point with only gravity value, the third type is a point with only depth value, the fourth type is a point without both gravity value and depth value, and the fifth type is a point with dangerous route environment such as underwater icebergs; the matchable degree of the fourth type of grid points is set to be one fifth of the sum of the matchable degrees of the second type of grid points and the third type of grid points, and the matchable degree of the fifth type of grid points in the environment with dangerous air routes such as underwater icebergs is set to be 0.
4. The method for polar region underwater vehicle database-assisted navigation path planning as claimed in claim 1, wherein the priority of step five is controlled by a weight k, and the matchable degree of the grid window participates in the calculation of the weight k.
5. The method for the database-assisted navigation path planning for the underwater vehicle in the polar region as claimed in claim 1, wherein the path planning algorithm in the seventh step uses Dijkstra's algorithm, specifically:
firstly, creating three sets, namely an open set, a close set and a distance set S, wherein the open set stores nodes of which the shortest paths are not determined, all the nodes are stored in the close set during initialization, the nodes of which the shortest paths are determined are stored in the close set, the nodes are empty during initialization, the S set stores the shortest distances among all the nodes, the first value is set to be 0 during initialization, and the rest are set to be infinite;
taking out the starting point from the open set, adding the starting point into the close set, calculating the distance from each node in the open set to the starting point, and setting the starting point as a father node of each node in the open set;
selecting a node of a path with the minimum calculated value of the starting point, taking the node out of the open set, and adding the node into the close set;
updating the distance set S, recording the minimum path calculation value into the distance set S, and setting the shortest path values of other starting points to be infinite;
starting from the node in the selected close set, calculating the distance from other points in the open set to the point, and setting the point as a parent node of the rest nodes in the open set;
selecting the node with the minimum calculated value of the point, taking the node out of the open set, and adding the node into the close set;
updating the distance set S, comparing the minimum path calculation value with the value in the original distance set S, if a smaller value exists, updating and storing the minimum value into the distance set, and if the smaller value exists, keeping the minimum value;
and maintaining circulation until the target point is taken out from the open set and added into the close set, finishing the iteration of the algorithm, sequentially searching the father node of the target point from the target point until the starting point, connecting the father node and the starting point into a line to obtain the shortest path from the starting point to the target end point, and calculating the relative value of the relative node in the distance set S to obtain the shortest path value.
6. The method for polar region underwater vehicle database assisted navigation path planning as claimed in claim 1, wherein the database matching algorithm in step eight uses an improved ICCP algorithm, specifically:
resolving pose information through IMU data carried by an underwater vehicle in a polar region, measuring gravity and depth values of the same timestamp position at the same time, taking the course as constraint, and rapidly determining a matching point by calculating a distance evaluation value function delta;
δ＝10+50*k
wherein: l isIMUResolving distance, L, for IMU data in short timecalIn order to calculate the distance between two points of the gravity and the depth of the timestamp on the corresponding contour line, k is the weight value in claim 4, and delta is the tolerance distance threshold value with the value range of 10 m-60 m.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111357833.XA CN114088098B (en) | 2021-11-16 | 2021-11-16 | Auxiliary navigation path planning method for polar region underwater vehicle database |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111357833.XA CN114088098B (en) | 2021-11-16 | 2021-11-16 | Auxiliary navigation path planning method for polar region underwater vehicle database |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114088098A true CN114088098A (en) | 2022-02-25 |
| CN114088098B CN114088098B (en) | 2024-06-28 |
Family
ID=80301065
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111357833.XA Active CN114088098B (en) | 2021-11-16 | 2021-11-16 | Auxiliary navigation path planning method for polar region underwater vehicle database |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114088098B (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116067376A (en) * | 2023-04-06 | 2023-05-05 | 青岛哈船海智科技有限公司 | Method for analyzing and evaluating route safety of underwater carrier |
| CN116127611A (en) * | 2023-04-13 | 2023-05-16 | 中国人民解放军国防科技大学 | Dynamic simulation method for underwater vehicle |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5339684A (en) * | 1991-12-10 | 1994-08-23 | Textron Inc. | Gravity aided inertial navigation system |
| CN107957271A (en) * | 2017-11-22 | 2018-04-24 | 哈尔滨工程大学 | A kind of initial accurate alignment method for being used for underwater unmanned vehicle in polar navigation |
| CN111504302A (en) * | 2020-05-13 | 2020-08-07 | 中国人民解放军61540部队 | Gravity beacon navigation path planning method and system combining sea power information |
| CN111928850A (en) * | 2020-03-20 | 2020-11-13 | 中国科学院沈阳自动化研究所 | Combined navigation method of autonomous underwater robot suitable for environment under polar ice frame |
| CN112229404A (en) * | 2020-08-31 | 2021-01-15 | 中国空间技术研究院 | Method for improving ocean gravity field interpolation precision based on three-dimensional optimization principle of submarine topography |
-
2021
- 2021-11-16 CN CN202111357833.XA patent/CN114088098B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5339684A (en) * | 1991-12-10 | 1994-08-23 | Textron Inc. | Gravity aided inertial navigation system |
| CN107957271A (en) * | 2017-11-22 | 2018-04-24 | 哈尔滨工程大学 | A kind of initial accurate alignment method for being used for underwater unmanned vehicle in polar navigation |
| CN111928850A (en) * | 2020-03-20 | 2020-11-13 | 中国科学院沈阳自动化研究所 | Combined navigation method of autonomous underwater robot suitable for environment under polar ice frame |
| CN111504302A (en) * | 2020-05-13 | 2020-08-07 | 中国人民解放军61540部队 | Gravity beacon navigation path planning method and system combining sea power information |
| CN112229404A (en) * | 2020-08-31 | 2021-01-15 | 中国空间技术研究院 | Method for improving ocean gravity field interpolation precision based on three-dimensional optimization principle of submarine topography |
Non-Patent Citations (4)
| Title |
|---|
| WANG RUPENG ET AL.: "Underwater digital elevation map gridding method based on optimal partition of suitable matching area", 《INTERNATIONAL JOURNAL OF ADVANCED ROBOTIC SYSTEMS》, vol. 16, no. 2, pages 1 - 16 * |
| 林沂;晏磊;童庆禧;: "针对水下辅助导航相关匹配算法的特征区最优航迹规划", 吉林大学学报(工学版), no. 02, pages 193 - 197 * |
| 欧阳明达等: "基于改进A*算法的水下重力匹配导航路径规划", 《地球物理学报》, vol. 63, no. 12, pages 4361 - 4368 * |
| 黄玉龙等: "自主水下航行器导航方法综述", 《水下无人系统学报》, vol. 27, no. 3, pages 232 - 253 * |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116067376A (en) * | 2023-04-06 | 2023-05-05 | 青岛哈船海智科技有限公司 | Method for analyzing and evaluating route safety of underwater carrier |
| CN116067376B (en) * | 2023-04-06 | 2023-07-21 | 青岛哈船海智科技有限公司 | Method for analyzing and evaluating route safety of underwater carrier |
| CN116127611A (en) * | 2023-04-13 | 2023-05-16 | 中国人民解放军国防科技大学 | Dynamic simulation method for underwater vehicle |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114088098B (en) | 2024-06-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US6259988B1 (en) | Real-time mission adaptable route planner | |
| CN107314768B (en) | Underwater terrain matching auxiliary inertial navigation positioning method and positioning system thereof | |
| Szczerba et al. | Robust algorithm for real-time route planning | |
| CN109254591B (en) | Dynamic track planning method based on Anytime restoration type sparse A and Kalman filtering | |
| Han et al. | An improved TERCOM-based algorithm for gravity-aided navigation | |
| CN106643714B (en) | A kind of autonomous airborne profile aided inertial navigation method and system in real time | |
| CN107504972A (en) | A kind of aircraft's flight track method and device for planning based on dove group's algorithm | |
| CN106371445A (en) | Unmanned vehicle planning control method based on topology map | |
| CN109724592B (en) | AUV geomagnetic bionic navigation method based on evolutionary gradient search | |
| Lavis et al. | Dynamic space reconfiguration for Bayesian search and tracking with moving targets | |
| Chen et al. | Research on Ship Meteorological Route Based on A‐Star Algorithm | |
| CN113252038B (en) | Course planning terrain auxiliary navigation method based on particle swarm optimization | |
| CN114088098A (en) | Auxiliary navigation path planning method for polar region underwater vehicle database | |
| CN107525502B (en) | Method for improving inertial terrain matching navigation average precision of underwater vehicle | |
| CN110514203A (en) | A kind of underwater Combinated navigation method based on ISR-UKF | |
| CN111307143B (en) | Bionic navigation algorithm for multi-target evolution search based on geomagnetic gradient assistance | |
| Han et al. | A matching algorithm based on the nonlinear filter and similarity transformation for gravity-aided underwater navigation | |
| CN110220510A (en) | A kind of underwater robot sea-floor relief matching navigation path planning method considering map accuracy | |
| Guo et al. | Algorithm for geomagnetic navigation and its validity evaluation | |
| CN114596360A (en) | Double-stage active instant positioning and graph building algorithm based on graph topology | |
| CN109855623A (en) | Geomagnetic model online approximating method based on Legendre multinomial and BP neural network | |
| Wang et al. | Land vehicle navigation using odometry/INS/vision integrated system | |
| CN113252039B (en) | Terrain-assisted navigation-oriented particle swarm fast matching method | |
| Stepanov et al. | Algorithm for planning an informative route for map-aided navigation | |
| CN109307513B (en) | Real-time road matching method and system based on driving record |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |