Unmanned surface vehicle formation path planning method

Abstract

The invention discloses an unmanned surface vehicle formation path planning method, and relates to the technical field of formation path planning, and the method comprises the steps: carrying out the rasterization processing of a map, adjusting the initial unmanned surface vehicle formation, determining the starting point and terminal point information of a navigator, and determining the position of a dynamic obstacle and the information of a static obstacle. An attenuation coefficient is introduced into a heuristic function of the A-Star algorithm, and path planning is carried out by adopting the improved A-Star algorithm; path smoothing processing is carried out through a Bezier curve of a self-adaptive selection control point; introducing an improved dynamic window method to carry out dynamic local obstacle avoidance; and according to the initial unmanned ship formation pattern and the positions of followers in the formation, combining the global path of the navigator to generate a planned path of the followers, and keeping the followers at the same speed and deflection angle as the navigator. The method ensures the optimality of the path planning of the navigator, also ensures that the follower can adaptively adjust in real time, and effectively improves the efficiency, stability and practicability of the formation path planning.

Description

Technical Field

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

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

[0003] At present, the research on path planning of unmanned boat formations is gradually increasing. Many algorithms have also been applied to the path planning problem of unmanned boat formations, such as A-Star algorithm, genetic algorithm, ant colony algorithm, deep learning algorithm, etc. These algorithms have their own advantages. In particular, the A-Star algorithm has a natural advantage in grid maps, can optimize targets and improve search efficiency, so it is widely used in the path planning research of unmanned boat formations. However, the A-Star algorithm has problems such as large consumption of computing resources and inability to effectively cope with dynamic environments, resulting in difficulties such as inability to avoid obstacles, occupying a large amount of memory, and the path is not smooth, and the corner is too large to meet the actual navigation requirements during the path planning process. At the same time, the A-Star algorithm does not consider the kinematic characteristics of the unmanned boat itself and its physical limitations such as angular velocity and linear velocity, so it is difficult to meet the actual navigation needs. For the navigator of the unmanned boat 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 its 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 a surface unmanned boat formation.

[0005] The technical solution adopted by the present invention to solve the technical problem is: a method for planning a path for a surface unmanned boat formation, comprising the following steps: 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 sequence.

[0006] In the above-mentioned method for planning the path of a surface unmanned boat formation, 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 is a relevant parameter.

[0007] The above-mentioned method for planning the path of a surface unmanned boat formation, the relevant parameters in the heuristic function and The values ​​are , , the attenuation coefficient is: .

[0008] In the above-mentioned method for path planning of a surface unmanned boat formation, 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, 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.

[0009] In the above-mentioned method for planning a path for a surface unmanned boat formation, 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.

[0010] In the above-mentioned method for planning the path of a surface unmanned boat formation, 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 or path length 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 It is the speed reference value of the lead boat or formation target.

[0011] In the above-mentioned method for planning the path of a surface unmanned boat formation, the speed space that can be sampled in step 4.2 is : ; ; ; ; ; 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.

[0012] The beneficial effect of the present invention is that by improving the A-Star algorithm and adding the attenuation coefficient constructed by the Gaussian function, the efficiency of the unmanned boat path planning can be improved, especially near the target point, which greatly reduces unnecessary searches. And the turning angle of the unmanned boat can be effectively reduced.

[0013] By introducing a Bezier curve smoothing method that can adaptively select control points, the safety of path smoothing can be effectively improved, making path planning more practical.

[0014] By introducing the improved DWM algorithm, the movement of the unmanned boat is more consistent with its own kinematic characteristics, and can better consider its own angular velocity, linear velocity and other limitations. It can also always maintain the position and speed with other unmanned boats, improve the safety of formation navigation, and maintain the formation of the unmanned boats.

[0015] The present invention generates the optimal track by performing global path planning for the leader. At the same time, other sub-ships dynamically adjust the course and speed according to the relative path of the leader through the DWM algorithm to avoid obstacles and maintain the stability of the formation. In this way, the optimality of the path planning of the leader is ensured, and all ships in the formation can be adaptively adjusted according to the real-time environment, thereby effectively improving the efficiency, stability and practicality of the formation path planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Figure 1 It is a schematic diagram of the process of the present invention; Figure 2 is a detailed method flow chart of an embodiment of the present invention; Figure 3 is a schematic diagram of a diamond formation of unmanned boats of the present invention; Figure 4 Schematic diagram of modeling a three-degree-of-freedom unmanned boat model in an embodiment of the present invention. DETAILED DESCRIPTION

[0017] In order to enable those skilled in the art to better understand the technical solution of the present invention, the present invention is described in detail below in conjunction with the accompanying drawings and specific implementation methods.

[0018] This embodiment discloses a surface unmanned boat formation path planning method based on the A-Star algorithm and the DWM algorithm, and its overall flow chart is as follows: Figure 1 As shown, the following steps are included: Step 1: Perform map rasterization processing, binarize and rasterize the real map, and convert it into a raster map to facilitate the subsequent processing of the algorithm. In particular, use smaller grids for areas with dense obstacles to improve accuracy.

[0019] Step 2: Adjust the initial formation of the unmanned boats, use the diamond formation as its basic formation shape, and determine the initial relative position, speed, steering angle, etc. of the leader and the follower, such as Figure 3 shown.

[0020] Step 3: Improve the A-Star algorithm, characterized in that the heuristic function of the A-Star algorithm is improved, and the heuristic function of the improved A-Star algorithm is: ; ; In the formula, is the total cost function, From the starting point to the node The cost function of Represents a slave node The 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 is its related parameter (adjustable). In order to facilitate calculation and reduce the amount of calculation, set , .

[0021] When the navigator is far away from the target point, ,at this time The weight of is very large, and the algorithm becomes the Dijkstra algorithm. When the distance to the target point is close, More weight, The weight of gradually decreases, and the A-Star algorithm becomes The advantage of designing the attenuation coefficient in this way is that in the initial stage Occupies a larger weight, allowing the algorithm to search faster near the target point, at the end of the planning process, Occupying a larger weight can avoid repeated node searches that may occur near the target point and reduce the total turning angle of the path.

[0022] 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 calculations. This allows the leader in the unmanned boat formation to generate a smooth path that is more in line with its own reality.

[0023] 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, as well as the position and velocity information of followers in other unmanned boat formations. After the global path is generated in step 4, the navigator moves along the designed global path. If a dynamic obstacle suddenly appears, this method is used to perform local dynamic obstacle avoidance to avoid dynamic obstacles, improve the safety of path planning, and effectively make a path planning path that conforms to real navigation.

[0024] Step 6: After the leader generates the global path, it generates the planned paths for other followers based on 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.

[0025] 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 keep the relative positions between the unmanned boats and the formation to prevent accidents during the unmanned boat formation.

[0026] Step 8: The formation maintains its formation while performing local obstacle avoidance. For areas with complex obstacles, especially narrow obstacle channels, the diamond formation is difficult to pass through the complex obstacle area in the form of a formation due to its large size. 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.

[0027] In this embodiment, detailed path planning steps are given, such as Figure 2 As shown, specifically: (1) Binarize and rasterize the map, divide it into feasible and infeasible domains, determine the starting and ending points of the leader in the unmanned boat formation in the map, and determine the location of dynamic obstacles and the information of static obstacles. Mark this information in the image.

[0028] (2) The A-Star algorithm is improved and an improved heuristic function of the A-Star algorithm is proposed. The attenuation coefficient constructed by the Gaussian function is added to it. The expression of the heuristic function of the improved A-Star algorithm is: ; ; In the formula, is the total cost function, From the starting point to the node The cost function of Represents a slave node The 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 is its relevant parameter (adjustable). In practical applications, the design , , so its attenuation coefficient becomes: .

[0029] The advantage of designing the attenuation coefficient in this way is that when the formation leader is far away from the target point, The value is large enough. Become smaller, middle The proportion of becomes larger, and the A-Star algorithm is To be the leader, because represents the cost of the path that has been explored, represents the heuristic function cost, so when When dominant, The value is much larger than Therefore, the search efficiency of the A-Star algorithm will be improved. In other words, When taking the lead, the heuristic estimation can quickly eliminate inappropriate paths, which reduces redundant exploration in the search space. With a stronger heuristic function, the algorithm can more accurately determine which paths are worth expanding, avoiding a large number of invalid expansion nodes. When the formation leader is close to the target point or has already reached the target point, by the same logic, Occupies a dominant position, the A-Star algorithm is now Dominant search algorithm, in which the algorithm pays more attention to the cost of the path that has been traveled when selecting nodes to expand, rather than relying on heuristic functions , so its turning angle will become smaller, especially when there is a sudden change of direction, because The dominant algorithm prefers paths with smaller costs, which are usually natural continuations, and the path changes are not too drastic. It can also avoid repeated node searches that may occur near the target point. join in The advantage is that the algorithm transformation can be completed as quickly as possible, thus improving the algorithm's advantages.

[0030] (3) The improved A-Star algorithm is used for path planning. The specific method is to create 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 is selected as the next mother node. When the new mother node performs node search, if the neighboring node has been searched, this node will not be selected as the new mother node, but will be skipped. This process continues until the target point is found. When the location information of the target point is included in the surrounding child nodes of the new mother node, the target point is directly selected as the mother node of the next moment, which is also the end point. The algorithm is considered to be terminated at this point.

[0031] (4) Perform path smoothing and propose an improved Bezier curve smoothing method that can adaptively select control points. The specific smoothing measure is: select all the parent node information generated in step (3), including the starting point and the end point, and name them The starting point is , the end point is , from the starting point First, divide the nodes into three consecutive groups, each with three nodes. For example: the first group {}, the second group { }. If the node has Finally, there will be one node left that cannot form three nodes, so take { } as a group. If the nodes have Each group of three nodes will be used to generate a Bezier curve. Select a group of nodes in the previous step. .

[0032] Define the path composed of this set of nodes as , select the middle node , check the path Obstacles on both sides, calculate the distance between the obstacles on both sides and the middle node Calculate the shortest distance from the obstacle on the path A to Distance , calculate the obstacle on the B side of the path to Distance , where the distance to the middle node is The coordinates of the nearest obstacles on both sides are defined as , define the node The vectors to the coordinates of the two obstacles are , calculates the sum of two vectors . Take the midpoint of the vector sum , then its control point The coordinate vector of .because The distribution of obstacles may cause the path to bend due to its own amplitude being too large, so a scaling factor is added. , so the coordinates of its control points The coordinate vector can be designed as: .

[0033] The advantage of this design is that this method calculates the vector form of the obstacles on both sides and the middle node, takes the middle vector point, ensures the adaptive selection of the control point, reduces the workload of manual calculation, and can well avoid the problem of unilateral deviation, making the path more balanced and natural in the process of obstacle avoidance, rather than forcibly deviating to one side, avoiding transition offset or unnatural, discontinuous turns, and avoiding drastic changes in path curvature. Moreover, combined with the scaling factor The introduction of can make the control points of the path flexibly adjusted in the actual environment and avoid excessive bending of the path.

[0034] (5) The improved dynamic window method is introduced for dynamic obstacle avoidance to prevent dynamic obstacles from suddenly appearing 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 motion of the unmanned boat, it is necessary to establish a reference coordinate system for the unmanned boat. Because the main research is on the path planning of the unmanned boat, the simulation experiment is mainly carried out in the grid map, so only the three-degree-of-freedom model needs to be established.

[0035] Therefore, the rolling, pitching and heaving motion models of the unmanned boat are not considered, and the modeling of the three-degree-of-freedom unmanned boat model is as follows: Figure 4 As shown, represents its inertial coordinate axis, Axis and The axes are parallel to the horizontal plane and point to the geographic east and north respectively. ,origin Usually defined as the center of mass of the unmanned boat, Axis and The axes are parallel to the sea level and point from the origin to the bow and starboard of the unmanned boat respectively. represents its vertical linear velocity, represents its lateral linear velocity, Indicates the angular velocity of its bow roll. Represents the deflection angle of the unmanned vehicle (USV) relative to the inertial coordinate system. In the path planning process, its kinematic characteristics are the key factors, and the main focus is on the motion state and geometric relationship of the unmanned vehicle. Therefore, in order to simplify the model and improve the computational efficiency, this study is only based on the kinematic three-degree-of-freedom model, without considering complex dynamic characteristics (such as hydrodynamic effects, inertia moments, etc.). In most planar path planning and obstacle avoidance scenarios, the kinematic model can accurately describe the motion characteristics of the unmanned vehicle in a two-dimensional plane, especially under low or medium speed operating conditions. Therefore, the three-degree-of-freedom kinematic model of its unmanned vehicle is: ; in , represents the Euler angle vector. represents its angular velocity vector, represents the transformation matrix, where The definition under the three-degree-of-freedom model is .

[0036] Therefore, the motion equation of its three-degree-of-freedom kinematic model is: ; ; ; The state of the system can be expressed as .

[0037] The dynamic window method (DWM) is a method based on predictive control theory. It can safely and effectively avoid obstacles in unknown environments. It also has the advantages of low computational complexity and rapid response. The principle is: first collect the robot's velocity samples through the mathematical model of the USV, and predict and simulate the motion trajectory in the next time period at the sample speed, and finally select the optimal path by evaluating these motion trajectories. 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: velocity sampling, trajectory prediction, and trajectory evaluation.

[0038] According to the characteristics of USV and environmental restrictions, the speed of USV has certain boundary restrictions, and the speed space that can be sampled is It can be expressed as: ; in, and Respectively represent the minimum linear speed and maximum linear speed of USV. and represent the minimum angular velocity and the maximum angular velocity respectively.

[0039] Since the USV is driven by a motor, its linear acceleration and angular acceleration are subject to certain limitations. Therefore, the speed space that can be sampled when considering acceleration is for: ; in, 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 USV respectively.

[0040] This embodiment uses the DWM algorithm for dynamic obstacle avoidance, so it is necessary to consider the obstacle factors around the USV during the dynamic obstacle avoidance process. The constraint conditions for the USV not to collide with obstacles during the dynamic obstacle avoidance process are: ; in, Indicates the shortest distance between the simulated trajectory and the surrounding obstacles at the current speed. Its definition is: ; in Indicates the distance between the USV and the dynamic obstacle, Indicates the distance to the surrounding static obstacles. Before the trigger condition of dynamic obstacles is reached, Only the speed constraints for static obstacles are included. When the trigger condition for dynamic obstacle avoidance is met, quilt and Control at the same time.

[0041] In summary, the final USV speed sampling space is the intersection of three speed spaces, namely: ; At each time step The above kinematic equations are used to update and determine the position and heading angle of the USV at the next moment.

[0042] 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: ; ; .

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

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

[0045] 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 collides with an obstacle at a certain moment, then the speed combination is considered unfeasible and needs to be eliminated. 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 to avoid excessive speed differences or collision risks.

[0046] Specifically, the evaluation function will give a comprehensive score to each trajectory based on the following aspects: azimuth deviation, obstacle distance, current speed of USV, predicted time of trajectory, and formation stability.

[0047] Finally, for each feasible trajectory, the following weighted evaluation function is used to score: ; in, Indicates the direction deviation of the current trajectory. The greater the deviation from the target angle, the greater the value. Indicates the minimum distance to obstacles. represents the smoothness cost, reflecting the magnitude of speed change, Represents the time cost to reach the target, or the path length cost, Indicates 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 expression is: ; in, is the next position predicted by the current speed combination of the navigator, Indicates the ideal formation position calculated based on the formation structure and the position of the lead boat.

[0048] ; in and is the linear velocity and angular velocity of the current velocity combination, and It is the speed reference value of the leading boat or formation target. It can be the current speed of the leading boat or the average speed of the formation. The smaller the value is, the closer the current speed is to the reference speed, and the better the synchronization of the formation.

[0049] (6) After the leader generates a global path using the improved A-Star algorithm, the global path of the follower is generated based on the formation positions of the leader and the follower. The follower's path is generated based on the leader's path, taking the formation position into consideration. After the follower's global path is generated, the dynamic window method (DWM) is used for local obstacle avoidance to avoid dynamic obstacles in real time. When encountering a narrow channel, since the distance between obstacles is smaller than the formation size, the obstacle avoidance priority is set (such as Figure 3 As shown in the figure): The leader has priority, followed by No. 2, No. 3, and No. 4 unmanned boats in sequence. This priority ensures that the unmanned boats in the formation pass through narrow areas in sequence, avoiding collisions and keeping the formation stable. When the formation reaches the target point, the path planning ends.

[0050] The above embodiments are only exemplary embodiments of the present invention and are not intended to limit the present invention. Those skilled in the art may make various modifications or equivalent substitutions to the present invention within the essence and protection scope of the present invention, and such modifications or equivalent substitutions shall also be deemed to fall 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 sequence.

2. A method for planning a path for a surface unmanned boat formation according to claim 1, characterized in that: 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 is a relevant parameter.

3. A method for planning a path for a surface unmanned boat formation according to claim 2, characterized in that: The relevant parameters in the heuristic function are and The values ​​are , , the attenuation coefficient is: 。 4. A method for planning a path for a surface unmanned boat formation according to claim 2, 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.

5. A method for planning a path for a surface unmanned boat formation according to claim 4, 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.

6. A method for planning a path for a surface unmanned boat formation according to claim 1, characterized in that: 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 or path length 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 It is the speed reference value of the lead boat or formation target.

7. A method for planning a path for a surface unmanned boat formation according to claim 6, characterized in that: 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.

Images