Multi-unmanned ship area coverage path planning method considering dynamic collision avoidance

Abstract

The invention discloses a multi-unmanned ship area coverage path planning method considering dynamic collision avoidance, belongs to the field of unmanned surface ship path planning, and aims to solve the problems that a coverage dead zone exists, backtracking cannot be adapted and dynamic collision avoidance is not considered when a traditional DARP area coverage algorithm is adopted to plan a multi-unmanned ship path. The method comprises the following steps: step 1, loading electronic chart data, and extracting static obstacle information in the electronic chart data; rasterization modeling is carried out; 2, according to the initial positions of the m unmanned ships, a task area is allocated to each unmanned ship based on a DARP algorithm; 3, for each task area, selecting a heuristic fusion algorithm with the minimum moving step number and action cost to obtain a static path, and completing full coverage of areas outside static obstacles; and step 4, introducing a DWA algorithm, performing real-time dynamic collision avoidance path planning on the basis of the static path, and when the angle of the advancing direction and the connecting line of the target point is too large, performing re-planning by using an APF algorithm so as to prevent the unmanned ship from falling into local optimum.

Description

Technical Field

[0001] The invention relates to a method for planning a path for regional coverage of multiple unmanned boats taking dynamic collision avoidance into consideration, and belongs to the field of path planning for surface unmanned boats. Background Art

[0002] In recent years, with the development of ocean development and intelligent technology, unmanned boats have been widely used in marine ranching, water quality testing, communication relay, search and rescue, and oil cleaning. In order to meet the needs of efficient execution of tasks in complex marine environments, intelligent perception and regional coverage technologies have become key research directions. Compared with land mobile robots, unmanned boats have wider mission areas and more complex environments, which puts higher requirements on the efficiency and robustness of regional coverage path planning algorithms. In addition, due to limited endurance and low efficiency, a single unmanned boat cannot complete a large range of sea area coverage tasks, so the collaborative coverage technology of multiple unmanned boats has become a research hotspot.

[0003] For the problem of multiple unmanned boats covering an area, reasonable task allocation and path planning strategies can significantly improve coverage efficiency and enhance the robustness of the system. The traditional DARP (Dynamic Adaptive Resource Partitioning) area coverage algorithm can only be used for path planning that covers all target points at once, and cannot generate a solution when it needs to backtrack. In addition, this method does not take into account the existence of dynamic obstacles, so the actual application scenarios are very limited. Summary of the invention

[0004] Aiming at the problems that the traditional DARP area coverage algorithm is used to plan the paths of multiple unmanned boats, such as dead zones in coverage, inability to adapt to backtracking, and failure to consider dynamic collision avoidance, the present invention provides a method for planning the area coverage paths of multiple unmanned boats considering dynamic collision avoidance.

[0005] The present invention provides a method for planning a path for a multi-unmanned boat area coverage considering dynamic collision avoidance, the method comprising the following steps:

[0006] Step 1: Load the electronic chart data and extract the static obstacle information in the electronic chart data; perform raster modeling according to the static obstacle area information in the environment;

[0007] Step 2: According to the initial positions of the m unmanned boats, a task area is assigned to each unmanned boat based on the DARP algorithm;

[0008] Step 3: For each task area, select the heuristic fusion algorithm with the minimum number of moving steps and action cost to obtain the static path, and complete the full coverage of the area outside the static obstacles;

[0009] Step 4: Introduce the DWA (Dynamic Window Approaches) algorithm to perform real-time dynamic collision avoidance path planning based on the static path. When the angle between the travel direction and the line connecting the target point is too large, use the APF (Artificial Potential Field) algorithm to re-plan to avoid the unmanned boat falling into the local optimum.

[0010] Preferably, the process of step 1 includes:

[0011] Step 11: Analyze the electronic chart information according to the theoretical data model and data structure of the S-57 chart. The plane where the surface unmanned boat operation area is located is the OXY plane of the Cartesian rectangular coordinate system. The Mercator projection transformation is used to project the navigation obstruction area where the static obstacle is located onto the OXY plane of the Cartesian rectangular coordinate system to obtain the position information of the static obstacle in the Cartesian rectangular coordinate system.

[0012] Step 12: According to the static obstacle area information in the environment and the scanning range of the unmanned boat, select the appropriate grid granularity for raster modeling.

[0013] Preferably, the process of step 2 is:

[0014] Step 21: According to the initial position of the unmanned boat, based on the DARP algorithm, by continuously correcting the evaluation matrix of each unmanned boat, a connected area is finally formed, and the map is evenly divided among the m unmanned boats;

[0015] Step 22: By rewarding the area around the unmanned boat and penalizing other non-connected areas, a connectivity matrix is ​​constructed so that all cells assigned to each unmanned boat gradually form a closed area as the mission area of ​​the unmanned boat.

[0016] Preferably, the process of step 3 is:

[0017] Step 31: In any task area, determine the starting point and calculate the cost matrix and step matrix;

[0018] Step 32, determine whether the next target point can be found at present, if yes, execute step 33; otherwise, execute step 34;

[0019] Step 33: When the next target point can be found, a heuristic coverage search is performed, and the next target point is added to the static path until all target points are covered to form a trajectory; then step 35 is executed;

[0020] Step 34: When the next target point cannot be found, perform A* backtracking search to reconstruct the initial static path. The backtracking path constructed by the backtracking search includes two types: horizontal backtracking path and vertical backtracking path; then return to step 32;

[0021] Step 35: Compare the trajectories formed by the four heuristic algorithms and select the trajectory with the least action cost and number of steps as the final static path.

[0022] Preferably, the four heuristic algorithms include a Manhattan distance heuristic algorithm, a Chebyshev distance heuristic algorithm, a horizontal distance heuristic algorithm and a vertical distance heuristic algorithm.

[0023] Preferably, the process of step 4 is:

[0024] Step 41, process the repeated path points of the backtracking, move the target point of the horizontal backtracking to the right by 0.5 grid, move the target point of the vertical backtracking to the bottom by 0.5 grid, and circularize the right-angle track in the path;

[0025] Step 42: Use the DWA algorithm to sample and search for the optimal path to determine whether the next target point can be found. If so, execute step 43; otherwise, execute step 44.

[0026] Step 43, move along the path to the next target point, return to step 42 and continue searching using the DWA algorithm; until all target points are searched and the unmanned boat reaches the end point, dynamic obstacle avoidance path planning is completed;

[0027] Step 44, switch to the APF algorithm for re-planning, by updating the position and direction, the unmanned boat avoids obstacles and gradually approaches the target point, and then returns to execute step 42.

[0028] Preferably, the process of using the DWA algorithm to sample and search for the optimal path in step 42 is as follows:

[0029] Step 42-1, determine the velocity space [a, b, c, d], where a is the minimum linear velocity, b is the maximum linear velocity, c is the minimum angular velocity, and d is the maximum angular velocity; the determination process is:

[0030] First, the lower limit threshold v of the linear velocity of the unmanned boat is obtained from the dynamic parameters of the unmanned boat. low , Line speed upper limit threshold v high 、Angular velocity lower limit threshold w low 、 Angular velocity upper threshold w high , Linear acceleration upper threshold a vmax and the angular acceleration upper threshold a wmax , denoted as the initial speed limit V m :

[0031]

[0032] Secondly, the velocity space is calculated again based on the current linear velocity v and angular velocity w of the unmanned boat, which is recorded as the velocity space limit V d :

[0033]

[0034] Among them, the linear velocity and angular velocity are changed to: v low =va vmax ×dt,v high =v+a vmax ×dt

[0035] w low =wa wmax ×dt,w high =w+a wmax ×dt;

[0036] Next, calculate the safe speed range of the unmanned boat in the current state, and record the speed limit of the dynamic obstacle as V a :

[0037]

[0038] Among them, the linear velocity and angular velocity are changed to: v low =va vmax ×dt,

[0039] Where, d o is the distance between the current unmanned boat and the dynamic obstacle;

[0040] Combined with the above, the speed space [a,b,c,d] is finally determined:

[0041] Minimum linear speed a: V m 、V d and V a The maximum value of the minimum linear velocity;

[0042] Maximum linear velocity b, take V m 、V d and V a The minimum value of the maximum linear velocity;

[0043] Minimum angular velocity c, take V m 、V d and V a The maximum value of the minimum angular velocity;

[0044] The maximum angular velocity d is V m 、V d and V a The minimum value of the maximum angular velocity in ;

[0045] Step 42-2, using the unmanned boat kinematics model to simulate the motion trajectory of the unmanned boat under a given control input command within a limited range of the speed space;

[0046] Update the horizontal and vertical coordinates x, y and heading angle yaw of the unmanned boat according to the current linear velocity v and heading angle yaw of the unmanned boat, keep the speed unchanged, and return the updated state and predicted trajectory:

[0047] x=x+v·cos(yaw)·dt,y=y+v·sin(yaw)·dt

[0048] yaw=yaw+w·dt

[0049] Sample the linear velocity v and angular velocity w in the velocity space window, and for each pair of v and w, generate a predicted trajectory;

[0050] Step 42-3, performing trajectory evaluation for each predicted trajectory, wherein the trajectory evaluation includes distance evaluation dist, direction evaluation head and speed evaluation vel;

[0051] Step 42-4, normalization processing, the distance evaluation normalization dist_n, direction evaluation head_n and speed evaluation vel_n are:

[0052]

[0053] Among them, α, β, and γ are distance evaluation weights, direction evaluation weights, and speed evaluation weights, respectively;

[0054] Step 42-5, calculate the comprehensive evaluation score of each trajectory: G = dist_n + head_n + vel_n

[0055] The trajectory with the highest evaluation score and the corresponding speed combination are selected as the optimal trajectory and optimal control amount for the DWA algorithm to sample and search for the optimal path.

[0056] Preferably, when the angle between the traveling direction and the line connecting the target points in step 4 is too large, the DWA algorithm cannot search for the next target point.

[0057] Preferably, the process of re-planning the APF algorithm in step 44 is:

[0058] Step 44-1, gravity vector calculation:

[0059]

[0060] Among them, η att is the gravitational gain coefficient, P i is the current i-th node coordinate, P g is the target node coordinate, and the next node is taken as the target node;

[0061] Step 44-2, calculation of repulsive force vector:

[0062]

[0063] in, is the repulsion gain coefficient, P obs is the node coordinate of the obsth obstacle;

[0064] Step 44-3: Calculate the resultant force vector F according to steps 44-1 and 44-2 sum and direction of the combined force

[0065]

[0066] Step 44-4: Obtain the expected direction θ of the predicted trajectory e :

[0067] in, is the vertical axis, is the horizontal axis;

[0068] Step 44-5, obtain the coordinates P of the next target point of the unmanned boat i+1 And the travel angle θ:

[0069] θ=θ e ,P i+1 =P i +l·(cos(θ),sin(θ))

[0070] Where l is the step size.

[0071] Beneficial effects of the present invention:

[0072] (1) Based on the traditional area coverage algorithm, the present invention uses the heuristic fusion A* algorithm to achieve full coverage with backtracking and select the optimal path with the shortest number of turns and length;

[0073] (2) The present invention introduces the DWA algorithm, which can perform real-time dynamic collision avoidance for various dynamic obstacles that may appear on the sea surface, which is more in line with the needs of actual application scenarios.

[0074] (3) When the DWA algorithm is trapped in a local optimal solution, the present invention calls the APF algorithm to enable the unmanned vehicle to escape from the position where it is trapped in the local optimal solution, and then continues to use the DWA algorithm to plan the path, thereby reducing the limitations of the DWA algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0075] Figure 1 It is a structural schematic diagram of a method for planning a path for a multi-unmanned boat area coverage considering dynamic collision avoidance according to the present invention;

[0076] Figure 2 It is a diagram of the electronic map data structure;

[0077] Figure 3 It is a schematic diagram of the visualization result of the electronic nautical chart;

[0078] Figure 4 It is a schematic diagram of the electronic chart grid modeling results;

[0079] Figure 5 is the area division diagram of the unmanned boat of the present invention, wherein Figure 5 (a) is a schematic diagram of the starting positions of the four unmanned boats. Figure 5 (b) is the result of the division of the four unmanned boat areas;

[0080] Figure 6 It is a bar chart of the number of steps and costs of four heuristic algorithms in different starting directions of the unmanned boat No. 1 of the present invention;

[0081] Figure 7 It is a full coverage diagram of the four heuristic algorithms of the present invention;

[0082] Figure 8 It is a full coverage path map with the least number of moving steps and the least cost of the present invention;

[0083] Fig. 9 It is a real-time dynamic collision avoidance full coverage path map;

[0084] Fig.10 It is the local collision avoidance path diagram of unmanned boat No. 2;

[0085] Fig.11 This is the local collision avoidance path map of unmanned boat No. 4. DETAILED DESCRIPTION

[0086] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0087] It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments may be combined with each other.

[0088] The present invention will be further described below in conjunction with the accompanying drawings and specific embodiments, but they are not intended to limit the present invention.

[0089] Specific implementation method 1: The following is combined Figures 1 to 11 This embodiment describes a method for planning a path for multiple unmanned boats covering an area considering dynamic collision avoidance. Figure 1 , the method comprises the following steps:

[0090] Step 1: Load the electronic chart data and extract the static obstacle information in the electronic chart data; perform raster modeling according to the static obstacle area information in the environment;

[0091] Step 2: According to the initial positions of the m unmanned boats, a task area is assigned to each unmanned boat based on the DARP algorithm;

[0092] Step 3: For each task area, select the heuristic fusion algorithm with the minimum number of moving steps and action cost to obtain the static path, and complete the full coverage of the area outside the static obstacles;

[0093] Step 4: Introduce the DWA algorithm to perform real-time dynamic collision avoidance path planning based on the static path. When the angle between the direction of travel and the line connecting the target point is too large, use the APF algorithm to re-plan to avoid the unmanned boat from falling into the local optimum.

[0094] The process for Step 1 includes:

[0095] Step 11: Analyze the electronic chart information according to the theoretical data model and data structure of the S-57 chart. The plane where the surface unmanned boat operation area is located is the OXY plane of the Cartesian rectangular coordinate system. The Mercator projection transformation is used to project the navigation obstruction area where the static obstacle is located onto the OXY plane of the Cartesian rectangular coordinate system to obtain the position information of the static obstacle in the Cartesian rectangular coordinate system.

[0096] Step 12: According to the static obstacle area information in the environment and the scanning range of the unmanned boat, select the appropriate grid granularity for raster modeling.

[0097] Import as Figure 2 The electronic map data shown is used to obtain the longitude and latitude of the electronic chart and obstacle information. In this embodiment, the longitude range of the elevation map is 104.2°E to 104.7°E, and the latitude range is 0.75°N to 1.25°N; the location information of the electronic chart data is recorded, and the electronic chart visualization result in two-dimensional space is as follows Figure 3 As shown in the figure, the present invention assumes that the radius of the unmanned boat scanning radar is about 5km, selects 0.05 latitude and longitude as the resolution, and constructs a 10*10 rectangular grid area. The grid modeling result is as follows: Figure 4 shown.

[0098] The process of step 2 is:

[0099] Step 21: According to the initial position of the unmanned boat, based on the DARP algorithm, by continuously correcting the evaluation matrix of each unmanned boat, a connected area is finally formed, and the map is evenly divided among the m unmanned boats;

[0100] Step 22: By rewarding the area around the unmanned boat and penalizing other non-connected areas, a connectivity matrix is ​​constructed so that all cells assigned to each unmanned boat gradually form a closed area as the mission area of ​​the unmanned boat.

[0101] The present invention selects four unmanned boats to cover the area, such as Figure 5 As shown in (a), the black grid is the obstacle, the white grid is the feasible area, and the blue, green, orange, and purple are the starting positions of the four unmanned boats. After the DARP algorithm area division, as shown in Figure (b), the corresponding color is the mission area of ​​each unmanned boat.

[0102] Step 3 uses the heuristic fusion A* algorithm to cover the entire area in two-dimensional space. The specific process is as follows:

[0103] Step 31: In any task area, determine the starting point and calculate the cost matrix and step matrix;

[0104] Step 32, determine whether the next target point can be found at present, if yes, execute step 33; otherwise, execute step 34;

[0105] Step 33: When the next target point can be found, a heuristic coverage search is performed, and the next target point is added to the static path until all target points are covered to form a trajectory; then step 35 is executed;

[0106] Step 34: When the next target point cannot be found, perform A* backtracking search to reconstruct the initial static path. The backtracking path constructed by the backtracking search includes two types: horizontal backtracking path and vertical backtracking path; then return to step 32;

[0107] Step 35, compare the trajectories formed by the four heuristic algorithms, and select the trajectory with the least action cost and number of steps as the final static path. The four heuristic algorithms include Manhattan distance heuristic algorithm, Chebyshev distance heuristic algorithm, horizontal distance heuristic algorithm and vertical distance heuristic algorithm.

[0108] Heuristic search process: First, create a matrix with the same size as the map and initialize all elements to 0. Then traverse each node of the map, calculate the four heuristic distance costs from the node to the target node, and store the results in the heuristic matrix. Set the number of steps of the starting node to 0, add it to the frontier queue, and use the breadth-first search algorithm: take the current node from the frontier queue and traverse its upper, lower, left and right adjacent nodes. If the number of steps of the adjacent node is greater than the number of steps of the current node plus 1, update the number of steps and add it to the queue. The path planning cost consists of three parts: the cumulative path cost (the total cost from the starting point to the current node), the action cost (0.1 for forward movement, 0.2 for left / right turn, and 0.4 for backward movement), and the heuristic cost (estimate the shortest path from the current node to the target node) to select the node with the lowest cost and form the final path.

[0109] The process of A* backtracking search: Create two lists: the open list stores the nodes to be checked (initially only contains the starting point), and the closed list stores the checked nodes (initially empty). Select the node with the smallest total cost from the open list as the current node, and move it from the open list to the closed list. Traverse the neighbor nodes of the current node (up, down, left, right), and update the cost value of the nodes that are not in the closed list:

[0110] g=g+1,f=g+h

[0111] Where g is the distance cost, h is the heuristic cost, and f is the total cost. If the neighboring node is not in the open list, add it to the list and set the current node as the parent node of the neighboring node to facilitate path backtracking. If the neighboring node is already in the open list and the new g value is smaller, update its g value, f value and parent node. Repeat this process until the target node is reached or the open list is empty. Once the target node is found, the path can be reconstructed by backtracking through the parent node to generate a complete path from the target node to the starting point.

[0112] Figure 6 is the number of steps and cost of the four heuristic algorithms for different starting directions of unmanned boat 1. The coverage paths of the four unmanned boats are as follows: Figure 7 As shown in the figure, the final static paths of the four unmanned boats are as follows: Figure 8 shown.

[0113] Step 4: Initialize the state of the unmanned boat, the target point, and the obstacle position, and use the DWA algorithm to sample and search for the optimal path. If the trajectory can reach the target point, it moves along the path; if it falls into a local optimum, it switches to the APF algorithm for replanning. By updating the position and direction, the unmanned boat avoids obstacles and gradually approaches the target point, and repeats this process until all target points are covered. The specific process is:

[0114] Step 41, the repeated path points of the backtracking are processed, the target point of the horizontal backtracking is moved to the right by 0.5 grids, and the target point of the vertical backtracking is moved down by 0.5 grids, so that the unmanned boat can complete the backtracking in accordance with the motion model; and the right-angle trajectory in the path is circularized;

[0115] Step 42: Use the DWA algorithm to sample and search for the optimal path to determine whether the next target point can be found. If so, execute step 43; otherwise, execute step 44.

[0116] Step 43, move along the path to the next target point, return to step 42 and continue searching using the DWA algorithm; until all target points are searched and the unmanned boat reaches the end point, dynamic obstacle avoidance path planning is completed;

[0117] Step 44, switch to the APF algorithm for re-planning, by updating the position and direction, the unmanned boat avoids obstacles and gradually approaches the target point, and then returns to execute step 42.

[0118] The process of using the DWA algorithm to sample and search for the optimal path in step 42 is as follows:

[0119] Step 42-1, determine the velocity space [a, b, c, d], where a is the minimum linear velocity, b is the maximum linear velocity, c is the minimum angular velocity, and d is the maximum angular velocity; the determination process is:

[0120] First, the lower limit threshold v of the linear velocity of the unmanned boat is obtained from the dynamic parameters of the unmanned boat. low , Line speed upper limit threshold v high 、Angular velocity lower limit threshold w low 、 Angular velocity upper threshold w high , Linear acceleration upper threshold a vmax and the angular acceleration upper threshold a wmax , denoted as the initial speed limit V m :

[0121]

[0122] Secondly, the velocity space is calculated again based on the current linear velocity v and angular velocity w of the unmanned boat, which is recorded as the velocity space limit V d :

[0123]

[0124] Among them, the linear velocity and angular velocity are changed to: v low =va vmax ×dt,v high =v+a vmax ×dt

[0125] w low =wa wmax×dt,w high =w+a wmax ×dt;

[0126] Next, calculate the safe speed range of the unmanned boat in the current state, and record the speed limit of the dynamic obstacle as V a :

[0127]

[0128] Among them, the linear velocity and angular velocity are changed to: v low =va vmax ×dt,

[0129] w low =wa wmax ×dt,

[0130] Where, d o is the distance between the current unmanned boat and the dynamic obstacle;

[0131] Combined with the above, the speed space [a,b,c,d] is finally determined:

[0132] Minimum linear speed a: V m 、V d and V a The maximum value of the minimum linear velocity;

[0133] Maximum linear velocity b, take V m 、V d and V a The minimum value of the maximum linear velocity;

[0134] Minimum angular velocity c, take V m 、V d and V a The maximum value of the minimum angular velocity;

[0135] The maximum angular velocity d is V m 、V d and V a The minimum value of the maximum angular velocity in ;

[0136] Step 42-2, using the unmanned boat kinematics model to simulate the motion trajectory of the unmanned boat under a given control input command within a limited range of the speed space;

[0137] Update the horizontal and vertical coordinates x, y and heading angle yaw of the unmanned boat according to the current linear velocity v and heading angle yaw of the unmanned boat, keep the speed unchanged, and return the updated state and predicted trajectory:

[0138] x=x+v·cos(yaw)·dt,y=y+v·sin(yaw)·dt

[0139] yaw=yaw+w·dt

[0140] Sample the linear velocity v and angular velocity w in the velocity space window, and for each pair of v and w, generate a predicted trajectory;

[0141] Step 42-3, performing trajectory evaluation for each predicted trajectory, wherein the trajectory evaluation includes distance evaluation dist, direction evaluation head and speed evaluation vel;

[0142] Step 42-4, normalization processing, the distance evaluation normalization dist_n, direction evaluation head_n and speed evaluation vel_n are:

[0143]

[0144] Among them, α, β, and γ are distance evaluation weights, direction evaluation weights, and speed evaluation weights, respectively;

[0145] Step 42-5, calculate the comprehensive evaluation score of each trajectory: G = dist_n + head_n + vel_n

[0146] The trajectory with the highest evaluation score and the corresponding speed combination are selected as the optimal trajectory and optimal control amount for the DWA algorithm to sample and search for the optimal path.

[0147] When the angle between the traveling direction and the line connecting the target point in step 4 is too large, the DWA algorithm cannot search for the next target point.

[0148] The process of re-planning the APF algorithm in step 44 is as follows:

[0149] Step 44-1, gravity vector calculation:

[0150]

[0151] Among them, η att is the gravitational gain coefficient, P i is the current i-th node coordinate, P g is the target node coordinate, and the next node is taken as the target node;

[0152] Step 44-2, calculation of repulsive force vector:

[0153]

[0154] in, is the repulsion gain coefficient, P obs is the node coordinate of the obsth obstacle;

[0155] Step 44-3: Calculate the resultant force vector F according to steps 44-1 and 44-2 sum and direction of the combined force

[0156]

[0157] Step 44-4: Obtain the expected direction θ of the predicted trajectory e :

[0158] in, is the vertical axis, is the horizontal axis;

[0159] Step 44-5, obtain the coordinates P of the next target point of the unmanned boat i+1 And the travel angle θ:

[0160] θ=θ e ,P i+1 =P i +l·(cos(θ),sin(θ))

[0161] Where l is the step size.

[0162] The present invention sets six horizontally moving dynamic obstacles, which move back and forth left and right within the range of horizontal coordinates 0 to 9.

[0163] Finally, the four unmanned boats were simulated by DWA fusion algorithm considering the dynamic model. Fig. 9 As shown in the figure, the small red circle represents a dynamic obstacle. When encountering a dynamic obstacle moving perpendicular to the direction of travel, the unmanned boat will slow down and wait for the dynamic obstacle to pass until the distance between the obstacle and the unmanned boat exceeds the obstacle radius. For dynamic obstacles on the same straight line as the direction of travel of the unmanned boat, the unmanned boat will bypass the obstacle. In order to better demonstrate this dynamic collision avoidance effect, this paper selects two local areas (trajectory 2 and trajectory 4) for enlargement processing, and the results are shown as follows: Fig.10 and Fig.11 shown.

[0164] Although the present invention is described herein with reference to specific embodiments, it should be understood that these embodiments are merely examples of the principles and applications of the present invention. It should therefore be understood that many modifications may be made to the exemplary embodiments and that other arrangements may be devised without departing from the spirit and scope of the present invention as defined by the appended claims. It should be understood that the various dependent claims and features described herein may be combined in a manner different from that described in the original claims. It should also be understood that the features described in conjunction with a single embodiment may be used in other described embodiments.

Claims

1. A method for planning a path for multiple unmanned boats covering an area considering dynamic collision avoidance, characterized in that: The method comprises the following steps: Step 1: Load the electronic chart data and extract the static obstacle information in the electronic chart data; perform raster modeling according to the static obstacle area information in the environment; Step 2: According to the initial positions of the m unmanned boats, a task area is assigned to each unmanned boat based on the DARP algorithm; Step 3: For each task area, select the heuristic fusion algorithm with the minimum number of moving steps and action cost to obtain the static path, and complete the full coverage of the area outside the static obstacles; Step 4: Introduce the DWA algorithm to perform real-time dynamic collision avoidance path planning based on the static path. When the angle between the direction of travel and the line connecting the target point is too large, use the APF algorithm to re-plan to avoid the unmanned boat from falling into the local optimum.

2. According to claim 1, a method for planning a path for multiple unmanned boats covering an area considering dynamic collision avoidance is characterized in that: The process for Step 1 includes: Step 11: Analyze the electronic chart information according to the theoretical data model and data structure of the S-57 chart. The plane where the surface unmanned boat operation area is located is the OXY plane of the Cartesian rectangular coordinate system. The Mercator projection transformation is used to project the navigation obstruction area where the static obstacle is located onto the OXY plane of the Cartesian rectangular coordinate system to obtain the position information of the static obstacle in the Cartesian rectangular coordinate system. Step 12: According to the static obstacle area information in the environment and the scanning range of the unmanned boat, select the appropriate grid granularity for raster modeling.

3. According to claim 2, a method for planning a path for multiple unmanned boats covering an area considering dynamic collision avoidance is characterized in that: The process of step 2 is: Step 21: According to the initial position of the unmanned boat, based on the DARP algorithm, by continuously correcting the evaluation matrix of each unmanned boat, a connected area is finally formed, and the map is evenly divided among the m unmanned boats; Step 22: By rewarding the area around the unmanned boat and penalizing other non-connected areas, a connectivity matrix is ​​constructed so that all cells assigned to each unmanned boat gradually form a closed area as the mission area of ​​the unmanned boat.

4. According to claim 3, a method for planning a path for multiple unmanned boats covering an area considering dynamic collision avoidance is characterized in that: The process of step 3 is: Step 31: In any task area, determine the starting point and calculate the cost matrix and step matrix; Step 32, determine whether the next target point can be found at present, if so, execute step 33; Otherwise, execute step 34; Step 33: When the next target point can be found, a heuristic coverage search is performed, and the next target point is added to the static path until all target points are covered to form a trajectory; then step 35 is executed; Step 34: When the next target point cannot be found, perform A* backtracking search to reconstruct the initial static path. The backtracking path constructed by the backtracking search includes two types: horizontal backtracking path and vertical backtracking path; then return to step 32; Step 35: Compare the trajectories formed by the four heuristic algorithms and select the trajectory with the least action cost and number of steps as the final static path.

5. According to claim 4, a method for planning a path for multiple unmanned boats covering an area considering dynamic collision avoidance is characterized in that: The four heuristic algorithms include Manhattan distance heuristic algorithm, Chebyshev distance heuristic algorithm, horizontal distance heuristic algorithm and vertical distance heuristic algorithm.

6. According to claim 4, a method for planning a path for multiple unmanned boats covering an area considering dynamic collision avoidance is characterized in that: The process of step 4 is: Step 41, process the repeated path points of the backtracking, move the target point of the horizontal backtracking to the right by 0.5 grid, move the target point of the vertical backtracking to the bottom by 0.5 grid, and circularize the right-angle track in the path; Step 42: Use the DWA algorithm to sample and search for the optimal path to determine whether the next target point can be found. If so, execute step 43; otherwise, execute step 44. Step 43, move along the path to the next target point, return to step 42 and continue searching using the DWA algorithm; until all target points are searched and the unmanned boat reaches the end point, dynamic obstacle avoidance path planning is completed; Step 44, switch to the APF algorithm for re-planning, by updating the position and direction, the unmanned boat avoids obstacles and gradually approaches the target point, and then returns to execute step 42.

7. A method for planning a path for multiple unmanned boats covering an area considering dynamic collision avoidance according to claim 6, characterized in that: The process of using the DWA algorithm to sample and search for the optimal path in step 42 is as follows: Step 42-1, determine the velocity space [a, b, c, d], where a is the minimum linear velocity, b is the maximum linear velocity, c is the minimum angular velocity, and d is the maximum angular velocity; the determination process is: First, the lower limit threshold v of the linear velocity of the unmanned boat is obtained from the dynamic parameters of the unmanned boat. low , Line speed upper limit threshold v high 、Angular velocity lower limit threshold w low 、 Angular velocity upper threshold w high , Linear acceleration upper limit threshold a vmax and the angular acceleration upper threshold a wmax , denoted as the initial speed limit V m : Secondly, the velocity space is calculated again based on the current linear velocity v and angular velocity w of the unmanned boat, which is recorded as the velocity space limit V d : Among them, the linear velocity and angular velocity are changed to: v low =va vmax ×dt,v high =v+a vmax ×dt w low =time wmax ×dt,w high =w+a wmax ×dt; Next, calculate the safe speed range of the unmanned boat in the current state, and record the speed limit of the dynamic obstacle as V a : Among them, the linear velocity and angular velocity are changed to: v low =va vmax ×dt, w low =time wmax ×dt, Where, d o is the distance between the current unmanned boat and the dynamic obstacle; Combined with the above, the speed space [a,b,c,d] is finally determined: Minimum linear speed a: V m 、V d and V a The maximum value of the minimum linear velocity; Maximum linear velocity b, take V m 、V d and V a The minimum value of the maximum linear velocity; Minimum angular velocity c, take V m 、V d and V a The maximum value of the minimum angular velocity; The maximum angular velocity d is V m 、V d and V a The minimum value of the maximum angular velocity in ; Step 42-2, using the unmanned boat kinematics model to simulate the motion trajectory of the unmanned boat under a given control input command within a limited range of the speed space; Update the horizontal and vertical coordinates x, y and heading angle yaw of the unmanned boat according to the current linear velocity v and heading angle yaw of the unmanned boat, keep the speed unchanged, and return the updated state and predicted trajectory: x=x+v·cos(yaw)·dt,y=y+v·sin(yaw)·dt yaw=yaw+w·dt Sample the linear velocity v and angular velocity w in the velocity space window, and for each pair of v and w, generate a predicted trajectory; Step 42-3, performing trajectory evaluation for each predicted trajectory, wherein the trajectory evaluation includes distance evaluation dist, direction evaluation head and speed evaluation vel; Step 42-4, normalization processing, the distance evaluation normalization dist_n, direction evaluation head_n and speed evaluation vel_n are: Among them, α, β, and γ are distance evaluation weights, direction evaluation weights, and speed evaluation weights, respectively; Step 42-5, calculate the comprehensive evaluation score of each trajectory: G = dist_n + head_n + vel_n The trajectory with the highest evaluation score and the corresponding speed combination are selected as the optimal trajectory and optimal control amount for the DWA algorithm to sample and search for the optimal path.

8. The method for planning a path for multiple unmanned boats covering an area considering dynamic collision avoidance according to claim 7, characterized in that: When the angle between the traveling direction and the line connecting the target point in step 4 is too large, the DWA algorithm cannot search for the next target point.

9. A method for planning a path for multiple unmanned boats covering an area considering dynamic collision avoidance according to claim 8, characterized in that: The process of re-planning the APF algorithm in step 44 is as follows: Step 44-1, gravity vector calculation: Among them, η att is the gravitational gain coefficient, P i is the current i-th node coordinate, P g is the target node coordinate, and the next node is taken as the target node; Step 44-2, calculation of repulsive force vector: in, is the repulsion gain coefficient, P obs is the node coordinate of the obsth obstacle; Step 44-3: Calculate the resultant force vector F according to steps 44-1 and 44-2 sum and direction of the combined force Step 44-4: Obtain the expected direction θ of the predicted trajectory e : in, is the vertical axis, is the horizontal coordinate; Step 44-5, obtain the coordinates of the next target point of the unmanned boat P i+1 And the travel angle θ: θ=θ e ,P i+1 =P i +l·(cos(θ),sin(θ)) Where l is the step size.

Images