CN109798899B - Tree diffusion heuristic path planning method for submarine unknown terrain search - Google Patents
Tree diffusion heuristic path planning method for submarine unknown terrain search Download PDFInfo
- Publication number
- CN109798899B CN109798899B CN201910091593.XA CN201910091593A CN109798899B CN 109798899 B CN109798899 B CN 109798899B CN 201910091593 A CN201910091593 A CN 201910091593A CN 109798899 B CN109798899 B CN 109798899B
- Authority
- CN
- China
- Prior art keywords
- point
- points
- potential
- diffusion
- path
- 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.)
- Active
Links
Images
Landscapes
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
- Information Retrieval, Db Structures And Fs Structures Therefor (AREA)
Abstract
The invention discloses a tree diffusion heuristic path planning method for submarine unknown terrain search, which comprises the following steps: firstly, modeling a map space; generating an O-XYZ coordinate system in a two-dimensional space, wherein O is an original point, X, Y, Z respectively corresponds to X, Y, Z axes, discretizing the space, and generating density pixels in the coordinate system, wherein the density represents the density, and each pixel represents a position point; dividing the position points into reachable points and unreachable points, marking the reachable points as 0 and the unreachable points as 1 in the algorithm; the invention can calculate the currently searched path information in real time in the process of gradually detecting the submarine information by the aircraft, and does not need to calculate after the detection of all the submarine information is finished, thereby saving the time wasted in the exploration process.
Description
Technical Field
The invention relates to the technical field of path planning, in particular to a tree diffusion heuristic path planning method for submarine unknown terrain search.
Background
The ocean is a huge resource treasury, contains rich biological resources, petroleum resources, combustible ice resources and mineral resources besides rich water resources, and is huge in quantity. Meanwhile, the ocean area accounts for about 70% of the earth surface area, and is an important activity space for human beings in the future. Since a plurality of centuries, the development of land resources is continuously increased by human beings, so that the land resources are gradually exhausted, and the scarce land resources are not enough to support the requirement of rapid development of the human society and have great restriction and influence on the human beings. In this regard, the world has begun to explore and develop marine resources. In the new century, countries explore the sea while developing land resources, can widen the living space of human beings, promote cooperative communication of countries, and have great development potential and strategic significance.
At present, countries compete for exploration and development of oceans, but exploration of oceans is only 5%, and 95% of sea bottoms are unknown. It can be inferred from known seafloor information that unknown marine environments tend to be complex and dangerous, and so it is of significant value to enhance the research into marine exploration equipment. With the development of science and technology, Autonomous Underwater Vehicles (AUVs) have been developed in many countries, and compared with manned submersible vehicles, AUVs have a very small size, are relatively flexible, require less energy, and can be mass produced. Each research institution collocates various detection devices, such as sensors, for the AUV, so that it can detect an unknown area; and each AUV is internally provided with a computer system, can sense complex and dangerous underwater environment and make decision and judgment, has strong intelligence of autonomous learning, and can provide efficient environment analysis for users.
In the AUV exploration process, a path planning technology is needed to guide the exploration of an aircraft, a feasible path from a starting point to a terminating point is searched for in a complex seabed environment space of the aircraft, and the path is required to have the characteristics of not touching seabed obstacles and being as short as possible in path length; for the requirements, a great deal of related path planning research is available at home and abroad. Among them, there are methods based on virtual potential field and navigation function search, such as artificial potential field method, harmonic function potential field, flow field, etc.; there are mathematical optimization based methods such as rolling optimization, linear programming, level sets, support vector machines, etc.; algorithms based on biological intelligence, such as ant colony algorithm, particle swarm algorithm, genetic and evolutionary algorithm, neural network method and the like; however, the above methods are all performed on the premise that map information is completely known, and belong to global information search, but in actual situations, it is often difficult to obtain all information of terrain, and in local information search, an aircraft is required to gradually detect to obtain the information.
Disclosure of Invention
The invention aims to overcome the defects of the prior art and provide a tree diffusion heuristic path planning method for searching for unknown terrains on the seabed.
The purpose of the invention is realized by the following technical scheme:
a tree diffusion heuristic path planning method for submarine unknown terrain search comprises the following steps:
firstly, modeling a map space; generating an O-XYZ coordinate system in a two-dimensional space, wherein O is an original point, X, Y, Z respectively corresponds to X, Y, Z axes, discretizing the space, and generating density pixels in the coordinate system, wherein the density represents the density, and each pixel represents a position point; dividing the position points into reachable points and unreachable points, marking the reachable points as 0 and the unreachable points as 1 in the algorithm; dividing a connecting line between two points into a feasible path and an infeasible path, wherein the feasible path refers to a path where the aircraft can pass, and the infeasible path refers to a path where the path can touch an obstacle; when the obstacle is analyzed, if a pixel point of the terrain space is included by the obstacle, the pixel point is marked as an obstacle point; for reachable points, the reachable points are divided into common reachable points, edge reachable points and potential reachable points, and the potential reachable points are subsets of the edge reachable points;
step two, in the face of unknown submarine space, the aircraft needs to gradually detect the terrain environment and evaluate a feasible path; the aircraft carries out detection, namely diffusion operation, of a first layer based on the starting point, records the detected obstacle points to an obstacle point set, and records the detected potential points to a potential point set;
wherein the diffusion operation specifically comprises: traversing all currently obtained pixel point information based on a diffusion point, judging whether the pixel point meets the definition of the potential point, if so, setting the potential point as a child node of the diffusion point, setting the diffusion point as a parent node of the potential point, and calculating the feasible path distance from the current potential point to a root node;
step three, entering a detection cycle, and exiting the cycle until the potential point set of the current layer is empty; in circulation, traversing all potential points of the current layer, respectively taking the potential points as diffusion points to carry out diffusion operation, setting new potential points obtained by diffusion as child nodes corresponding to the diffusion points, marking the new potential points as the next layer of the current layer, and taking the diffusion points as father nodes of the new potential points corresponding to the child nodes; because different diffusion points may diffuse to obtain the same potential point, for the condition that the same potential point has different paths reaching the starting point, only the shortest path is meaningful in practice, and therefore, the correction operation on the tree structure is utilized to achieve that any detected potential point has only one father node; after traversing the potential points of the current layer, traversing the potential points of the next layer, and jumping out of the cycle until the potential point set of the current layer is empty;
wherein the correcting operation is specifically as follows: when the diffusion point executes diffusion operation, if the searched potential reachable point has a parent node, and the feasible path distance from the potential reachable point to the root node along the original parent node is marked as oldDist, and the feasible path distance from the current diffusion point to the root node is marked as newDist, comparing the oldDist and the newDist: if the oldDist needs to be short, the parent-child relationship of the potential reachable point is unchanged; and if the newDist needs to be short, canceling the original parent-child relationship, setting the current diffusion point as the father of the potential reachable point, and setting the potential reachable point as the child of the current diffusion point.
And fourthly, finally obtaining a complete tree, wherein the root node is a starting point, the leaf nodes are potential points which can be directly connected with the termination point, traversing each leaf node, calculating the length of each path by utilizing the connection relation between the children and the father of the tree, finding out the shortest path from the paths, and ending the algorithm.
Preferably, in the loop of step three, if the distance from the current diffusion point to the root node is greater than the distance bestLen of the complete path which has reached the termination point at present, which means that the path obtained after diffusion is necessarily longer than bestLen, then the diffusion point does not perform the diffusion operation.
Preferably, in the diffusion operation, the step length of the neighborhood of the potential reachable point can be set to 1l, 2l and 3l, the neighborhood refers to a pixel point covered by a region formed by taking the current point as a central point and the distance of the step length along the longitudinal direction and the transverse direction, different step lengths of the neighborhood set influence the number of detected potential edge points, and finally influence the shortest path obtained by the algorithm.
Preferably, the specific steps of determining whether any two points in the space can be directly connected are as follows:
taking the coordinates of the point i and the point j, respectively finding the minimum value lower and the maximum value upper of the x, y and z coordinates, and then respectively taking the x, y and z coordinates from the lower by step length 1 x 、lower y And lower z Increments are made, and the other two elements affected by each increment are given by:
then the coordinates (x) obtained after increasing are processed 0 ,y 0 ,z 0 ) Reducing to the nearest position point and judging obstacle, if the top point is a non-feasible point, judging the line segment between the point i and the point j at the point (x) 0 ,y 0 ,z 0 ) When the touch is touched with an obstacle, the point i and the point j cannot be directly connected; if the connecting line does not touch the obstacle in the incremental obstacle judgment in the x direction, the y direction and the z direction, the point i and the point j can be directly connected.
Compared with the prior art, the invention has the following beneficial effects:
(1) in the process that the aircraft gradually detects the submarine information, the method can calculate the currently searched path information in real time without calculating after the detection of all the submarine information is finished, so that the time wasted in the exploration process is saved;
(2) the invention only records the edge information of the barrier, thereby greatly reducing the storage cost and improving the operation efficiency.
(3) According to the invention, not only information of the neighborhood of the current position can be detected each time, but also sonar technology of an aircraft is well attached, the terrain information can be collected as far as possible at the current position and the next position point can be analyzed and processed, so that the efficiency of exploring the terrain environment is improved;
(4) the invention can obtain a search result of a tree structure, the hierarchical structure is clear, and the user can search out the shortest path and analyze the characteristics of the terrain.
Drawings
FIG. 1 is an overall flow chart of the present invention;
FIG. 2 is a schematic diagram illustrating determination of potential reachable points according to the present invention;
FIG. 3 is a two-dimensional top view of a circular obstacle and edge point of the present invention;
FIG. 4 is a schematic diagram illustrating a method for determining whether two points are directly connected according to the present invention;
FIG. 5 is a schematic view of the diffusion operation detection of the present invention;
FIG. 6 is a diagram illustrating the neighborhood region arrangement of the present invention.
Detailed Description
The present invention will be described in further detail with reference to examples and drawings, but the present invention is not limited thereto.
The method comprises the steps of firstly modeling a map space, then detecting potential reachable points of a terrain through a sonar based on the current point of an aircraft, then detecting based on the potential reachable points each time until all the potential reachable points are detected completely, and finally calculating the shortest path through the detected potential reachable points.
Specifically, as shown in fig. 1 to 6, a heuristic path planning method for tree diffusion for submarine unknown terrain search includes the following steps:
(1) modeling a map space; generating an O-XYZ coordinate system in a two-dimensional space, wherein O is an original point, X, Y, Z corresponds to X, Y, Z axes respectively, discretizing the space, and generating density pixels in the coordinate system, wherein the density represents the density, and each pixel represents a position point. And meanwhile, dividing the position points into reachable points and unreachable points, marking the reachable points as 0 and the unreachable points as 1 in the algorithm. In addition, a connecting line between two points is divided into a feasible path and an infeasible path, wherein the feasible path refers to a path where the aircraft can pass, and the infeasible path refers to a path where the path can touch an obstacle. When the obstacle is analyzed, if the pixel point of the terrain space is included by the obstacle, the pixel point is marked as the obstacle point. Reachable points are divided into common reachable points, edge reachable points, and potential reachable points, where potential reachable points are a subset of edge reachable points. Wherein the definition of the edge reachable point is: at least one obstacle point exists in the range of the neighborhood H of the reachable point. The definition of the potential reachable point is: the reachable point is an edge reachable point, and at least one edge reachable point which cannot be directly connected with the diffusion point exists in the range of the neighborhood H. As shown in fig. 2, the current diffusion point is point a, and it is to be determined whether point B is a potential reachable point, where the point set of the current neighborhood H is C, D, E, F, G, H, I, J, and it is found that a connection line between point G and point a touches an obstacle, so that point G cannot be directly connected to point a, and point B satisfies the condition of the potential reachable point.
(2) In the face of unknown seafloor space, the vehicle needs to progressively explore the terrain environment and evaluate feasible paths. Firstly, the aircraft carries out detection of a first layer, namely diffusion operation, based on the starting point, records the detected obstacle points into an obstacle point set and records the detected potential points into a potential point set. Wherein, the diffusion operation means: based on a diffusion point, traversing all the currently obtained pixel point information, judging whether the pixel point meets the definition of the potential point, if so, setting the potential point as a child node of the diffusion point, setting the diffusion point as a father node of the potential point, and calculating the feasible path distance from the current potential point to the root node.
(3) Here, a probing loop is entered, and the process does not exit until the set of potential points of the current layer is empty. And in the circulation, traversing all potential points of the current layer, respectively taking the potential points as diffusion points to carry out diffusion operation, setting the new potential points obtained by diffusion as child nodes corresponding to the diffusion points, marking the child nodes as the next layer of the current layer, and correspondingly taking the diffusion points as father nodes of the new potential points. Since different diffusion points may diffuse to obtain the same potential point, for the case that the same potential point has different paths to the starting point, only the shortest path is meaningful in practice, so that any detected potential point has only one parent node by using the correction operation of the tree structure. And after traversing the potential point of the current layer, traversing the potential point of the next layer. And jumping out of the loop until the potential point set of the current layer is empty. In addition, in the loop, if the distance from the current diffusion point to the root node is greater than the distance bestLen of the complete path which has reached the termination point at present, which means that the path obtained after diffusion is necessarily longer than bestLen, then the diffusion operation is not executed by the diffusion point. In each diffusion operation in the cycle, a correction operation is performed: when the diffusion point executes diffusion operation, if the searched potential reachable point has a parent node, and the feasible path distance from the potential reachable point to the root node along the original parent node is marked as oldDist, and the feasible path distance from the current diffusion point to the root node is marked as newDist, comparing the oldDist and the newDist: if the oldDist needs to be short, the parent-child relationship of the potential reachable point is unchanged; and if the newDist needs to be short, canceling the original parent-child relationship, setting the current diffusion point as the father of the potential reachable point, and setting the potential reachable point as the child of the current diffusion point.
(4) And finally obtaining a complete tree, wherein the root node is a starting point, and the leaf nodes are potential points which can be directly connected with the terminating point. Then traversing each leaf node, calculating the length of each path by utilizing the connection relation between the children and the parents of the tree, finding out the shortest path from the paths, and finishing the algorithm.
In the diffusion operation of the algorithm steps (2) and (3), the step lengths of the neighborhoods of the potential reachable points can be set to be 1l, 2l and 3l, the neighborhoods refer to pixel points covered by a region which takes the current point as a central point and is formed by the distance of the step lengths along the longitudinal direction and the transverse direction, and different step lengths of the neighborhoods influence the number of the detected potential edge points and finally influence the shortest path obtained by the algorithm.
In the algorithm steps (2), (3) and (4), the judgment whether any two points i and j in the space are directly connected is used: taking the coordinates of the point i and the point j, respectively finding the minimum value lower and the maximum value upper of the x, y and z coordinates, and then respectively taking the x, y and z coordinates from the lower by step length 1 x 、 lower y And lower z Increments are made, and the other two elements affected by each increment are given by:
then the coordinates (x) obtained after increasing are processed 0 ,y 0 ,z 0 ) Reducing to the nearest position point and judging obstacle, if the top point is a non-feasible point, judging the line segment between the point i and the point j at the point (x) 0 ,y 0 ,z 0 ) When the touch is touched with an obstacle, the point i and the point j cannot be directly connected; if the connecting line does not touch the obstacle in the incremental obstacle judgment in the x direction, the y direction and the z direction, the point i and the point j can be directly connected.
The following is a specific embodiment of the present invention:
(1) modeling a map space;
an O-XYZ coordinate system is generated in a two-dimensional space, O is used as an origin, X, Y, Z corresponds to X, Y, Z axes respectively, the step length is 1, resolution is generated as (sensitivity) three-dimensional pixel points, the value ranges of x, y and z coordinates are all [0, sensitivity-1 ], the three-dimensional pixel points are initialized to be common reachable points, then the common reachable points and unreachable points are gradually divided along with the algorithm, and the reachable points are divided into common reachable points, edge reachable points and potential reachable points for analyzing and searching the shortest path. Taking the density as an example 10, assuming that there are 3 circular obstacles with the same or different radius and length, the obstacles are distributed as shown in fig. 3, the obstacle point is a solid origin in the circle, the upper triangle is a starting point, the lower triangle is a terminating point, and the small square is a potential reachable point. The four graphs are sequentially spatial information obtained by diffusion from the starting point.
Aiming at the problem that the position of one pixel needs x, y and Z coordinates, a large space is occupied, and the operation is inconvenient, the algorithm allocates a unique space number i to each pixel, wherein i belongs to Z ^ i belongs to [0, density-1], and then when the pixels are classified into different sets, the space numbers are directly stored. The formula for converting the x and y coordinates into the number i is as follows:
i＝x+y*density+z*density*density
the formula for converting the serial number i into x and y coordinates is as follows:
x＝i％density
y＝(i％(density 2 ))/density
z＝i/(density 2 )
after the map is constructed, the standard of direct connection is defined as whether a connecting line of two points does not pass through an obstacle, if not, the connecting line of the two points can be directly connected, otherwise, the connecting line of the two points is not directly connected. Because the map is modeled by discrete pixel points, and the line segment between two points is continuous, the line segment needs to be sampled by equidistant points, so that the judgment accuracy is improved as much as possible, and the increase of the time running cost is controlled, so that the two points reach a good balance. As shown in FIG. 4, the distance between adjacent points is 1, M 2 As a dot on the face ABCD, M 3 Points on the surface ADHE, M 4 For points on the surface JEHI, assume that M is to be judged 1 And M 5 Whether it can be directly connected, at line segment M 1 M 5 To M 1 And respectively increasing the step length to 1 in the x direction, the y direction and the z direction, and judging whether the points passing along the way touch the obstacle or not. Sequentially judging M from increasing in the x-axis direction 1 M 2 M 4 M 5 Whether an obstacle is touched; then increasing the y-axis direction, and sequentially judging M 1 M 5 (ii) a Finally, increasing the z-axis direction, and sequentially judging M 1 M 3 M 5 . The judged point in the mark is prevented from being repeatedly accessed next time. And when judging whether one point touches the obstacle, reducing the point to a position point closest to the point, and if the position point is the obstacle point, judging that the current point touches the obstacle.
(2) A tree diffusion heuristic method;
as shown in fig. 5, when the aircraft intends to reach the end point E from the starting point S, firstly, a barrier is found in the direction of ═ BSA by reconnaissance, and after the point S is subjected to diffusion operation, it is found that the point a and the point B are potential points, the two points are set as child nodes of the diffusion point S, fathers of the two points are set as the point S, and the feasible distances from the father to the root node S are calculated to be 0.3 respectively. The point a then continues to spread, searching for point D as a potential point and calculating the distance of the feasible path SAD from point D to the root node S, which is 0.8 in length. Then, point B performs a diffusion operation, and searches for points C and D as potential points, where point D is a potential point that has been detected, the original distance to the root node is 0.8, and the currently detected distance length of the feasible path SBD to the root node is 0.7, which is shorter than SAD, so that here a correction operation is performed, an edge AD is cut off, that is, the parent of point D is set as point B, and the feasible path length from point D to the root node is updated to be 0.7, and then the child node D of point a is deleted. And finally, when the point C and the point D are subjected to diffusion operation, the point C and the point D are both directly connected with the termination point E, and the diffusion of the two points is stopped. In the corresponding tree data structure, a point S is a root node, a point C and a point D are leaf nodes of the tree, and a termination point E is not in the tree structure; the number of layers of the dots S is 1, the number of layers of the dots a and B is 2, and the number of layers of the dots C and D is 3.
The neighborhood range setting diagram is shown in fig. 6, and it is assumed that the current diffusion point is a and the point B to be determined is a potential reachable point. Under the field setting of 1H, the neighborhood of the point B has 8 points; in the neighborhood setting of 2H, there are 24 points in the neighborhood of point B. Therefore, in the judgment of the potential reachable point, the larger the step length is, the looser the judgment is, the larger the chance of finally judging the judgment point as the potential reachable point is, the larger the grasp on the terrain information is, and the more accurate the shortest path finally searched.
(3) Potential reachable point diffusion operation;
after the map is constructed, the starting points are diffused to obtain a first batch of potential reachable points, the potential reachable points are all located on the second layer of the tree, and the first layer only has the starting points. And then entering a diffusion cycle of potential reachable points, wherein the potential reachable points are continuously increased along with the progress of diffusion operation until new potential points can not be explored any more. And the number of layers of the tree where the potential reachable points obtained by each diffusion operation are located is the next layer of the diffusion points, after all the potential reachable points of the current layer are diffused, the next layer is entered for continuous diffusion until no new potential reachable points exist in the current layer, a cycle is skipped, and then the lengths of feasible paths corresponding to all leaf nodes are calculated and shortest path information is output.
For each diffusion operation, firstly marking the current diffusion point to be diffused, then judging whether the diffusion point can be directly connected with the termination point or not, if so, marking the diffusion point to be directly connected with the termination point, then calculating the length of the complete feasible path, if the length is smaller than the length of the historical optimal feasible path, setting the length of the historical optimal feasible path as the length of the complete feasible path, and finally exiting the function; if the diffusion point can not be directly connected with the termination point, the function is continuously executed. Judging whether the feasible path length from the current diffusion point to the root node is longer than the historical optimal feasible path, if so, indicating that the complete feasible path constructed later is certainly longer than the historical optimal feasible path, and the diffusion is meaningless and exits the function; and if the distance is shorter than the historical optimal feasible path, traversing the currently acquired position points, and adding the points meeting the potential point condition into the candidate set. Setting potential reachable points which are not accessed in the candidate set as children of the diffusion points, and setting the diffusion points as parents of the diffusion points; for an accessed potential reachable point, firstly, judging whether the feasible path distance newDis from the point to a root node is smaller than the feasible path distance oldDis from the point to the root node originally, if so, executing correction operation, and executing diffusion operation on the point; if not, no operation is performed.
(4) Calculating the length of the shortest path;
after all the potential reachable points are diffused, the lengths of the corresponding feasible paths are calculated through all the leaf nodes of the tree, then each feasible path is compared, and the path with the shortest length is found out and is used as the shortest path to be output.
The invention discloses a tree diffusion path optimization-based technology, which is divided into two stages: spatial modeling and path searching. In the space modeling stage, firstly, discretization processing is carried out on a map space, each point is defined as a pixel point, the relation of edges formed between the two points is divided into connectable and non-connectable, and each feasible path is formed by the connectable edges; then, an O-XYZ coordinate system is constructed based on the discretization space, corresponding pixel points are marked as barrier points according to the shape characteristics of the barriers, the area enclosed by the barrier points is ensured to cover the area of the original barriers, and the driving safety of the aircraft is ensured; after the space modeling is completed, a tree diffusion method is applied to carry out path search; firstly, diffusing based on an initial point to obtain a first batch of potential reachable points, then entering a diffusion cycle, diffusing the points in a potential reachable point set one by one, diffusing the points of the next layer after the potential reachable points of the current layer in the corresponding tree structure are diffused, and stopping diffusion until the potential reachable point set of the current layer is empty; and finally, calculating the length of the corresponding complete feasible path through leaf nodes of the tree, comparing all paths, outputting the shortest path, and finishing the algorithm. Compared with the existing algorithm for solving the path planning problem, the method provided by the invention has the advantages of high efficiency, high speed and the like, can calculate the currently searched path information in real time in the process of gradually detecting the submarine information by the aircraft, does not need to calculate after the detection of all the submarine information is finished, and saves the time wasted in the exploration process; only the edge information of the barrier is recorded, so that the storage cost can be greatly reduced, and the operation efficiency is improved; the system does not only detect the information of the neighborhood of the current position every time, but well fits the sonar technology of the aircraft, can collect the terrain information as far as possible at the current position and analyze and process the situation of the next position point, thereby improving the efficiency of exploring the terrain environment; the search result of a tree structure can be obtained, the hierarchical structure is clear, and users can search out the shortest path and analyze the characteristics of the terrain from the shortest path.
The present invention is not limited to the above embodiments, and any other changes, modifications, substitutions, combinations, and simplifications which do not depart from the spirit and principle of the present invention should be construed as equivalents and are included in the scope of the present invention.
Claims (4)
1. A tree diffusion heuristic path planning method for submarine unknown terrain search is characterized by comprising the following steps:
firstly, modeling a map space; generating an O-XYZ coordinate system in a two-dimensional space, wherein O is an original point, X, Y, Z respectively corresponds to X, Y, Z axes, discretizing the space, and generating density pixels in the coordinate system, wherein the density represents the density, and each pixel represents a position point; dividing the position points into reachable points and unreachable points, marking the reachable points as 0 and the unreachable points as 1 in the algorithm; dividing a connecting line between two points into a feasible path and an infeasible path, wherein the feasible path refers to a path where the aircraft can pass, and the infeasible path refers to a path where the path can touch an obstacle; when the obstacle is analyzed, if a pixel point of the terrain space is included by the obstacle, the pixel point is marked as an obstacle point; for reachable points, the reachable points are divided into common reachable points, edge reachable points and potential reachable points, and the potential reachable points are subsets of the edge reachable points;
step two, in the face of unknown submarine space, the aircraft needs to gradually detect the terrain environment and evaluate a feasible path; the aircraft carries out detection, namely diffusion operation, of a first layer based on the starting point, records the detected obstacle points to an obstacle point set, and records the detected potential points to a potential point set;
wherein the diffusion operation specifically comprises: traversing all currently obtained pixel point information based on a diffusion point, judging whether the pixel point meets the definition of the potential point, if so, setting the potential point as a child node of the diffusion point, setting the diffusion point as a parent node of the potential point, and calculating the feasible path distance from the current potential point to a root node;
step three, entering a detection cycle, and exiting the cycle until the potential point set of the current layer is empty; in circulation, traversing all potential points of the current layer, respectively taking the potential points as diffusion points to carry out diffusion operation, setting new potential points obtained by diffusion as child nodes corresponding to the diffusion points, marking the new potential points as the next layer of the current layer, and taking the diffusion points as father nodes of the new potential points corresponding to the child nodes; because different diffusion points may diffuse to obtain the same potential point, for the condition that the same potential point has different paths reaching the starting point, only the shortest path is meaningful in practice, and therefore, the correction operation on the tree structure is utilized to achieve that any detected potential point has only one father node; after traversing the potential points of the current layer, traversing the potential points of the next layer, and jumping out of the cycle until the potential point set of the current layer is empty;
wherein the correcting operation is specifically as follows: when the diffusion point executes diffusion operation, if the searched potential reachable point has a parent node, and the feasible path distance from the potential reachable point to the root node along the original parent node is marked as oldDist, and the feasible path distance from the current diffusion point to the root node is marked as newDist, comparing the oldDist and the newDist: if the oldDist needs to be short, the parent-child relationship of the potential reachable point is unchanged; if the newDist needs to be short, the original parent-child relationship is cancelled, the current diffusion point is set as the father of the potential reachable point, and the potential reachable point is set as the child of the current diffusion point;
and fourthly, finally obtaining a complete tree, wherein the root node is a starting point, the leaf nodes are potential points which can be directly connected with the termination point, traversing each leaf node, calculating the length of each path by utilizing the connection relation between the children and the father of the tree, finding out the shortest path from the paths, and ending the algorithm.
2. The method for planning the tree diffusion heuristic path facing the submarine unknown terrain search according to claim 1, wherein in the loop of the third step, if the distance from the current diffusion point to the root node is greater than the distance bestLen of the complete path which has reached the termination point at present, which means that the path obtained after diffusion is necessarily longer than bestLen, then the diffusion operation is not executed for the diffusion point.
3. The tree diffusion heuristic path planning method facing to seabed unknown terrain search of claim 1, is characterized in that in the diffusion operation, the step length of a neighborhood of the potential reachable point is set to be 1l, 2l or 3l, the neighborhood refers to a pixel point covered by a region formed by taking the current point as a center point and the distance of the step length along the longitudinal direction and the transverse direction, different step length settings of the neighborhood affect the number of detected potential edge points, and finally affect the shortest path obtained by the algorithm.
4. The tree diffusion heuristic path planning method for searching for unknown topography at the sea floor as claimed in claim 1, wherein the specific step of determining whether any two points in space can be directly connected is as follows:
taking the coordinates of the point i and the point j, respectively finding the minimum value lower and the maximum value upper of the x, y and z coordinates, and then respectively taking the x, y and z coordinates from the lower by step length 1 x 、lower y And lower z Increments are made, and the other two elements affected by each increment are given by:
then the coordinates (x) obtained after increasing are processed 0 ,y 0 ,z 0 ) Reducing to the nearest position point and judging obstacles if the coordinate (x) 0 ,y 0 ,z 0 ) If it is a non-feasible point, then the line segment between point i and point j is determined to be at point (x) 0 ,y 0 ,z 0 ) When the touch is touched with an obstacle, the point i and the point j cannot be directly connected; if the connecting line does not touch the obstacle in the incremental obstacle judgment in the x direction, the y direction and the z direction, the point i and the point j can be directly connected.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910091593.XA CN109798899B (en) | 2019-01-30 | 2019-01-30 | Tree diffusion heuristic path planning method for submarine unknown terrain search |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910091593.XA CN109798899B (en) | 2019-01-30 | 2019-01-30 | Tree diffusion heuristic path planning method for submarine unknown terrain search |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109798899A CN109798899A (en) | 2019-05-24 |
| CN109798899B true CN109798899B (en) | 2022-08-02 |
Family
ID=66559370
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910091593.XA Active CN109798899B (en) | 2019-01-30 | 2019-01-30 | Tree diffusion heuristic path planning method for submarine unknown terrain search |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109798899B (en) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110220510B (en) * | 2019-06-03 | 2023-03-31 | 哈尔滨工程大学 | Underwater robot submarine topography matching navigation path planning method considering map accuracy |
| CN110081889A (en) * | 2019-06-11 | 2019-08-02 | 广东工业大学 | A kind of robot path planning method based on stochastical sampling and intensified learning |
| CN113716001B (en) * | 2021-11-04 | 2022-01-18 | 单县多米石墨烯科技有限公司 | Underwater robot system based on power supply of graphene electric brush |
Family Cites Families (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8626444B2 (en) * | 2009-03-16 | 2014-01-07 | The Hong Kong University Of Science And Technology | Safety based road map navigation |
| CN105487535A (en) * | 2014-10-09 | 2016-04-13 | 东北大学 | Mobile robot indoor environment exploration system and control method based on ROS |
| CN104950883A (en) * | 2015-05-14 | 2015-09-30 | 西安电子科技大学 | Mobile robot route planning method based on distance grid map |
| CN106444769B (en) * | 2016-10-31 | 2019-05-21 | 湖南大学 | A kind of optimum path planning method of indoor mobile robot increment type environmental information sampling |
| CN107085437A (en) * | 2017-03-20 | 2017-08-22 | 浙江工业大学 | A kind of unmanned aerial vehicle flight path planing method based on EB RRT |
| CN107368086B (en) * | 2017-07-04 | 2020-09-11 | 哈尔滨工程大学 | Unmanned underwater vehicle path planning device and method based on detection threat domain |
| CN107990903B (en) * | 2017-12-29 | 2021-01-05 | 东南大学 | Indoor AGV path planning method based on improved A-x algorithm |
| CN108268042A (en) * | 2018-01-24 | 2018-07-10 | 天津大学 | A kind of path planning algorithm based on improvement Visual Graph construction |
| CN108871364A (en) * | 2018-06-28 | 2018-11-23 | 南京信息工程大学 | A kind of underwater robot paths planning method based on Node Algorithm |
| CN108983781B (en) * | 2018-07-25 | 2020-07-07 | 北京理工大学 | Environment detection method in unmanned vehicle target search system |
| CN108981710B (en) * | 2018-08-07 | 2019-10-11 | 北京邮电大学 | A kind of complete coverage path planning method of mobile robot |
-
2019
- 2019-01-30 CN CN201910091593.XA patent/CN109798899B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN109798899A (en) | 2019-05-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Li et al. | Path planning technologies for autonomous underwater vehicles-a review | |
| Han et al. | Ant-colony-based complete-coverage path-planning algorithm for underwater gliders in ocean areas with thermoclines | |
| CN108680163B (en) | Unmanned ship path searching system and method based on topological map | |
| CN109798899B (en) | Tree diffusion heuristic path planning method for submarine unknown terrain search | |
| CN110887484A (en) | Mobile robot path planning method based on improved genetic algorithm and storage medium | |
| CN103744428B (en) | A kind of unmanned surface vehicle paths planning method based on neighborhood intelligent water drop algorithm | |
| Guo et al. | Research progress of path planning methods for autonomous underwater vehicle | |
| Macwan et al. | Target-motion prediction for robotic search and rescue in wilderness environments | |
| CN110471426A (en) | Unmanned intelligent vehicle automatic Collision Avoidance method based on quantum wolf pack algorithm | |
| CN108981716A (en) | A kind of paths planning method suitable for inland and coastal waters unmanned boat | |
| Xu et al. | Heuristic and random search algorithm in optimization of route planning for Robot’s geomagnetic navigation | |
| CN112612290A (en) | Underwater vehicle three-dimensional multi-task path planning method considering ocean currents | |
| Du et al. | An optimized path planning method for coastal ships based on improved DDPG and DP | |
| Yang et al. | Cooperative survey of seabed ROIs using multiple USVs with coverage path planning | |
| Wang et al. | Using DTW to measure trajectory distance in grid space | |
| Cui et al. | Multi-strategy adaptable ant colony optimization algorithm and its application in robot path planning | |
| CN112215395B (en) | Underwater equipment adaptability information guarantee system based on ocean big data | |
| Wang et al. | Research on autonomous planning method based on improved quantum Particle Swarm Optimization for Autonomous Underwater Vehicle | |
| CN109977455B (en) | Ant colony optimization path construction method suitable for three-dimensional space with terrain obstacles | |
| CN116952239A (en) | Unmanned ship path planning method based on fusion of improved A and DWA | |
| CN110716547A (en) | 3D exploration method based on wavefront algorithm | |
| CN114397894B (en) | Mobile robot target searching method imitating human memory | |
| CN113110530B (en) | Underwater robot path planning method for three-dimensional environment | |
| Li et al. | Comparison of biological swarm intelligence algorithms for AUVs for three-dimensional path planning in ocean currents’ conditions | |
| Czyzowicz et al. | Optimal exploration of terrains with obstacles |
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 |