A multi-candidate solution-guided trajectory planning method for autonomous underwater vehicles in complex waters

Abstract

This invention discloses a multi-candidate solution-guided trajectory planning method for autonomous underwater vehicles (AUVs) in complex waters. It integrates a global multi-candidate solution guidance strategy, a real-time collision detection mechanism, and a tangent mapping mechanism to achieve efficient obstacle avoidance planning for AUVs in dynamic 3D environments. The method acquires the AUV's position and visible area at each moment, generates a target guidance line, and performs collision detection. If a straight line is feasible, the vehicle proceeds directly; if a collision occurs, the tangent point is extracted from the collision plane and mapped to the next motion plane to form a candidate waypoint. Furthermore, a multi-trajectory planner generates global guidance points, calculates risk and distance coefficients, comprehensively evaluates the guidance value of candidate points, and selects the optimal waypoint for iterative updates. Finally, a smooth and executable trajectory is generated. This method achieves synergy between local obstacle avoidance and global optimization, improving trajectory safety, smoothness, and real-time performance, and is suitable for autonomous navigation tasks in complex dynamic underwater environments.

Description

Technical Field

[0001] This invention belongs to the field of autonomous navigation and intelligent trajectory planning technology for autonomous underwater vehicles (AUVs), specifically involving a trajectory planning method guided by multiple candidate solutions for autonomous underwater vehicles in complex waters, and belongs to the field of real-time obstacle avoidance and trajectory optimization control technology for autonomous underwater vehicles in complex three-dimensional dynamic environments. Background Technology

[0002] With the rapid development of marine resource development, seabed exploration, underwater archaeology, and marine environmental monitoring, autonomous underwater vehicles (AUVs), as important underwater operational equipment, have become a key technological platform in marine engineering and marine scientific research. AUVs can complete predetermined tasks without human intervention, and their autonomous navigation and path planning capabilities directly affect the safety, efficiency, and success rate of mission execution. Therefore, how to achieve safe, reliable, and efficient trajectory planning in complex underwater environments has become an important research direction in the field of underwater robotics.

[0003] Compared to terrestrial or aerial environments, underwater environments exhibit significant differences and complexities. First, the underwater environment is a three-dimensional spatial structure, with obstacles distributed in a spatially three-dimensional manner, including seabed topography, reef structures, underwater facilities, and dynamic biological communities. Second, the underwater environment is dynamically changing, influenced by ocean currents, tides, and environmental disturbances, causing obstacles to move slowly or drift randomly. Furthermore, underwater communication is limited; GPS signals cannot be used directly, and AUVs typically rely on sonar or inertial navigation systems for positioning and environmental awareness, which have limited sensing range and are subject to measurement errors. These factors collectively increase the complexity of the trajectory planning problem.

[0004] Existing path planning methods mainly include graph search-based algorithms, sampling algorithms, and potential field algorithms. Graph search algorithms, such as the A* algorithm and its improved forms, can find optimal paths in a known static map, but in dynamic environments, they require frequent replanning, resulting in significant computational overhead. 。 While the D* algorithm can achieve a certain degree of dynamic updating, its computational complexity is high in high-dimensional environments. Sampling-based algorithms, such as the fast exploratory random tree algorithm, are suitable for high-dimensional spaces, but the generated paths often exhibit significant jitter, requiring additional smoothing processing, and their responsiveness to dynamic obstacles is limited. Potential field-based algorithms, such as the artificial potential field method, have advantages such as simple implementation and fast computation speed, but they are prone to getting trapped in local optima and suffer from oscillation problems in complex obstacle environments.

[0005] In dynamic underwater environments, traditional methods typically suffer from the following shortcomings: First, the planning process often relies on a single global path. When the local environment changes, the global path becomes invalid, requiring a complete replanning and impacting real-time performance. Second, there is insufficient coupling between local obstacle avoidance strategies and the global path, which can easily lead to local optima or excessive detours. Third, there is a lack of multi-strategy alternatives when facing dynamic obstacles, resulting in insufficient planning robustness. Fourth, high-frequency real-time calculations in three-dimensional space place high demands on algorithm efficiency, making it difficult for traditional methods to balance accuracy and real-time performance.

[0006] In recent years, researchers have begun to explore methods that combine multi-path planning with local real-time obstacle avoidance to improve the stability and adaptability of path planning in complex environments. However, most existing methods still focus on single evaluation indicators, such as shortest distance or lowest energy consumption, and lack a comprehensive assessment mechanism for environmental risk, path reliability, and dynamic adaptability. Furthermore, in the local planning stage, the generation of candidate waypoints is relatively simplistic, making it difficult to fully utilize global information to guide decision-making.

[0007] Therefore, in dynamic underwater environments, there is an urgent need for a trajectory planning method that can simultaneously combine multiple global trajectory information, perceive local environmental conditions in real time, and achieve efficient waypoint generation through rapid mapping and guidance mechanisms. This method should possess the following capabilities: first, the ability to establish a reasonable environmental model in complex three-dimensional space; second, the ability to generate multiple candidate global paths as guiding references to improve planning robustness; third, the ability to quickly generate safe and feasible waypoints in local environments; and fourth, the ability to maintain continuous and stable real-time planning capabilities even in the presence of dynamic obstacles.

[0008] Based on the above requirements, a trajectory planning method for autonomous underwater vehicles (AUVs) with multiple candidate solutions is proposed to combine global guidance with rapid local decision-making, thereby improving the navigation safety and real-time performance of AUVs in complex dynamic environments. Summary of the Invention

[0009] To address the problems of insufficient real-time performance, low coupling between local obstacle avoidance and global path, susceptibility to local optima, and poor planning robustness in existing technologies for autonomous underwater vehicles (AUVs) when planning trajectories in dynamic underwater environments, this invention provides a trajectory planning method for AUVs in complex waters guided by multiple candidate solutions.

[0010] This method constructs a three-dimensional environment model and combines multiple global trajectory guidance strategies with a local tangent mapping mechanism to effectively integrate global information with local real-time decision-making, thereby improving the navigation safety, real-time performance, and stability of autonomous underwater vehicles in complex dynamic environments.

[0011] To achieve the above objectives, the present invention provides the following technical solution.

[0012] 1. A trajectory planning method for autonomous underwater vehicles (AUVs) guided by multiple candidate solutions in complex waters, characterized by comprising the following steps:

[0013] S1. Problem Modeling: Establish a dynamic underwater environment model, including static obstacles, moving suspended obstacles, and three-dimensional ocean current field information; discretize the underwater space into a Cartesian coordinate system, with each discrete point marked by a grid to indicate its passability, and define the movement rules and threshold triggering mechanism for suspended obstacles; construct an AUV kinematic model to describe the AUV's six degrees of freedom motion (sway, roll, pitch, and yaw) in three-dimensional space, as well as its visible area; update the dynamic environment state using discrete time steps and record the AUV's real-time position at each time step;

[0014] S2. Algorithm Design: At each time step, obtain the current position of the AUV and perform the following operations:

[0015] S2.1 Real-time collision detection: Generate a target guide line from the current position to the destination and perform collision detection within the AUV's visible area;

[0016] S2.2 If the target guide line does not intersect with the obstacle, generate the next waypoint along it and update the waypoint as the current position to the next time step;

[0017] S2.3 Tangent Mapping and Local Trajectory Planning: If the target guide line collides with an obstacle, a set of tangent points is generated on the collision plane, and the tangent points are mapped to the next motion plane along the tangent to form a set of candidate waypoints;

[0018] S2.4 Introduce a multi-candidate solution global guidance strategy to generate multiple global trajectories and extract the intersection points with the next motion plane as a set of global guidance points. At the same time, calculate the risk coefficient and distance coefficient for each global guidance point, and select the optimal guidance point according to the weighted evaluation function.

[0019] S2.5 Fast Waypoint Generation: Combining the candidate waypoint set with the guidance vector of the optimal global guidance point, calculate the angle between the candidate waypoint and the guidance vector, and select the candidate waypoint with the smallest angle as the next waypoint, thus achieving a combination of local obstacle avoidance and global guidance.

[0020] S2.6. Set the selected waypoint as the current position and repeat steps S2.1 to S2.5 until the AUV reaches the destination.

[0021] S3. The continuous dynamic trajectory of the AUV is formed by the iteratively generated waypoint sequence, which satisfies the requirements of obstacle avoidance constraints, kinematic constraints and trajectory smoothness, and the complete trajectory sequence is recorded as the final output.

[0022] As a preferred embodiment of the present invention, the specific content of the S1 problem modeling is as follows:

[0023] S4.1 Underwater 3D Space Modeling:

[0024] To accurately describe the operating environment of autonomous underwater vehicles, a three-dimensional Cartesian coordinate system is established, and the underwater operating space is abstracted into a finite discrete cubic region:

[0025]

[0026] The space dimension is defined as , where any point is represented as .

[0027]

[0028] satisfy:

[0029]

[0030] A dynamic environment grid model is constructed through the discretization process described above.

[0031] S4.2 Obstacle Set Modeling:

[0032] Define the set of underwater obstacles as follows.

[0033]

[0034] And construct the obstacle indication function.

[0035]

[0036] in, This indicates that the space grid is occupied by an obstacle; This indicates that the spatial grid is a passable area. The obstacles include fixed seamount obstacles and mobile, suspended obstacles that can move with ocean currents.

[0037] S4.3, Ocean Current Field Model Construction:

[0038] A distributed ocean current field model is constructed in the three-dimensional spatial domain Q, and the influence of ocean currents in the X–Y plane is simulated by superimposing multiple vortices.

[0039] For a suspended obstacle O, its ocean current components in the X and Y directions are defined as follows:

[0040]

[0041]

[0042] in: Indicates the number of grid cells occupied by obstacle O; Representing positions respectively The ocean current component at that location. This model is used to drive the dynamic changes of suspended obstacles over time.

[0043] S4.4 Construction of Time Step Discretization Mechanism:

[0044] To implement the dynamic programming process, a discrete time step is defined. The interval between adjacent time steps is 1. Within each time step, the following operations are performed:

[0045] Update the position of the floating obstacle;

[0046] Update the current pose of the AUV;

[0047] Update the information of the partially visible area.

[0048] The system updates iteratively according to time steps, achieving spatiotemporal coupling modeling.

[0049] S4.5 Dynamic Model of Moving Suspended Obstacles:

[0050] Define the threshold sign function:

[0051]

[0052] in, This is the threshold parameter.

[0053] The cumulative ocean flow of the suspended obstacle at time step t is:

[0054]

[0055]

[0056] When the accumulated amount exceeds the threshold, the center coordinates of the suspended obstacle move one grid unit in the corresponding direction, realizing discretized dynamic drift.

[0057] S4.6 Spatiotemporal Representation of AUV Trajectory:

[0058] Define AUV in the first i The position of each time step is:

[0059]

[0060] The complete trajectory is represented as:

[0061]

[0062] in: Starting point; The target point is [point 1]. The trajectory evolves synchronously over time.

[0063] S4.7 Obstacle Avoidance Constraints:

[0064] The trajectory must meet the following requirements:

[0065]

[0066] That is, the waypoint at any time step must not be located within the area occupied by obstacles; otherwise, it is determined to be an infeasible trajectory.

[0067] S4.8, AUV kinematic angle constraints:

[0068] The yaw angle and pitch angle of an AUV are defined as follows:

[0069]

[0070]

[0071] The constraints are satisfied:

[0072]

[0073]

[0074] If any angle exceeds the limit, the path segment is deemed infeasible.

[0075] S4.9, Modeling of Locally Visible Regions:

[0076] At the current location Construct a visible area:

[0077]

[0078] in:

[0079] Forward detection range;

[0080] For horizontal detection width;

[0081] For vertical height detection.

[0082] This area is used to build the basis for local obstacle avoidance decision-making.

[0083] As a preferred embodiment of the present invention, the specific content of the S2 real-time collision detection is as follows;

[0084] S5.1 Target guide line generation;

[0085] At time step According to the current location of the AUV and the finish line Generate target guide lines:

[0086]

[0087] This straight line is used to indicate the optimal forward direction of the AUV, serving as a reference for collision detection.

[0088] S5.2 Collision detection within the visible area:

[0089] In the visible area Inside, the target guide line Perform discrete grid detection:

[0090]

[0091] Determine if there are any impassable grid cells. If not, the line is considered feasible (Case 1); otherwise, it is deemed infeasible (Case 2).

[0092] S5.3 Collision Point Location:

[0093] When the target guide line is detected to intersect with an obstacle, a collision point is defined:

[0094]

[0095] And record the collision plane:

[0096] This plane is used for subsequent tangent mapping and local obstacle avoidance calculations.

[0097] S5.4 Situation Handling Mechanism:

[0098] S5.4.1 Feasible straight line case (Case 1):

[0099] The AUV moves directly along the target guide line to the next YZ plane:

[0100]

[0101] No obstacle avoidance sub-process needs to be triggered.

[0102] S5.4.2 Infeasible straight line case (Case 2):

[0103] To trigger the obstacle avoidance algorithm, first generate a set of tangent points on the collision plane. Then it is mapped to the next motion plane. Form a set of candidate waypoints This is to inform the selection of the optimal waypoint in the next step.

[0104] As a preferred embodiment of the present invention, the specific details of the S2 tangent mapping and local trajectory planning are as follows:

[0105] S6.1 Generation of tangent points on the collision plane:

[0106] At the point of collision Plane Above, detect passable grid points along the obstacle boundary in four directions (up, down, left, and right), and record the center of each grid point as the tangent point. :

[0107]

[0108] in The number of pointcuts, set This is the set of candidate cut points.

[0109] S6.2, Tangent generation:

[0110] Relate each cut point to the current position Connecting lines forms tangents :

[0111]

[0112] in This represents the distance between the collision plane and the current position plane.

[0113] S6.3, Mapping the tangent point to the next motion plane:

[0114] According to the tangent Map the tangent point to the next motion plane. , obtain the mapping point :

[0115]

[0116] A set of mapping points is formed.

[0117]

[0118] gather These are candidate waypoints for the current time step of the AUV.

[0119] S6.4 Candidate point evaluation and local guidance preparation:

[0120] For each candidate point in the set of mapping points It can calculate the deflection angle with the global guidance vector (used in S7 later) and, combined with local environmental constraints, provide data support for the next step of quickly generating the optimal waypoint.

[0121] As a preferred embodiment of the present invention, the specific details of the S2 multi-solution global guidance strategy are as follows:

[0122] S7.1 Global Multi-Track Generation:

[0123] Generate on a known static 3D raster map using the multi-track planner MTrajPlanner A diverse global trajectory:

[0124]

[0125] Each trajectory It consists of multiple planar points and is used for subsequent real-time guidance of the AUV's local trajectory selection.

[0126] S7.2, Global Guide Point Extraction:

[0127] Place each trajectory in the next motion plane The intersection point on the map is defined as the global guide point:

[0128]

[0129] Form the global bootstrap point set for the current time step:

[0130]

[0131] S7.3 Global Guide Point Risk Assessment:

[0132] Calculate each global boot point risk factor :

[0133]

[0134] in This indicates the number of impassable lattice cells within the visible area of ​​the guiding point. A lower risk coefficient indicates a safer and more reliable point.

[0135] S7.4 Global Guide Point Distance Assessment: Calculate distance coefficient To depict the guide point and the current position Spatial relationships:

[0136]

[0137] Track points that are closer together are generally more reliable and can provide a more reasonable obstacle avoidance direction.

[0138] S7.5 Comprehensive Assessment of Guiding Value:

[0139] By combining the risk factor and the distance factor, the guiding value of each global guiding point is calculated. :

[0140]

[0141] in and These are the risk weight and the distance weight, respectively, both set to 0.5 in this embodiment. The smaller the value, the higher the guiding value.

[0142] S7.6 Optimal Global Guiding Point Selection:

[0143] Based on guiding value from the collection Select the optimal global boot point :

[0144]

[0145] Selected Used as a reference point for subsequent local decisions, guiding the AUV to select a waypoint in the next time step.

[0146] As a preferred embodiment of the present invention, the specific details of the S2 fast waypoint generation are as follows:

[0147] S8.1 Determine the optimal guiding vector:

[0148] The optimal global guiding point selected in step S7 As a reference point, calculate the current position. point to Optimal guiding vector :

[0149]

[0150] This vector indicates the global direction for local trajectory selection, providing a reference for candidate waypoints.

[0151] S8.2 Calculation of turning vectors for candidate waypoints:

[0152] For the set of mapping points Each candidate waypoint Calculate its position relative to the current position Steering vector :

[0153]

[0154] in For the first There are 10 candidate waypoints. This vector is used to measure the degree of deviation of the candidate point from the global guidance direction.

[0155] S8.3 Evaluation of the angle between candidate waypoints:

[0156] Calculate the turning vector for each candidate waypoint With the optimal guiding vector The angle between :

[0157]

[0158] The smaller the angle, the more consistent the candidate waypoint is with the global guidance direction, and the better it matches the overall navigation objective.

[0159] S8.4, Next Waypoint Selection:

[0160] In the set The candidate waypoint with the smallest included angle is selected as the next waypoint. :

[0161]

[0162] Ensure that the AUV moves along the globally optimal direction as much as possible while avoiding obstacles locally, achieving a dynamic balance between local and global aspects.

[0163] S8.5 Waypoint Generation and Iteration:

[0164] Select the waypoint in step S8.4 As the target point for the AUV in the next time step, the S5–S8 process is repeated until the endpoint is reached. :

[0165] Update current location

[0166] Update visible environment information

[0167] Recalculate the collision point, tangent mapping, global guide point, and waypoint.

[0168] This cycle ensures continuous, safe, and efficient trajectory planning for the AUV in dynamic underwater environments.

[0169] As a preferred embodiment of the present invention, the specific content of the S3 trajectory output is as follows:

[0170] S9.1 Record the waypoint sequence:

[0171] The waypoints calculated at each time step Record sequentially to form a continuous trajectory sequence:

[0172]

[0173] in Starting point Each subsequent All are obtained from the fast waypoint generation in step S8.

[0174] S9.2 Trajectory Smoothing Processing:

[0175] for waypoint sequence Perform smoothing optimization, such as using B-spline methods or other smoothing interpolation methods, to eliminate local polylines or abrupt changes, and obtain the final executable trajectory. ::

[0176]

[0177] Smoothing ensures the continuity and dynamic feasibility of AUV execution.

[0178] S9.3, Destination arrival judgment and trajectory output:

[0179] Determine the current position Have you reached the destination?

[0180] If the conditions are met, terminate the real-time planning loop and finalize the trajectory. Output;

[0181] If the destination is not reached, return to S2 and continue iterating to generate the next waypoint.

[0182] S9.4 Output trajectory and navigation information:

[0183] Final trajectory It provides information to the AUV control system and outputs the corresponding speed, direction, and risk assessment information for each waypoint, forming a complete dynamic navigation command set to provide a reference for the autonomous movement of the AUV.

[0184] Compared with the prior art, the advantages of the present invention are:

[0185] 1. Multi-solution global guidance strategy: This invention introduces the concept of multi-solution global optimization into dynamic trajectory planning for the first time. By generating multiple global candidate trajectories and comprehensively evaluating them using risk and distance coefficients, the optimal global guidance point is dynamically selected for the generation of local waypoints. This strategy enables trajectory planning to possess both global strategic scope and local flexibility, significantly improving the robustness of planning and the success rate of tasks compared to existing methods that rely solely on a single path or purely local obstacle avoidance.

[0186] 2. A Fast Waypoint Generation Strategy Based on Tangent Mapping: For the local dynamic obstacle avoidance problem, this invention proposes a fast waypoint generation mechanism based on tangent mapping. By obtaining the tangent point at the collision plane and mapping it to the next motion plane, and combining this with the optimal guidance vector for angle evaluation and selection, candidate waypoints are quickly generated. This strategy avoids high-dimensional search and complex optimization calculations, significantly improving the real-time performance of dynamic programming, enabling AUVs to update their trajectories frequently and accurately in complex three-dimensional underwater environments.

[0187] 3. Local-Global Cooperative Decision-Making Strategy: In the dynamic trajectory planning process, this invention closely integrates rapid local obstacle avoidance with global information from multiple solutions to achieve local-global cooperative decision-making. The local strategy ensures safe and smooth obstacle avoidance, while the global strategy provides directional guidance and overall optimization support. Their synergistic effect effectively avoids getting trapped in local optima, improving trajectory continuity and target orientation. In dynamically changing environments with limited visibility, this strategy maintains the feasibility and efficiency of trajectory planning, significantly outperforming existing single-solution or purely local obstacle avoidance methods. Attached Figure Description

[0188] Figure 1 This is a schematic diagram of an underwater space model;

[0189] Figure 2 A schematic diagram of the kinematic model of an AUV;

[0190] Figure 3 This is a schematic diagram of the visible area;

[0191] Figure 4 A flowchart illustrating the proposed trajectory planning method;

[0192] Figure 5 A schematic diagram of the set of tangent points of the collision plane;

[0193] Figure 6 This is a schematic diagram of obtaining mapping points based on tangents; Detailed Implementation

[0194] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0195] This invention provides an implementation method, taking the trajectory planning of an underwater submarine as an example.

[0196] like Figure 1As shown, this invention first establishes an underwater space model to describe the motion constraints and obstacle distribution of an autonomous underwater vehicle (AUV) in a complex three-dimensional underwater environment. The underwater space is represented using a three-dimensional Cartesian coordinate system, where the X-axis, Y-axis, and Z-axis correspond to the AUV's forward direction, vertical plane direction, and depth direction, respectively. This three-dimensional model includes static obstacles, dynamic obstacles, and environmental factors such as water flow disturbances (e.g.,...). Figure 1 (c) shows the model to ensure it reflects the complexity and uncertainty of the actual underwater environment. In the model, obstacles are discretized into grid cells, each with a drivability marker (e.g., ...). Figure 1 (b) is shown and used for subsequent trajectory planning and collision detection.

[0197] like Figure 2 As shown, the schematic diagram of the AUV kinematic model is used to characterize the motion characteristics of an AUV in an underwater environment. The state variables of the AUV include position coordinates (…). x , y , z ) and velocity vector ( u , v , w The AUV updates its position along its direction of motion at each time step t, while being affected by water flow disturbances and dynamic constraints. The motion model considers not only linear velocity changes but also acceleration and steering angle constraints to ensure that the waypoints generated by trajectory planning can be executed by the actual control system, thereby guaranteeing the feasibility and stability of the planned trajectory.

[0198] like Figure 3 As shown, this invention defines the visible area of ​​an AUV for real-time acquisition of local environmental information. Visible Area Based on the current location of the AUV Centered on a central point, a perception range of a certain size is formed within three-dimensional space. The traversability information of grid points within the visible area is updated in real time, dynamically reflecting factors such as obstacle movement and ocean current changes. By defining the visible area, this invention enables real-time response to local collision detection and obstacle avoidance strategies, while reducing the computational load of global search and improving planning efficiency.

[0199] In this embodiment of the invention, the trajectory planning problem is formalized as starting from the starting point... To the finish line This is a dynamic programming problem. The goal of this problem is to achieve efficient movement of an AUV while ensuring safe obstacle avoidance, that is, to generate a continuous and smooth trajectory in three-dimensional space, enabling the AUV to avoid obstacles within the visible area and comply with kinematic constraints. Specifically, the planning problem can be expressed as solving for a set of waypoints at a series of time steps t, given environmental constraints, dynamic constraints, and global navigation objectives. ,in Starting from point S, With E as the destination, each waypoint meets the requirements for navigability and local obstacle avoidance.

[0200] By modeling the above problems and combining them Figures 1 to 3 The three-dimensional spatial model, AUV kinematic model, and visible area settings shown in this invention provide a rigorous and feasible mathematical and engineering foundation for subsequent dynamic trajectory planning algorithms. Based on this model, this invention further proposes a trajectory planning algorithm to achieve dynamic trajectory planning based on multi-solution optimization, thereby addressing dynamic obstacles and environmental disturbances that may occur in complex underwater environments.

[0201] Figure 4 This is a complete flowchart of the algorithm proposed in this invention. The flowchart clearly shows the overall framework of dynamic trajectory planning and the logical relationships between each sub-module. In the diagram, the starting point S of the AUV is taken as the navigation start position, and the ending point E is taken as the target position. The algorithm executes the following main processes at each time step: First, the time step is initialized. And the current position of the AUV, and obtain the surrounding visible area. and environmental information; then, a target guide line pointing to the endpoint is generated. And perform real-time collision detection within the visible area to determine if the line is feasible; if the line is feasible, then follow... Generate the next waypoint; if a straight line is not feasible, trigger the obstacle avoidance algorithm, perform collision point detection, tangent point generation and mapping point calculation, and combine with the multi-solution global guidance strategy to generate a set of candidate waypoints and select the optimal waypoint; finally, update the generated waypoint to the current position, and repeat the above process until the destination E is reached. Figure 4 The document marks the core modules such as local trajectory planning, tangent mapping, multi-trajectory guidance, and rapid waypoint generation, and displays the input-output relationships between each module, providing an overall reference for subsequent detailed explanations of each sub-module.

[0202] Specifically, when using it:

[0203] 1. Initialization: First, the AUV's state, mission objective, and environmental information need to be initialized. The algorithm sets the AUV's initial time step to t = 0 to record the position update for each iteration during trajectory planning. The size of the time step is determined based on the AUV's speed, environmental complexity, and sensor update frequency to ensure real-time performance and accuracy. Then, the AUV's current position is used as the mission starting point S, including its 3D coordinates and initial heading angle. Simultaneously, the 3D coordinates of the mission endpoint E are defined to generate a target guidance line and provide motion direction reference during planning. In environmental initialization, known suspended objects, seamount obstacles, and other static obstacles need to be marked to form a preliminary 3D grid map. Each obstacle corresponds to an impassable grid point for subsequent collision detection. Simultaneously, the algorithm constructs the visible area based on the AUV's sensor performance. This represents the three-dimensional spatial range that the AUV can perceive at the current time step, used for real-time collision detection and local trajectory planning. In the initial stage, the starting point... It is also set as the first waypoint, providing a reference for generating the next waypoint. After completing the above initialization, the algorithm can enter the main loop and begin the dynamic trajectory planning process;

[0204] 2. Real-time collision detection mechanism: In the embodiments of this invention, the algorithm first performs real-time collision detection at each time step. At time step... Inside, the AUV adjusts its position based on the current location. and the finish line Generate target guide line ,

[0205] This target guideline is used to indicate the optimal direction of travel for the AUV and serves as a reference for collision detection. Within the visible area... Inside, the target guide line Discrete grid detection is performed to determine passability, using the following formula:

[0206]

[0207] If the straight line does not collide with any obstacle within the visible area, it is considered a feasible situation (Case 1), and the AUV can directly generate the next waypoint along the target guide line:

[0208]

[0209] Otherwise, if the target guide line is detected to intersect with an obstacle (case 2), the obstacle avoidance algorithm is triggered. The collision point is located on the collision plane. ::

[0210]

[0211] And record the collision plane ,in This represents the distance between the collision plane and the current plane. This collision plane provides the basis for subsequent tangent mapping and local obstacle avoidance calculations.

[0212] In Case 1, the AUV moves smoothly along the target guidance line without needing to perform an obstacle avoidance sub-process; in Case 2, a set of tangent points will be generated on the collision plane and mapped to the next motion plane, thus providing a basis for the selection of candidate waypoints.

[0213] 3. Tangent mapping and local trajectory planning strategy: When the AUV detects that the target guideline is infeasible (i.e., a collision point exists), it will attempt to detour. Afterwards, tangent mapping and local trajectory planning strategies will be triggered to generate candidate waypoints and guide the AUV safely and efficiently through the obstacle area. The specific process is as follows:

[0214] First, at the point of collision Plane Detect passable grid points along the obstacle boundary in the four directions (up, down, left, and right), and record the center point of each grid point as the tangent point. This forms a set of tangent points:

[0215]

[0216] in, The number of pointcuts, set That is, the set of candidate tangent points ( Figure 5 (As shown).

[0217] Then, compare each cut point with the current position of the AUV. Connect the lines to obtain the tangent:

[0218]

[0219] in, Indicates the distance between the collision plane and the current position plane ( Figure 5 (Indication of tangent line).

[0220] Next, the tangent points on each tangent line are mapped to the next motion plane. , obtain the mapping point :

[0221]

[0222] This results in a set of mapping points:

[0223]

[0224] gather These are the candidate waypoints that the AUV can select at the current time step. Figure 6 (As shown).

[0225] After obtaining the set of mapped points, each candidate point This information will be combined with global guidance information for subsequent rapid waypoint generation. Using this method, AUVs can quickly select multiple feasible route directions under local collision constraints, achieving a balance between local obstacle avoidance and global navigation objectives.

[0226] 4. Multi-solution global guidance strategy: After completing local tangent mapping, the AUV will utilize a multi-solution global guidance strategy to optimize the selection of candidate waypoints, balancing local obstacle avoidance with overall trajectory planning. The specific process is as follows:

[0227] First, generate a multi-track planner MTrajPlanner on a known static 3D raster map. A diverse global trajectory:

[0228]

[0229] Each trajectory It consists of multiple planar points and is used to provide global guidance information for the AUV in the next time step.

[0230] Then, each trajectory is mapped onto the next motion plane. The intersection point on the map is defined as the global guide point:

[0231]

[0232] Form the global bootstrap point set for the current time step:

[0233]

[0234] To ensure the feasibility of global boot points, a risk factor is introduced. It is based on the number of impassable lattice blocks within the visible region of the guiding point. calculate:

[0235]

[0236] The lower the risk coefficient, the safer and more reliable the guiding point is.

[0237] At the same time, calculate the distance coefficient. To depict the guide point and the current position Spatial relationships between them:

[0238]

[0239] Closer guide points are generally more reliable and can provide reasonable obstacle avoidance directions.

[0240] Next, the risk coefficient and distance coefficient are combined to calculate the comprehensive guidance value of each global guidance point. :

[0241]

[0242] in, and These are risk weight and distance weight, respectively, both set to [value] in this embodiment. The smaller the value, the higher the guiding value.

[0243] Ultimately, the global guiding point with the optimal guiding value is selected at the current time step. :

[0244]

[0245] Selected This will serve as a reference point for the next step of generating fast waypoints, guiding the AUV to select a waypoint from local candidate points that aligns with the globally optimal direction, thereby achieving a balance between local obstacle avoidance and global trajectory planning.

[0246] 5. Fast waypoint generation:

[0247] As a preferred embodiment of the present invention, the fast waypoint generation method incorporates local tangent mapping points. With global boot point To quickly generate the next waypoint in a dynamic underwater environment, as detailed below:

[0248] First, determine the optimal global guiding vector. .

[0249]

[0250] This vector represents the current position. Pointing to the optimal global boot point The direction provides a global reference for candidate waypoints.

[0251] For the set of mapping points Each candidate waypoint Calculate relative to the current position Local steering vector :

[0252]

[0253] This vector is used to measure the degree of deviation of the candidate point from the global guiding direction.

[0254] Subsequently, the angle between the steering vector and the optimal guidance vector at each candidate waypoint is calculated. :

[0255]

[0256] The smaller the angle, the more consistent the candidate waypoint is with the global guidance direction, and the better it matches the overall navigation goal.

[0257] Based on the principle of minimizing the included angle, the next waypoint is selected from the candidate points. :

[0258]

[0259] Through the above steps, the AUV can avoid obstacles locally while moving in the globally optimal direction as much as possible, achieving a dynamic balance between local and global aspects.

[0260] Select waypoints Update to the current position and repeat the process of collision detection, tangent mapping, multiple solution guidance and fast waypoint generation until the destination is reached.

Claims

1. A trajectory planning method for autonomous underwater vehicles (AUVs) in complex waters, guided by multiple candidate solutions, characterized in that, Specifically, the following steps are included: S1. Problem Modeling: Establish a dynamic underwater environment model, including static obstacles, moving suspended obstacles, and three-dimensional ocean current field information; discretize the underwater space into a Cartesian coordinate system, with each discrete point marked by a grid to indicate its passability, and define the movement rules and threshold triggering mechanism for suspended obstacles; construct an AUV kinematic model to describe the AUV's six degrees of freedom motion (sway, roll, pitch, and yaw) in three-dimensional space, as well as its visible area; update the dynamic environment state using discrete time steps and record the AUV's real-time position at each time step; S2. Algorithm Design: At each time step, obtain the current position of the AUV and perform the following operations: S2.1 Real-time collision detection: Generate a target guide line from the current position to the destination and perform collision detection within the AUV's visible area; S2.2 If the target guide line does not intersect with the obstacle, generate the next waypoint along it and update the waypoint as the current position to the next time step; S2.3 Tangent Mapping and Local Trajectory Planning: If the target guide line collides with an obstacle, a set of tangent points is generated on the collision plane, and the tangent points are mapped to the next motion plane along the tangent to form a set of candidate waypoints; S2.4 Introduce a multi-solution global guidance strategy to generate multiple global trajectories and extract the intersection points with the next motion plane as a set of global guidance points. At the same time, calculate the risk coefficient and distance coefficient for each global guidance point, and select the optimal guidance point according to the weighted evaluation function. S2.5 Fast Waypoint Generation: Combining the candidate waypoint set with the guidance vector of the optimal global guidance point, calculate the angle between the candidate waypoint and the guidance vector, and select the candidate waypoint with the smallest angle as the next waypoint, thus achieving a combination of local obstacle avoidance and global guidance. S2.

6. Set the selected waypoint as the current position and repeat steps S2.1 to S2.5 until the AUV reaches the destination. S3. The continuous dynamic trajectory of the AUV is formed by the iteratively generated waypoint sequence, which satisfies the requirements of obstacle avoidance constraints, kinematic constraints and trajectory smoothness, and the complete trajectory sequence is recorded as the final output.

2. The trajectory planning method for autonomous underwater vehicles (AUVs) guided by multiple candidate solutions in complex waters, as described in claim 1, is characterized in that: The modeling of the S1 problem specifically includes the following: S4.1 Underwater 3D Space Modeling: To accurately describe the operating environment of autonomous underwater vehicles, a three-dimensional Cartesian coordinate system is established, and the underwater operating space is abstracted into a finite discrete cubic region: ； The space dimension is defined as Any point therein is represented as: ； satisfy: ； A dynamic environment grid model is constructed through the above discretization process; S4.2 Obstacle Set Modeling: Define the set of underwater obstacles as: ； And construct the obstacle indication function: ； in, This indicates that the space grid is occupied by an obstacle; This indicates that the spatial grid is a passable area. The obstacles include fixed seamount obstacles and movable, suspended obstacles that can move with ocean currents; S4.3, Ocean Current Field Model Construction: In the three-dimensional space domain An internally constructed distributed ocean current field model was used to simulate the influence of X–Y plane ocean currents by superimposing multiple vortices. For suspended obstacles Its ocean current components in the X and Y directions are defined as follows: ； ； in: Indicates obstacles Number of grid cells occupied; Representing positions respectively The ocean current component at that location. This model is used to drive the dynamic changes of suspended obstacles over time; S4.4, Construction of Time Step Discretization Mechanism: To implement the dynamic programming process, a discrete time step is defined. t The interval between adjacent time steps is 1; the following operations are performed within each time step: 1) Update the position of floating obstacles; 2) Update the current pose of the AUV; 3) Update the information of the locally visible area; The system updates iteratively according to time steps, achieving spatiotemporal coupling modeling; S4.5 Dynamic Model of Moving Suspended Obstacles: Define the threshold sign function: ； in, For threshold parameters; The cumulative ocean flow of the suspended obstacle at time step t is: ； ； When the accumulated amount exceeds the threshold, the center coordinates of the suspended obstacle move one grid unit in the corresponding direction to achieve discretized dynamic drift. S4.6 Spatiotemporal Representation of AUV Trajectory: Define AUV in the first The position of each time step is: ； The complete trajectory is represented as: ； in: Starting point; The target point is [location]. The trajectory evolves synchronously over time. S4.7 Obstacle Avoidance Constraints: The trajectory must meet the following requirements: ； That is, the waypoint at any time step must not be located within the area occupied by obstacles; otherwise, it will be judged as an infeasible trajectory. S4.8, AUV kinematic angle constraints: The yaw angle and pitch angle of an AUV are defined as follows: ； ； The constraints are satisfied: ； ； If any angle exceeds the limit, the path segment is deemed infeasible. S4.9, Modeling of Locally Visible Regions: At the current location Construct a visible area: ； in: · Forward detection range; · For horizontal detection width; · For vertical height detection; This area is used to build the basis for local obstacle avoidance decisions.

3. The trajectory planning method for autonomous underwater vehicles (AUVs) guided by multiple candidate solutions in complex waters, as described in claim 2, is characterized in that: The real-time collision detection specifically includes the following steps: S5.1 Target Guide Line Generation: At time step According to the current location of the AUV and the finish line Generate target guide lines: ； This straight line is used to indicate the optimal forward direction of the AUV, serving as a reference for collision detection; S5.2 Collision detection within the visible area: In the visible area Inside, the target guide line Perform discrete grid detection: ； Determine if there are any impassable grid cells. If not, the line is considered feasible (Case 1); otherwise, it is deemed infeasible (Case 2). S5.3 Collision Point Location: When the target guide line is detected to intersect with an obstacle, a collision point is defined: ； And record the collision plane: ； This plane is used for subsequent tangent mapping and local obstacle avoidance calculations; S5.4 Situation Handling Mechanism: S5.4.1 Feasible straight line case (Case 1): The AUV moves directly along the target guide line to the next YZ plane: ； No obstacle avoidance sub-process needs to be triggered; S5.4.2 Infeasible straight line case (Case 2): To trigger the obstacle avoidance algorithm, first generate a set of tangent points on the collision plane. Then it is mapped to the next motion plane. Form a set of candidate waypoints This is to inform the selection of the optimal waypoint in the next step.

4. The trajectory planning method for autonomous underwater vehicles (AUVs) guided by multiple candidate solutions in complex waters, as described in claim 1, is characterized in that: The specific steps of tangent mapping and local trajectory planning are as follows: S6.1 Generation of tangent points on the collision plane: At the point of collision Plane Above, detect passable grid points along the obstacle boundary in four directions (up, down, left, and right), and record the center of each grid point as the tangent point. : ； in The number of pointcuts, set For the set of candidate tangent points; S6.2, Tangent generation: Relate each cut point to the current position Connecting lines forms tangents : ； in This represents the distance between the collision plane and the current position plane; S6.3, Mapping the tangent point to the next motion plane: According to the tangent Map the tangent point to the next motion plane. , obtain the mapping point : ； Form a set of mapping points: ； gather Candidate waypoints for the AUV at the current time step; S6.4 Candidate point evaluation and local guidance preparation: For each candidate point in the set of mapping points It can calculate the deflection angle with the global guidance vector (used in S7 later) and, combined with local environmental constraints, provide data support for the next step of quickly generating the optimal waypoint.

5. The trajectory planning method for autonomous underwater vehicles (AUVs) guided by multiple candidate solutions in complex waters, as described in claim 1, is characterized in that: The specific steps of the multi-solution global guidance strategy are as follows: S7.1 Global Multi-Track Generation: Generate on a known static 3D raster map using the multi-track planner MTrajPlanner A diverse global trajectory: ； Each trajectory It consists of multiple planar points and is used for subsequent real-time guidance of the AUV's local trajectory selection; S7.2, Global Guide Point Extraction: Place each trajectory in the next motion plane The intersection point on the map is defined as the global guide point: ； Form the global bootstrap point set for the current time step: ； S7.3 Global Guide Point Risk Assessment: Calculate each global boot point risk coefficient : ； in This indicates the number of impassable lattice cells within the visible area of ​​the guiding point. A lower risk coefficient indicates a safer and more reliable point. S7.4 Global Guide Point Distance Assessment: Calculate distance coefficient To depict the guide point and the current position Spatial relationships: ； Track points that are closer together are generally more reliable and can provide more reasonable obstacle avoidance directions; S7.5 Comprehensive Assessment of Guiding Value: By combining the risk factor and the distance factor, the guiding value of each global guiding point is calculated. : ； in and These are the risk weight and the distance weight, respectively, both set to 0.5 in this embodiment. A smaller value indicates higher guidance value. S7.6 Optimal Global Guiding Point Selection: Based on guiding value from the collection Select the optimal global boot point : ； Selected Used as a reference point for subsequent local decisions, guiding the AUV to select a waypoint in the next time step.

6. The trajectory planning method for autonomous underwater vehicles (AUVs) guided by multiple candidate solutions in complex waters, as described in claim 1, is characterized in that: The specific steps for generating fast waypoints are as follows: S8.1 Determine the optimal guiding vector: The optimal global guiding point selected in step S7 As a reference point, calculate the current position. point to Optimal guiding vector : ； This vector indicates the global direction for local trajectory selection, providing a reference for candidate waypoints; S8.2 Calculation of turning vectors for candidate waypoints: For the set of mapping points Each candidate waypoint Calculate its position relative to the current position Steering vector : ； in For the first There are 10 candidate waypoints; this vector is used to measure the degree of deviation of the candidate point from the global guidance direction. S8.3 Evaluation of the angle between candidate waypoints: Calculate the turning vector for each candidate waypoint With the optimal guiding vector The angle between : ； The smaller the angle, the more consistent the candidate waypoint is with the global guidance direction and the better it matches the overall navigation objective; S8.4, Next Waypoint Selection: In the set The candidate waypoint with the smallest included angle is selected as the next waypoint. : ； Ensure that the AUV moves along the globally optimal direction as much as possible while avoiding obstacles locally, achieving a dynamic balance between local and global aspects; S8.5 Waypoint Generation and Iteration: Select the waypoint in step S8.4 As the target point for the AUV in the next time step, the process of S5–S8 is repeated until the endpoint is reached. : (1) Update the current position ; (2) Update visible environment information ; (3) Recalculate the collision point, tangent point mapping, global guidance point, and waypoint; This cycle ensures continuous, safe, and efficient trajectory planning for the AUV in dynamic underwater environments.

7. The trajectory planning method for autonomous underwater vehicles (AUVs) guided by multiple candidate solutions in complex waters, as described in claim 1, is characterized in that: The specific steps for trajectory output are as follows: S9.1 Record the waypoint sequence: The waypoints calculated at each time step Record sequentially to form a continuous trajectory sequence: ； in Starting point Each subsequent All are obtained from the fast waypoint generation in step S8; S9.2 Trajectory Smoothing Processing: for waypoint sequence Perform smoothing optimization, such as using B-spline methods or other smoothing interpolation methods, to eliminate local polylines or abrupt changes, and obtain the final executable trajectory. : ； Smoothing ensures the continuity and dynamic feasibility of AUV execution; S9.3, Destination arrival judgment and trajectory output: 1) Determine the current position Have you reached the destination? ; 2) If the conditions are met, terminate the real-time planning loop and determine the final trajectory. Output; 3) If the destination is not reached, return to S2 and continue iterating to generate the next waypoint; S9.4 Output trajectory and navigation information: Final trajectory It provides information to the AUV control system and outputs the corresponding speed, direction, and risk assessment information for each waypoint, forming a complete dynamic navigation command set to provide a reference for the autonomous movement of the AUV.

Images