A deep fusion path planning method for autonomous navigation of USV

Abstract

The application discloses a kind of depth fusion path planning method for USV autonomous navigation, it belongs to the field of surface unmanned vehicle path planning.The application solves the problems of poor path smoothness, low navigation efficiency and difficult to guarantee navigation safety of existing hierarchical path planning scheme.The method of the application comprises loading electronic chart data, extracting static obstacle information in the electronic chart data;According to the static obstacle region information in the environment, grid modeling is carried out;Global path planning is carried out using an improved algorithm based on environment perception and target orientation, and the improved initial path is smoothed;Finally, the DWA algorithm improved by evaluation function is used to plan the local path of USV, and USV navigates according to the local path planning result.The application can be applied to surface unmanned vehicle path planning.

Description

Technical Field

[0001] This invention belongs to the field of path planning for unmanned surface vessels (USVs), specifically involving a deep fusion path planning method for USV autonomous navigation. Background Technology

[0002] With the advancement of artificial intelligence technology, unmanned surface vehicles (USVs) have become core equipment in fields such as marine environmental monitoring, maritime search and rescue, and port security. The level of intelligence of a USV determines its ability to navigate autonomously, safely, and efficiently in complex environments involving wind, waves, currents, and dynamic and static obstacles. Path planning technology is key to achieving this goal. In restricted waters with numerous islands and narrow channels, extremely high requirements are placed on the safety, real-time performance, smoothness, and robustness of planning algorithms.

[0003] To address these challenges, existing technologies generally employ a hierarchical hybrid path planning approach, decoupling the USV's navigation task into two subsystems: global path planning and local path planning. Global planning, based on prior map information, generates a reference route from the starting point to the target point at a macroscopic scale; local planning, while following this reference route, utilizes sensors to perceive local environmental information in real time for dynamic obstacle avoidance. However, the overall performance of this hierarchical architecture is highly dependent on the individual performance of the algorithms at each level and their collaborative effectiveness, and current mainstream algorithm combinations exhibit significant shortcomings in practical applications.

[0004] At the global path planning level The algorithm and its variants are widely used due to their efficient heuristic search capabilities. However, these algorithms have the following inherent drawbacks: First, the paths generated by their grid-based expansion mechanism often contain a large number of unnecessary turning points, resulting in poor path smoothness and severely violating the kinematic constraints of the USV. If navigation is directly carried out according to the paths planned by such algorithms, it will lead to unstable navigation and low energy efficiency of the USV. Second, their traditional heuristic functions are computationally inefficient in areas with dense obstacles, which can easily lead to an indiscriminate expansion of the search range. Existing improvement schemes are mostly limited to local adjustments and fail to fundamentally optimize the quality of the initial path.

[0005] At the local path planning level, the Dynamic Window Approach (DWA) has become a commonly used technique for dynamic obstacle avoidance because it can directly search in velocity space by combining kinematic constraints. However, traditional DWA algorithms have the following core limitations: First, their evaluation function is simple in design, making them prone to getting stuck in local optima in complex multi-obstacle encounter scenarios, causing the USV to stagnate or oscillate in decision-making; second, as a typical reactive strategy, this algorithm lacks the ability to predict the future motion state of dynamic obstacles, resulting in decision-making lag and making it difficult to guarantee safety in high-speed encounter scenarios. Although some studies have attempted to fine-tune the evaluation function by introducing methods such as fuzzy logic or reinforcement learning, most improvements only remain at the indicator level and have failed to systematically improve the algorithm's forward-looking risk prediction and compliance decision-making capabilities.

[0006] At the level of fusion methodology, existing technologies mostly employ a simple cascaded mixing strategy, that is, from The algorithm generates a global pathpoint sequence, which is then used by the DWA algorithm for pathpoint tracking and local obstacle avoidance. This simple combination suffers from a fundamental problem of insufficient synergy: on the one hand, the traditional... The convoluted and poor-quality global path generated by the algorithm will impose a huge tracking burden on the downstream DWA algorithm, forcing it to make frequent and drastic adjustments, which will seriously affect the smoothness of navigation and overall efficiency. On the other hand, a traditional DWA algorithm that lacks advanced decision-making capabilities may still make unsafe or non-compliant avoidance actions when faced with complex dynamic scenarios, even under the guidance of the global path, and cannot fundamentally make up for the defects of a single algorithm.

[0007] In summary, due to the inherent defects of the two core algorithms and the insufficient synergy of the fusion method, existing algorithms based on... The hierarchical path planning schemes of the algorithm and the DWA algorithm still cannot meet the requirements of practical applications in terms of path smoothness, navigation efficiency and navigation safety. Summary of the Invention

[0008] The purpose of this invention is to address the problems of poor path smoothness, low navigation efficiency, and difficulty in ensuring navigation safety in existing hierarchical path planning schemes, and to propose a deep fusion path planning method for USV autonomous navigation.

[0009] The technical solution adopted by this invention to solve the above-mentioned technical problems is: a deep fusion path planning method for USV autonomous navigation, the method specifically including the following steps:

[0010] Step 1: Load the electronic chart data corresponding to the USV operation area. Process the electronic chart data to obtain a two-dimensional raster map, which includes restricted areas corresponding to static obstacles.

[0011] Step 2: Adopt the improved The algorithm performs global planning on the USV's navigation path to obtain an initial global path planning result, and then smooths the initial global path planning result to obtain a smoothed global path.

[0012] Step 3: Based on the smoothed global path, the improved DWA algorithm is used to plan the local path of the USV, and the USV navigates according to the local path planning results.

[0013] Furthermore, the specific process of step one is as follows:

[0014] Step 1: Load the electronic chart data corresponding to the USV operation area, parse the loaded electronic chart data, and obtain the area where the static obstacle is located in the electronic chart data;

[0015] The Mercator projection transformation is used to project the obstruction zone where the static obstacle is located onto the OXY plane of the Cartesian rectangular coordinate system, so as to obtain the position of the static obstacle in the OXY plane of the Cartesian rectangular coordinate system;

[0016] Step 1 and 2: Next, divide the OXY plane of the Cartesian coordinate system into grids to determine the grids where static obstacles are located;

[0017] Step 13: In the grid map, the area where the static obstacle is located is expanded, and the expanded area is used as the no-entry zone in the grid map.

[0018] Furthermore, the improved The cost function of the algorithm is:

[0019]

[0020] in, Represents the cost function, This represents the distance from the starting point to the current node. The actual cost, Indicates from node Heuristic cost estimation to the target point Dynamic weights;

[0021]

[0022] in, The base of the natural logarithm. Represents the obstacle factor;

[0023]

[0024] in, This represents the number of prohibited grid cells along the straight line connecting the starting point and the target point. This represents the total number of grid cells traversed along the straight line connecting the starting point and the target point.

[0025] Furthermore, the improved The algorithm employs a 5-neighborhood search strategy, specifically:

[0026] Step 1: In the OXY plane of the Cartesian rectangular coordinate system, take the positive X-axis as... direction;

[0027] Will The region is the region corresponding to the third neighbor of the current node;

[0028] Will The region is the region corresponding to the second neighbor of the current node;

[0029] Will The region is the region corresponding to the first neighbor of the current node;

[0030] Will The region is the region corresponding to the 4th neighbor of the current node;

[0031] Will The region is the region corresponding to the 6th neighbor of the current node;

[0032] Will The region is the region corresponding to the 7th neighbor of the current node;

[0033] Will The region is the region corresponding to the 8th neighbor of the current node;

[0034] Will The region is the region corresponding to the 5th neighbor of the current node;

[0035] Step 2: Calculate the guide angle based on the positions of the parent node to be expanded and the target point. :

[0036]

[0037] in, This indicates the position coordinates of the parent node to be expanded. This represents the position coordinates of the target point, and atan2 represents the arctangent function in the four quadrants.

[0038] Step 3: Determine the guide angle After the area is located, guide the angle. Location and guiding angle The two areas to the left of the location and the guiding angle The two regions to the right of the current region are considered as the five neighboring regions to be searched.

[0039] Furthermore, the specific process of smoothing the initial global path planning results is as follows:

[0040] Let each node in the initial global path planning result be denoted as... , This represents the total number of nodes in the initial global path planning result;

[0041] In the initial global path planning results Filter the nodes so that the remaining nodes are the first ones. Nodes With the Nodes The area is not visually accessible, that is, it connects to the first Nodes With the Nodes The straight line segment needs to pass through the restricted area grid determined in step one or three.

[0042] Furthermore, the evaluation function used in the improved DWA algorithm is... for:

[0043]

[0044] in, This indicates the heading angle deviation term; Indicates the distance to obstacles; Indicates the velocity term; , and These are the weighting coefficients; Represents the target forward gain function. This represents the path dynamic risk assessment function. This represents the collision avoidance cost function.

[0045] Furthermore, the target forward gain function The calculation method is as follows:

[0046]

[0047] in, Indicates the current location of the USV. Indicates the location of the target point. This indicates the USV's prediction duration. The location reached later For any small positive number, This indicates the linear velocity of the USV. This represents the angular velocity of the USV. This represents the L2 norm.

[0048] Furthermore, the calculation method for the path dynamic risk assessment function is as follows:

[0049] The predicted path of dynamic obstacles is discretized into a set of risk points, and the first... The coordinates of each risk point are: ;

[0050] Predicted duration within, no. Collision risk indicators at individual risk points for:

[0051]

[0052] in, Indicates the prediction duration Inside, USV and the The closest distance to each risk point; This represents the risk perception distance threshold.

[0053] No. Weight of each risk point :

[0054]

[0055] in, Indicates that the dynamic obstacle has reached the first The time difference between the time of each risk point and the current time;

[0056] Calculate the path dynamic risk assessment function :

[0057]

[0058] in, Indicates the total number of risk points. Indicates safety reward items;

[0059]

[0060] in, Indicates the reward coefficient; Indicates the prediction duration Inside, the USV operates according to linear velocity. and angular velocity Minimum distance from all risk points during navigation; This represents the upper limit of the optimal safe distance.

[0061] Furthermore, the collision avoidance cost function The calculation method is as follows:

[0062] (1) For encounter scenarios, the collision avoidance cost is:

[0063]

[0064] in, This represents the basic reward and punishment coefficient;

[0065] (2) For the starboard crossing scenario, the collision avoidance cost is:

[0066]

[0067] (3) For the port side crossing scenario, the collision avoidance cost is:

[0068]

[0069] (4) For the scenario of being overtaken, the collision avoidance cost is:

[0070]

[0071] (5) For the stern crossing scenario, the collision avoidance cost is:

[0072]

[0073] (6) For other scenarios, collision avoidance costs All are 0.

[0074] Furthermore, the weighting coefficients , and The fuzzy controller is used to obtain the following:

[0075] Step S1: Record the distance between the current position of the USV and the target point as... The minimum distance between the USV and the obstacle at the current moment is denoted as... ;

[0076] Define the first fuzzy subset respectively Second fuzzy subset and the third fuzzy subset ;

[0077] in, , This indicates the maximum detection range of the sensors deployed on the USV. , ;

[0078] Step S2: Calculate separately For the first fuzzy subset Second fuzzy subset and the third fuzzy subset The membership degree is then calculated separately. For the first fuzzy subset Second fuzzy subset and the third fuzzy subset Membership degree;

[0079] Step S3: Define the fourth fuzzy subset The fifth fuzzy subset and the sixth fuzzy subset ,in, , , ;

[0080] A fuzzy rule base is defined, comprising 9 fuzzy rules, which are as follows:

[0081] Within the first fuzzy rule: Corresponding to the third fuzzy subset, Corresponding to the third fuzzy subset, weight Corresponding to the fifth fuzzy subset, weight Corresponding to the fifth fuzzy subset, weight Corresponding to the sixth fuzzy subset;

[0082] Within the second fuzzy rule: Corresponding to the third fuzzy subset, Corresponding to the first fuzzy subset, weight Corresponding to the fourth fuzzy subset, weight Corresponding to the sixth fuzzy subset, weight Corresponding to the fourth fuzzy subset;

[0083] Within the third fuzzy rule: Corresponding to the first fuzzy subset, Corresponding to the third fuzzy subset, weight Corresponding to the sixth fuzzy subset, weight Corresponding to the fourth fuzzy subset, weight Corresponding to the sixth fuzzy subset;

[0084] Within the fourth fuzzy rule: Corresponding to the first fuzzy subset, Corresponding to the first fuzzy subset, weight Corresponding to the sixth fuzzy subset, weight Corresponding to the sixth fuzzy subset, weight Corresponding to the fourth fuzzy subset;

[0085] Within the 5th fuzzy rule: Corresponding to the third fuzzy subset, Corresponding to the second fuzzy subset, weight Corresponding to the fifth fuzzy subset, weight Corresponding to the fifth fuzzy subset, weight Corresponding to the sixth fuzzy subset;

[0086] Within the 6th fuzzy rule: Corresponding to the first fuzzy subset, Corresponding to the second fuzzy subset, weight Corresponding to the sixth fuzzy subset, weight Corresponding to the fifth fuzzy subset, weight Corresponding to the fifth fuzzy subset;

[0087] Within the 7th fuzzy rule: Corresponding to the second fuzzy subset, Corresponding to the third fuzzy subset, weight Corresponding to the fifth fuzzy subset, weight Corresponding to the fourth fuzzy subset, weight Corresponding to the sixth fuzzy subset;

[0088] Within the 8th fuzzy rule: Corresponding to the second fuzzy subset, Corresponding to the first fuzzy subset, weight Corresponding to the fifth fuzzy subset, weight Corresponding to the sixth fuzzy subset, weight Corresponding to the fourth fuzzy subset;

[0089] Within the 9th fuzzy rule: Corresponding to the second fuzzy subset, Corresponding to the second fuzzy subset, weight Corresponding to the fifth fuzzy subset, weight Corresponding to the fifth fuzzy subset, weight Corresponding to the sixth fuzzy subset;

[0090] Step S4: Calculate the weights based on the fuzzy rules and the results from step S2. Weight and weight The value;

[0091] The specific process of step S4 is as follows:

[0092] Step S41: For the first row of fuzzy rules in the fuzzy rule base, compare... For the third fuzzy subset Membership degree and For the third fuzzy subset The smaller of the two membership degrees is selected and used as the membership degree of the first row of fuzzy rules.

[0093] Then, iterate through each row of fuzzy rules in the fuzzy rule base to obtain the membership degree of each row of fuzzy rules;

[0094] Step S42: Compare the membership degrees of the fuzzy rules in each row to obtain the row with the highest membership degree, and calculate the weight of the fuzzy rule corresponding to the row with the highest membership degree. Weight and weight The value of .

[0095] The beneficial effects of this invention are:

[0096] The method of this invention first extracts static obstacle information from electronic nautical chart data, and then performs raster modeling based on static obstacle area information in the environment; [This method] improves upon existing methods. The algorithm is improved, based on the improvement The algorithm performs global path planning and adopts improved methods. The initial path obtained by the algorithm is smoothed; finally, the evaluation function of the traditional DWA algorithm is improved, and an improved DWA algorithm is designed for application to local path planning of unmanned surface vessels.

[0097] (1) At the global path planning level, this invention improves upon traditional methods. The algorithm has been improved by introducing an environment-aware heuristic function, adopting a goal-oriented five-neighbor expansion strategy, and combining a line-of-sight smoothing mechanism, which significantly improves the initial quality and smoothness of the global path.

[0098] (2) At the local path planning level, the present invention redesigns the evaluation system of the DWA algorithm: by constructing a forward-looking dynamic risk field, quantifying and integrating international maritime collision avoidance rules, and using fuzzy logic to adaptively adjust the weights, the traditional reactive obstacle avoidance mode is transformed into an active and compliant decision-making mode, which can avoid frequent and drastic adjustments to the speed of the USV and improve the overall navigation efficiency and safety. Attached Figure Description

[0099] Figure 1 This is a flowchart of a deep fusion path planning method for USV autonomous navigation according to the present invention;

[0100] Figure 2 It is a raster visualization result of an electronic nautical chart;

[0101] Figure 3 It is tradition A diagram illustrating the algorithm's search method;

[0102] Figure 4This is an improvement adopted in the present invention. A diagram illustrating the algorithm's search method;

[0103] Figure 5(a) shows the improved [system / mechanism] in an open water environment. Algorithms and Fundamentals Comparison chart of the planning results of the algorithm;

[0104] Figure 5(b) shows the improved [system / mechanism] in a complex aquatic environment. Algorithms and Fundamentals Comparison chart of the planning results of the algorithm;

[0105] Figure 5(c) shows the improved design in a maze-like aquatic environment. Algorithms and Fundamentals Comparison chart of the planning results of the algorithm;

[0106] Figure 6 It is a mathematical motion model of USV based on COLREGs (International Maritime Collision Prevention Regulations);

[0107] Figure 7(a) shows the experimental results for the encounter scenario;

[0108] Figure 7(b) shows the experimental results for the starboard crossing scenario;

[0109] Figure 7(c) shows the experimental results for the port side crossing scenario;

[0110] Figure 7(d) shows the experimental results in the case of being overtaken;

[0111] Figure 8(a) shows the dynamic collision avoidance experimental results of the basic DWA algorithm;

[0112] Figure 8(b) shows the dynamic collision avoidance experimental results of the improved DWA algorithm;

[0113] Figure 9(a) shows the experimental results of the traditional fusion algorithm under static conditions;

[0114] Figure 9(b) shows the experimental results of the fusion algorithm of the present invention under static conditions;

[0115] Figure 10(a) shows the experimental results of the traditional fusion algorithm in a dynamic environment under a meeting situation;

[0116] Figure 10(b) shows the experimental results of the traditional fusion algorithm in a port-side crossing situation under dynamic conditions.

[0117] Figure 10(c) shows the experimental results of the fusion algorithm of the present invention in dynamic environments under encounter and crossover situations. Detailed Implementation

[0118] Specific implementation method one: Combining Figure 1This embodiment describes a deep fusion path planning method for USV autonomous navigation, which specifically includes the following steps:

[0119] Step 1: Load the electronic chart data corresponding to the USV operation area. Process the electronic chart data to obtain a two-dimensional raster map, which includes restricted areas corresponding to static obstacles.

[0120] The specific process of step one is explained in detail below:

[0121] Step 11: Load the electronic chart data corresponding to the USV operation area. Analyze the loaded electronic chart data according to the S-57 chart theoretical data model and data structure to obtain the area where the static obstacle is located in the electronic chart data.

[0122] Since the plane where the USV operation area is located is the OXY plane of the Cartesian rectangular coordinate system, the Mercator projection transformation is used to project the obstruction area where the static obstacle is located onto the OXY plane of the Cartesian rectangular coordinate system, so as to obtain the position of the static obstacle in the OXY plane of the Cartesian rectangular coordinate system.

[0123] Steps 1 and 2: Select an appropriate grid granularity, then divide the OXY plane of the Cartesian coordinate system into grids to determine the grids containing static obstacles. The gridded modeling result is as follows: Figure 2 As shown;

[0124] Step 13: In the grid map, expand the area where the static obstacle is located, and use the expanded area as the no-entry area in the grid map;

[0125] A safety buffer zone is established by expanding the point-like obstacle from an abstract geometric point into a two-dimensional circular restricted area, and an outward expansion operation is performed on each boundary of the surface obstacle.

[0126] Step 2: Adopt the improved The algorithm performs global planning on the USV's navigation path to obtain an initial global path planning result, and then smooths the initial global path planning result to obtain a smoothed global path.

[0127] Compared to the existing The algorithm and method of this invention mainly make two improvements, which are described in detail below:

[0128] 1. Cost function

[0129] The improvement The cost function of the algorithm is:

[0130]

[0131] in, Represents the cost function, This represents the distance from the starting point to the current node. The actual cost, Indicates from node Heuristic cost estimation to the target point Dynamic weights;

[0132]

[0133] in, The base of the natural logarithm. Represents the obstacle factor;

[0134]

[0135] in, This represents the number of prohibited grid cells along the straight line connecting the starting point and the target point. This represents the total number of grid cells traversed along the straight line connecting the starting point and the target point.

[0136] 2. Neighborhood search strategy

[0137] The improvement The algorithm employs a 5-neighborhood search strategy, specifically:

[0138] Step 1: In the OXY plane of the Cartesian rectangular coordinate system, take the positive X-axis as... direction;

[0139] Will The region is the region corresponding to the third neighbor of the current node;

[0140] Will The region is the region corresponding to the second neighbor of the current node;

[0141] Will The region is the region corresponding to the first neighbor of the current node;

[0142] Will The region is the region corresponding to the 4th neighbor of the current node;

[0143] Will The region is the region corresponding to the 6th neighbor of the current node;

[0144] Will The region is the region corresponding to the 7th neighbor of the current node;

[0145] Will The region is the region corresponding to the 8th neighbor of the current node;

[0146] Will The region is the region corresponding to the 5th neighbor of the current node;

[0147] Furthermore, it should be noted that for the current node, the numbering rule for its 8 surrounding neighbors is as follows: First, the neighbors in the first row are numbered from left to right as 1, 2, and 3; then, the neighbors in the second row are numbered from left to right as 4 and 5; and finally, the neighbors in the third row are numbered from left to right as 6, 7, and 8.

[0148] Step 2, as follows Figure 3 As shown, the traditional algorithm uses an 8-neighborhood search strategy. This invention introduces global target information into the local node expansion process and selects 5 neighborhoods from the 8 neighborhoods as the search area through a dynamic pruning mechanism.

[0149] Define the leading angle The guide angle is the angle between the vector pointing from this node to the final target point and the positive X-axis of the Cartesian coordinate system; calculated based on the positions of the current parent node to be expanded and the target point. :

[0150]

[0151] in, This indicates the position coordinates of the parent node to be expanded. This represents the position coordinates of the target point, and atan2 represents the arctangent function in the four quadrants.

[0152] Step 3: Determine the guide angle After the area is located, guide the angle. Location and guiding angle The two areas to the left of the location and the guiding angle The two regions to the right of the current region are considered as the five neighboring regions to be searched.

[0153] like Figure 4 As shown, during the node expansion phase, the guiding angle is determined according to the mapping rules. Mapped to the third region, the two regions to the left of the third region are region 1 and region 2, and the two regions to the right of the third region are region 5 and region 8. Then, the first, second, third, fifth and eighth regions are the five neighborhoods to be searched. That is, only the five neighboring nodes that are consistent with the global guidance direction are retained, and the other three neighboring nodes are directly deleted and do not participate in the subsequent cost function calculation.

[0154] After establishing the above search rules, in order to ensure the smoothness of all planned paths, the initial global path planning results need to be smoothed. The specific process for smoothing the initial global path planning results is as follows:

[0155] Let each node in the initial global path planning result be denoted as... , This represents the total number of nodes in the initial global path planning result;

[0156] In the initial global path planning results Filter the nodes so that the remaining nodes are the first ones. Nodes With the Nodes The area is not visually accessible, that is, it connects to the first Nodes With the Nodes The straight line segment needs to pass through the restricted area grid determined in step one three, where, , indicating the first Nodes With the Nodes Not adjacent nodes.

[0157] Taking the first and third nodes as examples, the selection principle of this invention is as follows:

[0158]

[0159] in, For connecting nodes With nodes The straight line segment, It is the set of all the grid cells corresponding to the restricted areas in the raster map. For set Any grid cell in the array;

[0160] like The value is Then the node Direct access to nodes , i.e., node If it can be removed, then use the 1st, 3rd, and 4th nodes to determine if the 3rd node can be removed; otherwise, remove the node. This should be retained, and then the second, third, and fourth nodes should be evaluated.

[0161] The process continues until no more nodes can be removed, at which point all remaining nodes are considered the remaining nodes after filtering.

[0162] To verify the improvement To assess the planning performance of the algorithm under different environments, this invention designed multiple sets of simulation experiments. The results are shown in Figures 5(a), 5(b), and 5(c). The experimental results demonstrate that the improvement proposed in this invention... The algorithm outperforms the basic algorithm in both planning efficiency and path quality. The algorithm effectively reduces invalid node expansion and redundant searches by combining environmental perception and goal-oriented search strategies, lowering the average planning time by over 90%. In path selection, the algorithm favors open water areas with lower obstacle density; although the path length increases slightly in some cases, the overall collision risk is reduced. Furthermore, based on the Line of Sight (LOS) smoothing mechanism, the algorithm eliminates redundant nodes in the path, further shortening the path length and reducing the number of turns.

[0163] Step 3: Based on the smoothed global path, the improved DWA algorithm is used to plan the local path of the USV, and the USV navigates according to the local path planning results.

[0164] Before starting local path planning, the motion model of the USV is first simplified to a three-degree-of-freedom (3-DOF) model, considering only its sway, roll, and bow roll. For example... Figure 6 As shown, the mathematical description of this model includes both kinematics and dynamics. The kinematic model describes the geometric relationship between the USV's velocity, position, and rate of change of attitude, without involving forces. Its state equation can be expressed as:

[0165]

[0166] in, Let be the pose vector of the USV in the global coordinate system. For the direction of navigation, Let be the velocity vector in the ship's coordinate system, with the components of the velocity vector being the longitudinal velocity. lateral velocity and angular velocity of heading , The rotation matrix representing the velocity transformation from the ship's coordinate system to the global coordinate system is defined as follows:

[0167]

[0168] The dynamic model follows Newton's second law, establishing the relationship between the external forces / torques acting on the USV and its acceleration. A general 3-DOF model's dynamic equations can be expressed as:

[0169]

[0170] in, This represents the system inertia matrix, including the added mass. and These are the Coriolis force matrix and the damping matrix, respectively. To advance the active control inputs generated by the system, This represents environmental disturbances caused by wind, waves, currents, etc.

[0171] Based on the aforementioned motion model, an improved DWA algorithm is used for local path planning, which can eliminate velocity combinations that do not meet dynamic constraints. Specifically, the improved DWA algorithm modifies the evaluation function of the original DWA algorithm. The evaluation function used in the improved DWA algorithm is... for:

[0172] in, This represents the heading angle deviation term, used to assess the closeness of the flight path to the target; The obstacle distance parameter is used to assess the safety of the flight path; The velocity term is used to evaluate the efficiency of the trajectory execution. , and These are the weighting coefficients; Represents the target forward gain function. This represents the path dynamic risk assessment function. This represents the collision avoidance cost function.

[0173] , and The calculation method is the same as that of the traditional DWA algorithm. The target forward gain function quantitatively evaluates the advancement efficiency of a path by measuring the effective distance traveled towards the target by each candidate path (i.e., the path under each combination of linear velocity and angular velocity) within the prediction period. The target forward gain function is described below. Path dynamic risk assessment function Collision avoidance cost function The calculation process is explained in detail below:

[0174] 1. Target forward gain function The calculation method is as follows:

[0175]

[0176] in, Indicates the current location of the USV. Indicates the location of the target point. This indicates the USV's prediction duration. The location reached later It is an arbitrarily small positive number, used to prevent the denominator from being zero. This indicates the linear velocity of the USV. This represents the angular velocity of the USV. This represents the L2 norm.

[0177] 2. Path dynamic risk assessment function

[0178] Sensors deployed on the USV are used to perceive dynamic obstacles. This invention employs a uniform velocity model to predict the path of dynamic obstacles, assuming that the velocity and heading of the dynamic obstacle remain constant in the short time domain. Based on this, a forward-looking dynamic risk field is established to quantify the potential collision risk in future spacetime and integrate it into the path evaluation function. Specifically, the calculation method of the path dynamic risk evaluation function is as follows:

[0179] The predicted path of a dynamic obstacle is discretized into a set of risk points. That is, after predicting the path of the dynamic obstacle, the path of the dynamic obstacle is discretized at each time step. Each location reached is considered a risk point, and the first... The coordinates of each risk point are: ;

[0180] Predicted duration within, no. Collision risk indicators at individual risk points for:

[0181]

[0182] in, Indicates the prediction duration Inside, USV and the The closest distance to each risk point; The risk perception distance threshold (which can be set based on experience) is used; the smaller the value of the collision risk index, the higher the collision risk.

[0183] No. Weight of each risk point :

[0184]

[0185] in, Indicates that the dynamic obstacle has reached the first The time difference between the time of each risk point and the current time The smaller the value, the more likely a collision will occur, and the higher the risk.

[0186] Calculate the path dynamic risk assessment function :

[0187]

[0188] in, Indicates the total number of risk points. Indicates safety reward items;

[0189]

[0190] in, Indicates the reward coefficient (adjusts the reward intensity); Indicates the prediction duration Inside, the USV operates according to linear velocity. and angular velocity Minimum distance from all risk points during navigation (encourages USVs to actively avoid obstacles and expand safe navigation space); The upper limit of the optimal safe distance (can be set based on experience).

[0191] This invention sets a uniform radius of influence for each risk point. When a path point falls within the influence range of a risk point, the risk weight corresponding to that path point is accumulated, forming a dynamic risk penalty. This penalty increases significantly with the number of times the path traverses a high-risk area, thereby effectively suppressing the selection of high-risk paths. Furthermore, by introducing a safety reward, it can further incentivize USVs to actively seek wider, safer routes.

[0192] The collision avoidance cost function The calculation method is as follows:

[0193] (1) For encounter scenarios, both parties are required to turn right, and the other party must turn left (angular velocity). Imposing a significant penalty on the path of turning right ( ), while also penalizing the path of turning right ( ) A small reward is given for the path specified in the example; otherwise, the reward remains the same. This item is controlled by the base reward / penalty coefficient P. The collision avoidance cost is:

[0194]

[0195] in, This represents the basic reward and punishment coefficient;

[0196] (2) In a starboard crossing scenario, the vessel, acting as the give-way vessel, must turn to starboard. Therefore, the strategy for maneuvering to port ( (Impose penalties.) The collision avoidance cost is:

[0197]

[0198] (3) In port-side crossing scenarios, the vessel, as a straight-ahead vessel, should maintain its course. To prevent dangerous maneuvers that could interfere with other vessels, this strategy applies to maneuvers to starboard ( Penalties will be imposed to encourage maintaining the course or, if necessary, swerving to the left to avoid a collision. The collision avoidance cost is as follows:

[0199]

[0200] (4) In the scenario of being overtaken, the vessel being overtaken should maintain its course and speed. Therefore, the strategy is ineffective against any significant course adjustments ( To maintain navigational stability, penalties will be imposed. Collision avoidance costs are as follows:

[0201]

[0202] (5) In the event of a stern-to-stern collision, the leading vessel should maintain a stable course and speed to provide a predictable basis for avoidance for the following vessel. The collision avoidance cost is as follows:

[0203]

[0204] (6) For other scenarios, collision avoidance costs All are 0.

[0205] This invention systematically models typical COLREG encounter scenarios (such as encounters, intersections, and overtaking), and integrates the quantified avoidance rules into the path evaluation function of the DWA algorithm, which can better ensure the safety of the path.

[0206] Furthermore, it should be specifically noted that the weighting coefficients of this invention... , and The fuzzy controller is used to obtain the following:

[0207] Step S1: Record the distance between the current position of the USV and the target point as... The minimum distance between the USV and the obstacle at the current moment is denoted as... ;

[0208] Define the first fuzzy subset respectively Second fuzzy subset and the third fuzzy subset ;

[0209] in, , This indicates the maximum detection range of the sensors deployed on the USV. , ;

[0210] Step S2: Calculate separately For the first fuzzy subset Second fuzzy subset and the third fuzzy subset The membership degree is then calculated separately. For the first fuzzy subset Second fuzzy subset and the third fuzzy subset Membership degree;

[0211] by The calculation process of membership degree will be explained using an example:

[0212] like In Within the range, the membership function is used for calculation. For the first fuzzy subset The degree of membership, if Not in Within the range, then For the first fuzzy subset The membership degree is 0;

[0213] like In Within the range, the membership function is used for calculation. For the second fuzzy subset The degree of membership, if Not in Within the range, then For the second fuzzy subset The membership degree is 0;

[0214] like In Within the range, the membership function is used for calculation. For the third fuzzy subset The degree of membership, if Not in Within the range, then For the third fuzzy subset The membership degree is 0;

[0215] The membership function used in this invention is:

[0216]

[0217] in, For input (substitute) or ), The lower bound is (substitute the first element of the fuzzy subset). The vertex (substitute the second element of the fuzzy subset). The upper bound is (substitute the third element of the fuzzy subset);

[0218] Step S3: Define the fourth fuzzy subset The fifth fuzzy subset and the sixth fuzzy subset ,in, , , ;

[0219] A fuzzy rule base is defined, comprising 9 fuzzy rules, which are as follows:

[0220] Within the first fuzzy rule: Corresponding to the third fuzzy subset, Corresponding to the third fuzzy subset, weight Corresponding to the fifth fuzzy subset, weight Corresponding to the fifth fuzzy subset, weight Corresponding to the sixth fuzzy subset;

[0221] Within the second fuzzy rule: Corresponding to the third fuzzy subset, Corresponding to the first fuzzy subset, weight Corresponding to the fourth fuzzy subset, weight Corresponding to the sixth fuzzy subset, weight Corresponding to the fourth fuzzy subset;

[0222] Within the third fuzzy rule: Corresponding to the first fuzzy subset, Corresponding to the third fuzzy subset, weight Corresponding to the sixth fuzzy subset, weight Corresponding to the fourth fuzzy subset, weight Corresponding to the sixth fuzzy subset;

[0223] Within the fourth fuzzy rule: Corresponding to the first fuzzy subset, Corresponding to the first fuzzy subset, weight Corresponding to the sixth fuzzy subset, weight Corresponding to the sixth fuzzy subset, weight Corresponding to the fourth fuzzy subset;

[0224] Within the 5th fuzzy rule: Corresponding to the third fuzzy subset, Corresponding to the second fuzzy subset, weight Corresponding to the fifth fuzzy subset, weight Corresponding to the fifth fuzzy subset, weight Corresponding to the sixth fuzzy subset;

[0225] Within the 6th fuzzy rule: Corresponding to the first fuzzy subset, Corresponding to the second fuzzy subset, weight Corresponding to the sixth fuzzy subset, weight Corresponding to the fifth fuzzy subset, weight Corresponding to the fifth fuzzy subset;

[0226] Within the 7th fuzzy rule: Corresponding to the second fuzzy subset, Corresponding to the third fuzzy subset, weight Corresponding to the fifth fuzzy subset, weight Corresponding to the fourth fuzzy subset, weight Corresponding to the sixth fuzzy subset;

[0227] Within the 8th fuzzy rule: Corresponding to the second fuzzy subset, Corresponding to the first fuzzy subset, weight Corresponding to the fifth fuzzy subset, weight Corresponding to the sixth fuzzy subset, weight Corresponding to the fourth fuzzy subset;

[0228] Within the 9th fuzzy rule: Corresponding to the second fuzzy subset, Corresponding to the second fuzzy subset, weight Corresponding to the fifth fuzzy subset, weight Corresponding to the fifth fuzzy subset, weight Corresponding to the sixth fuzzy subset;

[0229] The defined fuzzy rule base is shown in Table 1:

[0230] Table 1

[0231]

[0232] Step S4: Calculate the weights based on the fuzzy rules and the results from step S2. Weight and weight The value of .

[0233] The specific process of step S4 is as follows:

[0234] Step S41: For the first row of fuzzy rules in the fuzzy rule base, compare... For the third fuzzy subset Membership degree and For the third fuzzy subset The smaller of the two membership degrees is selected and used as the membership degree of the first row of fuzzy rules.

[0235] Then, iterate through each row of fuzzy rules in the fuzzy rule base to obtain the membership degree of each row of fuzzy rules;

[0236] Step S42: Compare the membership degrees of the fuzzy rules in each row to obtain the row with the highest membership degree, and calculate the weight of the fuzzy rule corresponding to the row with the highest membership degree. Weight and weight The value of .

[0237] For example, when the fuzzy rule with the highest membership degree is the fuzzy rule in the eighth row: define the membership degree of 0.5 in the fifth fuzzy subset as the membership degree of the eighth row, the membership degree of 0.3 in the fifth fuzzy subset as 0, and the membership degree of 0.7 in the fifth fuzzy subset as 0; define the membership degree of endpoint 1 in the sixth fuzzy subset as the membership degree of the eighth row, and the membership degree of 0.6 in the sixth fuzzy subset as 0; define the membership degree of endpoint 0 in the fourth fuzzy subset as the membership degree of the eighth row, and the membership degree of 0.4 in the fourth fuzzy subset as 0.

[0238] Assuming the membership degree of the eighth row is 0.3, the weights are calculated using the centroid (CoG) method. The value is:

[0239]

[0240] in, The first interval set according to the fifth fuzzy subset One sampling point, Indicates the first sampling points The corresponding membership value;

[0241] A sampling point is obtained every 0.1. The value of the first sampling point is recorded as 0.3, the value of the second sampling point as 0.4, the value of the third sampling point as 0.5, the value of the fourth sampling point as 0.6, and the value of the fifth sampling point as 0.7. Except for the sampling points located at the endpoints, which have a membership degree of 0, that is, except for the first and fifth sampling points, which have a membership degree of 0, the membership degree of other sampling points located in the interval is 0.3.

[0242] The calculation methods for the other two weight parameters are the same as those for the weights. The calculation method is the same.

[0243] The fuzzy logic controller of this invention can adaptively adjust the weights of some evaluation items according to the current state, which can ensure the overall stability and robustness of the algorithm in multi-objective and high-risk environments.

[0244] To verify the compliance of the improved DWA algorithm with maritime rules, this invention designed simulation experiments under four typical encounter scenarios. The experimental results for the four typical encounter scenarios are shown in Figure 7(a), Figure 7(b), Figure 7(c) and Figure 7(d), respectively.

[0245] To further evaluate the practical performance of the improved DWA algorithm in dynamic obstacle environments, a comparative experiment was conducted between the improved DWA algorithm and the basic DWA algorithm under the same kinematic model and obstacle velocity conditions. The experimental results of the basic DWA algorithm and the improved DWA algorithm are shown in Figure 8(a) and Figure 8(b), respectively. The experimental results show that the improved DWA algorithm exhibits significant advantages in terms of safety and environmental adaptability. The improved DWA algorithm can accurately identify and strictly follow the COLREGs maritime code to handle various typical encounter scenarios.

[0246] Comparative experiments were conducted on the fusion path planning method in both static and dynamic environments. The results of the static environment experiments are shown in Figures 9(a) and 9(b). The experimental results show that the improved fusion algorithm of this invention exhibits better obstacle avoidance performance, preferentially generating paths along sparse obstacle regions, and effectively reducing potential collision risks while maintaining path smoothness. Although the path length increases slightly in some scenarios, it significantly improves planning efficiency, motion smoothness, and navigation safety.

[0247] The comparative experimental results under dynamic environment are shown in Figure 10(a) and Figure 10(b). The experimental results show that when facing various types of dynamic collision threats, the improved algorithm of this invention can flexibly formulate and execute avoidance strategies in accordance with COLREGs maritime rules, which fully ensures navigation safety. This proves that by introducing dynamic risk field and fuzzy control, the method of this invention can significantly enhance the autonomous and compliant obstacle avoidance capability of unmanned surface vessels in complex multi-target dynamic conflicts.

[0248] The above examples of the present invention are merely illustrative of the computational model and process of the present invention, and are not intended to limit the implementation of the present invention. Those skilled in the art will recognize that other variations or modifications can be made based on the above description. It is impossible to exhaustively list all possible implementations here. Any obvious variations or modifications derived from the technical solutions of the present invention are still within the scope of protection of the present invention.

Claims

1. A deep fusion path planning method for USV autonomous navigation, characterized in that, The method specifically includes the following steps: Step 1: Load the electronic chart data corresponding to the USV operation area. Process the electronic chart data to obtain a two-dimensional raster map, which includes restricted areas corresponding to static obstacles. Step 2: Adopt the improved The algorithm performs global planning on the USV's navigation path to obtain an initial global path planning result, and then smooths the initial global path planning result to obtain a smoothed global path. Step 3: Based on the smoothed global path, the improved DWA algorithm is used to plan the local path of the USV, and the USV navigates according to the local path planning results.

2. The deep fusion path planning method for USV autonomous navigation according to claim 1, characterized in that, The specific process of step one is as follows: Step 1: Load the electronic chart data corresponding to the USV operation area, parse the loaded electronic chart data, and obtain the area where the static obstacle is located in the electronic chart data; The Mercator projection transformation is used to project the obstruction zone where the static obstacle is located onto the OXY plane of the Cartesian rectangular coordinate system, so as to obtain the position of the static obstacle in the OXY plane of the Cartesian rectangular coordinate system; Step 1 and 2: Next, divide the OXY plane of the Cartesian coordinate system into grids to determine the grids where static obstacles are located; Step 13: In the grid map, the area where the static obstacle is located is expanded, and the expanded area is used as the no-entry zone in the grid map.

3. The deep fusion path planning method for USV autonomous navigation according to claim 1, characterized in that, The improvement The cost function of the algorithm is: in, Represents the cost function, This represents the distance from the starting point to the current node. The actual cost, Indicates from node Heuristic cost estimation to the target point Dynamic weights; in, The base of the natural logarithm. Represents the obstacle factor; The obstacle factor for: in, This represents the number of prohibited grid cells along the straight line connecting the starting point and the target point. This represents the total number of grid cells traversed along the straight line connecting the starting point and the target point.

4. The deep fusion path planning method for USV autonomous navigation according to claim 1, characterized in that, The improvement The algorithm employs a 5-neighborhood search strategy, specifically: Step 1: In the OXY plane of the Cartesian rectangular coordinate system, take the positive X-axis as... direction; Will The region is the region corresponding to the third neighbor of the current node; Will The region is the region corresponding to the second neighbor of the current node; Will The region is the region corresponding to the first neighbor of the current node; Will The region is the region corresponding to the 4th neighbor of the current node; Will The region is the region corresponding to the 6th neighbor of the current node; Will The region is the region corresponding to the 7th neighbor of the current node; Will The region is the region corresponding to the 8th neighbor of the current node; Will The region is the region corresponding to the 5th neighbor of the current node; Step 2: Calculate the guide angle based on the positions of the parent node to be expanded and the target point. : in, This indicates the position coordinates of the parent node to be expanded. This represents the position coordinates of the target point, and atan2 represents the arctangent function in the four quadrants. Step 3: Determine the guide angle After the area is located, guide the angle. Location and guiding angle The two areas to the left of the location and the guiding angle The two regions to the right of the current region are considered as the five neighboring regions to be searched.

5. The deep fusion path planning method for USV autonomous navigation according to claim 1, characterized in that, The specific process for smoothing the initial global path planning results is as follows: Let each node in the initial global path planning result be denoted as... , This represents the total number of nodes in the initial global path planning result; In the initial global path planning results Filter the nodes so that the remaining nodes are the first ones. Nodes With the Nodes The area is not visually accessible, that is, it connects to the first Nodes With the Nodes The straight line segment needs to pass through the restricted area grid determined in step one or three.

6. The deep fusion path planning method for USV autonomous navigation according to claim 1, characterized in that, The improved DWA algorithm uses the following evaluation function. for: in, This indicates the heading angle deviation term; Indicates the distance to obstacles; Indicates the velocity term; , and These are the weighting coefficients; Represents the target forward gain function. This represents the path dynamic risk assessment function. This represents the collision avoidance cost function.

7. The deep fusion path planning method for USV autonomous navigation according to claim 1, characterized in that, The target forward gain function The calculation method is as follows: in, Indicates the current location of the USV. Indicates the location of the target point. This indicates the USV's prediction duration. The location reached later For any small positive number, This indicates the linear velocity of the USV. This represents the angular velocity of the USV. This represents the L2 norm.

8. The deep fusion path planning method for USV autonomous navigation according to claim 1, characterized in that, The calculation method for the path dynamic risk assessment function is as follows: The predicted path of dynamic obstacles is discretized into a set of risk points, and the first... The coordinates of each risk point are: ; Predicted duration within, no. Collision risk indicators at individual risk points for: in, Indicates the prediction duration Inside, USV and the The closest distance to each risk point; This represents the risk perception distance threshold. No. Weight of each risk point : in, Indicates that the dynamic obstacle has reached the first The time difference between the time of each risk point and the current time; Calculate the path dynamic risk assessment function : in, Indicates the total number of risk points. Indicates safety reward items; in, Indicates the reward coefficient; Indicates the prediction duration Inside, the USV operates according to linear velocity. and angular velocity Minimum distance from all risk points during navigation; This represents the upper limit of the optimal safe distance.

9. The deep fusion path planning method for USV autonomous navigation according to claim 1, characterized in that, The collision avoidance cost function The calculation method is as follows: (1) For encounter scenarios, the collision avoidance cost is: in, This represents the basic reward and punishment coefficient; (2) For the starboard crossing scenario, the collision avoidance cost is: (3) For the port side crossing scenario, the collision avoidance cost is: (4) For the scenario of being overtaken, the collision avoidance cost is: (5) For the stern crossing scenario, the collision avoidance cost is: (6) For other scenarios, collision avoidance costs All are 0.

10. A deep fusion path planning method for USV autonomous navigation according to claim 1, characterized in that, The weighting coefficient , and The fuzzy controller is used to obtain the following: Step S1: Record the distance between the current position of the USV and the target point as... The minimum distance between the USV and the obstacle at the current moment is denoted as... ; Define the first fuzzy subset respectively Second fuzzy subset and the third fuzzy subset ; in, , This indicates the maximum detection range of the sensors deployed on the USV. , ; Step S2: Calculate separately For the first fuzzy subset Second fuzzy subset and the third fuzzy subset The membership degree is then calculated separately. For the first fuzzy subset Second fuzzy subset and the third fuzzy subset Membership degree; Step S3: Define the fourth fuzzy subset The fifth fuzzy subset and the sixth fuzzy subset ,in, , , ; A fuzzy rule base is defined, comprising 9 fuzzy rules, which are as follows: Within the first fuzzy rule: Corresponding to the third fuzzy subset, Corresponding to the third fuzzy subset, weight Corresponding to the fifth fuzzy subset, weight Corresponding to the fifth fuzzy subset, weight Corresponding to the sixth fuzzy subset; Within the second fuzzy rule: Corresponding to the third fuzzy subset, Corresponding to the first fuzzy subset, weight Corresponding to the fourth fuzzy subset, weight Corresponding to the sixth fuzzy subset, weight Corresponding to the fourth fuzzy subset; Within the third fuzzy rule: Corresponding to the first fuzzy subset, Corresponding to the third fuzzy subset, weight Corresponding to the sixth fuzzy subset, weight Corresponding to the fourth fuzzy subset, weight Corresponding to the sixth fuzzy subset; Within the fourth fuzzy rule: Corresponding to the first fuzzy subset, Corresponding to the first fuzzy subset, weight Corresponding to the sixth fuzzy subset, weight Corresponding to the sixth fuzzy subset, weight Corresponding to the fourth fuzzy subset; Within the 5th fuzzy rule: Corresponding to the third fuzzy subset, Corresponding to the second fuzzy subset, weight Corresponding to the fifth fuzzy subset, weight Corresponding to the fifth fuzzy subset, weight Corresponding to the sixth fuzzy subset; Within the 6th fuzzy rule: Corresponding to the first fuzzy subset, Corresponding to the second fuzzy subset, weight Corresponding to the sixth fuzzy subset, weight Corresponding to the fifth fuzzy subset, weight Corresponding to the fifth fuzzy subset; Within the 7th fuzzy rule: Corresponding to the second fuzzy subset, Corresponding to the third fuzzy subset, weight Corresponding to the fifth fuzzy subset, weight Corresponding to the fourth fuzzy subset, weight Corresponding to the sixth fuzzy subset; Within the 8th fuzzy rule: Corresponding to the second fuzzy subset, Corresponding to the first fuzzy subset, weight Corresponding to the fifth fuzzy subset, weight Corresponding to the sixth fuzzy subset, weight Corresponding to the fourth fuzzy subset; Within the 9th fuzzy rule: Corresponding to the second fuzzy subset, Corresponding to the second fuzzy subset, weight Corresponding to the fifth fuzzy subset, weight Corresponding to the fifth fuzzy subset, weight Corresponding to the sixth fuzzy subset; Step S4: Calculate the weights based on the fuzzy rules and the results from step S2. Weight and weight The value; The specific process of step S4 is as follows: Step S41: For the first row of fuzzy rules in the fuzzy rule base, compare... For the third fuzzy subset Membership degree and For the third fuzzy subset The smaller of the two membership degrees is selected and used as the membership degree of the first row of fuzzy rules. Then, iterate through each row of fuzzy rules in the fuzzy rule base to obtain the membership degree of each row of fuzzy rules; Step S42: Compare the membership degrees of the fuzzy rules in each row to obtain the row with the highest membership degree, and calculate the weight of the fuzzy rule corresponding to the row with the highest membership degree. Weight and weight The value of .

Images