An Unmanned Surface Vehicle Formation Path Planning Method

Abstract

The present invention discloses a method for path planning of unmanned surface vehicle formations, which relates to the technical field of formation path planning. It includes rasterizing the map, adjusting the formation of the initial unmanned surface vehicle formation, determining the starting and ending point information of the leader, and determining the positions of dynamic obstacles and the information of static obstacles; introducing an attenuation coefficient into the heuristic function of the A-Star algorithm and using the improved A-Star algorithm for path planning; performing path smoothing through a Bezier curve with adaptively selected control points; introducing an improved dynamic window method for dynamic local obstacle avoidance; generating a planned path for the follower according to the initial unmanned surface vehicle formation and the position of the follower in the formation, in combination with the global path of the leader, and the follower maintains the same speed and deflection angle as the leader. The present invention ensures the optimality of the path planning of the leader and also ensures that the follower can be adaptively adjusted in real time, effectively improving the efficiency, stability and practicality of the formation path planning.

Description

Technical Field

[0001] The present invention relates to the technical field of formation path planning, and in particular to a path planning method for an unmanned surface vehicle formation. Background Art

[0002] As a core equipment in ocean exploration and resource development, unmanned surface vehicles play a crucial role in ocean research. Path planning, as a key link in the autonomous navigation control technology of unmanned surface vehicles, is a powerful tool for exploring the ocean. With the continuous increase in ocean exploration tasks and the increasing complexity of tasks in recent years, the formation control of unmanned surface vehicles has become a research hotspot. Through a formation composed of multiple unmanned surface vehicles, ocean tasks can be effectively disassembled and coordinated, improving the execution efficiency and reliability of tasks.

[0003] Currently, the research on the path planning of unmanned surface vehicle formations is gradually increasing. Many algorithms have also been applied to the problem of unmanned surface vehicle formation path planning, such as the A-Star algorithm, genetic algorithm, ant colony algorithm, deep learning algorithm, etc. These algorithms each have their own advantages. In particular, the A-Star algorithm is widely used in the research on unmanned surface vehicle formation path planning because of its natural advantages in grid maps, which can optimize targets and improve search efficiency. However, the A-Star algorithm has problems such as high consumption of computing resources and inability to effectively handle dynamic environments, resulting in difficulties in avoiding obstacles, occupying a large amount of memory, and having an uneven path and too large turning angles that do not conform to the actual navigation during the path planning process. At the same time, the A-Star algorithm does not consider the kinematic characteristics of the unmanned surface vehicle itself and its physical limitations such as angular velocity and linear velocity, so it is difficult to fit the actual navigation requirements. For the leader of an unmanned surface vehicle formation, reasonable path planning can not only ensure the stable control and movement of the formation, but also effectively reduce the computational complexity, improve the practicality of path planning and the fit with real navigation. Summary of the Invention

[0004] In order to overcome the above problems existing in the prior art, the present invention proposes a path planning method for an unmanned surface vehicle formation.

[0005] The technical solution adopted by the present invention to solve its technical problems is: a path planning method for an unmanned surface vehicle formation, including the following steps:

[0006] Step 1, rasterize the map, adjust the initial formation of the unmanned surface vehicle formation, determine the starting point and ending point information of the leader of the unmanned surface vehicle formation, and determine the positions of dynamic obstacles and the information of static obstacles;

[0007] Step 2, introduce an attenuation coefficient into the heuristic function of the A-Star algorithm, and use the improved A-Star algorithm for path planning;

[0008] Step 3: Perform path smoothing through a Bezier curve with adaptively selected control points;

[0009] Step 4: Introduce an improved dynamic window method for dynamic local obstacle avoidance to generate the global path of the leader;

[0010] Step 5: According to the initial formation of the unmanned surface vehicle fleet and the positions of the followers in the fleet, combined with the global path of the leader obtained in Step 4, generate the planned paths of the followers, and the followers maintain the same speed and deflection angle as the leader;

[0011] In the above Step 5, the dynamic window method described in Step 4 is used for the dynamic local obstacle avoidance of the followers. When the distance between obstacles is less than the size of the unmanned surface vehicle fleet formation, set the obstacle avoidance priorities for different unmanned surface vehicles to ensure that the unmanned surface vehicles in the fleet pass through in sequence.

[0012] For the above method for path planning of an unmanned surface vehicle fleet, the heuristic function of the A-Star algorithm in Step 2 is specifically:

[0013] ;

[0014] ;

[0015] Among them, is the total cost function; represents the cost function from the starting point to node ; represents the heuristic evaluation function from node to the target node; represents the attenuation coefficient constructed by combining a Gaussian function, which is used to dynamically adjust the weight ratio of the heuristic function; and are related parameters.

[0016] For the above method for path planning of an unmanned surface vehicle fleet, the related parameters and take values of , respectively, and the attenuation coefficient is:

[0017] .

[0018] For the above-mentioned method for path planning of a formation of unmanned surface vessels, the specific process of using the improved A-Star algorithm for path planning in step 2 is as follows: Traverse the eight child nodes around the starting point, and select the node with the lowest total cost function value as the next parent node. When searching at the new parent node, if a neighbor node has already been searched, skip this node until the target point is approached; when the position information of the target point is included in the child nodes around the new parent node, directly select the target point as the next parent node.

[0019] For the above-mentioned method for path planning of a formation of unmanned surface vessels, step 3 specifically includes:

[0020] Step 3.1, Select all the parent node information generated in step 2, denoted as , where the starting point is , and the ending point is , Starting from the starting point , divide all the parent nodes into three consecutive groups, with three nodes in each group;

[0021] Step 3.2, Use the three nodes in each group obtained in step 3.1 to generate a Bezier curve for piecewise smoothing, and calculate the vector sum of the distances from the middle node to the obstacles on both sides ;

[0022] Step 3.3, According to the vector sum obtained in step 3.2, calculate the control point coordinate vector:

[0023] ;

[0024] Among them, is the coordinate vector of the control point, is the scaling factor, is the coordinate vector of the middle node.

[0025] For the above-mentioned method for path planning of a formation of unmanned surface vessels, the dynamic local obstacle avoidance in step 4 specifically includes:

[0026] Step 4.1, Establish a three-degree-of-freedom kinematic model of the unmanned vessel:

[0027] ;

[0028] ;

[0029] Among them, , represents the Euler angle vector; represents its angular velocity vector; represents the transformation matrix; represents the deflection angle of the unmanned vessel relative to the inertial coordinate system;

[0030] Step 4.2: Conduct speed sampling based on the three-degree-of-freedom kinematic model of the unmanned boat obtained in Step 4.1;

[0031] Step 4.3: According to the three-degree-of-freedom kinematic model of the unmanned boat obtained in Step 4.1 and the speed obtained in Step 4.2, update the position and heading angle of the unmanned boat at the next moment within each time step. Select a time period T and continuously sample at each time interval a to generate a predicted trajectory;

[0032] Step 4.4: Evaluate the predicted trajectory generated in Step 4.3 through an evaluation function. The evaluation function has the following expression:

[0033] ;

[0034] ;

[0035] ;

[0036] where represents the heading deviation of the current trajectory; represents the minimum distance from the obstacle; represents the smoothness cost; represents the time cost to reach the target or the path length cost; represents the distance deviation between the current unmanned boat and the expected position of the formation; represents the speed error between the current unmanned boat and other members; represents the weight coefficient; is the next position predicted by the current speed combination of the leader; represents the ideal formation position calculated based on the formation structure and the position of the leader boat; and are the linear velocity and angular velocity of the current speed combination, and are the speed reference values of the leading boat or the formation target.

[0037] For the above-mentioned method for path planning of a surface unmanned boat formation, the speed space that can be sampled in Step 4.2 is as follows:

[0038] ;

[0039] ;

[0040] ;

[0041] ;

[0042] ;

[0043] Among them, and respectively represent the minimum linear velocity and the maximum linear velocity of the unmanned boat; and respectively represent the minimum angular velocity and the maximum angular velocity of the unmanned boat; and represent the values of the linear velocity and the angular velocity at the current moment; and respectively represent the maximum linear acceleration and the maximum angular acceleration of the unmanned boat; represents the closest distance between the simulated trajectory corresponding to the current speed and the surrounding obstacles; represents the distance between the unmanned boat and the dynamic obstacle, represents the distance between the unmanned boat and the surrounding static obstacles.

[0044] The beneficial effect of the present invention is that by improving the A-Star algorithm and adding a decay coefficient constructed by a Gaussian function, the efficiency of the path planning of the unmanned boat can be improved, especially near the target point, and a large number of unnecessary searches are reduced. And it can effectively reduce the turning angle of the unmanned boat.

[0045] By introducing a Bezier curve smoothing method that can adaptively select control points, the safety of path smoothing can be effectively improved, making the path planning more in line with the actual situation.

[0046] By introducing an improved DWM algorithm, the movement of the unmanned boat is more in line with its own kinematic characteristics, and its angular velocity, linear velocity and other limitations can be better considered. And it can always maintain the position and speed with other unmanned boats, improve the safety of formation navigation, and can maintain the formation shape of the unmanned boat.

[0047] In the present invention, the global path planning is carried out on the leader to generate the optimal track. At the same time, other sub-vessels dynamically adjust the heading and speed according to the relative path of the leader through the DWM algorithm, avoid obstacles and maintain the formation stability. In this way, both the optimality of the path planning of the leader is ensured, and it is also ensured that all vessels in the formation can make adaptive adjustments according to the real-time environment, thereby effectively improving the efficiency, stability and practicability of the formation path planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] Figure 1 is a schematic flow chart of the present invention;

[0049] Figure 2 is a detailed method flow chart of an embodiment of the present invention;

[0050] Figure 3 is a schematic diagram of the diamond formation of the unmanned boat of the present invention;

[0051] Figure 4 It is a schematic diagram of the modeling of a three - degree - of - freedom unmanned surface vehicle model in an embodiment of the present invention. Detailed implementation manners

[0052] To enable those skilled in the art to better understand the technical solution of the present invention, the present invention will be described in detail below in conjunction with the accompanying drawings and specific implementation manners.

[0053] This embodiment discloses a method for path planning of an unmanned surface vehicle formation based on the A - Star algorithm and the DWM algorithm. Its overall flowchart is as Figure 1 shown, and includes the following steps:

[0054] Step 1: Perform map rasterization processing. Binary - ize and rasterize the real map to convert it into a raster map for subsequent processing of the algorithm. In particular, smaller rasters are used for areas with dense obstacles to improve accuracy.

[0055] Step 2: Adjust the formation of the initial unmanned surface vehicle formation. Use a diamond formation as its basic formation shape, and determine the initial relative positions, speeds, steering angles, etc. of the leader and followers, as Figure 3 shown.

[0056] Step 3: Improve the A - Star algorithm. Specifically, improve the heuristic function of the A - Star algorithm. The improved heuristic function of the A - Star algorithm is:

[0057] ;

[0058] ;

[0059] In the formula, is the total cost function, represents the cost function from the starting point to node , represents the heuristic evaluation function from node to the target node, represents the attenuation coefficient constructed by combining the Gaussian function, which is used to dynamically adjust the weight ratio of the heuristic function, and are its related parameters (adjustable). For the convenience of calculation and to reduce the amount of computation, set , .

[0060] When the leader is far from the target point, at this time , at this time the weight of is very large, and the algorithm becomes the Dijkstra algorithm at this time. When the distance from the target point is relatively close, the weight is larger, The weight gradually decreases, and at this time, the A-Star algorithm becomes an algorithm dominated by This design of the attenuation coefficient has the advantage that in the initial stage occupies a larger weight, enabling the algorithm to search near the target point faster. At the end stage of the planning process, occupies a larger weight, which can avoid repeated node searches that may occur near the target point and reduce the total turning angle of the path.

[0061] Step 4: Design a Bezier curve smoothing method for adaptively selecting control points to smooth the path generated in Step 3 and reduce the cost of manual operations, enabling the leader in the unmanned boat formation to generate a smoother path that better suits its actual situation.

[0062] Step 5: Design a dynamic window method (DWM) with multiple evaluation functions, which not only considers its own kinematic characteristics, but also physical characteristics such as linear velocity and angular velocity, and also considers the position and velocity information of the followers in other unmanned boat formations. After the global path is generated in Step 4, the leader moves along the designed global path. If a dynamic obstacle suddenly appears, this method is used for local dynamic obstacle avoidance to avoid dynamic obstacles and improve the safety of path planning, effectively making a path planning that conforms to actual navigation.

[0063] Step 6: After the leader generates the global path, generate the planned paths of other followers according to the formation of the unmanned boat formation and the positions of the followers in other formations. Other followers move according to the generated planned paths and maintain the same speed and deflection angle as the leader, which can ensure that the unmanned boat formation maintains a certain formation during navigation.

[0064] Step 7: Other followers use the dynamic window method in Step 5 to perform local dynamic obstacle avoidance while following the planned path and always maintain the relative positions between the unmanned boats to keep the formation, preventing accidents during the navigation of the unmanned boat formation.

[0065] Step 8: While the formation maintains the formation, perform local obstacle avoidance. In areas with complex obstacles, especially in narrow obstacle passages, due to its large size, it is difficult for the diamond formation to pass through the complex obstacle area in formation. Therefore, it is necessary to set the obstacle avoidance weights of different unmanned boats to reduce path conflicts. Finally, the formation reaches the target point according to the planned path, completing the path planning task.

[0066] In this embodiment, detailed path planning steps are given, as Figure 2 shown, specifically:

[0067] (1) Binarize and rasterize the map, divide it into a feasible region and an infeasible region, determine the starting point and ending point information of the leader in the unmanned boat formation in the map, and determine the positions of dynamic obstacles and the information of static obstacles. Mark this information in the image.

[0068] (2) Improve the A-Star algorithm, propose a heuristic function for the improved A-Star algorithm, and add a decay coefficient constructed by a Gaussian function. The expression of the heuristic function of the improved A-Star algorithm is:

[0069] ;

[0070] ;

[0071] In the formula, is the total cost function, represents the cost function from the starting point to node , represents the heuristic evaluation function from node to the target node, represents the decay coefficient constructed by combining the Gaussian function, which is used to dynamically adjust the weight ratio of the heuristic function, and are its related parameters (adjustable). In practical applications, design , , so its decay coefficient becomes:

[0072] .

[0073] The advantage of designing the decay coefficient like this is that when the formation leader is far from the target point, the value is large enough, becomes smaller, in the proportion of becomes larger. At this time, the A-Star algorithm is dominated by Since represents the cost of the explored path, represents the heuristic function cost. Therefore, when dominates, the value of is much larger than occupies the dominant position, and the A-Star algorithm is the dominant search algorithm. At this time, when the algorithm selects a node for expansion, it pays more attention to the cost of the path that has been traveled rather than relying on the heuristic function , so its turning angle will become smaller, especially when there is a sudden change in direction. Because the dominant algorithm tends to choose paths with lower costs, and these paths are usually naturally continuous, and the changes in the paths are not too drastic. And it can avoid repeated node searches that may occur when approaching the target point. And in Adding has the advantage of being able to complete the transformation of the algorithm as soon as possible and improve the advantages of the algorithm.

[0074] (3) Use the improved A-Star algorithm for path planning. The specific method is to establish OPEN and CLOSE tables for the starting point and add the starting point to the OPEN and CLOSE tables. Traverse the eight surrounding child nodes and select the node with the lowest value as the next parent node. When searching for nodes at the new parent node, if the neighbor node has already been searched, this node will not be selected as the new parent node and will be skipped. And so on until near the target point. When the position information of the target point is included in the surrounding child nodes of the new parent node, the target point will be directly selected as the next parent node, that is, the end point. In this way, it can be considered that the algorithm ends.

[0075] (4) Perform path smoothing. Propose an improved Bezier curve smoothing method that can adaptively select control points. The specific smoothing measures are as follows: Select all the parent node information generated in step (3), including the starting point and the end point, and name it . Among them, the starting point is , and the end point is . Starting from the starting point , divide these nodes into continuous groups of three nodes each. For example: The first group { }, the second group { }. If there are nodes, there will be one node left at the end that cannot form a group of three, then take { } as a group. If there are a total of nodes, they can be exactly divided into groups of three each. Each group of three nodes will be used to generate a Bezier curve. Select a group of nodes from the previous step .

[0076] Define the path formed by this group of nodes as , select the middle node among them, and check the path Obstacles on both sides, calculate the shortest distances from the obstacles on both sides to the middle node Calculate the distance from the obstacles on side A of the path to the distance Calculate the distance from the obstacles on side B of the path to the distance Among them, the coordinates of the obstacles on both sides that are closest to the middle node are defined as Define the vector from node to the coordinates of the two obstacles as Calculate the sum of the two vectors Take the midpoint of the vector sum Then the coordinate vector of its control point is Since may have an excessive amplitude due to the distribution of obstacles, resulting in excessive path bending. Therefore, a scaling factor is added. Therefore, the coordinate of its control point can be designed as:

[0077] .

[0078] The advantage of this design is that this method ensures the adaptive selection of the control point by calculating the vectors of the obstacles on both sides and the middle node, taking the middle vector point, reducing the workload of manual operation. Moreover, this method can well avoid the problem of unilateral bias, enabling the path to bypass obstacles more balanced and natural during obstacle avoidance, rather than forcibly deviating to one side, avoiding excessive deviation or unnatural and discontinuous turning, and avoiding drastic changes in path curvature. Moreover, combined with the introduction of the scaling factor , it can enable flexible adjustment of the control point of the path in the actual environment and also avoid excessive bending of the path.

[0079] (5) Introduce an improved dynamic window method for dynamic obstacle avoidance to prevent suddenly appearing dynamic obstacles in the path and also to solve the problem that the A-Star algorithm cannot perform dynamic obstacle avoidance. In order to consider the kinematic characteristics of the unmanned boat in the DWM algorithm, it is necessary to model the unmanned boat. In order to accurately describe the movement of the unmanned boat, it is necessary to establish a reference coordinate system for the unmanned boat. Since the main research is on the path planning of the unmanned boat and mainly conducts simulation experiments in the grid map, only a three-degree-of-freedom model needs to be established.

[0080] Therefore, the rolling, pitching, and heaving motion models of the unmanned boat are not considered. The modeling of its three-degree-of-freedom unmanned boat model is as Figure 4 shown represents its inertial coordinate axes axis and The axes are all parallel to the horizontal plane and point to the due east and due north directions of geography respectively. The body coordinate system is , and the origin is usually defined as the centroid of the unmanned surface vehicle, the axis and the axis are both parallel to the sea level and point from the origin to the bow and starboard of the unmanned surface vehicle respectively. Among them represents its linear velocity of surge, represents its linear velocity of sway, represents its angular velocity of yaw. represents the deflection angle of the unmanned surface vehicle (USV) relative to the inertial coordinate system. During the path planning process, its kinematic characteristics are key factors, and the motion state and geometric relationship of the unmanned surface vehicle are mainly concerned. Therefore, to simplify the model and improve the calculation efficiency, this study is only based on the three-degree-of-freedom kinematic model without considering complex dynamic characteristics (such as hydrodynamic effects, inertial moments, etc.). In most planar path planning and obstacle avoidance scenarios, the kinematic model can accurately describe the motion characteristics of the unmanned surface vehicle in the two-dimensional plane, especially under low-speed or medium-speed operating conditions. Therefore, the three-degree-of-freedom kinematic model of the unmanned surface vehicle is:

[0081] ;

[0082] Among them , represents the Euler angle vector. represents its angular velocity vector, represents the transformation matrix, where is defined under the three-degree-of-freedom model as

[0083] .

[0084] Therefore, the motion equation of its three-degree-of-freedom kinematic model is:

[0085] ;

[0086] ;

[0087] ;

[0088] The state of its system can be expressed as .

[0089] The Dynamic Window Method (DWM) is a method based on predictive control theory. It can safely and effectively avoid obstacles in an unknown environment. At the same time, it also has the advantages of small computational complexity and rapid response. Its principle is as follows: First, the speed samples of the robot are collected through the mathematical model of the USV, and the motion trajectories in the next time period under the sample speeds are predicted and simulated. Then, by evaluating these motion trajectories, the optimal path is finally selected. The USV moves along the optimal path, and its motion posture and direction are jointly determined by the current linear velocity and angular velocity of the USV. The DWM algorithm mainly includes three steps: speed sampling, trajectory prediction, and trajectory evaluation.

[0090] According to the characteristics of the USV itself and environmental limitations, there are certain boundary limitations for the speed of the USV, and the speed space that can be sampled can be expressed as:

[0091] ;

[0092] where and represent the minimum linear velocity and the maximum linear velocity of the USV respectively. and represent the minimum angular velocity and the maximum angular velocity respectively.

[0093] Since the USV is motor-driven, there are certain limitations on its linear acceleration and angular acceleration. Therefore, when considering acceleration, the speed space that can be sampled is:

[0094] ;

[0095] where and represent the values of the linear velocity and angular velocity at the current moment, and represent the maximum linear acceleration and the maximum angular acceleration of the USV respectively.

[0096] In this embodiment, the DWM algorithm is used for dynamic obstacle avoidance. Therefore, it is necessary to consider the obstacle factors around the USV during the dynamic obstacle avoidance process. The constraint condition for the USV not to collide with obstacles during a certain dynamic obstacle avoidance process is:

[0097] ;

[0098] where represents the minimum distance between the simulated trajectory corresponding to the current speed and the surrounding obstacles. Its definition is:

[0099] ;

[0100] where represents the distance between the USV and the dynamic obstacle, and represents the distance from the surrounding static obstacles. Before the triggering condition of the dynamic obstacle is reached, it only contains the speed constraints of the static obstacles. When the triggering condition for dynamic obstacle avoidance is reached, is and simultaneously controlled.

[0101] In summary, the final speed sampling space of the USV is the intersection of the three speed spaces, that is:

[0102] ;

[0103] At each time step within, the above kinematic equations are used to update and determine the position and heading angle of the USV at the next moment.

[0104] According to the state space equation at the current moment, the state of the USV at the next moment can be calculated by the following equation:

[0105] ;

[0106] ;

[0107] .

[0108] The core of the estimation and prediction is trajectory sampling. Assuming a time period is selected, a series of future moment trajectory points can be predicted by continuously sampling at each time interval a. In all simulations in this paper, the time interval a = 0.1 second. During the prediction process, in addition to calculating the trajectory, it is also necessary to check whether the position at the next moment conflicts with the obstacle. If the predicted trajectory point at a certain moment collides with the obstacle, the current speed combination under the sampling is considered unacceptable, and a new speed combination needs to be selected for obstacle avoidance.

[0109] After determining the constrained speed range of the robot, some simulated trajectories may be feasible, but too many trajectories will cause the navigation judgment of the USV to become chaotic. Therefore, it is necessary to evaluate the multiple sets of trajectories obtained by sampling to select the speed corresponding to the optimal trajectory as the driving speed. To achieve this goal, an evaluation function is designed, which is in the form of a weighted sum of multiple sub-functions, and each sub-function is optimized for a specific performance criterion. These criteria include: azimuth deviation, obstacle distance, the current linear speed and angular speed of the USV, the prediction time of the trajectory, etc. In addition, considering the state of the follower ships in the formation, the evaluation function also needs to combine the position and speed information of other ships to ensure that the follower can maintain a reasonable formation and avoid collisions with each other.

[0110] At each speed combination sampling, a predicted trajectory is generated, and the distance between each trajectory point and the obstacle is calculated during the prediction process. If the trajectory at a certain moment collides with the obstacle, then this speed combination is considered infeasible and needs to be excluded. At the same time, in order to ensure the stability of the formation, the evaluation function also needs to consider the distance relationship between the follower and the ship in front, ensuring that the motion states of all ships can work together and avoiding excessive speed differences or collision risks between each other.

[0111] Specifically, the evaluation function comprehensively scores each trajectory based on the following aspects: azimuth deviation, obstacle distance, the current speed of the USV, the prediction time of the trajectory, and formation stability.

[0112] Finally, for each feasible trajectory, the following weighted evaluation function is used for scoring:

[0113] ;

[0114] where represents the heading deviation of the current trajectory, the larger the deviation from the target angle, the larger the value, represents the minimum distance from the obstacle, represents the smoothness cost, reflecting the magnitude of the speed change, represents the time cost to reach the target, or the path length cost, represents the distance deviation between the current USV and the expected position of the formation, represents the speed error between the current USV and other members, where represents the weight coefficient (adjustable), where and The expressions of are:

[0115] ;

[0116] where is the next position predicted by the current speed combination of the leader, represents the ideal formation position calculated according to the formation structure and the position of the leader boat.

[0117] ;

[0118] where and are the linear velocity and angular velocity of the current speed combination, and are the speed reference values of the leading boat or the formation target, and the current speed or the average formation speed of the leader boat can be taken. The smaller this value is, the closer the current speed is to the reference speed, and the better the formation synchronization is.

[0119] (6) After the leader generates the global path by improving the A-Star algorithm, based on the formation positions of the leader and the followers, the global paths of the followers are generated. The paths of the followers are generated on the basis of the leader's path, taking into account the formation positions. When the global paths of the followers are generated, the Dynamic Window Method (DWM) is used for local obstacle avoidance to avoid dynamic obstacles in real time. When encountering a narrow passage, since the distance between the obstacles is less than the formation size, the obstacle avoidance priorities are set (as shown in Figure 3 ): The leader passes first, followed by the No. 2, No. 3, and No. 4 unmanned boats passing in sequence. This priority ensures that the unmanned boats in the formation pass through the narrow area in sequence, avoiding collisions and maintaining the stability of the formation. When the formation reaches the target point, the path planning ends.

[0120] The above embodiments are only exemplary embodiments of the present invention and are not used to limit the present invention. Those skilled in the art can make various modifications or equivalent replacements to the present invention within the essence and protection scope of the present invention, and such modifications or equivalent replacements should also be regarded as falling within the protection scope of the present invention.

Claims

1. A method for planning a path for a surface unmanned boat formation, characterized in that: The steps include: Step 1: rasterize the map, adjust the initial unmanned boat formation, determine the starting and ending information of the leader of the unmanned boat formation, and determine the location of dynamic obstacles and the information of static obstacles; Step 2, introducing the attenuation coefficient into the heuristic function of the A-Star algorithm, and using the improved A-Star algorithm for path planning; Step 3, path smoothing is performed by adaptively selecting a Bezier curve of control points; Step 4: Introduce the improved dynamic window method to perform dynamic local obstacle avoidance and generate the global path of the navigator; Step 5: Generate a planned path for the follower based on the initial unmanned boat formation and the position of the follower in the formation, combined with the global path of the leader obtained in step 4, and the follower maintains the same speed and deflection angle as the leader; In step 5, the dynamic window method described in step 4 is used to perform dynamic local obstacle avoidance for the follower. When the distance between obstacles is smaller than the size of the unmanned boat formation, obstacle avoidance priorities are set for different unmanned boats to ensure that the unmanned boats in the unmanned boat formation pass in order; The heuristic function of the A-Star algorithm in step 2 is specifically: ; ; in, is the total cost function; From the starting point to the node The cost function of Represents a slave node Heuristic evaluation function to the target node; Represents the attenuation coefficient constructed in combination with the Gaussian function, which is used to dynamically adjust the weight ratio of the heuristic function; and are the relevant parameters; The dynamic local obstacle avoidance in step 4 specifically includes: Step 4.1, establish the unmanned boat kinematic three-degree-of-freedom model: ; ; in, , represents the Euler angle vector; represents its angular velocity vector; represents the transformation matrix; Indicates the deflection angle of the unmanned boat relative to the inertial coordinate system; Step 4.2, performing velocity sampling based on the unmanned boat kinematic three-degree-of-freedom model obtained in step 4.1; Step 4.3, based on the kinematic three-degree-of-freedom model of the unmanned boat obtained in step 4.1 and the speed obtained in step 4.2, in each time step, update the position and heading angle of the unmanned boat at the next moment, select a time period T, continuously sample at each time interval a, and generate a predicted trajectory; Step 4.4: The predicted trajectory generated in step 4.3 is evaluated by the evaluation function. The expression is: ; ; ; in, Indicates the direction deviation of the current trajectory; Indicates the minimum distance to obstacles; represents the smoothness cost; Indicates the time cost to reach the target; Indicates the distance deviation between the current unmanned boat and the expected position of the formation; Indicates the speed error between the current unmanned boat and other members; represents the weight coefficient; is the next position predicted by the current speed combination of the navigator; It indicates the ideal formation position calculated based on the formation structure and the position of the pilot boat; and is the linear velocity and angular velocity of the current velocity combination, and is the speed reference value of the leading boat; The velocity space that can be sampled in step 4.2 : ; ; ; ; ; in, and They represent the minimum linear speed and maximum linear speed of the unmanned boat respectively; and They represent the minimum angular velocity and maximum angular velocity of the unmanned boat respectively; and Indicates the value of linear velocity and angular velocity at the current moment; and They represent the maximum linear acceleration and maximum angular acceleration of the unmanned boat respectively; Indicates the shortest distance between the corresponding simulated trajectory and surrounding obstacles at the current speed; Indicates the distance between the unmanned boat and the dynamic obstacle. Indicates the distance between the unmanned boat and surrounding static obstacles.

2. A method for planning a path for a surface unmanned boat formation according to claim 1, characterized in that: The relevant parameters in the heuristic function are and The values ​​are , , the attenuation coefficient is: 。 3. A method for planning a path for a surface unmanned boat formation according to claim 1, characterized in that: The specific process of path planning using the improved A-Star algorithm in step 2 is as follows: traverse the eight child nodes around the starting point, select the node with the lowest total cost function value as the next mother node, and when searching at the new mother node, if the neighboring node has been searched, then transfer the node until the vicinity of the target point is searched; when the child nodes around the new mother node contain the target point location information, directly select the target point as the next mother node.

4. A method for planning a path for a surface unmanned boat formation according to claim 3, characterized in that: The step 3 specifically includes: Step 3.1, select all the parent node information generated in step 2, recorded as , where the starting point is , the end point is , from the starting point To begin with, divide all parent nodes into three consecutive groups, each with three nodes; Step 3.2: Each group of three nodes obtained in step 3.1 is used to generate a Bezier curve, perform piecewise smoothing, and calculate the vector sum of the distance between the middle node and the obstacles on both sides. ; Step 3.3, calculate the control point based on the vector sum obtained in step 3.2 Coordinate vector: ; in, is the coordinate vector of the control point, is the scaling factor, is the coordinate vector of the midpoint.

Images