Underwater Glider Path Planning Method Based on Improved RRT* Algorithm

Abstract

The present invention discloses a method for underwater glider path planning based on an improved RRT* algorithm, comprising: S1, preparing depth average flow data for a rectangular sea trial area; S2, gridding the rectangular sea trial area; S3, extracting a preferred sampling area for the RRT* algorithm within the rectangular sea trial area; and S4, looping through the following "path acquisition method" based on the preferred sampling area, obtaining a path after each execution of the "path acquisition method," and outputting the current optimal path when the number of iterations reaches K, where the number of iterations is the sum of the number of times step 1 is executed during the loop execution of the "path acquisition method." The underwater glider path planning method of the present invention reorganizes edges based on motion time, improving exploration efficiency, and proposes a "method for determining the validity of paths between adjacent path points," ensuring the feasibility and applicability of the outputted current optimal path in a marine environment.

Description

Technical Field

[0001] The present invention belongs to the technical field of underwater gliders, and in particular relates to an underwater glider path planning method based on an improved RRT* algorithm. Background Art

[0002] Underwater gliders are a new type of underwater vehicle with advantages such as low power consumption, long range, extended operating time, and high stealth. Therefore, underwater gliders offer significant advantages when performing long-term marine resource exploration missions. To improve the efficiency of underwater glider reconnaissance and exploration, path planning plays a crucial role.

[0003] Currently, path planning algorithms for traditional mobile robots mainly include graph search algorithms, biological intelligence algorithms, machine learning, and rapidly expanding random trees (RRT) algorithms. Among them, the A* and D* graph search algorithms are not suitable for large-scale or high-precision grid maps, and their runtime increases dramatically as the map size increases. Commonly used algorithms in biological intelligence algorithms include the ant colony algorithm (ACO) and the particle swarm optimization algorithm (PSO). The ACO is easily integrated with ocean currents and has obvious advantages for shortest path planning, but it suffers from slow convergence, long computation time, and a tendency to get stuck in local optima. The particle swarm optimization algorithm suffers from premature convergence due to the lack of information exchange between particles. In machine learning, reinforcement learning, as a path planning technique, often achieves the maximum-reward path, but it suffers from numerous issues such as dimensionality explosion, high experimental costs, and mathematical model uncertainty. The RRT algorithm generates sampling points, constructs edges between the root node, sampling points, and the target point, and ultimately generates an efficient path from the starting point to the destination. The RRT* algorithm introduces ChooseParent and Rewire based on the Rapidly Expanding Random Tree (RRT) algorithm to optimize the edge structure of the Rapidly Expanding Random Tree during the process of randomly generating sampling points. ChooseParent and Rewire are two built-in functions of the RRT* algorithm.

[0004] However, the RRT* algorithm performs path planning with the goal of progressively searching for the shortest collision-free path. It does not take into account the impact of ocean currents on path planning and whether the solved path meets the conditions for the navigation or passage of underwater gliders. Therefore, the RRT* algorithm cannot meet the path planning needs of underwater gliders in complex ocean environments. Summary of the Invention

[0005] In view of the shortcomings of the existing technology, the purpose of the present invention is to provide an underwater glider path planning method based on an improved RRT* algorithm.

[0006] The present invention is achieved through the following technical solutions.

[0007] The underwater glider path planning method based on the improved RRT* algorithm includes the following steps:

[0008] S1. Prepare depth-averaged current data for a rectangular sea trial area. The depth-averaged current data includes velocity vectors of coordinate points within the rectangular sea trial area. The velocity vector of each coordinate point includes the velocity magnitude and direction of the coordinate point. The direction is the average of the current velocity directions at different depths for the coordinate point, and the velocity magnitude is the average of the current velocity magnitudes at different depths for the coordinate point.

[0009] S2, grid the rectangular sea trial area and get x in the longitude direction. num The same grid sea area, in the latitude direction to obtain y num the same grid sea area; wherein the average value of the depth average flow data of the four vertices of each grid sea area is used as the depth average flow data of the grid sea area;

[0010] S3, extract the preferred sampling area of the RRT* algorithm in the rectangular sea trial area;

[0011] S4, based on the preferred sampling area, execute the following "method for obtaining a path" cyclically, obtain a path after each execution of the "method for obtaining a path", and output the current best path when the number of iterations reaches K, wherein, after each execution of the "method for obtaining a path", compare the time consumed by the path obtained by the execution of the "method for obtaining a path" with the path with the shortest time consumed among all paths generated by the previous "method for obtaining a path", retain the path with the shortest time as the current best path, and use the random search tree obtained by each execution of the "method for obtaining a path" for the next execution of the "method for obtaining a path"; wherein, the number of iterations is the total number of times step 1 is executed during the cyclic execution of the "method for obtaining a path", and K is the set upper limit of the number of iterations, i.e., the maximum number of iterations;

[0012] The methods for obtaining the path include:

[0013] Step 1: Determine the sampling point x rand :The target point x goal As the sampling point x rand Or randomly select a point in the preferred sampling area as the sampling point x ran ,

[0014] Step 2: Based on the RRT* algorithm, the starting point x init As the root node of the random search tree, the distance from the sampling point x in the random search tree rand The nearest point is taken as the sampling point x rand The nearest neighbor node x nearest , where, according to the sampling point x rand and the nearest neighbor node xnearest The direction vector and expansion step s, determine x new The longitude and latitude coordinates of x nearest to x new Validity of the path between: If x nearest to x new The path between them is valid, and x nearest As x new The parent node of x new and its parent node x parent Make a connection if x nearest to x new If the path between is not valid, return to step 1 and execute again; nearest and x new As adjacent path points, judge x according to the "method for judging the validity of the path between adjacent path points" nearest to x new the validity of the paths between them;

[0015] Step 3: Update x according to the impact of ocean current I new The parent node x parent , including steps 3-1 and 3-2:

[0016] Step 3-1, with x new Construct x as the center and R as the radius new The circular neighborhood of the random search tree is used as x new The set of neighboring nodes of n is the number of nodes in the random search tree, c is a constant that is set according to the application scenario, and d is the dimension of the search space in the RRT* algorithm;

[0017] Step 3-2, traverse x new The neighborhood node set of x new And each neighboring node x in the neighboring node set near As adjacent path points, according to the “method for judging the validity of paths between adjacent path points”, each neighboring node x in the neighboring node set is judged near to x new Is the path valid, and make the neighborhood node x corresponding to the valid path nea Form a valid neighborhood node set; calculate each neighborhood node x in the valid neighborhood node set near Corresponding ocean current influence degree I total , where I total =I1+I2, I1 is the neighboring node x near to x newThe degree of influence of the ocean current on the path, I2 is the degree of influence from the starting point x init Move through the current random search tree node to the neighboring node x near The sum of the ocean current influence degrees of all paths between adjacent nodes in the path; select the largest ocean current influence degree I in the valid neighborhood node set total The corresponding neighboring node x near As x new The parent node x parent , and x new and its parent node x parent Perform connections where each node in the random search tree has exactly one parent node;

[0018] Step 4: Remove x from the valid neighborhood node set in step 3-2. new Each x outside the parent node near The calculation is performed according to the reorganization edge method, which includes steps 4-1 and 4-2:

[0019] Step 4-1, use the "method for calculating the movement time of the path between adjacent path points" to calculate the time from the starting point x init Move through the current random search tree node to x near The movement time t1 is calculated by using the “method for calculating the movement time of the path between adjacent path points”. init Move through the current random search tree node to x new Then from x new Move along the straight line to x near Movement time t2;

[0020] Step 4-2, if t1>t2, then let x near Disconnect from its parent node and change x new As the x near The parent node of x new With the x near Connect and update the current random search tree. near The judgment method ends; if t1≤t2, the current random search tree is not updated, and the x near The judgment method ends;

[0021] Step 5: Determine x new and the target point x goal Is the Euclidean distance between them less than the distance threshold d? threshhold :If x ne and the target point x goal The Euclidean distance between them is greater than or equal to the distance threshold d threshhold , then return to step 1; if x new and the target point x goalThe Euclidean distance between them is less than the distance threshold d threshhold , then go to step 6;

[0022] Step 6: According to the “method for judging the validity of paths between adjacent path points”, determine the path from x new To target point x goal Validity of the path: If from x new To target point x goal If the path is valid, then x new As the target point x goal The parent node of x new and the target point x goal Make a connection and update the current random search tree; if new To target point x goal If the path is invalid, then the Monte Carlo circular area sampling strategy is used to new and the target point x goal Supplement the path points between them, and add the supplemented path points and their corresponding edges to the random search tree, and update the current random search tree;

[0023] Step 7: In the random search tree obtained in step 6, the target point x goal As the current path point, find the parent node of the current path point according to the random search tree as the next current path point, continue to find the parent node of the next current path point, and repeat the above operation until the starting point x is traced back. init So far, we get the value from the target point x goal To starting point x init All path points from the starting point x init To target point x goal A path.

[0024] In the above technical solution, S3 includes: S3-1, S3-2, S3-3, S3-4 and S3-5:

[0025] S3-1, set the starting point x of the underwater glider mission in the rectangular sea trial area init and the target point x goal , set the starting point x of the underwater glider's mission init and the target point x of the underwater glider mission goal The direction of the line is used as the direction vector Fixed starting point x init and direction vector Rotate clockwise and counterclockwise by ω, and the angle range enclosed is the threshold range ω threshhold , extract the threshold range ω threshhold All coordinate points within the rectangular sea trial area are formed into an array;

[0026] S3-2, including S3-2-1, S3-2-2, S3-2-3, S3-2-4 and S3-2-5, the specific steps are as follows:

[0027] S3-2-1, let j = 1;

[0028] S3-2-2, create an empty cluster C j ;

[0029] S3-2-3, select any coordinate point in the array Array and add it to cluster C j , and delete the coordinate point in the array Array, and use the deleted array Array as the array Array′, making i=1;

[0030] S3-2-4, calculate the coordinates of the i-th point in the array Array′ to the cluster C j The Euclidean distance of all coordinate points in the , and the distance set d is obtained i ;

[0031] S3-2-5, determine the distance set d i Is there a distance less than the set threshold l threshold Euclidean distance: If it exists, add the i-th coordinate point in array Array′ to cluster C j and keep the array Array′ unchanged, execute S3-2-6; if it does not exist, execute S3-2-6 directly;

[0032] S3-2-6, let the value of i increase by 1. If i is less than or equal to the total number of coordinate points in the array Array′, then execute S3-2-4 to S3-2-5; if i is greater than the total number of coordinate points in the array Array′, then delete the array Array′ that is related to cluster C. j For all the same coordinate points, delete the array Array′ and use it as the array Array″. Clear the array Array in S3-2-3 and assign all the coordinate points in the array Array″ to the array Array in S3-2-3. Add 1 to the value of j and repeat steps S3-2-2 to S3-2-6 until the array Array″ is empty. At this time, j=J, and J clusters C1...C are obtained. J ;

[0033] S3-3, J clusters C1...C J As input, repeat the following cluster merging method until the number of clusters no longer changes, and obtain J′ merged clusters, wherein the cluster merging method includes: if the Euclidean distance between two coordinate points in any two clusters is less than the set distance threshold l threshold , then merge the two clusters;

[0034] S3-4, cluster selection: select the starting point x of the task that also includes the underwater glider in J′ merged clusters init and the target point x goal A cluster of is taken as the target cluster;

[0035] S3-5, based on the alpha shapes algorithm, the outer contour of the target cluster is extracted, and the inner area of the outer contour of the target cluster is used as the preferred sampling area.

[0036] In the above technical solution, the specific operations of step 1 in the method for obtaining the path are as follows:

[0037] Generate a random number rand in the interval [0,1] and compare the size of the generated random number rand with the preset threshold p: if the random number rand is less than the threshold p, then the target point x goal As the sampling point x rand If the random number rand is greater than or equal to the preset threshold p, random sampling is performed in the rectangular sea trial area to obtain the sampling point x rand , determine the sampling point x rand Is it located in the preferred sampling area? If the sampling point x rand If it is not in the preferred sampling area, random sampling will be carried out again in the rectangular sea trial area until the sampling point x rand Located within the preferred sampling area.

[0038] In the above technical solution, in step 3-2 of the method for obtaining a path, the calculation formula for calculating the ocean current influence degree I of the path between two nodes in the random search tree is as follows:

[0039]

[0040] Where m is the number of grid sea areas that the path between two nodes passes through, θ k The angle d represents the angle between the forward direction of the path between the two nodes in the kth square sea area among all the square sea areas passed by the path between the two nodes and the direction corresponding to the depth average flow data of the kth square sea area. k represents the path length between two nodes in the kth square sea area, v k Indicates the velocity of the depth-averaged flow data of the k-th square sea area passed by the path between two nodes.

[0041] In the above technical solution, in step 6 of the method for obtaining the path, the Monte Carlo circular area sampling strategy is used to obtain the path of the new and the target point x goal The specific steps to add path points are as follows:

[0042] x new To the target point xgoal The number of square sea areas that the path passes through is a, let Q be x new To the target point x goal The first intersection of the path and the grid sea boundary of its invalid sub-path; according to the "method for judging the validity of the path between adjacent path points", determine x new Is the path to Q valid? If so, execute (1); if not, execute (2);

[0043] (1) Take Q as the path point and add another path point M so that the path x new Q, path QM, path Mx goal All are valid, and the current random search tree is updated. The specific steps for obtaining the path point M are as follows:

[0044] With Q and x goal The midpoint of the line is the center of the circle, Q and x goal The length of the connecting line is used as the radius to construct a circular sampling area, and the Monte Carlo algorithm is used to sample points in the circular sampling area to obtain points that meet the requirements as path points M, while discarding points that do not meet the requirements and taking path points M as the target point x goal The parent node of the target point x goal Connect to path point M, make path point Q the parent node of path point M, connect path point M with path point Q, and connect x new As the parent node of the path point Q, make the path point Q and x new connect;

[0045] (2) By increasing the path point M, x new The path to M is valid, and M to x goal The path is valid, and the specific steps to update the current random search tree and obtain the path point M are as follows:

[0046] x new and x goal The midpoint of the connecting line is the center of the circle, x new and x goal The length of the connecting line is used as the radius to construct a circular sampling area, and the Monte Carlo algorithm is used to sample points in the circular sampling area to obtain points that meet the requirements as path points M, while discarding points that do not meet the requirements and taking path points M as the target point x goal The parent node of the target point x goal Connect with the path point M and change x new As the parent node of the path point M, make the path point M and x new connect.

[0047] In the above technical solution, the method for determining the validity of the path between adjacent path points is:

[0048] (1) Determine whether the path between adjacent path points passes through an inaccessible area. If so, the path between the adjacent path points is invalid; if not, perform the following steps:

[0049] (2) The part of the path that passes through each grid sea area is regarded as a sub-path, and e = 1;

[0050] (3) Calculate the v of the e-th sub-path according to the speed calculation formula f and θ ug , where the speed calculation formula is as follows:

[0051] v ug ×cosθ ug +v cur ×cosθ cur =v f ×cosθ f

[0052] v ug ×sinθ ug +v cur ×sinθ cur =v f ×sinθ f ,

[0053] in, is the velocity vector of the underwater glider in the horizontal direction of the e-th sub-path, v ug for Medium speed magnitude, θ ug for The angle between the mid-direction and the east direction; is the depth average flow data of the grid sea area where the e-th sub-path is located, v cur for Medium speed magnitude, θ cur for The angle between the mid-direction and the east direction; for and The resultant velocity vector, v f for Medium speed magnitude, θ f for The angle between the mid-direction and the east direction;

[0054] In the above formula, v ug 、v cur ,θ cur ,θ f All are known quantities, and v can be calculated using the known quantities. f and θ ug ;

[0055] (4), judge v f Is it greater than 0: If v f ≤0, then the e-th sub-path is invalid;

[0056] (5), if v f >0, then the e-th sub-path is valid. Let the value of e be increased by 1, and repeat (3)-(4) until e is greater than the number of sub-paths in the path. If all sub-paths in the path are valid, then the path is valid. Among them, the inaccessible area is the sea area with shallow seabed depth or extreme environment.

[0057] The present invention has the following advantages due to the adoption of the above technical solution:

[0058] 1. The underwater glider path planning method of the present invention ensures that the sampling points avoid the influence of the upstream sea area as much as possible by extracting the preferred sampling area of the RRT* algorithm in the rectangular sea trial area, solves the problem of blind sampling of the RRT* algorithm in the marine environment, and effectively improves the sampling efficiency of the underwater glider path planning method of the present invention.

[0059] 2. The underwater glider path planning method of the present invention introduces the ocean current influence degree I and uses the ocean current influence degree I as a powerful basis for selecting the parent node of the random search tree, thereby guiding the links of the random search tree according to the ocean current influence degree. The underwater glider path planning method of the present invention reorganizes the links based on the movement time, thereby improving the exploration efficiency.

[0060] 3. The underwater glider path planning method of the present invention, based on consideration of inaccessible areas, proposes a "method for determining the validity of the path between adjacent path points", ensuring the feasibility and applicability of the path obtained by the underwater glider path planning method of the present invention in the marine environment.

[0061] 4. The underwater glider path planning method of the present invention proposes a Monte Carlo circular area sampling strategy for the invalid path between the last two path points in the path planned by the present invention, so as to make the path planned by the present invention valid. This solves the problem that the traditional RRT* algorithm does not consider whether the path is valid (whether it is passable) in the sea environment, and improves the planning efficiency of the underwater glider path planning method of the present invention.

[0062] 5. The underwater glider path planning method of the present invention optimizes the planned path based on the path consumption time, ensuring that the planned path better meets actual needs, and improving the applicability of the underwater glider path planning method of the present invention in the marine environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0063] Figure 1 is a flow chart of the underwater glider path planning method of the present invention;

[0064] Figure 2 For the determination of the present invention new Schematic diagram of;

[0065] Figure 3 The x of the present invention new Get the schematic diagram of the waypoint M when the path to Q is valid;

[0066] Figure 4 The x of the present invention new Get the schematic diagram of the waypoint M when the path to Q is invalid;

[0067] Figure 5 A velocity vector diagram of the underwater glider of the present invention in the horizontal direction;

[0068] Figure 6 This is a schematic diagram of the preferred sampling area extraction of the present invention;

[0069] Figure 7 This is a simulation diagram of the underwater glider path planning method of the present invention. DETAILED DESCRIPTION

[0070] The underwater glider path planning method based on the improved RRT* algorithm of the present invention is described in detail below with reference to the accompanying drawings and embodiments.

[0071] Example 1

[0072] like Figure 1 As shown in FIG, the underwater glider path planning method based on the improved RRT* algorithm includes the following steps:

[0073] S1. Prepare depth-averaged current data for a rectangular sea trial area. The depth-averaged current data includes velocity vectors of coordinate points within the rectangular sea trial area. The velocity vector of each coordinate point includes the velocity magnitude and direction of the coordinate point. The direction is the average of the current velocity directions at different depths for the coordinate point, and the velocity magnitude is the average of the current velocity magnitudes at different depths for the coordinate point.

[0074] The method for obtaining depth-averaged flow data includes: preparing a data set D of a rectangular sea trial area, wherein the data set D includes the longitude, latitude, depth, current velocity, and current velocity direction of different coordinate points at a certain point in time, calculating the average value of the current velocity and the average value of the current velocity direction at different depths for each coordinate point as the velocity vector of the coordinate point, and then obtaining the depth-averaged flow data of the rectangular sea trial area.

[0075] S2, grid the rectangular sea trial area and get x in the longitude direction. mum The same grid sea area, in the latitude direction to obtain y numThe same grid sea area is used. The average of the depth average flow data of the four vertices of each grid sea area is used as the depth average flow data of the grid sea area. The grid processing makes the depth average flow data of different coordinate points more convenient for the path planning algorithm to call.

[0076] S3, based on the depth average flow data of the rectangular sea trial area and the starting point x of the underwater glider's mission init and the target point x of the underwater glider mission goal , extract the preferred sampling areas of the RRT* algorithm in the rectangular sea trial area, including: S3-1, S3-2, S3-3, S3-4 and S3-5:

[0077] S3-1, set the starting point x of the underwater glider mission in the rectangular sea trial area init and the target point x goal , set the starting point x of the underwater glider's mission init and the target point x of the underwater glider mission goal The direction of the line is used as the direction vector Fixed starting point x init and direction vector Rotate clockwise and counterclockwise by ω, and the angle range enclosed is the threshold range ω threshhold , extract the threshold range ω threshhold All coordinate points within the rectangular sea trial area are formed into an array;

[0078] S3-2, including S3-2-1, S3-2-2, S3-2-3, S3-2-4 and S3-2-5, the specific steps are as follows:

[0079] S3-2-1, let j = 1;

[0080] S3-2-2, create an empty cluster C j ;

[0081] S3-2-3, select any coordinate point in the array Array and add it to cluster C j , and delete the coordinate point in the array Array, and use the deleted array Array as the array Array′, making i=1;

[0082] S3-2-4, calculate the coordinates of the i-th point in the array Array′ to the cluster C j The Euclidean distance of all coordinate points in the , and the distance set d is obtained i ;

[0083] S3-2-5, determine the distance set d i Is there a distance less than the set threshold l thresholdEuclidean distance: If it exists, add the i-th coordinate point in array Array′ to cluster C j and keep the array Array′ unchanged, execute S3-2-6; if it does not exist, execute S3-2-6 directly;

[0084] S3-2-6, let the value of i increase by 1. If i is less than or equal to the total number of coordinate points in the array Array′, then execute S3-2-4 to S3-2-5; if i is greater than the total number of coordinate points in the array Array′, then delete the array Array′ that is related to cluster C. j For all the same coordinate points, delete the array Array′ and use it as the array Array″. Clear the array Array in S3-2-3 and assign all the coordinate points in the array Array″ to the array Array in S3-2-3. Add 1 to the value of j and repeat steps S3-2-2 to S3-2-6 until the array Array″ is empty. At this time, j = J, and J clusters C1...C are obtained. J ;

[0085] Step S3-2 can be replaced by the DBSCAN algorithm (for DBSCAN algorithm, see: Deng, Dingsheng. "DBSCAN clustering algorithm based on density." 2020 7th International Forum on Electrical Engineering and Automation (IFEEA). IEEE, 2020.) to achieve the same effect and obtain J clusters C1...C J .

[0086] S3-3, J clusters C1...C J As input, repeat the following cluster merging method until the number of clusters no longer changes, and obtain J′ merged clusters, wherein the cluster merging method includes: if the Euclidean distance between two coordinate points in any two clusters is less than the set distance threshold l threshold , then merge the two clusters;

[0087] S3-4, cluster selection: select the starting point x of the task that also includes the underwater glider in J′ merged clusters init and the target point x goal A cluster of is taken as the target cluster;

[0088] S3-5, based on the alpha shapes algorithm (Edelsbrunner H, Kirkpatrick D, Seidel R. On the shape of a set of points in the plane [J]. IEEE Transactions on information theory, 1983, 29 (4): 551-559.), the outer contour of the target cluster is extracted, and the inner area of the outer contour of the target cluster is used as the preferred sampling area.

[0089] In step S3, extracting the preferred sampling area of the RRT* algorithm in the rectangular sea trial area can ensure that the sampling points avoid the influence of the upstream sea area as much as possible, while solving the disadvantage of global blind sampling of the traditional RRT* algorithm.

[0090] S4, based on the preferred sampling area, execute the following "method for obtaining a path" cyclically, and obtain a path after each execution of the "method for obtaining a path". When the number of iterations reaches K, output the current best path, wherein, after each execution of the "method for obtaining a path", compare the time consumed by the path obtained by the execution of the "method for obtaining a path" with the path with the shortest time consumed among all the paths generated by the previous "method for obtaining a path", and retain the path with the shortest time as the current best path. The random search tree obtained by each execution of the "method for obtaining a path" is used for the next execution of the "method for obtaining a path"; wherein, the number of iterations is the total number of times step 1 is executed during the cyclic execution of the "method for obtaining a path", and K is the set upper limit of the number of iterations, that is, the maximum number of iterations.

[0091] The methods for obtaining the path include:

[0092] Step 1: Determine the sampling point x rand :The target point x goal As the sampling point x rand Or randomly select a point in the preferred sampling area as the sampling point x rand The specific operation is as follows: Generate a random number rand in the interval [0,1] and compare the size relationship between the generated random number rand and the preset threshold p:

[0093] If the random number rand is less than the threshold p, the target point x goal As the sampling point x rand ;

[0094] If the random number rand is greater than or equal to the preset threshold p, random sampling is performed in the rectangular sea trial area to obtain the sampling point x rand , determine the sampling point x rand Is it located in the preferred sampling area? If the sampling point x randIf it is not in the preferred sampling area, random sampling will be carried out again in the rectangular sea trial area until the sampling point x rand located within the preferred sampling area;

[0095] Sampling is performed within the preferred sampling area, avoiding the drawbacks of global blind sampling in the traditional RRT* algorithm, thereby improving the sampling efficiency in ocean current environments.

[0096] Among them, the sampling point x rand Generation strategy:

[0097]

[0098] Where rand is a random number in the interval [0,1], and p is the preset threshold;

[0099] Determine the sampling point x rand For methods to determine whether a point is within the preferred sampling area, that is, to determine whether a point is inside a polygon, see: WRFranklin, Pnpoly-point inclusion in polygon test, Web site: http: / / www.ecse.rpi.edu / Homepages / wrf / Research / Short_Notes / pnpoly.html, (2006);

[0100] Step 2: Based on the RRT* algorithm, the starting point x init As the root node of the random search tree, the distance from the sampling point x in the random search tree rand The nearest point is taken as the sampling point x rand The nearest neighbor node x nearest , where, according to the sampling point x rand and the nearest neighbor node x nearest The direction vector and expansion step s (with the nearest node x nearest The starting point along the direction vector Move in the direction of expansion step s to get x new ), determine x new The longitude and latitude coordinates of Figure 2 As shown, judge x nearest to x new Validity of the path between: If x nearest to x new The path between them is valid, and x nearest As x new The parent node of x new and its parent node x parent Make a connection if x nearest to x newIf the path between is not valid, return to step 1 and execute again; nearest and x ne As adjacent path points, judge x according to the "method for judging the validity of the path between adjacent path points" nearest to x new the validity of the paths between them;

[0101] Step 3: Update x according to the impact of ocean current I new The parent node x parent , including steps 3-1 and 3-2:

[0102] Step 3-1, with x new Construct x as the center and R as the radius new The circular neighborhood of the random search tree is used as x new The set of neighboring nodes of n is the number of nodes in the random search tree, c is a constant that is set according to the application scenario, and d is the dimension of the search space in the RRT* algorithm;

[0103] Step 3-2, traverse x new The neighborhood node set of x new And each neighboring node x in the neighboring node set near As adjacent path points, according to the “method for judging the validity of paths between adjacent path points”, each neighboring node x in the neighboring node set is judged near to x new Is the path valid, and make the neighborhood node x corresponding to the valid path nea Form a valid neighborhood node set; calculate each neighborhood node x in the valid neighborhood node set near Corresponding ocean current influence degree I total , where I total =I1+I2, I1 is the neighboring node x near to x new The degree of influence of the ocean current on the path, I2 is the degree of influence from the starting point x init Move through the current random search tree node to the neighboring node x near The sum of the ocean current influence degrees of the paths between all adjacent nodes in the path, that is, the ocean current influence degree I2 is the sum of the ocean current influence degrees I of the paths between multiple nodes; select the largest ocean current influence degree I in the valid neighborhood node set total The corresponding neighboring node x near As x new The parent node x parent , and x new and its parent node x parentPerform connections where each node in the random search tree has exactly one parent node;

[0104] The calculation formula for calculating the ocean current influence degree I of the path between two nodes in the random search tree is as follows:

[0105]

[0106] Where m is the number of grid sea areas that the path between two nodes passes through, θ k The angle d represents the angle between the forward direction of the path between the two nodes in the kth square sea area among all the square sea areas passed by the path between the two nodes and the direction corresponding to the depth average flow data of the kth square sea area. k represents the path length between two nodes in the kth square sea area, v k Indicates the velocity of the depth-averaged flow data of the k-th square sea area passed by the path between two nodes.

[0107] In step 3-2, under the premise of satisfying the path validity, the degree of the path affected by the ocean current I is used as the selected x new The parent node x parent Based on this, the influence of ocean current on the path is introduced into the RRT* algorithm.

[0108] Step 4: Remove x from the valid neighborhood node set in step 3-2. new Each x outside the parent node near The calculation is performed according to the reorganization edge method, which includes steps 4-1 and 4-2:

[0109] Step 4-1, use the "method for calculating the movement time of the path between adjacent path points" to calculate the time from the starting point x init Move through the current random search tree node to x near The movement time t1 is calculated by using the “method for calculating the movement time of the path between adjacent path points”. init Move through the current random search tree node to x new Then from x new Move along the straight line to x near Movement time t2;

[0110] Step 4-2, if t1>t2, then let x near Disconnect from its parent node and change x new As the x near The parent node of x new With the x near Connect and update the current random search tree. nearThe judgment method ends; if t1≤t2, the current random search tree is not updated, and the x near The judgment method ends.

[0111] Step 5: Determine x new and the target point x goal Is the Euclidean distance between them less than the distance threshold d? threshhold :If x ne and the target point x goal The Euclidean distance between them is greater than or equal to the distance threshold d threshhold , then return to step 1; if x new and the target point x goal The Euclidean distance between them is less than the distance threshold d threshhold , then go to step 6;

[0112] Step 6: According to the “method for judging the validity of paths between adjacent path points”, determine the path from x new To target point x goal Validity of the path: If from x new To target point x goal If the path is valid, then x new As the target point x goal The parent node of x new and the target point x goal Make a connection and update the current random search tree; if new To target point x goal If the path is invalid, then the Monte Carlo circular area sampling strategy is used to new and the target point x goal Supplement the path points between them, and add the supplemented path points and their corresponding edges to the random search tree, and update the current random search tree;

[0113] Among them, the Monte Carlo circular area sampling strategy proposed in this invention is new and the target point x goal The specific steps to add path points are as follows:

[0114] x new To the target point x goal The number of square sea areas that the path passes through is a, let Q be x new To the target point x goal The first intersection of the path and the grid sea boundary of its invalid sub-path; according to the "method for judging the validity of the path between adjacent path points", determine x new Is the path to Q valid? If so, execute (1); if not, execute (2);

[0115] (1), such as Figure 3As shown, Q is used as the path point and the path point M is added so that the path x new Q, path QM, path Mx goal All are effective (effective paths such as Figure 3 The “feasible trajectory” in the figure is updated, and the specific steps for obtaining the path point M are as follows:

[0116] With Q and x goal The midpoint of the line is the center of the circle, Q and x goal The length of the connecting line is used as the radius to construct a circular sampling area, and the Monte Carlo algorithm (Metropolis, Nicholas, et al. "Equation of state calculations by fast computing machines." The journal of chemical physics 21.6 (1953): 1087-1092.) is used to sample points in the circular sampling area that meet the requirements as path points M, while discarding points that do not meet the requirements and taking path points M as the target point x. goal The parent node of the target point x goal Connect to path point M, make path point Q the parent node of path point M, connect path point M with path point Q, and connect x new As the parent node of the path point Q, make the path point Q and x new connect;

[0117] (2), such as Figure 4 As shown, by increasing the path point M, x new The path to M is valid, and M to x goal The path is valid (valid path is as follows Figure 4 The specific steps to update the current random search tree and obtain the path point M are as follows:

[0118] x new and x goal The midpoint of the connecting line is the center of the circle, x new and x goal The length of the connecting line is used as the radius to construct a circular sampling area, and the Monte Carlo algorithm is used to sample points in the circular sampling area to obtain points that meet the requirements as path points M, while discarding points that do not meet the requirements and taking path points M as the target point x goal The parent node of the target point x goal Connect with the path point M and change x new As the parent node of the path point M, make the path point M and x new connect;

[0119] The Monte Carlo circular area sampling strategy solves the problem of invalid paths between the last two path points by adding more path points to ensure the feasibility of the trajectory.

[0120] Step 7: In the random search tree obtained in step 6, the target point x goal As the current path point, find the parent node of the current path point according to the random search tree as the next current path point, continue to find the parent node of the next current path point, and repeat the above operation until the starting point x is traced back. init So far, we get the value from the target point x goal To starting point x init All path points from the starting point x init To target point x goal a path;

[0121] The method for determining the validity of paths between adjacent path points is as follows:

[0122] (1) Determine whether the path between adjacent path points passes through an inaccessible area. If so, the path between the adjacent path points is invalid; if not, perform the following steps:

[0123] (2) The part of the path that passes through each grid sea area is regarded as a sub-path, and e = 1;

[0124] (3) Calculate the v of the e-th sub-path according to the speed calculation formula f and θ ug , where the speed calculation formula is as follows:

[0125] v ug ×cosθ ug +v cur ×cosθ cur =v f ×cosθ f

[0126] v ug ×sinθ ug +v cur ×sinθ cur =v f ×sinθ f ,

[0127] like Figure 5 The figure shows the velocity vector diagram of the underwater glider in the horizontal plane of the e-th sub-path, where: is the velocity vector of the underwater glider in the horizontal direction of the e-th sub-path, v ug for Medium speed magnitude, θ ug for The angle between the mid-direction and the east direction; is the depth average flow data of the grid sea area where the e-th sub-path is located, v cur for Medium speed magnitude, θ cur for The angle between the mid-direction and the east direction; for and The resultant velocity vector, v f for Medium speed magnitude, θ f for The angle between the mid-direction and the east direction;

[0128] In the above formula, v ug 、v cur ,θ cur ,θ f All are known quantities, and v can be calculated using the known quantities. f and θ ug ;

[0129] (4), judge v f Is it greater than 0: If v f ≤0, then the e-th sub-path is invalid;

[0130] (5), if v f >0, then the e-th subpath is valid. Let the value of e be increased by 1, and repeat (3)-(4) until e is greater than the number of subpaths in the path. If all subpaths in the path are valid, the path is valid.

[0131] Among them, impassable areas are sea areas with shallow seabed depths or extreme environments;

[0132] The method for calculating the movement time of the path between adjacent path points is: according to the v corresponding to each sub-path f The straight-line distance between the sub-path and the sub-path is used to calculate the movement time of the sub-path; the movement time corresponding to all sub-paths in the path is added up to obtain the movement time of the path between adjacent path points.

[0133] Example 2

[0134] The current optimal path (K=100) is obtained according to the underwater glider path planning method based on the improved RRT* algorithm in Example 1. The dataset D is from the Ocean University of China. The parameters set by the underwater glider path planning method are shown in Table 1. The type of underwater glider is Tiantong 1300 underwater glider. The preferred sampling area obtained based on the rectangular sea trial area is as follows: Figure 6As shown in Table 1, the underwater glider path planning method of the present invention is used to simulate the underwater glider path planning according to the parameters in Table 1. The running results are shown in FIG. Figure 7 As shown, Figure 7 The “planned path” is the current optimal path output when the number of iterations is 100 in the underwater glider path planning method of the present invention.

[0135] Table 1

[0136]

[0137] Example 3

[0138] Underwater glider path planning was performed according to the underwater glider path planning method based on the improved RRT* algorithm in Example 1 and the RRT* algorithm (Karaman, Sertac, and Emilio Frazzoli. "Sampling-based algorithms for optimal motion planning." The international journal of robotics research 30.7 (2011): 846-894.), respectively, to obtain the current optimal path (K = 300). The parameters set for underwater glider path planning are shown in Table 2. The obtained path planning results are shown in Table 3.

[0139] Data set D comes from the experimental data of the Hycom website (experimental data: the magnitude and direction of the ocean current speed at different depths for each coordinate point at different longitudes and latitudes). The date of the experimental data is August 7, 2024. A rectangular sea trial area downstream, a rectangular sea trial area upstream, a rectangular sea trial area downstream with an impassable area, and a rectangular sea trial area upstream with an impassable area are selected as scenes 1, 2, 3, and 4, respectively. The underwater glider path planning method and the RRT* algorithm of the present invention are used to plan the underwater glider path in different scenes. The maximum number of iterations K of the underwater glider path planning method and the RRT* algorithm of the present invention in the four scenes is 300. In summary, the underwater glider path planning method of the present invention has certain advantages in average running time and path passability probability index.

[0140] Table 2

[0141]

[0142] Table 3

[0143]

[0144]

[0145] The above is an exemplary description of the present invention. It should be noted that, without departing from the core of the present invention, any simple deformation, modification or other equivalent replacement that can be made by other skilled in the art without expending creative labor falls within the scope of protection of the present invention.

Claims

1. The underwater glider path planning method based on the improved RRT* algorithm is characterized by: The following steps are involved: S1. Prepare depth-averaged current data for a rectangular sea trial area. The depth-averaged current data includes velocity vectors of coordinate points within the rectangular sea trial area. The velocity vector of each coordinate point includes the velocity magnitude and direction of the coordinate point. The direction is the average of the current velocity directions at different depths for the coordinate point, and the velocity magnitude is the average of the current velocity magnitudes at different depths for the coordinate point. S2, grid the rectangular sea trial area and get x in the longitude direction. num The same grid sea area, in the latitude direction to obtain y num the same grid sea area; wherein the average value of the depth average flow data of the four vertices of each grid sea area is used as the depth average flow data of the grid sea area; S3, extract the preferred sampling area of the RRT* algorithm in the rectangular sea trial area; S4, looping through the "method for obtaining a path" based on the preferred sampling area, obtaining a path after each execution of the "method for obtaining a path", and outputting the current best path when the number of iterations reaches K. After each execution of the "method for obtaining a path", the time consumed by the path obtained by that execution of the "method for obtaining a path" is compared with the path with the shortest time consumed among all paths previously generated by the "method for obtaining a path", and the path with the shortest time is retained as the current best path. The random search tree obtained by each execution of the "method for obtaining a path" is used for the next execution of the "method for obtaining a path". The number of iterations is the total number of times step 1 is executed during the loop execution of the "method for obtaining a path", and K is a set upper limit for the number of iterations. The methods for obtaining the path include: Step 1: Determine the sampling point x rand :The target point x goal As the sampling point x rand Or randomly select a point in the preferred sampling area as the sampling point x rand ; Step 2: Based on the RRT* algorithm, the starting point x init As the root node of the random search tree, the distance from the sampling point x in the random search tree rand The nearest point is taken as the sampling point x rand The nearest neighbor node x nearest , where, according to the sampling point x rand and the nearest neighbor node x nearest The direction vector and expansion step s, determine x new The longitude and latitude coordinates of x nearest to x new Validity of the path between: If x nearest to x new The path between them is valid, and x nearest As x new The parent node of x new and its parent node x parent Make a connection if x nearest to x new If the path between is not valid, return to step 1 and execute again; nearest and x new As adjacent path points, judge x according to the "method for judging the validity of the path between adjacent path points" nearest to x new the validity of the paths between them; Step 3: Update x according to the impact of ocean current I new The parent node x parent , including steps 3-1 and 3-2: Step 3-1, with x new Construct x as the center and R as the radius new The circular neighborhood of the random search tree is used as x new The set of neighboring nodes of n is the number of nodes in the random search tree, c is a constant, and d is the dimension of the search space in the RRT* algorithm; Step 3-2, traverse x new The neighborhood node set of x new And each neighboring node x in the neighboring node set near As an adjacent path point, according to the "method for judging the validity of the path between adjacent path points", each neighboring node x in the neighboring node set is judged near to x new Is the path valid, and make the neighborhood node x corresponding to the valid path nea Form a valid neighborhood node set; calculate each neighborhood node x in the valid neighborhood node set near Corresponding ocean current influence degree I total , where I total =I1+I2, I1 is the neighboring node x near to x new The degree of influence of the ocean current on the path, I2 is the degree of influence from the starting point x init Move through the current random search tree node to the neighboring node x near The sum of the ocean current influence degrees of all paths between adjacent nodes in the path; select the largest ocean current influence degree I in the valid neighborhood node set total The corresponding neighboring node x near As x new The parent node x parent , and x new and its parent node x parent Perform connections where each node in the random search tree has exactly one parent node; Step 4: Remove x from the valid neighborhood node set in step 3-2. new Each x outside the parent node near Calculation is performed using the reorganization edge method, which includes steps 4-1 and 4-2: Step 4-1, use the "method for calculating the movement time of the path between adjacent path points" to calculate the time from the starting point x init Move through the current random search tree node to x near The movement time t1 is calculated by using the "method for calculating the movement time of the path between adjacent path points" init Move through the current random search tree node to x new Then from x new Move along the straight line to x near Movement time t2; Step 4-2, if t1>t2, then let x near Disconnect from its parent node and change x new As the x near The parent node of x new With the x near Connect and update the current random search tree. near The judgment method ends; if t1≤t2, the current random search tree is not updated, and the x near The judgment method ends; Step 5: Determine x new and the target point x goal Is the Euclidean distance between them less than the distance threshold d? threshhold :If x ne and the target point x goal The Euclidean distance between them is greater than or equal to the distance threshold d threshhold , then return to step 1; if x new and the target point x goal The Euclidean distance between them is less than the distance threshold d threshhold , then go to step 6; Step 6: According to the "method for determining the validity of the path between adjacent path points", determine the path from x new To target point x goal Validity of the path: If from x new To target point x goal If the path is valid, then x new As the target point x goal The parent node of x new and the target point x goal Make a connection and update the current random search tree; if new To target point x goal If the path is invalid, then the Monte Carlo circular area sampling strategy is used to new and the target point x goal Supplement the path points between them, and add the supplemented path points and their corresponding edges to the random search tree, and update the current random search tree; Step 7: In the random search tree obtained in step 6, the target point x goal As the current path point, find the parent node of the current path point according to the random search tree as the next current path point, continue to find the parent node of the next current path point, and repeat the above operation until the starting point x is traced back. init So far, we get the value from the target point x goal To starting point x init All path points from the starting point x init To target point x goal A path.

2. The underwater glider path planning method based on the improved RRT* algorithm according to claim 1 is characterized in that: S3 Includes: S3-1, S3-2, S3-3, S3-4 and S3-5: S3-1, set the starting point x of the underwater glider mission in the rectangular sea trial area init and the target point x goal , set the starting point x of the underwater glider's mission init and the target point x of the underwater glider mission goal The direction of the line is used as the direction vector Fixed starting point x init and direction vector Rotate clockwise and counterclockwise by ω, and the angle range enclosed is the threshold range ω threshhold , extract the threshold range ω threshhold All coordinate points within the rectangular sea trial area are formed into an array; S3-2, including S3-2-1, S3-2-2, S3-2-3, S3-2-4 and S3-2-5, the specific steps are as follows: S3-2-1, let j = 1; S3-2-2, create an empty cluster C j ; S3-2-3, select any coordinate point in the array Array and add it to cluster C j , and delete the coordinate point in the array Array, and use the deleted array Array as the array Array′, making i=1; S3-2-4, calculate the coordinates of the i-th point in the array Array′ to the cluster C j The Euclidean distance of all coordinate points in the , and the distance set d is obtained i ; S3-2-5, determine the distance set d i Is there a distance less than the set threshold l threshold Euclidean distance: If it exists, add the i-th coordinate point in array Array′ to cluster C j and keep the array Array′ unchanged, execute S3-2-6; if it does not exist, execute S3-2-6 directly; S3-2-6, let the value of i increase by 1. If i is less than or equal to the total number of coordinate points in the array Array′, then execute S3-2-4 to S3-2-5; If i is greater than the total number of coordinate points in the array Array′, then delete the points in the array Array′ that are related to cluster C. j For all the same coordinate points, delete the array Array′ and use it as the array Array″. Clear the array Array in S3-2-3 and assign all the coordinate points in the array Array″ to the array Array in S3-2-3. Add 1 to the value of j and repeat steps S3-2-2 to S3-2-6 until the array Array″ is empty. At this time, j=J, and J clusters C1...C are obtained. J ; S3-3, J clusters C1...C J As input, repeat the following cluster merging method until the number of clusters no longer changes, and obtain J′ merged clusters, wherein the cluster merging method includes: if the Euclidean distance between two coordinate points in any two clusters is less than the set distance threshold l threshold , then merge the two clusters; S3-4, cluster selection: select the starting point x of the task that also includes the underwater glider in J′ merged clusters init and the target point x goal A cluster of is taken as the target cluster; S3-5, based on the alpha shapes algorithm, the outer contour of the target cluster is extracted, and the inner area of the outer contour of the target cluster is used as the preferred sampling area.

3. The underwater glider path planning method based on the improved RRT* algorithm according to claim 1 is characterized in that: Step 1 of the method for obtaining the path is as follows: Generate a random number rand in the interval [0,1] and compare the size of the generated random number rand with the preset threshold p: if the random number rand is less than the threshold p, then the target point x goal As the sampling point x rand If the random number rand is greater than or equal to the preset threshold p, random sampling is performed in the rectangular sea trial area to obtain the sampling point x rand , determine the sampling point x rand Is it located in the preferred sampling area? If the sampling point x rand If it is not in the preferred sampling area, random sampling will be carried out again in the rectangular sea trial area until the sampling point x rand Located within the preferred sampling area.

4. The underwater glider path planning method based on the improved RRT* algorithm according to claim 1, characterized in that: In step 3-2 of the method for obtaining a path, the calculation formula for calculating the ocean current influence degree I of the path between two nodes in the random search tree is as follows: Where m is the number of grid sea areas that the path between two nodes passes through, θ k The angle d represents the angle between the forward direction of the path between the two nodes in the kth square sea area among all the square sea areas passed by the path between the two nodes and the direction corresponding to the depth average flow data of the kth square sea area. k represents the path length between two nodes in the kth square sea area, v k Indicates the velocity of the depth-averaged flow data of the k-th square sea area passed by the path between two nodes.

5. The underwater glider path planning method based on the improved RRT* algorithm according to claim 1, characterized in that: In step 6 of the path acquisition method, the Monte Carlo circular area sampling strategy is used to obtain the path of the new and the target point x goa The specific steps to add path points are as follows: x new To the target point x goal The number of square sea areas that the path passes through is a, let Q be x new To the target point x goal The first intersection of the path and the grid sea boundary of its invalid sub-path; According to the "method for judging the validity of paths between adjacent path points", x is judged. new Is the path to Q valid? If so, execute (1); if not, execute (2); (1) Take Q as the path point and add the path point M so that the path x new Q, path QM, path Mx goal All are valid, and the current random search tree is updated. The specific steps for obtaining the path point M are as follows: With Q and x goal The midpoint of the line is the center of the circle, Q and x goal The length of the connecting line is used as the radius to construct a circular sampling area, and the Monte Carlo algorithm is used to sample points in the circular sampling area to obtain points that meet the requirements as path points M, while discarding points that do not meet the requirements and taking path points M as the target point x goal The parent node of the target point x goal Connect to path point M, make path point Q the parent node of path point M, connect path point M with path point Q, and connect x new As the parent node of the path point Q, make the path point Q and x new connect; (2) By increasing the path point M, x new The path to M is valid, and M to x goal The path is valid, and the specific steps to update the current random search tree and obtain the path point M are as follows: x new and x goal The midpoint of the connecting line is the center of the circle, x new and x goal The length of the connecting line is used as the radius to construct a circular sampling area, and the Monte Carlo algorithm is used to sample points in the circular sampling area to obtain points that meet the requirements as path points M, while discarding points that do not meet the requirements and taking path points M as the target point x goal The parent node of the target point x goal Connect with the path point M and change x new As the parent node of the path point M, make the path point M and x new connect.

6. The underwater glider path planning method based on the improved RRT* algorithm according to claim 1, characterized in that: The method for judging the validity of the path between adjacent path points is: (1) Determine whether the path between adjacent path points passes through an inaccessible area. If so, the path between the adjacent path points is invalid; if not, perform the following steps: (2) The part of the path that passes through each grid sea area is regarded as a sub-path, and e = 1; (3) Calculate the v of the e-th sub-path according to the speed calculation formula f and θ ug , The speed calculation formula is as follows: v ug ×cosθ ug +v cur ×cosθ cur =v f ×cosθ f v ug ×sinθ ug +v cur ×sinθ cur =v f ×sinθ f , in, is the velocity vector of the underwater glider in the horizontal direction of the e-th sub-path, v ug for Medium speed magnitude, θ ug for The angle between the mid-direction and the east direction; is the depth average flow data of the grid sea area where the e-th sub-path is located, v cur for Medium speed magnitude, θ cur for The angle between the mid-direction and the east direction; for and The resultant velocity vector, v f for Medium speed magnitude, θ f for The angle between the mid-direction and the east direction; In the above formula, v ug 、v cur ,θ cur ,θ f All are known quantities, and v can be calculated using the known quantities. f and θ ug ; (4), judge v f Is it greater than 0: If v f ≤0, then the e-th sub-path is invalid; (5), if v f >0, then the e-th sub-path is valid. Let the value of e be increased by 1, and repeat (3)-(4) until e is greater than the number of sub-paths in the path. If all sub-paths in the path are valid, then the path is valid. Among them, the inaccessible area is the sea area with shallow seabed depth or extreme environment.

Images