Underwater glider path planning method based on improved RRT* algorithm

Abstract

The invention discloses an underwater glider path planning method based on an improved RRT * algorithm. The method comprises the following steps: S1, preparing depth average flow data of a rectangular sea test area; s2, performing gridding processing on the rectangular sea test area; s3, extracting a preference sampling region of an RRT * algorithm in the rectangular sea test region; and S4, loop execution is carried out according to the following'path obtaining method 'based on the preference sampling area, a path is obtained after the'path obtaining method' is executed each time, when the number of iterations reaches K, the current optimal path is output, and the number of iterations is the sum of the number of times executed in the step 1 in the loop execution process of the'path obtaining method '. According to the underwater glider path planning method, recombination and edge connection are carried out based on the motion time, the exploration efficiency is improved, a method for judging the effectiveness of the path between the adjacent path points is provided, and the feasibility and applicability of the output current optimal path in the sea area environment are guaranteed.

Description

Technical Field

[0001] The 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 the advantages of low power consumption, long range, long working time, and high concealment. Therefore, underwater gliders have significant advantages in performing long-term marine resource exploration missions. In order to improve the reconnaissance and detection efficiency of underwater gliders, path planning plays a vital role.

[0003] At present, the path planning algorithms for traditional mobile robots mainly include graph search algorithms, biological intelligence algorithms, machine learning, and rapidly expanding random tree (RRT) algorithms. Among them, the graph search algorithms of A* and D* are not suitable for large-scale or high-precision grid maps. As the map size increases, the running time increases sharply; the commonly used algorithms in biological intelligence algorithms mainly include ant colony optimization (ACO) and particle swarm optimization (PSO). The ant colony algorithm is easy to combine with ocean currents and has obvious advantages for shortest path planning, but it has problems such as slow convergence speed, long calculation time, and easy to fall into local optimality. The particle swarm optimization algorithm has the problem of premature convergence due to the lack of information interaction between particles; reinforcement learning as a technical means of path planning in machine learning can usually obtain the effect of the maximum benefit path, but there are many problems such as dimensionality explosion, high experimental cost, and uncertainty of mathematical models. The rapidly expanding random tree algorithm generates sampling points, constructs the edges between the root node, sampling points and target points, and then generates an effective path from the starting point to the end point. 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 in 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 navigation or passage conditions of the underwater glider. Therefore, the RRT* algorithm cannot meet the needs of path planning for underwater gliders in complex ocean environments. Summary of the invention

[0005] In view of the deficiencies in the prior art, the object 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 the depth average flow data of the rectangular sea trial area, the depth average flow data includes: the velocity vectors of the coordinate points in the rectangular sea trial area, the velocity vector of each coordinate point includes the velocity magnitude and direction of the coordinate point, wherein the direction is the average value of the direction of the current velocity of the coordinate point at different depths, and the velocity magnitude is the average value of the magnitude of the current velocity of the coordinate point at different depths;

[0009] S2, grid the rectangular sea trial area and obtain x in the longitude direction. num The same grid sea area, in the latitude direction to obtain y num the same square sea area; wherein the average value of the depth average flow data of the four vertices of each square sea area is taken as the depth average flow data of the square 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, the following “method for obtaining a path” is executed cyclically, and a path is obtained after each execution of the “method for obtaining a path”. When the number of iterations reaches K, the current best path is output, wherein, after each execution of the “method for obtaining a path”, the time consumed by the path obtained by the execution of the “method for obtaining a path” is compared with the path with the shortest time consumed among all the paths generated by the previous “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”; 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;

[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 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 With its parent node x parent Connect 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, x is determined according to the method of "determining 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 currents I new The parent node x parent , including step 3-1 and step 3-2:

[0016] Step 3-1, with x new Construct x as the center and R as the radius new The circular neighborhood of , takes all nodes in the random search tree and within the circular neighborhood 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 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 newThe degree of influence of the ocean current on the path, I2 is the degree of influence of the ocean current on the path 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 With its parent node x parent Make connections, where each node in the random search tree has only 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 nodes of the current random search tree to x near The movement time t1 is calculated by using the method of calculating the movement time of the path between adjacent path points. init Move through the nodes of the current random search tree 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, 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 With the target point x goal Is the Euclidean distance between them less than the distance threshold d? threshhold :If x ne With 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 With the target point x goalThe Euclidean distance between them is less than the distance threshold d threshhold , then execute step 6;

[0022] Step 6: According to the method of judging the validity of the path between adjacent path points, determine the 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 With the target point x goal Connect and update the current random search tree; if from x new To target point x goal If the path is invalid, then the Monte Carlo circular area sampling strategy is used to new With 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, place 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 you trace back to the starting point x init So far, we get 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 , 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 used as the threshold range ω threshhold , extract the threshold range ω threshhold All coordinate points in the rectangular sea trial area form 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 , and the distance set d 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, directly execute S3-2-6;

[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 cluster C in the array Array′. j For all the same coordinate points, delete the array Array′ 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, let the value of j increase by 1, and repeat steps S3-2-2 to S3-2-6 until the array Array″ is empty, at which 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 does not change, 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 screening: 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, extract the outer contour of the target cluster based on the alpha shapes algorithm, and use the inner area of ​​the outer contour of the target cluster as the preferred sampling area.

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

[0037] Generate a random number rand in the interval [0,1], and compare 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 performed 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 square sea areas that the path between two nodes passes through, θ k The angle between the direction of the path between the two nodes in the k-th 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 k-th square sea area, d k represents the path length between two nodes in the kth square sea area, v k It represents the velocity of the depth average 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 sample the x new With 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" to 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 that meet the requirements in the circular sampling area as path points M. At the same time, points that do not meet the requirements are discarded and path points M are used as target points 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 to path point Q, and connect x new As the parent node of the waypoint Q, make the waypoint 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 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 that meet the requirements in the circular sampling area as path points M. At the same time, points that do not meet the requirements are discarded and path points M are used as target points x. goal The parent node of the target point x goal Connect to the path point M and convert 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 square sea area is regarded as a sub-path, and e = 1;

[0050] (3) Calculate the v of the e-th subpath 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 plane of the e-th subpath, 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. Calculate v using the known quantities. f and θ ug ;

[0055] (4), determine 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 sea 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 edges of the random search tree according to the ocean current influence degree. The underwater glider path planning method of the present invention reorganizes the edges 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, thereby solving the problem that the traditional RRT* algorithm does not consider whether the path is valid (whether it is passable) in the sea environment, and improving 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 the actual needs, thereby improving the applicability of the underwater glider path planning method of the present invention in the sea 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 a schematic diagram of the waypoint M when the path to Q is valid;

[0066] Figure 4 The x of the present invention new A schematic diagram of obtaining a 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 A schematic diagram of the preferred sampling area extraction of the present invention;

[0069] Figure 7 It 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 in conjunction with the accompanying drawings and embodiments.

[0071] Example 1

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

[0073] S1, prepare the depth average flow data of the rectangular sea trial area, the depth average flow data includes: the velocity vectors of the coordinate points in the rectangular sea trial area, the velocity vector of each coordinate point includes the velocity magnitude and direction of the coordinate point, wherein the direction is the average value of the direction of the current velocity of the coordinate point at different depths, and the velocity magnitude is the average value of the magnitude of the current velocity of the coordinate point at different depths;

[0074] The method for obtaining depth average 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 speed and current speed direction of different coordinate points at a certain point in time, calculating the average value of the current speed and the average value of the current speed direction of each coordinate point at different depths as the velocity vector of the coordinate point, and then obtaining the depth average flow data of the rectangular sea trial area.

[0075] S2, grid the rectangular sea trial area and obtain 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, wherein 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 gridding process makes the depth average flow data of different coordinate points easier to call the path planning algorithm.

[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 , 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 used as the threshold range ω threshhold , extract the threshold range ω threshhold All coordinate points in the rectangular sea trial area form 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 , and the distance set d 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, directly execute S3-2-6;

[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 cluster C in the array Array′. j For all the same coordinate points, delete the array Array′ 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 (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 does not change, 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 screening: 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.), extract the outer contour of the target cluster and use the inner area of ​​the outer contour of the target cluster 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 drawback 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 current execution of the "method for obtaining a path" with the path with the shortest time consumed among all the paths previously generated by the "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 performed again in the rectangular sea trial area until the sampling point x rand located within the preferred sampling area;

[0095] Sampling is performed in the preferred sampling area, avoiding the drawbacks of global blind sampling of the traditional RRT* algorithm, so as to improve the sampling efficiency in the ocean current environment.

[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 a preset threshold;

[0099] Determine the sampling point x rand The method of whether a point is located in the preferred sampling area, that is, the method of judging whether a point is inside the 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 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 (based on the nearest node x nearest The starting point along the direction vector Move in the direction and expand the step size 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 With its parent node x parent Connect 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, x is determined according to the method of "determining 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 currents I new The parent node x parent , including step 3-1 and step 3-2:

[0102] Step 3-1, with x new Construct x as the center and R as the radius new The circular neighborhood of , takes all nodes in the random search tree and within the circular neighborhood 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 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 of the ocean current on the path 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 With its parent node x parentMake connections, where each node in the random search tree has only 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 square sea areas that the path between two nodes passes through, θ k The angle between the direction of the path between the two nodes in the k-th 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 k-th square sea area, d k represents the path length between two nodes in the kth square sea area, v k It represents the velocity of the depth average 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 The influence of ocean current on the path is then 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 nodes of the current random search tree to x near The movement time t1 is calculated by using the method of calculating the movement time of the path between adjacent path points. init Move through the nodes of the current random search tree 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, 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 With the target point x goal Is the Euclidean distance between them less than the distance threshold d? threshhold :If x ne With 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 With the target point x goal The Euclidean distance between them is less than the distance threshold d threshhold , then execute step 6;

[0112] Step 6: According to the method of judging the validity of the path between adjacent path points, determine the 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 With the target point x goal Connect and update the current random search tree; if from x new To target point x goal If the path is invalid, then the Monte Carlo circular area sampling strategy is used to new With 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 the present invention is new With 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" to determine x new Is the path to Q valid? If so, execute (1); if not, execute (2);

[0115] (1) Figure 3As shown, Q is used as the path point and another path point M is added to make the path x new Q, path QM, path Mx goal All are effective (effective paths such as Figure 3 In the “feasible trajectory”), update the current random search tree, where 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 that meet the requirements in the circular sampling area 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 to path point Q, and connect x new As the parent node of the waypoint Q, make the waypoint Q and x new connect;

[0117] (2) Figure 4 As shown, by adding the path point M, x new The path to M is valid, and M to x goal The path is valid (valid path is 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 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 that meet the requirements in the circular sampling area as path points M. At the same time, points that do not meet the requirements are discarded and path points M are used as target points x. goal The parent node of the target point x goal Connect to the path point M and convert 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 path between the last two path points by adding path points to ensure the feasibility of its trajectory;

[0120] Step 7: In the random search tree obtained in step 6, place 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 you trace back to the starting point x init So far, we get 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 of

[0121] Among them, the method for judging the validity of the path between adjacent path points is:

[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 square sea area is regarded as a sub-path, and e = 1;

[0124] (3) Calculate the v of the e-th subpath 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 subpath, where: is the velocity vector of the underwater glider in the horizontal plane of the e-th subpath, 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. Calculate v using the known quantities. f and θ ug ;

[0129] (4), determine 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, the impassable areas are sea areas with shallow seabed depths or corresponding to extreme environments;

[0132] The method for calculating the movement time of the path between adjacent path points is as follows: according to the v corresponding to each sub-path f The movement time of the sub-path is calculated by comparing the straight-line distance of the sub-path with the path point. The movement time of all sub-paths in the path is added together to obtain the movement time of the path between adjacent path points.

[0133] Example 2

[0134] According to the underwater glider path planning method based on the improved RRT* algorithm in Example 1, the current optimal path (K=100) is obtained. The data set D comes 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 path planning of the underwater glider, and the operation results are shown in 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] 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.) are respectively used to perform underwater glider path planning, and the current optimal path (K = 300) is obtained. 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 test data of the Hycom website (test 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 test 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 scene 1, scene 2, scene 3, and scene 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 present invention is described above by way of example. 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 those skilled in the art without inventive effort falls within the protection scope 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 the depth average flow data of the rectangular sea trial area, the depth average flow data includes: the velocity vectors of the coordinate points in the rectangular sea trial area, the velocity vector of each coordinate point includes the velocity magnitude and direction of the coordinate point, wherein the direction is the average value of the direction of the current velocity of the coordinate point at different depths, and the velocity magnitude is the average value of the magnitude of the current velocity of the coordinate point at different depths; S2, grid the rectangular sea trial area and obtain x in the longitude direction. num The same grid sea area, in the latitude direction to obtain y num the same square sea area; wherein the average value of the depth average flow data of the four vertices of each square sea area is taken as the depth average flow data of the square sea area; S3, extract the preferred sampling area of ​​the RRT* algorithm in the rectangular sea trial area; S4, based on the preferred sampling area, the following "method for obtaining a path" is executed cyclically, and a path is obtained after each execution of the "method for obtaining a path". When the number of iterations reaches K, the current best path is output, wherein, after each execution of the "method for obtaining a path", the time consumed by the path obtained by the execution of the "method for obtaining a path" is compared with the path with the shortest time consumed among all the paths generated by the previous "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"; 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; 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 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 With its parent node x parent Connect 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, x is determined according to the "Method for determining the validity of paths 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 currents I new The parent node x parent , including step 3-1 and step 3-2: Step 3-1, with x new Construct x as the center and R as the radius new The circular neighborhood of , takes all nodes in the random search tree and within the circular neighborhood 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 of the ocean current on the path 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 With its parent node x parent Make connections, where each node in the random search tree has only 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 The calculation is performed according to 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 nodes of the current random search tree 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 nodes of the current random search tree to x new Then from x new Move along the straight line to x near Movement time t2; Step 4-2, if t1>t2, 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 With the target point x goal Is the Euclidean distance between them less than the distance threshold d? threshhold :If x ne With 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 With the target point x goal The Euclidean distance between them is less than the distance threshold d threshhold , then execute step 6; Step 6: According to the "method for determining the validity of paths between adjacent path points", determine the 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 With the target point x goal Connect and update the current random search tree; if from x new To target point x goal If the path is invalid, then the Monte Carlo circular area sampling strategy is used to new With 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, place 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 you trace back to the starting point x init So far, we get 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 , 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 used as the threshold range ω threshhold , extract the threshold range ω threshhold All coordinate points in the rectangular sea trial area form 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 , and the distance set d 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, directly execute S3-2-6; 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′ 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, let the value of j increase by 1, and repeat steps S3-2-2 to S3-2-6 until the array Array″ is empty, at which 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 does not change, 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 screening: 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, extract the outer contour of the target cluster based on the alpha shapes algorithm, and use the inner area of ​​the outer contour of the target cluster 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 generated random number rand with the preset threshold p: if the random number rand is less than the threshold p, then move 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 performed 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 is 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 square sea areas that the path between two nodes passes through, θ k The angle between the direction of the path between the two nodes in the k-th 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 k-th square sea area, d k represents the path length between two nodes in the kth square sea area, v k It represents the velocity of the depth average 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 is characterized in that: 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 With 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 determining the validity of paths between adjacent path points", x is determined new Is the path to Q valid? If so, execute (1); if not, execute (2); (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: 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 that meet the requirements in the circular sampling area as path points M. At the same time, points that do not meet the requirements are discarded and path points M are used as target points 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 to path point Q, and connect x new As the parent node of the waypoint Q, make the waypoint 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 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 that meet the requirements in the circular sampling area as path points M. At the same time, points that do not meet the requirements are discarded and path points M are used as target points x. goal The parent node of the target point x goal Connect to the path point M and convert 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 is 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 square sea area is regarded as a sub-path, and e = 1; (3) Calculate the v of the e-th subpath 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 plane of the e-th subpath, 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. Calculate v using the known quantities. f and θ ug ; (4), determine 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