Three-dimensional path planning method for autonomous underwater vehicle multi-target point task

Abstract

The application discloses a three-dimensional path planning method for multi-target point tasks of an autonomous underwater vehicle, adopts segmented planning to decompose multi-target point path planning into a plurality of sub-path planning problems between adjacent target points, generates an initial feasible sub-path through an improved RRT* algorithm, uniformly samples control points from the initial feasible sub-path to initialize a particle swarm, and performs iterative local optimization, generates a smooth sub-path through cubic spline interpolation, and finally splices each sub-path in the order of target points to form a complete three-dimensional track. The application mutually couples the path feasibility guarantee of RRT*, the PSO local optimization based on path skeleton initialization, the cubic spline smoothing, and the kinematic constraint of the AUV, and the particles in the PSO stage only need to search in the locally feasible corridor generated by RRT*, thereby improving the quality of the planning result and the executability in actual deployment.

Description

Technical Field

[0001] This invention belongs to the field of underwater autonomous robot path planning technology, specifically relating to a three-dimensional path planning method for autonomous underwater vehicles (AUVs) under a given multi-target point access order. Background Technology

[0002] When performing tasks such as seabed surveying, pipeline inspection, marine environmental monitoring, underwater search and rescue, and military reconnaissance, autonomous underwater vehicles (AUVs) typically need to sequentially pass through multiple pre-set target points for multi-location detection. These multi-target-point navigation missions require path planning algorithms capable of generating safe, smooth, and continuous tracks that satisfy AUV kinematic constraints in a three-dimensional environment containing complex seabed topography such as seamounts, trenches, and reefs. In recent years, research on the autonomy and intelligence of AUVs has continued to deepen, and planning safe, efficient, and low-energy navigation paths in complex and dynamic underwater environments has become one of the core technologies of intelligent underwater robot navigation and control systems.

[0003] Existing underwater path planning methods mainly fall into four categories: The first category is graph search-based methods, represented by the A* algorithm. These methods use heuristic functions to guide the search and estimate the cost from the current node to the target node. However, they suffer from the curse of dimensionality in three-dimensional space, and the path search results are highly dependent on the accuracy of the pre-established environment model. The second category is sampling-based methods, represented by the Rapidly-exploring Random Tree (RRT) algorithm and its asymptotically optimal variant RRT*. These methods do not require explicit modeling of the environment, and their computational complexity depends more on the difficulty of path discovery than on spatial dimension. They have unique advantages in high-dimensional space exploration; however, their initial paths suffer from jagged lines, lack of smoothness, and abrupt changes in turning angles. The third category is swarm intelligence methods, including Particle Swarm Optimization (PSO), Ant Colony Optimization (ACO), and Genetic Algorithms. The first category is represented by Algorithm (GA), which obtains an approximate optimal path through iterative optimization. However, random initialization of the particle swarm causes a large number of initial particles to fall into the obstacle region, which seriously reduces the search efficiency and convergence quality. The second category is smoothing methods based on curve fitting, represented by cubic spline interpolation. These methods can generate smooth curves that are continuously differentiable in the second order, but they lack obstacle avoidance mechanisms when used alone.

[0004] Furthermore, existing path planning methods generally separate path geometry generation from the AUV's own kinematic constraints (maximum climb angle, maximum dive angle, minimum turning radius, etc.), resulting in planning results that are unexecutable in actual deployment. To address these technical issues, a 3D path planning scheme is needed that integrates the asymptotic optimal feasibility guarantee of RRT*, the PSO local optimization capability based on path skeleton initialization, the smooth generation capability of cubic spline interpolation, and the AUV's kinematic constraints. Summary of the Invention

[0005] To address the issues in existing technologies, this invention provides a three-dimensional path planning method for multi-target point tasks of autonomous underwater vehicles. This method proposes a two-stage collaborative three-dimensional path planning algorithm called RSS-MLP (RRT* Swarm Synergy for Multi-Location Path).

[0006] The core idea of ​​this invention is as follows: Multi-objective path planning is decomposed into a sub-path planning problem between multiple adjacent objective points using segmented planning. For each sub-path, a two-stage planning strategy is adopted: The first stage generates initial feasible sub-paths using an improved RRT* algorithm, which includes mechanisms such as target bias sampling, 3D KD-Tree nearest neighbor search, spherical coordinate expansion, 3D collision detection based on seabed topographic height field, neighbor node search and parent node reselection, and random tree rewire. The second stage uniformly samples control points from the initial feasible sub-paths to initialize the particle swarm, designing a system that includes path length and terrain clearance. Multiple fitness functions, including constraints such as depth overrun penalties, path curvature / turning radius constraints, and climb / dive angle constraints, are used to iteratively optimize the particle swarm. Second-order continuous and differentiable smooth sub-paths are generated through cubic spline interpolation, and kinematic feasibility is verified using the minimum turning radius and maximum climb / dive angle of the AUV. If these criteria are not met, the optimization process returns to the particle swarm optimization stage, the penalty weights are adjusted, and re-optimization is performed, forming a closed-loop feedback mechanism. Finally, the sub-paths are stitched together in the order of the target points to form a complete 3D track. The stitching points achieve C0 continuity through shared endpoints, C1 continuity through endpoint tangent vector constraints, and C2 continuity through local spline refitting.

[0007] To achieve the above objectives, the specific technical solution adopted by the present invention is as follows:

[0008] A three-dimensional path planning method for multi-target point tasks of autonomous underwater vehicles includes the following steps:

[0009] S1: Obtain the underwater 3D environment model. The underwater 3D environment model includes 3D spatial boundaries, seabed topography height field, starting point, ending point, and other elements arranged in the order of task access. One target point.

[0010] S2: Decompose sub-paths based on segmented planning strategy.

[0011] According to the task access order, the starting point, The goal point sequence is obtained by combining the goal points and the destination points. Based on the goal point sequence, the multi-goal path planning is decomposed into n+1 sub-path planning problems.

[0012] S3: Generate initial feasible sub-paths based on the improved RRT* algorithm.

[0013] Target bias sampling is performed in the underwater 3D map to construct a fast exploration random tree; collision-free path checking and path reconnection optimization are performed to generate initial sub-paths.

[0014] S4: Perform improved particle swarm optimization based on path initialization on all initially feasible sub-paths.

[0015] S5: Path smoothing based on cubic spline interpolation.

[0016] Cubic spline interpolation is used to smooth the sub-path after particle swarm optimization. By interpolating discrete control points, the path corners introduced by discrete sampling of control points are eliminated.

[0017] S6: Subpath stitching and complete track generation. Smooth subpaths obtained by cubic spline interpolation are sequentially stitched together according to the target point sequence, with the stitching points achieved through shared endpoints. Continuity is achieved through endpoint tangent vector constraints. Continuity is achieved through local spline refitting. Continuous. Ultimately, a complete three-dimensional track of multiple targets is obtained.

[0018] In one possible implementation, the seabed topographic height field is generated from multibeam sonar bathymetry data or a parametric seamount model and stored in a discrete grid format. The depth range of the three-dimensional spatial boundary is set according to the diving depth capability of the AUV platform.

[0019] In one possible implementation, the specific steps for generating initial feasible sub-paths based on the improved RRT* algorithm are as follows:

[0020] For each sub-path planning problem, S3-1 uses the starting point of the sub-path as the root node of a random tree. A random tree node is defined as a triple containing three-dimensional coordinates, the index of the parent node, and the cumulative cost from the root node to the current node.

[0021] S3-2 Target Bias Sampling: A random sampling strategy is used to generate candidate points.

[0022] S3-3 uses a three-dimensional KD-Tree nearest neighbor search to determine the parent node of candidate points.

[0023] A three-dimensional KD-Tree data structure is used to maintain the three-dimensional coordinates of all nodes in the random tree. When candidate points are generated... Then, the distance is queried using a 3D KD-Tree. The nearest random tree node in Euclidean distance As the parent node.

[0024] S3-4 generates new nodes by expanding spherical coordinates.

[0025] S3-5 performs three-dimensional collision detection on the newly generated nodes based on the seabed topographic height field.

[0026] S3-6 performs parent node reselection through neighborhood search.

[0027] S3-7 continues to iterate and expand until the termination condition is met, generating an initial feasible sub-path.

[0028] In one possible implementation, the random sampling strategy employs a biased approach, that is, under a set probability, randomly sampling within the entire three-dimensional water space to generate candidate points for the current iteration. :

[0029]

[0030] in: , , , ; This represents the probability of random sampling. The target point.

[0031] In one possible implementation, the spherical coordinate extension operation is as follows:

[0032] Based on the parent node ( , , ) to candidate point ( , , Calculate the horizontal azimuth angle from the three-dimensional direction vector. and elevation angle :

[0033]

[0034]

[0035] In the formula for With candidate points Distance on the x-axis, for With candidate points The distance on the ordinate.

[0036] With preset expansion step size along Direction generates new nodes :

[0037] ,

[0038]

[0039]

[0040] In the formula , They are respectively The x-axis, y-axis, and y-axis coordinates.

[0041] In one possible implementation, the 3D collision detection operation is as follows:

[0042] From parent node to new node Collision detection is performed on the connections between them. The parent node to be detected is then compared with... The line segments between them are discretized according to a preset spacing Δd. Path sampling points ,in The coordinates are represented as For the coordinates of each sampling point, based on the seabed topographic height field... The corresponding terrain height Z is calculated using bilinear interpolation, and the formula is as follows:

[0043]

[0044] In the formula, These represent the grid coordinates and height mapping matrix of the terrain height field, respectively. This is a bilinear interpolation function. A path safety criterion is constructed by combining terrain height, AUV safety clearance, and depth constraints. First, a collision flag variable is set: if the sampling point height is less than or equal to the terrain height, a collision is determined and flagged. Set to 1 if the value is not 1, otherwise set to 0. The determination formula is:

[0045]

[0046] Based on this, a safety threshold constraint is introduced to preset the safe clearance height for AUVs. upper and lower limits of navigation depth , Only if the sampling points satisfy and If the path point is safe, the iteration is terminated; otherwise, the path is determined to have a collision risk or depth exceedance, and the target bias sampling is performed again.

[0047] In one possible implementation, the parent node reselection process is as follows:

[0048] After collision detection is completed, the new node is added to the tree, and the tree structure is updated. For new nodes that pass the collision detection... Given a neighborhood radius Searching in a 3D KD-Tree candidate node set .right Each node in , calculation When it is the parent node Cumulative cost: .

[0049] Choose to The node with the lowest cumulative cost is selected as the new parent node, thus completing the parent node reselection. A new 3D collision detection is performed based on the new parent node; if the detection passes, the node that fell into the collision is considered a suitable candidate. Neighborhood radius Reconnecting to nodes within the same node: For each node ,like:

[0050]

[0051] Then update The parent node is Otherwise, keep the original node connection.

[0052] In one possible implementation, S3-7 specifically operates as follows:

[0053] After reconnection, a new round of target bias sampling and subsequent operations will continue, continuously moving towards the target point on the sub-path. Extend until the new node is selected after the parent node is re-elected. and The Euclidean distance between them is less than a preset threshold. At that time, stop expanding, along Tracing back the parent node chain to the root node yields a result consisting of multiple path points. The initial feasible sub-path formed .

[0054] In one possible implementation, the improved particle swarm optimization based on path initialization operates as follows:

[0055] The particle structure of the traditional particle swarm optimization algorithm is improved by introducing a "multi-control point" strategy, enabling each improved particle to store three position points. Each particle Q consists of three position points, which are the three control points on the path:

[0056]

[0057] Each location point is a three-dimensional coordinate point, represented as:

[0058]

[0059] M points are uniformly sampled from the initial path generated by RRT* to initialize particles. The sampling sequence number is defined as:

[0060]

[0061] in: .

[0062] The sampled first The coordinates of each RRT* path point are:

[0063] The initial position of the j-th position of the k-th particle is represented as:

[0064]

[0065] In a three-dimensional coordinate system, the coordinate boundaries for each dimension are defined as follows:

[0066] in: ; The lower bound; The upper boundary.

[0067] For the j-th position of the k-th particle, in dimension The initial velocity is randomly generated from a uniform velocity, and its mathematical expression is:

[0068]

[0069] in: For obedience Uniformly distributed random variables are used to increase the randomness of the search.

[0070] After initializing the particle position and velocity, the particle velocity and position are iteratively updated to search for a better path solution.

[0071] After the t-th iteration, the velocity update formula for the j-th position of the k-th particle in dimension d is:

[0072]

[0073] in: Inertial weights control the extent to which a particle maintains its current velocity. As a cognitive learning factor, it controls the particle to move towards its historical best position. The degree of closeness; As a social learning factor, it controls the particles to move towards the global optimal position. The degree of closeness; and A random number between 0 and 1; t is the number of iterations; Let j be the position of the k-th particle after t iterations.

[0074] If the speed update exceeds the limit, then a constraint will be imposed:

[0075]

[0076] The particle's position is updated based on the updated velocity. The position of the j-th position of the k-th particle in the (t+1)-th iteration is given by:

[0077]

[0078] After multiple iterations, the global optimal position of each particle is obtained, thereby constructing an optimal sub-path.

[0079] In one possible implementation, path smoothing based on cubic spline interpolation specifically operates as follows:

[0080] For the three-dimensional sub-path, respectively in Cubic spline interpolation is used on the axis. The key point coordinates of the path are set as follows:

[0081]

[0082] in: , , and , , These represent the coordinates of the starting point and the ending point, respectively.

[0083] Generate equally spaced indices in the [0,1] interval:

[0084]

[0085]

[0086] in: It is an index of discrete control points; It is the path point index generated after interpolation.

[0087] To each Cubic spline interpolation of the axial coordinates:

[0088]

[0089] The final generated subpath is:

[0090]

[0091] Where: s represents the s-th segment of the path; .

[0092] The present invention has at least the following advantages over the prior art:

[0093] (1) This invention couples the path feasibility guarantee of RRT*, PSO local optimization based on path skeleton initialization, cubic spline smoothing, and AUV kinematic constraints together. The three-dimensional KD-Tree in the RRT* stage reduces the complexity of the nearest neighbor search from Down to In the PSO stage, particles only need to search within the locally feasible corridors generated by RRT*, which improves the quality of planning results and their executability in actual deployment.

[0094] (2) The present invention is based on a fine collision detection mechanism using a three-dimensional seabed topography height field and bilinear interpolation, combined with safety clearance constraints and dynamic penalty coefficients, to effectively ensure the safety of the seabed topography of the path and enhance the adaptability of collision penalty in complex topography areas.

[0095] (3) Multiple fitness functions simultaneously constrain path length, terrain safety, depth range, turning radius and climbing and diving angles, avoiding local over-adjustment problems caused by a single optimization objective. When optimizing along the ocean current direction, the ocean current energy consumption cost term can also be added to further improve the economic efficiency of the path.

[0096] (4) The RRT* stage introduces a neighborhood node search, parent node reselection and random tree reconnection mechanism to maintain asymptotic optimality, improve the quality and stability of the initial path, and provide a high-quality search starting point for subsequent PSO optimization. Attached Figure Description

[0097] Figure 1 This is a diagram illustrating the overall framework of the RSS-MLP two-stage collaborative three-dimensional path planning algorithm according to an embodiment of the present invention.

[0098] Figure 2 This is a schematic diagram of biased random particle generation.

[0099] Figure 3 This is a schematic diagram of traversing nodes in a 3D KD-Tree.

[0100] Figure 4 This is a schematic diagram of particle path updates for multiple control points.

[0101] Figure 5 This is an underwater 3D simulation map.

[0102] Figure 6 The results are experimental findings of the RSS-MLP algorithm under random task conditions and extreme underwater terrain conditions.

[0103] Figure 7 A comparison of 3D path planning results from different algorithms.

[0104] Figure 8 A top-down comparison of path planning algorithms.

[0105] Figure 9 Compare the convergence of different algorithm path planning methods. Detailed Implementation

[0106] The present invention will be further explained below with reference to the accompanying drawings and specific embodiments.

[0107] This application provides a three-dimensional path planning method for multi-target point missions of autonomous underwater vehicles, including the following steps:

[0108] S1: Obtain an underwater 3D environment model. The underwater 3D environment model includes a 3D spatial boundary ( ), seabed topographic elevation field ,starting point ,end And arranged in the order of task access. Target points The seabed topographic height field is generated from multibeam sonar bathymetry data or a parametric seamount model and stored in a discrete grid format. The depth range of the three-dimensional spatial boundary... to Based on the depth capability of the AUV platform.

[0109] S2: Decompose sub-paths based on segmented planning strategy.

[0110] The starting point will be accessed in the order of task visits. , The target point sequence is obtained by combining the target points and the endpoint G. Based on the sequence of target points, multi-objective point path planning is decomposed into n+1 sub-path planning problems. A segmented planning strategy decouples the tightly coupled multi-objective point problem into multiple single-objective point sub-problems, achieving a time complexity of O(n log n). The space complexity is Furthermore, the independence between sub-paths supports parallel computing to further reduce runtime.

[0111] S3: Generate initial feasible sub-paths based on the improved RRT* algorithm.

[0112] Target bias sampling is performed in the underwater 3D map to construct a fast exploration random tree; collision-free path checking and path reconnection optimization are performed to generate initial sub-paths.

[0113] S3-1 For each sub-path planning problem, the starting point of the sub-path is taken as the root node of a random tree, and the random tree nodes are... Defined as containing three-dimensional coordinates Parent node index and the cumulative cost from the root node to the current node Triples (defined as the sum of Euclidean distances).

[0114] S3-2 Target Bias Sampling. To perform a global search within the 3D water space, a random sampling strategy is employed to generate candidate points. This random sampling strategy uses a biased approach, meaning that candidate points for this iteration are generated by randomly sampling across the entire 3D water space with a set probability. :

[0115]

[0116] in: , , , ; This represents the probability of random sampling. The target point.

[0117] The initial value is set to 0.4 to 0.8, preferably 0.6. In this embodiment, during the RRT* iteration process, the value is dynamically adjusted based on the success rate of the most recent N expansion attempts. Increase when the success rate is below the preset first threshold. To strengthen goal orientation, reduce [the threshold] when the success rate exceeds a preset second threshold. This facilitates global exploration. The adaptive mechanism achieves a dynamic balance between global exploration and goal orientation.

[0118] Table 1 Target Bias Probability Ablation test results

[0119]

[0120] Standard test environment: 5 typical seamounts, initial distance 80m. It is optimal in terms of generation time, path length, and number of nodes.

[0121] S3-3 3D KD-Tree Nearest Proximity Search. A 3D KD-Tree data structure is used to maintain the 3D coordinates of all nodes in the random tree. When candidate points are generated... Then, the distance is queried using a 3D KD-Tree. The nearest random tree node in Euclidean distance As the parent node. Compared to the point-by-point traversal method, the 3D KD-Tree can significantly reduce the computational cost of nearest neighbor search in large-scale node sets, with a query complexity of only [missing information]. .

[0122] S3-4 spherical coordinate extension. Based on the parent node. ( , , ) to candidate point ( , , Calculate the horizontal azimuth angle from the three-dimensional direction vector. and elevation angle :

[0123]

[0124]

[0125] In the formula for With candidate points Distance on the x-axis, for With candidate points The distance on the ordinate.

[0126] With preset expansion step size along Direction generates new nodes :

[0127]

[0128]

[0129]

[0130] In the formula , They are respectively The x-axis, y-axis, and y-axis coordinates.

[0131] The preferred range for the step size is 2 to 15 meters, which can be adjusted according to environmental complexity and AUV platform size. The spherical coordinate expansion method allows the node expansion to simultaneously carry horizontal steering and depth change information, making it more suitable for the movement characteristics of AUVs in three-dimensional underwater space.

[0132] S3-5 is a 3D collision detection method based on the seabed topographic height field. After generating a new node, to ensure path safety, it is necessary to perform collision detection on the parent node (i.e., ...). (or the rewritten parent node) to the new node Collision detection is performed on the connections between them. The parent node to be detected is then compared with... The line segments between them are discretized according to a preset spacing Δd. Path sampling points ,in The coordinates are represented as For the coordinates of each sampling point, based on the seabed topographic height field... The corresponding terrain height Z is calculated using bilinear interpolation, and the formula is as follows:

[0133]

[0134] In the formula, These represent the grid coordinates and height mapping matrix of the terrain height field, respectively. This is a bilinear interpolation function. A path safety criterion is constructed by combining terrain height, AUV safety clearance, and depth constraints. First, a collision flag variable is set: if the sampling point height is less than or equal to the terrain height, a collision is determined and flagged. Set to 1 if the value is not 1, otherwise set to 0. The determination formula is:

[0135]

[0136] Based on this, a safety threshold constraint is introduced to preset the safe clearance height for AUVs. upper and lower limits of navigation depth , Only if the sampling points satisfy and If the path point is safe, the iteration is terminated; otherwise, the path is determined to have a collision risk or depth exceedance, and the target bias sampling is performed again.

[0137] S3-6 performs parent node reselection through neighborhood search.

[0138] After collision detection is completed, the new node is added to the tree, and the tree structure is updated. For new nodes that pass the collision detection... Given a neighborhood radius Searching in a 3D KD-Tree candidate node set .right Each node in , calculation When it is the parent node Cumulative cost: .

[0139] Choose to The node with the lowest cumulative cost is selected as the new parent node, thus completing the parent node reselection. A new 3D collision detection is performed based on the new parent node; if the detection passes, the node that fell into the collision is considered a suitable candidate. Neighborhood radius Reconnecting to nodes within the same node: For each node ,like:

[0140]

[0141] Then update The parent node is Otherwise, keep the original node connection.

[0142] S3-7 Termination conditions and initial feasible sub-paths.

[0143] After reconnection, a new round of target bias sampling and subsequent operations will continue, continuously moving towards the target point on the sub-path. Extend until the new node is selected after the parent node is re-elected. and The Euclidean distance between them is less than a preset threshold. At that time, stop expanding, along Tracing back the parent node chain to the root node yields a result consisting of multiple path points. The initial feasible sub-path formed .

[0144] S4: Perform improved particle swarm optimization based on path initialization on all initially feasible sub-paths.

[0145] To further improve the optimization performance of all initial sub-paths obtained by RRT*, the particle structure of the traditional particle swarm optimization algorithm was improved by introducing a "multi-control point" strategy, enabling each improved particle to store three position points. Each particle Q consists of three position points, which are the three control points on the path:

[0146]

[0147] Each location point is a three-dimensional coordinate point, represented as:

[0148]

[0149] The improved particles can inherit the initial feasible solution generated by the improved RRT* algorithm, obtaining a good initial solution distribution. M points are uniformly sampled from the initial path generated by RRT* to initialize the particles. The sampling sequence number is defined as:

[0150]

[0151] in: .

[0152] The sampled first The coordinates of each RRT* path point are:

[0153] The initial position of the j-th position of the k-th particle is represented as:

[0154]

[0155] In a three-dimensional coordinate system, the coordinate boundaries for each dimension are defined as follows:

[0156] in: ; The lower bound; The upper boundary.

[0157] For the j-th position of the k-th particle, in dimension The initial velocity is randomly generated from a uniform velocity, and its mathematical expression is:

[0158]

[0159] in: For obedience Uniformly distributed random variables are used to increase the randomness of the search. Expanding, we get:

[0160]

[0161] in: This is a speed scaling factor that controls the speed range.

[0162] After initializing the particle position and velocity, the particle velocity and position are iteratively updated to search for a better path solution.

[0163] After the t-th iteration, the velocity update formula for the j-th position of the k-th particle in dimension d is:

[0164]

[0165] in: Inertial weights control the extent to which a particle maintains its current velocity. As a cognitive learning factor, it controls the particle to move towards its historical best position. The degree of closeness; As a social learning factor, it controls the particles to move towards the global optimal position. The degree of closeness; and The random number between 0 and 1 enhances the randomness of the particle search process, thereby improving the particle's optimization ability; t is the number of iterations. Let j be the position of the k-th particle after t iterations.

[0166] If the speed update exceeds the limit, then a constraint will be imposed:

[0167]

[0168] The particle's position is updated based on the updated velocity. The position of the j-th position of the k-th particle in the (t+1)-th iteration is given by:

[0169]

[0170] After multiple iterations, the global optimal position of each particle is obtained, thereby constructing an optimal sub-path.

[0171] S5: Path smoothing based on cubic spline interpolation.

[0172] In the generated sub-paths, which are formed by connecting discrete control points, directly connecting these points would lead to discontinuities in path curvature, affecting the smoothness of the path. Cubic spline interpolation is used to smooth the sub-paths optimized by particle swarm optimization. By interpolating the discrete control points, the path corners introduced by the discrete sampling of control points are eliminated, so that the final generated sub-paths not only meet the obstacle avoidance requirements but also have high dynamic feasibility.

[0173] For the three-dimensional sub-path, respectively in Cubic spline interpolation is used on the axis. The key point coordinates of the path are set as follows:

[0174]

[0175] in: , , and , , These represent the coordinates of the starting point and the ending point, respectively.

[0176] Generate equally spaced indices in the [0,1] interval:

[0177]

[0178]

[0179] in: It is an index of discrete control points; It is the path point index generated after interpolation.

[0180] To each Cubic spline interpolation of the axial coordinates:

[0181]

[0182] The final generated subpath is:

[0183]

[0184] Where: s represents the s-th segment of the path; .

[0185] S6: Subpath stitching and complete track generation. Smooth subpaths obtained by cubic spline interpolation are sequentially stitched together according to the target point sequence, with the stitching points achieved through shared endpoints. Continuity is achieved through endpoint tangent vector constraints. Continuity is achieved through local spline refitting. Continuous. This ultimately yields a complete three-dimensional trajectory of multiple targets. The trajectory has global obstacle avoidance feasibility, second-order continuous differentiability, and kinematic constraint compatibility.

[0186] Example 1: AUV Multi-Object Point 3D Path Planning Based on RSS-MLP Algorithm

[0187] This embodiment presents the complete implementation process of the algorithm of the present invention. The present invention proposes a two-stage collaborative three-dimensional path planning algorithm of RSS-MLP, and the overall complete execution framework of the algorithm is as follows: Figure 1 As shown in the figure, the algorithm breaks down the multi-target cruise mission into several independent sub-paths. For each sub-path, a collision-free initial path skeleton is first generated by improving RRT*. Then, the particle swarm optimization is initialized with the path skeleton sampling control points. After smoothing by cubic spline interpolation, the kinematic constraints of the AUV are checked. If the turning and pitch angle constraints are not met, the algorithm is fed back to the particle swarm optimization module for re-iteration. Finally, all sub-paths are stitched together to form a continuous, smooth, and complete three-dimensional track.

[0188] In step S1, an underwater 3D environment model is obtained. The 3D spatial boundaries are then defined. The seabed topography is generated by a parametric seamount model, and the topographic height field is stored in a discrete grid with a 1m spacing. AUV kinematic parameters: minimum turning radius Maximum climb angle Maximum diving angle Safe airspace .starting point ,end and The three-dimensional coordinates of each target point are set according to the task.

[0189] In step S2, the starting point is... , Target points to and the end point Constituting a sequence, forming together This is a sub-path planning problem. Each sub-path is solved independently, and the solution order can be sequential or parallel.

[0190] Step S3 uses an improved RRT algorithm to generate initial feasible sub-paths, with unified configuration of algorithm parameters: target bias baseline probability. =0.6, Expanding step size Target termination threshold Neighborhood search radius . Specifically:

[0191] Step S3-1: Design an adaptive target bias sampling mechanism that balances global space exploration with fast target convergence. The sampling generation logic is as follows: Figure 2 As shown, the algorithm directly selects the sub-path endpoint as a candidate point with a probability of 0.6, and randomly samples in the three-dimensional feasible navigation space with a remaining probability of 0.4; at the same time, it dynamically adjusts the success rate based on the real-time expansion of random tree nodes. The numerical values ​​significantly reduce invalid sampling and shorten the time required to generate feasible paths.

[0192] Step S3-2 introduces a 3D KD-Tree to store all random tree nodes to optimize the efficiency of neighbor node retrieval. The KD-Tree topology and the 3D spatial nearest neighbor search process are as follows: Figure 3 As shown in (a)-(c) in the diagram. The traditional method of traversing nodes one by one has a time complexity of O(n). This solution reduces the retrieval complexity to In large-scale random tree scenarios, this significantly improves the speed of RRT node expansion. After completing node sampling and nearest neighbor queries, collision detection based on the seabed topography height field is performed on newly generated nodes. When there is no collision on a line segment, parent node reselection and neighbor node reconnection are performed, iterating continuously until the Euclidean distance between the new node and the child target point is less than a threshold. By tracing back the node chain, the initial feasible sub-path can be output.

[0193] In step S4, particle vectors are constructed by sampling control points at equal intervals from the initial feasible path output by RRT*. The particles rely on their individual optimality in three-dimensional space. Global Optimum Bidirectional guidance completes position iteration updates; the principle of three-dimensional particle update is as follows: Figure 4 As shown in the figure. This scheme differs from the traditional random initialization of particle swarms in that all particles are initially positioned within a collision-free safe corridor, significantly reducing invalid iterations. The initial value is 3. If the absolute value of the seabed topographic height difference crossed by the sub-path exceeds 50m, then... Automatically increments to 5. PSO parameter: Inertia weight. It decreases linearly from 1.2 to 0.4. velocity boundary The maximum number of iterations is 200, and the termination condition is that the relative change in fitness is less than 1 for 10 consecutive generations. The fitness function weights are adjusted based on experiments; typical values ​​can be set to... , , , Collision penalty coefficient According to the terrain undulation Dynamically adjusted upwards.

[0194] In step S5, the control points corresponding to the globally optimal particle are generated by cubic spline interpolation to form a second-order continuous smooth sub-path. This sub-path is then discretized into a sequence of track points with a 1m interval, and the turning curvature is verified point by point to be no more than [the specified value]. Furthermore, the absolute value of the pitch angle should not exceed 30°. If this condition is not met, return to S4, increase the corresponding penalty weight by 50%, and then re-optimize.

[0195] In step S6, the smooth sub-paths are stitched together in the order of the target points to obtain the complete AUV multi-target point three-dimensional track.

[0196]

[0197] in Let be the cost function of the i-th sub-path, and n be the total number of sub-paths.

[0198] Example 2: Path Planning System

[0199] This invention also provides a three-dimensional path planning system for multi-target point missions of autonomous underwater vehicles, comprising the following functional modules:

[0200] The environment modeling module is used to acquire underwater 3D environment models, seabed topography height fields, obstacle information, AUV kinematic parameters, start point, end point, and multiple target points.

[0201] The task segmentation module is used to generate a sequence of adjacent point pairs based on the order in which the target points are visited, forming multiple sub-path planning tasks.

[0202] An improved RRT* planning module was developed to generate initial feasible sub-paths based on target bias sampling, 3D KD-Tree nearest neighbor search, spherical coordinate expansion, neighborhood node search and parent node reselection, random tree reconnection, and line segment-level collision detection.

[0203] The collision detection module is used to determine whether path sampling points meet safety clearance and depth constraints based on the seabed topographic height field and bilinear interpolation.

[0204] The particle swarm optimization module is used to sample control points at equal intervals from the initial feasible sub-path, construct multi-control point composite particles, and perform iterative optimization based on multiple fitness functions including path length, terrain clearance penalty, depth overrun penalty, curvature penalty, and ascent angle penalty.

[0205] The spline generation module is used to generate second-order continuous smooth sub-paths based on the optimized control points through cubic spline interpolation.

[0206] The AUV kinematics verification module is used to determine whether the smooth sub-path meets the kinematic parameter constraints of the AUV, such as the minimum turning radius, maximum climb angle, and maximum dive angle. If it does not meet the constraints, it feeds back to the particle swarm optimization module to increase the corresponding penalty weight, thus forming a closed-loop optimization mechanism.

[0207] The path stitching module is used to stitch multiple smooth sub-paths according to the target point sequence. It ensures the smoothness of the stitching points by using shared endpoints, endpoint tangent vector constraints, or local spline refitting. Continuity.

[0208] The track output module is used to output a complete three-dimensional track that meets kinematic constraints and obstacle avoidance requirements to the navigation and control system of the autonomous underwater vehicle.

[0209] Example 3: Simulation Experiment and Performance Comparison

[0210] To verify the effectiveness of the method of the present invention (RSS-MLP), a simulation experiment was designed to evaluate its performance.

[0211] This embodiment constructs a 100m×100m×100m three-dimensional underwater simulation environment. The seabed consists of multiple parametric seamount obstacles. The 3D perspective view of the simulated terrain and the top-down view of the marked target points are shown below. Figure 5 As shown in (a) and (b) in the figure. The center coordinates, ridge size, and seabed height parameters of all seamounts in the simulation scene are shown in Table 4; the RSS-MLP algorithm, AUV motion constraints, and all parameter configurations of the task points are summarized in Table 5.

[0212] Table 2. Submarine mountain range data

[0213]

[0214] Table 3. RSS-MLP Algorithm Parameter Settings

[0215]

[0216] Combination Figure 5 As shown in (b), the simulation environment marks the starting point, three intermediate detection target points, and the end point of this multi-objective task. The scene has a dense distribution of seamounts and dramatic terrain undulations, which can fully verify the obstacle avoidance and continuous planning capabilities of the algorithm in a complex seabed environment.

[0217] The planning performance of the RSS-MLP algorithm of this invention was tested separately under different underwater conditions. Two test scenarios were divided: conventional random multi-target cruise and extremely steep seamount terrain. The complete 3D trajectory results generated under different conditions are shown below. Figure 7 As shown in (a)~(i) in the diagram. Figure 7 (a) to (f) in the text correspond to the normal random task conditions. Figure 7 In this context, (g)~(i) correspond to extreme seabed topographic conditions with significant elevation differences. (From...) Figure 7 It is evident that, regardless of whether the terrain is conventional or extreme, the algorithm-generated track and seamount obstacles always maintain a preset safe clearance, with no risk of depth overrun or collision with the seabed, and the entire track is smooth without any sudden inflection points.

[0218] To verify the technical advantages of this invention, six algorithms were selected for comparative simulation: RRT*+cubic spline, standard PSO, ant colony ACO, artificial bee colony ABC, genetic GA, and wolf pack WPA. The three-dimensional segmented tracks generated by each algorithm were compared under the same terrain. Figure 8 (a)~(f) in the text. Figure 8 In (a), the RSS-MLP planning result of this invention is shown. The four segments of the trajectory have smooth transitions and uniform obstacle avoidance distances. The other comparison algorithm paths have defects such as large redundant detours, close proximity to seamounts, and sharp turning angles, which cannot meet the kinematic constraints of AUV.

[0219] To visually compare the planar detour logic of each algorithm, the two-dimensional top-down projection of all algorithm paths is shown below. Figure 9 As shown in (a) to (f) in the figure. From the top view, it can be clearly seen that traditional swarm intelligence algorithms have problems such as unnecessary large-angle turns and traversing high-risk seamount areas. RSS-MLP can avoid areas with dense obstacles in advance, with a shorter total path length and smoother turns.

[0220] Simulation results show that the RSS-MLP method of this invention shortens the total path length by approximately 6%–15% compared to classic swarm intelligence algorithms (PSO, ACO, GA), and its planning efficiency is significantly superior to traditional swarm intelligence algorithms. Compared to group B, the method of this invention, due to the addition of the PSO local optimization stage, significantly reduces path curvature and pitch angle, and greatly improves the kinematic constraint satisfaction rate. Compared to group C, the method of this invention, because RRT* provides a feasible initial path skeleton, allows the particle swarm to start searching from the feasible region, resulting in a significantly faster convergence speed and a substantial reduction in the collision ratio. In summary, the RSS-MLP algorithm demonstrates superior overall performance in terms of path length, planning time, convergence characteristics, and kinematic constraint satisfaction rate.

[0221] The statistical results of quantitative indicators such as path length, planning time, collision ratio, and kinematic satisfaction rate of each algorithm are shown in Table 4.

[0222] Table 4. Path planning performance comparison between the RSS-MLP method of this invention and the comparison algorithm.

[0223]

[0224] As shown in Table 4, the path length of RSS-MLP is reduced by 6% to 15% compared with the swarm intelligence algorithm, and the planning time of 2.84s is significantly better than PSO (12.3s) / ACO (15.7s) / GA (18.2s). The collision rate is 0%, and the kinematic satisfaction rate and success rate are both 100%.

[0225] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit the technical solutions. Those skilled in the art should understand that any modifications or equivalent substitutions to the technical solutions of the present invention without departing from the spirit and scope of the present invention should be covered within the scope of the claims of the present invention.

Claims

1. A three-dimensional path planning method for multi-target point tasks of autonomous underwater vehicles, characterized in that, Includes the following steps: S1: Obtain the underwater 3D environment model; the underwater 3D environment model includes 3D spatial boundaries, seabed topography height field, starting point, ending point, and elements arranged according to the task access order. One target point; S2: Sub-path decomposition based on segmented planning strategy; According to the task access order, the starting point, The combination of target points and destination points yields a target point sequence. Based on the target point sequence, the multi-target point path planning is decomposed into n+1 sub-path planning problems. S3: Generate initial feasible sub-paths based on the improved RRT* algorithm; Target bias sampling is performed in underwater 3D maps to construct a fast exploration random tree; Perform collision-free path checks and path reconnection optimization to generate initial sub-paths; S4: Perform improved particle swarm optimization based on path initialization on each of the initially feasible sub-paths; S5: Path smoothing based on cubic spline interpolation; Cubic spline interpolation is used to smooth the sub-path after particle swarm optimization. By interpolating discrete control points, the path corners introduced by discrete sampling of control points are eliminated. S6: Subpath stitching and complete track generation; Smooth subpaths obtained by cubic spline interpolation are sequentially stitched together according to the target point sequence, with the stitching achieved through shared endpoints. Continuity is achieved through endpoint tangent vector constraints. Continuity is achieved through local spline refitting. Continuous; ultimately, a complete three-dimensional track of multiple targets is obtained.

2. The three-dimensional path planning method for multi-target point tasks of autonomous underwater vehicles according to claim 1, characterized in that, The seabed topographic height field is generated from multibeam sonar bathymetry data or a parameterized seamount model and stored in a discrete grid format; the depth range of the three-dimensional spatial boundary is set according to the diving depth capability of the AUV platform.

3. The three-dimensional path planning method for multi-target point tasks of autonomous underwater vehicles according to claim 1, characterized in that, The specific steps for generating initial feasible subpaths based on the improved RRT* algorithm are as follows: For each sub-path planning problem, S3-1 takes the starting point of the sub-path as the root node of a random tree. A random tree node is defined as a triple containing three-dimensional coordinates, the index of the parent node, and the cumulative cost from the root node to the current node. S3-2 Target Bias Sampling: Candidate points are generated using a random sampling strategy; S3-3 uses a three-dimensional KD-Tree nearest neighbor search to determine the parent node of the candidate point; A 3D KD-Tree data structure is used to maintain the 3D coordinates of all nodes in the random tree; when candidate points are generated... Then, the distance is queried using a 3D KD-Tree. The nearest random tree node in Euclidean distance As the parent node; S3-4 generates new nodes by expanding spherical coordinates; S3-5 performs three-dimensional collision detection on the newly generated nodes based on the seabed topographic height field; S3-6 performs parent node reselection through neighborhood search; S3-7 continues to iterate and expand until the termination condition is met, generating an initial feasible sub-path.

4. The three-dimensional path planning method for multi-target point tasks of autonomous underwater vehicles according to claim 3, characterized in that, The random sampling strategy employs a biased approach, meaning that candidate points for this iteration are generated by randomly sampling across the entire three-dimensional water space with a set probability. : in: , , , ; This represents the probability of random sampling. The target point.

5. The three-dimensional path planning method for multi-target point tasks of autonomous underwater vehicles according to claim 3, characterized in that, The specific operations for spherical coordinate expansion are as follows: Based on the parent node To candidate point Calculate the horizontal azimuth angle from the three-dimensional direction vector. and elevation angle : In the formula for With candidate points Distance on the x-axis for With candidate points Distance on the ordinate; With preset expansion step size along Direction generates new nodes : In the formula , They are respectively The x-axis, y-axis, and y-axis coordinates.

6. The three-dimensional path planning method for multi-target point tasks of autonomous underwater vehicles according to claim 5, characterized in that, The specific operation of 3D collision detection is as follows: From parent node to new node Collision detection is performed on the connections between them; the parent node to be detected is compared with... The line segments between them are discretized according to a preset spacing Δd. Path sampling points ,in The coordinates are represented as For the coordinates of each sampling point, based on the seabed topographic height field The corresponding terrain height Z is calculated using bilinear interpolation, and the formula is as follows: In the formula, These represent the grid coordinates and height mapping matrix of the terrain height field, respectively. The function is a bilinear interpolation function; a path safety criterion is constructed by combining terrain height, AUV safety clearance, and depth constraints; firstly, a collision flag variable is set: if the sampling point height is less than or equal to the terrain height, a collision is determined and flagged. Set to 1 if the value is not 1, otherwise set to 0. The determination formula is: Based on this, a safety threshold constraint is introduced to preset the safe clearance height for AUVs. upper and lower limits of navigation depth , Only if the sampling points satisfy and If the path point is safe, the iteration is terminated; otherwise, the path is determined to have a collision risk or depth exceedance, and the target bias sampling is performed again.

7. The three-dimensional path planning method for multi-target point tasks of autonomous underwater vehicles according to claim 6, characterized in that, The specific steps for reselecting a parent node are as follows: After collision detection is completed, the new node is added to the tree, and the tree structure is updated; for new nodes that pass the collision detection... Given a neighborhood radius Searching in a 3D KD-Tree candidate node set ;right Each node in , calculation When it is the parent node Cumulative cost: ; Choose to The node with the lowest cumulative cost is selected as the new parent node, thus completing the parent node reselection. A new 3D collision detection is then performed based on the new parent node; if the collision detection passes, the node that fell into the collision is considered a suitable candidate. Neighborhood radius Reconnecting to nodes within the same node: For each node ,like: Then update The parent node is Otherwise, keep the original node connection.

8. The three-dimensional path planning method for multi-target point tasks of autonomous underwater vehicles according to claim 7, characterized in that, The specific operation for S3-7 is as follows: After reconnection, a new round of target bias sampling and subsequent operations will continue, continuously moving towards the target point on the sub-path. Extend; until the new node is selected after the parent node is re-elected. and The Euclidean distance between them is less than a preset threshold. At that time, stop expanding, along Tracing back the parent node chain to the root node yields a result consisting of multiple path points. The initial feasible sub-path formed .

9. The three-dimensional path planning method for multi-target point tasks of autonomous underwater vehicles according to claim 1, characterized in that, The specific operations of the improved particle swarm optimization based on path initialization are as follows: The particle structure of the traditional particle swarm optimization algorithm is improved by introducing a "multi-control point" strategy, enabling each improved particle to store three position points; each particle Q consists of three position points, which are the three control points on the path: Each location point is a three-dimensional coordinate point, represented as: M points are uniformly sampled from the initial path generated by RRT* to initialize particles; the sampling sequence number is defined as: in: ; The sampled first The coordinates of each RRT* path point are: The initial position of the j-th position of the k-th particle is represented as: In a three-dimensional coordinate system, the coordinate boundaries for each dimension are defined as follows: in: ; The lower bound; The upper boundary; For the j-th position of the k-th particle, in dimension The initial velocity is randomly generated from a uniform velocity, and its mathematical expression is: in: For obedience Uniformly distributed random variables are used to increase the randomness of the search; After initializing the particle position and velocity, the particle velocity and position are iteratively updated to search for a better path solution; After the t-th iteration, the velocity update formula for the j-th position of the k-th particle in dimension d is: in: Inertial weights control the extent to which a particle maintains its current velocity. As a cognitive learning factor, it controls the particle to move towards its historical best position. The degree of closeness; As a social learning factor, it controls the particles to move towards the global optimal position. The degree of closeness; and A random number between 0 and 1; t is the number of iterations; This is the position of the j-th position of the k-th particle after t iterations; If the speed update exceeds the limit, then a constraint will be imposed: The particle's position is updated based on the updated velocity. The position of the j-th position of the k-th particle in the (t+1)-th iteration is given by: After multiple iterations, the global optimal position of each particle is obtained, thereby constructing an optimal sub-path.

10. The three-dimensional path planning method for multi-target point tasks of autonomous underwater vehicles according to claim 1, characterized in that, The specific operation of path smoothing based on cubic spline interpolation is as follows: For the three-dimensional sub-path, respectively in Cubic spline interpolation is used on the axis; the key point coordinates of the path are set as follows: in: , , and , , These represent the coordinates of the starting point and the ending point, respectively. Generate equally spaced indices in the [0,1] interval: in: It is an index of discrete control points; It is the path point index generated after interpolation; To each Cubic spline interpolation of the axial coordinates: The final generated subpath is: Where: s represents the s-th segment of the path; .

Images