Unmanned ship path planning method and system based on fusion of artificial potential field and dwa

Abstract

The application discloses an unmanned ship path planning method and system based on fusion of artificial potential field and DWA, belongs to the technical field of path planning, and solves the problem that global path planning and local obstacle avoidance are separately performed or simply combined in series in the prior unmanned ship path planning method, and an effective cooperation mechanism is lacked, the method comprises the following steps: constructing a grid map model of an unmanned ship working environment, adopting a bidirectional RRT* algorithm guided by an artificial potential field to perform global path planning on the unmanned ship, extracting key nodes from an initial global path and performing redundant node elimination optimization, and dynamically adjusting weights by constructing an adaptive evaluation function, in the application, the bidirectional RRT* algorithm guided by the artificial potential field improves the efficiency and directionality of global path planning of the unmanned ship, through a global-local closed loop fusion mechanism, the unmanned ship can follow an optimized global route and flexibly cope with dynamic obstacles in a complex sea environment, and the safety, efficiency and stability of navigation are comprehensively ensured.

Description

Technical Field

[0001] This invention belongs to the field of path planning technology, specifically relating to an unmanned vessel path planning method and system based on the fusion of artificial potential field and DWA. Background Technology

[0002] As an important type of intelligent marine equipment, unmanned surface vessels (USVs) have shown broad application prospects in recent years in fields such as marine environmental monitoring, resource exploration, and maritime patrol and search and rescue. With the increasing frequency of marine development activities, the autonomous navigation capability of USVs has become a key technological bottleneck restricting their effectiveness. Path planning, as the core link of autonomous navigation of USVs, directly determines the safety, efficiency, and adaptability of ships in complex marine environments.

[0003] Current unmanned surface vessel (USV) path planning technologies are mainly divided into two categories: global planning and local planning. At the global path planning level, traditional algorithms perform well in structured environments, but suffer from low computational efficiency and insufficient path smoothness in large-scale complex sea areas. While Probabilistic Route Maps (PRM) and Rapid Random Tree Exploration (RRT) algorithms can adapt to complex environments, they suffer from strong randomness and slow convergence. In particular, the standard RRT algorithm lacks directional guidance during expansion, easily generating a large number of redundant search nodes. For local obstacle avoidance, common methods include Dynamic Window (DWA) and artificial potential field methods. Although the DWA can adjust the USV's speed and direction in real time to avoid obstacles, it is prone to local oscillations when facing complex dynamic obstacles, causing the USV to hover near obstacles, unable to move effectively, and potentially even deadlocking and failing to reach the target point. Furthermore, the traditional artificial potential field method is easily affected by the strong repulsive force of obstacles when guiding USVs to avoid obstacles, causing the USV to veer sharply or deviate from the target direction when approaching obstacles, making it difficult to achieve a smooth obstacle avoidance trajectory.

[0004] Existing unmanned surface vessel (USV) path planning methods either separate global path planning and local obstacle avoidance or combine them in a simple, sequential manner, lacking an effective coordination mechanism. This simple sequential approach makes it difficult for USVs to achieve a good balance between global path guidance and local obstacle avoidance during actual navigation, resulting in compromised navigation safety, real-time performance, and overall efficiency in complex and dynamic sea areas. To address these issues, we propose a USV path planning method and system based on the fusion of artificial potential fields and Dynamical Obstacle Avoidance (DWA). Summary of the Invention

[0005] The purpose of this invention is to address the shortcomings of existing technologies by providing an unmanned surface vessel (USV) path planning method and system based on the fusion of artificial potential field and DWA. This solves the problem that in existing USV path planning methods, global path planning and local obstacle avoidance are performed separately or simply combined in sequence for obstacle avoidance, lacking an effective coordination mechanism. This results in the inability to guarantee the navigation safety, real-time performance, and global efficiency of USVs in complex and dynamic sea areas.

[0006] This invention is implemented as follows: an unmanned surface vessel path planning method based on the fusion of artificial potential field and DWA, the method comprising:

[0007] S10, Construct a grid map model of the unmanned vessel's working environment to determine the unmanned vessel's starting point, ending point, static obstacles, and temporary obstacle information;

[0008] S20, using the bidirectional RRT* algorithm guided by an artificial potential field to perform global path planning for the unmanned vessel and obtain the initial global path;

[0009] S30, the initial global path is optimized by extracting key nodes and removing redundant nodes to obtain an optimized global path formed by connecting key nodes in sequence;

[0010] S40: Load the optimized global path, divide the optimized global path into segments according to key nodes to obtain at least one set of segmented paths, and use the improved dynamic window method to perform local path planning and real-time obstacle avoidance on each segmented path. By constructing an adaptive evaluation function to dynamically adjust the weights, dynamic obstacle avoidance and trajectory smoothing are achieved.

[0011] Preferably, the method for global path planning of unmanned vessels using the bidirectional RRT* algorithm guided by an artificial potential field includes:

[0012] The bidirectional RRT* algorithm is used to construct two random trees, the start tree and the end tree, with the start and end points of the unmanned vessel in the grid map model as root nodes. During the expansion of the start and end trees, an artificial potential field method is introduced. When the distance between the new nodes of the two random trees is one step, the two random trees are connected to obtain an initial global path connecting the start and end points. When the artificial potential field method is introduced during the expansion of the start and end trees, the artificial potential field method includes an attraction function and a repulsion function.

[0013] Preferably, when the starting tree expands, the gravitational target is the endpoint, and the repulsive source is the obstacle. The gravitational function of the starting tree, defined as the artificial potential field, is expressed as:

[0014] (1)

[0015] In the formula, The gravitational coefficient, This is the Euclidean distance from the current unmanned vessel's position to the target point, and its direction vector points from the current node position to the target point position.

[0016] The improved repulsion function of the starting point tree is expressed as:

[0017] (2)

[0018] In the formula, Indicates the repulsion coefficient. This represents the Euclidean distance between the current position of the unmanned surface vessel and the obstacle. It is the radius of influence of the obstacle; when the distance exceeds... At that time, the repulsive force of the obstacle on the node will disappear;

[0019] The combined potential field of the starting tree is represented as:

[0020] (3)

[0021] in, Represents the combined potential field of the starting tree;

[0022] When the endpoint tree expands, the gravitational target is the starting position, and the repulsive force comes from obstacles. The gravitational function of the endpoint tree, defined as the artificial potential field, is expressed as:

[0023] (4)

[0024] In the formula, The Euclidean distance between the current position of the unmanned vessel and the starting point is given by the vector direction from the current node to the starting point.

[0025] The improved repulsion function of the endpoint tree is expressed as:

[0026] (5)

[0027] Indicates the repulsion coefficient. This represents the Euclidean distance between the current position of the unmanned surface vessel and the obstacle. It is the radius of influence of the obstacle.

[0028] Preferably, the method for optimizing the initial global path by extracting key nodes and removing redundant nodes includes:

[0029] Load and iterate through the initial global path, placing all path nodes in the initial global path into the node set in order. , , … };

[0030] The starting point and the target point are always considered key nodes in the node set. Traversing the node set, starting from the starting node... Start by connecting the nodes in the set sequentially until you reach the set. When the path between nodes encounters an obstacle, at this time It was identified as a critical node, and then from Begin by connecting the remaining nodes one by one until all the key nodes are found;

[0031] Starting from the origin, connect the key nodes and the target point in sequence to obtain an optimized global path formed by connecting the key nodes in sequence.

[0032] Preferably, when using the improved dynamic window method for local path planning and real-time obstacle avoidance on each segmented path, a kinematic model of the unmanned vessel is established based on the XOY inertial coordinate system, and the position state of the unmanned vessel is set as follows: ,in These represent the positions of the unmanned vessel in the XOY plane. This represents the ship's heading angle. When the unmanned surface vessel (USV) moves in a straight line for a very short period of time, it maintains a constant angular velocity. and linear velocity are In motion, the unmanned ship can move in a very short time. The trajectory within is obtained using the following formula:

[0033] (6)

[0034] In the formula, Indicates the ship's heading angle at the next moment. This represents the ship's heading angle at time t. This represents the angular velocity of the unmanned vessel. This represents the linear velocity of the unmanned vessel in the x and y directions.

[0035] Preferably, the adaptive evaluation function is expressed as:

[0036] (7)

[0037] In the formula, The deviation of the unmanned vessel's orientation from the target position at the current moving speed. This indicates the distance between the unmanned vessel and the nearest obstacle at its current speed. This indicates the current speed of the unmanned vessel. For smoothing coefficients, , Let r represent the first, second, and third evaluation coefficients of the evaluation function, r represent the effective radius of the obstacle, h represent the distance between the current position of the unmanned vessel and the target point, and R represent the distance between the starting point and the target point.

[0038] On the other hand, the present invention also provides an unmanned surface vessel (USV) path planning system based on the fusion of artificial potential field and DWA, wherein the USV path planning system based on the fusion of artificial potential field and DWA specifically includes:

[0039] The environment modeling module is used to construct a grid map model of the unmanned vessel's working environment and determine the unmanned vessel's starting point, ending point, static obstacles, and temporary obstacle information.

[0040] The initial path generation module uses a bidirectional RRT* algorithm guided by an artificial potential field to perform global path planning for the unmanned vessel and obtain the initial global path.

[0041] The global path optimization module is used to extract key nodes and remove redundant nodes from the initial global path to obtain an optimized global path formed by connecting key nodes in sequence.

[0042] The local path planning module is used to load and optimize the global path. The optimized global path is segmented according to key nodes to obtain at least one set of segmented paths. On each segmented path, the improved dynamic window method is used for local path planning and real-time obstacle avoidance. By constructing an adaptive evaluation function, the weights are dynamically adjusted to achieve dynamic obstacle avoidance and trajectory smoothing.

[0043] Compared with the prior art, the embodiments of this application have the following main advantages:

[0044] In this embodiment of the invention, the bidirectional RRT* algorithm guided by an artificial potential field significantly improves the efficiency and directionality of global path planning for unmanned surface vessels (USVs). This allows two random trees to intelligently expand towards each other under the combined guidance of attraction (target point) and repulsion (obstacles), drastically reducing the time required to find a feasible path initially. By extracting key nodes and removing redundant nodes from the initial path, a shorter and smoother optimized global path is generated, laying a solid foundation for efficient navigation. At the local planning level, the improved dynamic window method dynamically balances obstacle avoidance safety, target approach, and motion stability through an adaptive evaluation function, achieving real-time and smooth avoidance of sudden obstacles. Finally, through a global-local closed-loop fusion mechanism, USVs can both follow the optimized global route and flexibly respond to dynamic obstacles in complex marine environments, resulting in a significant improvement in path planning success rate, a substantial reduction in navigation time, and a marked improvement in trajectory smoothness, comprehensively ensuring the safety, efficiency, and stability of navigation. Attached Figure Description

[0045] Figure 1 This is a schematic diagram illustrating the implementation process of the unmanned vessel path planning method based on the fusion of artificial potential field and DWA provided by the present invention.

[0046] Figure 2A schematic diagram of a raster map model is shown.

[0047] Figure 3 A schematic diagram illustrating the principle of the bidirectional RRT* algorithm in an embodiment of the present invention is shown.

[0048] Figure 4 A schematic diagram illustrating the principle of initial global path optimization is shown.

[0049] Figure 5 This diagram illustrates the principle of integrating global path guidance and local obstacle avoidance optimization based on the improved dynamic window method.

[0050] Figure 6 A schematic diagram of global path optimization simulation is shown in an embodiment of the present invention.

[0051] Figure 7 A simulation diagram of unmanned vessel path planning based on a global-local fusion mechanism is shown in an embodiment of the present invention. Detailed Implementation

[0052] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs; the terminology used herein in the specification of the application is for the purpose of describing particular embodiments only and is not intended to be limiting of the application; the terms "comprising" and "having," and any variations thereof, in the specification, claims, and foregoing drawings of this application are intended to cover non-exclusive inclusion. The terms "first," "second," etc., in the specification, claims, or foregoing drawings of this application are used to distinguish different objects, not to describe a particular order.

[0053] In existing unmanned surface vessel (USV) path planning methods, global path planning and local obstacle avoidance are performed separately or in a simple sequential combination, lacking an effective coordination mechanism. This results in the inability to guarantee the navigation safety, real-time performance, and global efficiency of USVs in complex and dynamic sea areas. To address these issues, we propose a USV path planning method and system based on the fusion of artificial potential field and DWA. In short, the method first constructs a grid map model of the USV's working environment, then uses a bidirectional RRT* algorithm guided by artificial potential field to perform global path planning for the USV, optimizes the initial global path by extracting key nodes and removing redundant nodes, and dynamically adjusts the weights by constructing an adaptive evaluation function to achieve dynamic obstacle avoidance and trajectory smoothing. In this embodiment of the invention, the bidirectional RRT* algorithm guided by an artificial potential field significantly improves the efficiency and directionality of global path planning for unmanned surface vessels (USVs). This allows two random trees to intelligently expand towards each other under the combined guidance of attraction (target point) and repulsion (obstacles), drastically reducing the time required to find a feasible path initially. By extracting key nodes and removing redundant nodes from the initial path, a shorter and smoother optimized global path is generated, laying a solid foundation for efficient navigation. At the local planning level, the improved dynamic window method dynamically balances obstacle avoidance safety, target approach, and motion stability through an adaptive evaluation function, achieving real-time and smooth avoidance of sudden obstacles. Finally, through a global-local closed-loop fusion mechanism, USVs can both follow the optimized global route and flexibly respond to dynamic obstacles in complex marine environments, resulting in a significant improvement in path planning success rate, a substantial reduction in navigation time, and a marked improvement in trajectory smoothness, comprehensively ensuring the safety, efficiency, and stability of navigation.

[0054] This invention provides a path planning method for unmanned surface vessels based on the fusion of artificial potential field and DWA. Figure 1 A schematic diagram illustrating the implementation process of an unmanned surface vessel (USV) path planning method based on the fusion of artificial potential field and DWA is shown. The method specifically includes:

[0055] S10, Construct a grid map model of the unmanned vessel's working environment to determine the unmanned vessel's starting point, ending point, static obstacles, and temporary obstacle information;

[0056] in, Figure 2 A schematic diagram of a raster map model is shown. During the construction of the raster map model, a matrix structure is used, dividing the map into feasible and infeasible areas. Black grids represent infeasible areas, and white grids represent feasible areas. This model determines the starting and ending points of the unmanned vessel on the map, as well as the locations of temporary and static obstacles. Figure 2In the diagram, the working area of ​​the unmanned vessel is configured with a space of 30*30; the green triangle represents the starting position, the red triangle represents the ending position, and the red grid represents temporary obstacles that suddenly appear.

[0057] S20, using the bidirectional RRT* algorithm guided by an artificial potential field to perform global path planning for the unmanned vessel and obtain the initial global path;

[0058] The method for global path planning of unmanned surface vessels using a bidirectional RRT* algorithm guided by an artificial potential field includes:

[0059] A bidirectional RRT* algorithm is used to construct two random trees—a start-up tree and a finish-up tree—with the start and finish points of the unmanned surface vessel (USV) in the grid map model as root nodes. An artificial potential field method is introduced during the expansion of the start and finish trees to make the expansion process more directional. When the distance between the new nodes of the two random trees is one step, connecting the two random trees yields an initial global path connecting the start and finish points. The artificial potential field method, introduced during the expansion of the start and finish trees, includes an attraction function and a repulsion function. Figure 3 This diagram illustrates the principle of the bidirectional RRT* algorithm in an embodiment of the present invention. Figure 3 middle, Starting from, As the endpoint, The blue line represents the connection between the starting point and the target point, indicating that a globally valid path has been found.

[0060] When the starting tree expands, the gravitational target is the endpoint, and the repulsive force comes from obstacles. The gravitational function of the starting tree, defined as the artificial potential field, is expressed as:

[0061] (1)

[0062] In the formula, The gravitational coefficient, This is the Euclidean distance from the current unmanned vessel's position to the target point, and its direction vector points from the current node position to the target point position.

[0063] The improved repulsion function of the starting point tree is expressed as:

[0064] (2)

[0065] In the formula, Indicates the repulsion coefficient. This represents the Euclidean distance between the current position of the unmanned surface vessel and the obstacle. It is the radius of influence of the obstacle; when the distance exceeds... When the obstacle's repulsive force on the node disappears, the improved repulsive force function incorporates the influence of the distance between the target point and the current position, ensuring that both repulsive and attractive forces decrease to zero simultaneously only after the unmanned vessel reaches the target point. This enhances the vessel's ability to react to obstacles, preventing interference with its path when approaching the target point.

[0066] The combined potential field of the starting tree is represented as:

[0067] (3)

[0068] in, Represents the combined potential field of the starting tree;

[0069] When the endpoint tree expands, the gravitational target is the starting position, and the repulsive force comes from obstacles. The gravitational function of the endpoint tree, defined as the artificial potential field, is expressed as:

[0070] (4)

[0071] In the formula, The vector direction is the Euclidean distance between the current position of the unmanned vessel and the starting point. The vector direction is the direction from the current node to the starting point. Each time the endpoint tree expands, it expands towards the starting point tree under the guidance of the artificial potential field. In this way, the two trees can move closer to each other at the same time instead of extending towards a fixed global endpoint, which speeds up the generation of the global path.

[0072] The repulsive function of the endpoint tree and the repulsive function of the starting point tree are structurally similar, differing only in the definition of the gravitational target. The improved repulsive function of the endpoint tree, incorporating the influence of the distance between the starting point and the current position, is expressed as follows:

[0073] (5)

[0074] Indicates the repulsion coefficient. This represents the Euclidean distance between the current position of the unmanned surface vessel and the obstacle. It is the radius of influence of the obstacle, while the resultant potential field of the endpoint tree is the same as that in formula (3).

[0075] S30, the initial global path is optimized by extracting key nodes and removing redundant nodes to obtain an optimized global path formed by connecting key nodes in sequence;

[0076] S40, load the optimized global path, divide the optimized global path into segments according to key nodes to obtain at least one set of segmented paths, use the improved dynamic window method to perform local path planning and real-time obstacle avoidance on each segmented path, dynamically adjust the weights by constructing an adaptive evaluation function to achieve dynamic obstacle avoidance and trajectory smoothing, the unmanned vessel moves along the optimal trajectory, and it is determined whether the unmanned vessel has reached the target point.

[0077] If the unmanned surface vessel reaches the target point, the optimal path is determined. If the unmanned surface vessel does not reach the target point, the speed of the unmanned surface vessel continues to be sampled. The improved dynamic window method is used repeatedly for local path planning and real-time obstacle avoidance on each segment path. By constructing an adaptive evaluation function to dynamically adjust the weights, dynamic obstacle avoidance and trajectory smoothing are achieved.

[0078] In this embodiment of the invention, the bidirectional RRT* algorithm guided by an artificial potential field significantly improves the efficiency and directionality of global path planning for unmanned surface vessels (USVs). This allows two random trees to intelligently expand towards each other under the combined guidance of attraction (target point) and repulsion (obstacles), drastically reducing the time required to find a feasible path initially. By extracting key nodes and removing redundant nodes from the initial path, a shorter and smoother optimized global path is generated, laying a solid foundation for efficient navigation. At the local planning level, the improved dynamic window method dynamically balances obstacle avoidance safety, target approach, and motion stability through an adaptive evaluation function, achieving real-time and smooth avoidance of sudden obstacles. Finally, through a global-local closed-loop fusion mechanism, USVs can both follow the optimized global route and flexibly respond to dynamic obstacles in complex marine environments, resulting in a significant improvement in path planning success rate, a substantial reduction in navigation time, and a marked improvement in trajectory smoothness, comprehensively ensuring the safety, efficiency, and stability of navigation.

[0079] It should be noted that a hierarchical collaborative path planning architecture is established through a global-local fusion mechanism. This mechanism, through intelligent division of labor and organic integration, achieves efficient collaboration between global path optimization guidance and real-time local obstacle avoidance response. In terms of architecture design, the global-local fusion mechanism adopts a two-layer structure of "macro-guidance - micro-adjustment." The global planning layer is responsible for generating the overall optimized path from the starting point to the destination, providing directional guidance for navigation; the local planning layer is responsible for real-time obstacle avoidance and trajectory optimization within the framework of the global path. The fusion mechanism resolves the contradiction between real-time planning and global optimization through hierarchical processing. Although global planning involves significant computation, it does not require frequent execution; local planning is computationally lightweight and can meet real-time requirements. When environmental changes are within the local processing capacity, obstacle avoidance adjustments are performed independently by local planning; global replanning is only triggered when environmental changes exceed the local processing capacity. This on-demand replanning strategy ensures the system's real-time responsiveness while avoiding unnecessary computational overhead. The main advantages of the fusion mechanism in this invention are reflected in three aspects: First, global guidance ensures the overall optimality of the path and avoids the trap of local optima; second, local real-time adjustments ensure adaptability to dynamic environments; and finally, hierarchical processing enables the rational allocation of computing resources, ensuring the practicality of system operation.

[0080] This invention provides a method for optimizing an initial global path by extracting key nodes and removing redundant nodes. Specifically, this method includes:

[0081] S101, Load and traverse the initial global path, placing all path nodes in the initial global path into the node set in order. , , … };

[0082] S102, the starting point and the target point are always considered key nodes in the node set. Traverse the node set, starting from the starting node. Start by connecting the nodes in the set sequentially until you reach the set. When the path between nodes encounters an obstacle, at this time It was identified as a critical node, and then from Begin by connecting the remaining nodes one by one until all the key nodes are found;

[0083] S103: Starting from the origin, connect the key nodes and the target point in sequence to obtain the optimized global path formed by connecting the key nodes in sequence.

[0084] In this embodiment of the invention, Figure 4 This diagram illustrates the principle of path optimization for the initial global path. In the diagram, two adjacent key nodes... and If the straight path between nodes is free of obstructions, unnecessary intermediate nodes are removed, and two adjacent key nodes are connected. To improve the continuity and smoothness of the path, key node extraction and redundant node removal are repeated until the path cannot be simplified further. The optimized global path is shorter and smoother.

[0085] In this embodiment of the invention, when performing local path planning and real-time obstacle avoidance using the improved dynamic window method on each segmented path, a kinematic model of the unmanned vessel is established based on the XOY inertial coordinate system, and the position state of the unmanned vessel is assumed to be... ,in These represent the positions of the unmanned vessel in the XOY plane. This represents the ship's heading angle. When the unmanned surface vessel (USV) moves in a straight line for a very short period of time, it maintains a constant angular velocity. and linear velocity are In motion, the unmanned ship can move in a very short time. The trajectory within is obtained using the following formula:

[0086] (6)

[0087] In the formula, Indicates the ship's heading angle at the next moment. This represents the ship's heading angle at time t. This represents the angular velocity of the unmanned vessel. This represents the linear velocity of the unmanned vessel in the x and y directions.

[0088] It should be noted that DWA selects multiple combinations of linear and angular velocities within the velocity space and performs trajectory simulation for each combination to generate the corresponding motion trajectory. Using formula (6) and the collected unmanned surface vessel (USV) velocity information, the USV's motion trajectory for the next time period can be simulated. Subsequently, the trajectory set is scored using an evaluation function, and the trajectory with the highest score is selected as the optimal motion path for the current moment. Finally, the linear velocity minus the angular velocity corresponding to the trajectory with the highest score is used as the USV's velocity input.

[0089] To enable the unmanned surface vessel (USV) to quickly plan feasible paths during navigation using obstacle avoidance and target arrival mechanisms (DWA), an adaptive weighting method was introduced into the improved DWA evaluation function. This allows the USV to balance the objectives of obstacle avoidance, target arrival, and speed limitations. The adaptive evaluation function is expressed as follows:

[0090] (7)

[0091] In the formula, This represents the deviation of the unmanned vessel's orientation from the target position at its current moving speed. This value is maximized when the unmanned vessel is moving towards the target. This indicates the distance between the unmanned surface vessel and the nearest obstacle at its current speed, ensuring obstacle avoidance capabilities and excluding sampling points that are close to obstacles. This indicates the current speed of the unmanned vessel, including angular velocity and linear velocity. The smoothing coefficient is used to normalize the collected information. , Let r represent the first, second, and third evaluation coefficients of the evaluation function, r represent the effective radius of the obstacle, h represent the distance between the current position of the unmanned vessel and the target point, and R represent the distance between the starting point and the target point.

[0092] In this invention, by constructing an adaptive evaluation function to dynamically adjust weights, dynamic obstacle avoidance and trajectory smoothing are achieved. Then, by merging global and local algorithms in a closed loop, the unmanned surface vessel (USV) can maintain safety and real-time performance even in complex and unknown environments. An artificial potential field-improved bidirectional RRT* algorithm is used to complete global path planning, and the resulting global path is segmented based on key nodes. An improved DWA is applied to each segment to complete local path planning and real-time dynamic obstacle avoidance, thus achieving a fusion of global path guidance and local obstacle avoidance optimization. Figure 5This diagram illustrates the principle of integrating global path guidance and local obstacle avoidance optimization based on an improved dynamic window method. This method enables unmanned surface vessels (USVs) in complex sea areas to follow a globally optimized path guided by key nodes while simultaneously utilizing improved Dynamic Window (DWA) to avoid pre-set temporary obstacles in real time, ensuring both navigation safety and real-time performance.

[0093] Specifically, this invention is based on the fusion of artificial potential field-bidirectional RRT* and dynamic window method to establish a global-local fusion path planning framework, and finally plans a safe, smooth and efficient unmanned vessel driving path. First, establish an environmental model and global path planning. Construct a configuration space using the grid map model in step S10, and clarify the starting point, ending point, static and temporary obstacles. Then, use the artificial potential field-bidirectional RRT* algorithm described in S20 to perform a global search: generate two random trees with the starting point and ending point as roots respectively. When each tree expands a new node, it is guided by the attraction of the target point (the ending point for the starting point tree, and the starting point for the ending point tree) and the repulsive force of the obstacle, as described in formulas (1)-(5), making the expansion more directional. When the distance between the new nodes of the two trees is less than the step size, they are connected to form an initial global path. Optimize the global path. According to the method described in step S30, traverse the initial path node set, extract the key nodes that can be directly connected and are without obstacles, remove redundant nodes, and form a shorter and smoother optimized global path. The specific process is as follows: Figure 6 As shown.

[0094] Segmented local real-time obstacle avoidance is implemented. Based on steps S40 and S50, the optimized global path is segmented according to key nodes, and an improved dynamic window method is applied to each segment for local planning. The method is based on the kinematic model of the unmanned surface vessel (USV), sampling and simulating trajectories in the velocity space. An improved evaluation function with adaptive weights is introduced to score the trajectories, selecting the velocity corresponding to the optimal trajectory to control the USV, thereby achieving dynamic obstacle avoidance and trajectory smoothing. The final simulation results are shown below, through closed-loop fusion of the global and local algorithms. Figure 6 and Figure 7 As shown, where, Figure 6 This diagram illustrates a simulation of global path optimization in an embodiment of the present invention. Figure 6 (a) in the figure represents a simulation diagram of global path planning for an unmanned surface vessel using a bidirectional RRT* algorithm guided by an artificial potential field. Figure 6 (b) in the diagram represents a simulation illustration of the optimization process for extracting key nodes and removing redundant nodes in the initial global path. Figure 6 (c) in the diagram represents the simulation diagram where the optimized global path is segmented according to key nodes. Figure 7 The figure shows a simulation diagram of unmanned surface vessel path planning based on a global-local fusion mechanism in an embodiment of the present invention. In the diagram, green triangles represent the path start point, and red triangles represent the path end point. From the simulation... Figure 6It can be seen that the optimization method significantly reduces redundant nodes in the initial global path and reduces directional changes. Furthermore, it segments the path according to key nodes, as shown in the simulation. Figure 7 As shown, the closed-loop fusion planning of global path guidance and local obstacle avoidance optimization effectively avoids the three temporary obstacles, and there are no collisions between the path and the obstacles, ensuring the feasibility and safety of the path. This proves that the fusion algorithm has good global effectiveness and can dynamically avoid obstacles.

[0095] On the other hand, embodiments of the present invention also provide an unmanned surface vessel (USV) path planning system based on the fusion of artificial potential field and DWA, wherein the USV path planning system based on the fusion of artificial potential field and DWA specifically includes:

[0096] The environment modeling module is used to construct a grid map model of the unmanned vessel's working environment and determine the unmanned vessel's starting point, ending point, static obstacles, and temporary obstacle information.

[0097] The initial path generation module uses a bidirectional RRT* algorithm guided by an artificial potential field to perform global path planning for the unmanned vessel and obtain the initial global path.

[0098] The global path optimization module is used to extract key nodes and remove redundant nodes from the initial global path to obtain an optimized global path formed by connecting key nodes in sequence.

[0099] The local path planning module is used to load and optimize the global path. The optimized global path is segmented according to key nodes to obtain at least one set of segmented paths. On each segmented path, the improved dynamic window method is used for local path planning and real-time obstacle avoidance. By constructing an adaptive evaluation function, the weights are dynamically adjusted to achieve dynamic obstacle avoidance and trajectory smoothing.

[0100] In summary, this invention provides an unmanned surface vessel (USV) path planning method and system based on the fusion of artificial potential field and DWA. In the embodiments of this invention, the bidirectional RRT* algorithm guided by the artificial potential field significantly improves the efficiency and directionality of the USV's global path planning, enabling two random trees to intelligently expand towards each other under the cooperative guidance of attraction (target point) and repulsion (obstacles), greatly shortening the time to find a feasible path for the first time. By extracting key nodes and removing redundant nodes from the initial path, a shorter and smoother optimized global path is generated, laying a good foundation for efficient navigation. At the local planning level, the improved dynamic window method dynamically balances obstacle avoidance safety, target approach, and motion stability through an adaptive evaluation function, achieving real-time and smooth avoidance of sudden obstacles. Finally, through the global-local closed-loop fusion mechanism, the USV can follow the optimized global route and flexibly cope with dynamic obstacles in complex marine environments, achieving a significant improvement in path planning success rate, a significant reduction in navigation time, and a significant improvement in trajectory smoothness, comprehensively ensuring the safety, efficiency, and stability of navigation.

[0101] It should be noted that, for the sake of simplicity, the foregoing embodiments are all described as a series of actions. However, those skilled in the art should understand that the present invention is not limited to the described order of actions, as some steps may be performed in other orders or simultaneously according to the present invention. Furthermore, those skilled in the art should also understand that the embodiments described in the specification are preferred embodiments, and the actions and modules involved are not necessarily essential to the present invention.

[0102] It should be understood that the disclosed apparatus can be implemented in other ways, given the several embodiments provided in this application. For example, the apparatus embodiments described above are merely illustrative; the division of units described above is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or communication connections shown or discussed may be through some interfaces; the indirect coupling or communication connections between devices or units may be telecommunications or other forms.

[0103] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit the scope of protection of the invention. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on these embodiments, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art can still combine, add, delete, or otherwise adjust the features of the various embodiments of the present invention according to the circumstances without conflict or creative effort, thereby obtaining different technical solutions that do not fundamentally depart from the concept of the present invention. These technical solutions also fall within the scope of protection of the present invention.

Claims

1. A path planning method for unmanned surface vessels based on the fusion of artificial potential field and DWA, characterized in that, The method includes: S10, Construct a grid map model of the unmanned vessel's working environment to determine the unmanned vessel's starting point, ending point, static obstacles, and temporary obstacle information; S20, using the bidirectional RRT* algorithm guided by an artificial potential field to perform global path planning for the unmanned vessel and obtain the initial global path; S30, the initial global path is optimized by extracting key nodes and removing redundant nodes to obtain an optimized global path formed by connecting key nodes in sequence; S40, load the optimized global path, divide the optimized global path into segments according to key nodes to obtain at least one set of segmented paths, use the improved dynamic window method to perform local path planning and real-time obstacle avoidance on each segmented path, and dynamically adjust the weights by constructing an adaptive evaluation function to achieve dynamic obstacle avoidance and trajectory smoothing. The method for global path planning of unmanned surface vessels using a bidirectional RRT* algorithm guided by an artificial potential field includes: The bidirectional RRT* algorithm is used to construct two random trees, the start tree and the end tree, with the start and end points of the unmanned vessel in the grid map model as root nodes. During the expansion of the start and end trees, an artificial potential field method is introduced. When the distance between the new nodes of the two random trees is one step, the two random trees are connected to obtain an initial global path connecting the start and end points. When the artificial potential field method is introduced during the expansion of the start and end trees, the artificial potential field method includes an attraction function and a repulsion function.

2. The unmanned surface vessel path planning method based on the fusion of artificial potential field and DWA as described in claim 1, characterized in that: When the starting tree expands, the gravitational target is the endpoint, and the repulsive force comes from obstacles. The gravitational function of the starting tree, defined as the artificial potential field, is expressed as: （1） In the formula, The gravitational coefficient, This is the Euclidean distance from the current unmanned vessel's position to the target point, and its direction vector points from the current node position to the target point position. The improved repulsion function of the starting point tree is expressed as: （2） In the formula, Indicates the repulsion coefficient. This represents the Euclidean distance between the current position of the unmanned surface vessel and the obstacle. It is the radius of influence of the obstacle; The combined potential field of the starting tree is represented as: （3） in, This represents the combined potential field of the starting tree.

3. The unmanned surface vessel path planning method based on the fusion of artificial potential field and DWA as described in claim 2, characterized in that: The method for optimizing the initial global path by extracting key nodes and removing redundant nodes includes: Load and iterate through the initial global path, placing all path nodes in the initial global path into the node set in order. , , … }; The starting point and the target point are always considered key nodes in the node set. Traversing the node set, starting from the starting node... Start by connecting the nodes in the set sequentially until you reach the set. When the path between nodes encounters an obstacle, at this time It was identified as a critical node, and then from Begin by connecting the remaining nodes one by one until all the key nodes are found; Starting from the origin, connect the key nodes and the target point in sequence to obtain an optimized global path formed by connecting the key nodes in sequence.

4. The unmanned surface vessel path planning method based on the fusion of artificial potential field and DWA as described in claim 1, characterized in that: When using the improved dynamic window method for local path planning and real-time obstacle avoidance on each segmented path, a kinematic model of the unmanned vessel is established based on the XOY inertial coordinate system. The position state of the unmanned vessel is assumed to be... ,in These represent the positions of the unmanned vessel in the XOY plane. This represents the ship's heading angle. When the unmanned surface vessel (USV) moves in a straight line for a very short period of time, it maintains a constant angular velocity. and linear velocity are In motion, the unmanned ship can move in a very short time. The trajectory within is obtained using the following formula: （6） In the formula, The ship's heading angle at the next moment, This represents the ship's heading angle at time t. This represents the angular velocity of the unmanned vessel. This represents the linear velocity of the unmanned vessel in the x and y directions.

5. The unmanned surface vessel path planning method based on the fusion of artificial potential field and DWA as described in claim 4, characterized in that: The adaptive evaluation function is expressed as follows: (7), in the formula, The deviation of the unmanned vessel's orientation from the target position at the current moving speed. This indicates the distance between the unmanned vessel and the nearest obstacle at its current speed. This indicates the current speed of the unmanned vessel. For smoothing coefficients, , Let r represent the first, second, and third evaluation coefficients of the evaluation function, r represent the effective radius of the obstacle, h represent the distance between the current position of the unmanned vessel and the target point, and R represent the distance between the starting point and the target point.

6. An unmanned surface vessel (USV) path planning system based on the fusion of artificial potential field and DWA, used to implement the USV path planning method based on the fusion of artificial potential field and DWA as described in any one of claims 1-5, characterized in that: The unmanned surface vessel path planning system based on the fusion of artificial potential field and DWA specifically includes: The environment modeling module is used to construct a grid map model of the unmanned vessel's working environment and determine the unmanned vessel's starting point, ending point, static obstacles, and temporary obstacle information. The initial path generation module uses a bidirectional RRT* algorithm guided by an artificial potential field to perform global path planning for the unmanned vessel and obtain the initial global path. The global path optimization module is used to extract key nodes and remove redundant nodes from the initial global path to obtain an optimized global path formed by connecting key nodes in sequence. The local path planning module is used to load and optimize the global path. The optimized global path is segmented according to key nodes to obtain at least one set of segmented paths. On each segmented path, the improved dynamic window method is used for local path planning and real-time obstacle avoidance. By constructing an adaptive evaluation function, the weights are dynamically adjusted to achieve dynamic obstacle avoidance and trajectory smoothing.

Images