A dynamic planning method for ship paths based on collision risk field

Abstract

The invention discloses a ship path dynamic planning method based on a collision risk field, comprising the following steps: grid-dividing a map of the ship's navigation waters to obtain a grid map of the ship's navigation; using a path planning algorithm to search for paths on the grid map to obtain the ship's initial route; establishing a collision risk field model of a target ship according to the real-time navigation parameters of the target ship, substituting the coordinates of the center point of each grid in the grid map into the collision risk field model to obtain a collision risk value of each grid in the grid map; when the ship sails to a grid whose collision risk value is greater than a set threshold, taking the grid whose distance to the end point is closest and whose collision risk value is less than the set threshold as the local optimal point, and updating the ship's route; repeating the above steps until the ship arrives at the destination.

Description

Technical Field

[0001] The present invention relates to the technical field of ship traffic safety, and in particular to a ship path dynamic planning method based on a collision risk field. Background Art

[0002] With the rapid development of global trade, maritime transport has become an important link connecting all parts of the world. As the main means of maritime transport, the navigation safety of ships is directly related to the timely delivery of goods and the safety of personnel. In the complex marine environment, ships face a variety of potential risks, including collisions with other ships, grounding, bad weather, etc. Therefore, ensuring that ships sail safely and efficiently to their destinations is a core demand of the shipping industry. In order to meet these challenges, ship path planning technology has emerged, aiming to optimize routes, reduce navigation risks, and improve transportation efficiency through scientific methods.

[0003] At present, ship path planning mainly relies on automated navigation systems and advanced path planning algorithms. These systems usually include a global positioning system (GPS) to determine the real-time position of the ship, an automatic identification system (AIS) to obtain information about surrounding ships, and an electronic chart display and information system (ECDIS) to provide charts and navigation data. Path planning algorithms, such as the A* algorithm, the Dijkstra algorithm, and the artificial potential field method, are widely used to calculate the optimal path from the starting point to the end point.

[0004] Although existing ship path planning technologies have achieved certain results in navigation and path optimization, there are still some limitations. Traditional path planning methods are often based on static chart data and lack adaptability to real-time maritime traffic conditions and environmental changes. Therefore, these methods may not provide sufficient safety guarantees when dealing with complex maritime traffic and high-risk areas. In addition, existing technologies may not respond quickly enough in emergency situations and cannot adjust routes in time to avoid potential collisions. Therefore, it is necessary to develop a new path planning method that can evaluate and respond to maritime risks in real time and provide safer and more efficient navigation solutions. Summary of the invention

[0005] The present invention proposes a ship path dynamic planning method based on a collision risk field, which solves the problem that existing ship path planning methods are often based on static nautical chart data and lack adaptive adjustment to real-time maritime traffic conditions and environmental changes.

[0006] In order to solve the above technical problems, the present invention provides a ship path dynamic planning method based on a collision risk field, comprising the following steps:

[0007] Step S1: grid-dividing the map of the ship's navigation waters to obtain a grid map of the ship's navigation;

[0008] Step S2: using a path planning algorithm to search for a path on the grid map to obtain an initial route for the ship;

[0009] Step S3: establishing a collision risk field model of the target ship according to the real-time navigation parameters of the target ship, substituting the coordinates of the center point of each grid in the grid map into the collision risk field model, and obtaining a collision risk value of each grid in the grid map;

[0010] Step S4: When the ship sails to a grid where the collision risk value is greater than the set threshold, the grid closest to the end point and where the collision risk value is less than the set threshold is taken as the local optimal point, and the route of the ship is updated;

[0011] Step S5: Repeat steps S2 to S4 until the ship reaches the destination.

[0012] Preferably, the grid division of the map of the navigation waters of the ship in step S1 comprises the following steps:

[0013] Step S11: Divide the map of the ship's navigation area into nine sub-grids;

[0014] Step S12: Calculate the probability of each sub-grid having an obstacle, set the sub-grid with a probability of 1 to black, set the sub-grid with a probability of 0 to white, and set the remaining grids to gray;

[0015] Step S13: Divide the gray grid into nine sub-grids again, and repeat step S12 until the current number of iterations reaches the set maximum number of iterations and the size of the smallest sub-grid reaches the set grid threshold.

[0016] Preferably, in step S13, the grid threshold is set by the following steps:

[0017] Step S131: Based on the size of the ship 、Map size of the waters where this ship is sailing and set safe distance Calculate the first grid threshold r :

[0018] ;

[0019] In the formula, They are ship size factor, map size factor, and safety distance factor;

[0020] Step S132: Based on the distance of the own ship and initial diameter of the cycle Calculate the second grid threshold d :

[0021] ;

[0022] ;

[0023] ;

[0024] ;

[0025] ;

[0026] In the formula, , , B , L They represent the grid size, transverse size, longitudinal size, ship width and ship length respectively; is the ship's speed;

[0027] Step S133: selecting the larger one of the first grid threshold and the second grid threshold as the grid threshold.

[0028] Preferably, the step S3 of establishing a collision risk field model of the target ship according to the real-time navigation parameters of the target ship comprises the following steps: calculating the ship field of the target ship according to the length, advance distance, initial turning diameter and navigation speed of the target ship, taking the ship field as the collision risk field of the target ship, and establishing the collision risk field model of the target ship according to the collision risk field.

[0029] Preferably, the expression for calculating the ship field of the target ship is:

[0030] ;

[0031] ;

[0032] ;

[0033] ;

[0034] ;

[0035] In the above formula, The ship area for the target ship; are the coordinates of the point within the ship's domain; is the boundary value of the ship field; Shape control parameters for the ship domain; is a symbolic function; The controlling quaternion for the shape of the ship's domain; , , , They are the radius of the ship area in the four directions of bow, stern, left and right; is the risk value of the ship area along each semi-axis direction; is the semi-axis length; Control parameters for the shape of the ship domain.

[0036] Preferably, the expression of the collision risk field model in step S3 is:

[0037] ;

[0038] ;

[0039] In the formula, is the spatial collision risk value; is the longitudinal collision risk of the target ship; The risk of lateral collision with the target ship; are the coordinates of the point within the ship's domain; is a symbolic function; is a scaling factor to adjust the probability of ship collision; is the risk value of the ship area along each semi-axis direction; , Control parameters for the shape of the ship domain.

[0040] Preferably, in step S2, an ant colony algorithm is used to perform path search on the grid map, which includes the following steps:

[0041] Step S21: Initialize the parameters of the ant colony algorithm;

[0042] Step S22: Calculate the state transition probability of the k-th ant in the grid map, select the next grid for the k-th ant, and record the path of the k-th ant;

[0043] Step S23: repeat step S22 until the next grid is selected for all ants in the ant colony;

[0044] Step S24: updating the pheromone of each grid in the grid map according to the path of the ant colony, and setting the number of ants in the ant colony to M=M+1;

[0045] Step S25: Repeat steps S22 to S24 until the current number of iterations reaches the set maximum number of iterations, and output the optimal path.

[0046] Preferably, initializing the pheromone distribution of the grid map in step S21 includes the following steps:

[0047] Step S211: Connect the starting point and the end point of the own ship to obtain a straight line D, wherein the straight line D divides the grid map into two parts;

[0048] Step S212: Calculate the distance between each grid in the grid map and the straight line D, and initialize the pheromone distribution of the grid map according to the distance:

[0049] ;

[0050] ;

[0051] In the above formula, Q ( is the initial pheromone concentration; is the pheromone distribution factor; is the pheromone intensity constant; A, B, C For straight line D Parameters; The coordinates of the center point of the grid.

[0052] Preferably, the expression of the state transition probability in step S22 is:

[0053] ;

[0054] ;

[0055] ;

[0056] In the above formula, is the pheromone concentration index; is the heuristic factor index; for t Moment ants from the grid i Move to Grid j the concentration of pheromones left behind; for t Moment ants from the grid i Move to Grid j The path cost of for t Time Ant k The next step is to select the set and moving grid; , They are the weight coefficient of path length and the weight coefficient of static risk value respectively; For Grid j To Target Raster o distance; For Grid j The static risk cost value of is the smoothness heuristic function; is the weight coefficient of the straight-ahead mode; is the planning path smoothing coefficient; The direction of the ship's previous path planning; The direction in which the ship will turn next.

[0057] Preferably, the expression for updating the pheromone of each grid in the grid map in step S24 is:

[0058] ;

[0059] ;

[0060] In the above formula, For Ant The pheromone concentration at the iteration; ρ is the pheromone concentration volatility factor; is the number of ants from the grid at time t i Move to Grid j the concentration of pheromones left behind; is the amount of pheromone released by the ants on the path at time t; is the increment of pheromone concentration at time t; is the coefficient for increasing pheromone concentration.

[0061] The benefits of the present invention include at least:

[0062] 1. Dynamically adjust the risk field model according to the real-time navigation parameters of the target ship, and provide more flexible and reliable path planning in the case of multiple ships and multiple obstacles, so that the path planning of the ship can adapt to the real-time changing maritime traffic and environmental conditions;

[0063] 2. By specifying the risk value for each grid, a more refined assessment of the risk around the ship can be made, which is more accurate than the traditional method based on rough area division. When the ship enters a high-risk area, it can respond quickly, select the nearest low-risk grid as the local optimal point, update the route, and enhance the local collision avoidance capability. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] Figure 1 A schematic diagram of a method flow of an embodiment of the present invention;

[0065] Figure 2 This is a schematic diagram of the structure of a nine-fork tree in an embodiment of the present invention;

[0066] Figure 3 Schematic diagram of the process of segmenting a grid map in an embodiment of the present invention

[0067] Figure 4 A comparison diagram of processing results of a traditional grid method and a grid method of an embodiment of the present invention;

[0068] Figure 5 A schematic diagram of the collision risk of each grid in a grid map according to an embodiment of the present invention;

[0069] FIG6 (a) is an initial navigation path of a first ship according to an embodiment of the present invention;

[0070] FIG6 ( b ) is a diagram showing the encounter process 1 between the first ship and the target ship according to an embodiment of the present invention;

[0071] FIG6 (c) is a diagram showing the encounter process 2 between the first ship and the target ship according to an embodiment of the present invention;

[0072] FIG6 (d) is a diagram showing the final encounter effect of the first ship and the target ship according to an embodiment of the present invention;

[0073] Figure 7 is a distance and risk value variation curve during the dynamic collision avoidance process of the first ship in an embodiment of the present invention;

[0074] FIG8 (a) is an initial navigation path of a second ship according to an embodiment of the present invention;

[0075] FIG8( b ) is a diagram showing the encounter process 1 between the second ship and the target ship according to an embodiment of the present invention;

[0076] FIG8 (c) is a diagram showing the encounter process 2 between the second ship and the target ship according to an embodiment of the present invention;

[0077] FIG8 (d) is a diagram showing the final encounter effect of the second ship and the target ship according to the embodiment of the present invention;

[0078] Fig. 9 This is a curve showing the change of distance and risk value during the dynamic collision avoidance process of the second ship in an embodiment of the present invention. DETAILED DESCRIPTION

[0079] The following is a clear and complete description of the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings 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 belong to the protection scope of the present invention.

[0080] like Figure 1 As shown, an embodiment of the present invention provides a ship path dynamic planning method based on a collision risk field, comprising the following steps:

[0081] Step S1: grid-dividing the map of the ship's navigation waters to obtain a grid map of the ship's navigation.

[0082] Specifically, when the traditional grid method is used to model the navigation environment of a ship, the traditional grid method has a contradictory problem between grid division efficiency and grid accuracy, resulting in low modeling efficiency when a large area of ​​ship navigation environment modeling is required. At the same time, the traditional grid method does not take into account the size and maneuverability of the ship sufficiently, resulting in a gap between the navigation route planned based on the grid map and the actual required route of the ship. Based on the above considerations, an embodiment of the present invention proposes a grid division method based on the nine-tree method, which can simultaneously consider the distribution of obstacles, the boundary collision risk of the obstacle grid, the influence of the ship length and the maneuverability on the navigation safety of the ship and the accuracy of the modeling of the navigation environment of the ship. While expanding the amount of grid information, it can adaptively determine the optimal grid size value and construct a ship navigation adaptive grid map. Figure 2 As shown, the following steps are included:

[0083] Step S11: Based on the binary image of the waters where the ship is sailing, the waters map is divided into nine sub-grids.

[0084] Step S12: Determine whether the nine sub-grids are occupied by obstacles. If so, determine whether they are completely occupied by obstacles. If they are completely occupied, delete the node, assign a value of 1 to the sub-grid, and set the state to black; if they are not completely occupied, save the node, assign a value of 0.5 to the sub-grid, and set the state to gray; if they are not occupied by obstacles, directly assign a value of 0 to the sub-grid, and set the state to white.

[0085] Among them, calculate the i The expression for the probability that a subgrid is occupied is:

[0086] ;

[0087] ;

[0088] ;

[0089] ;

[0090] In the formula, For the i The probability that a subgrid is occupied; For ships and grids i The distance between the centers of The minimum distance detected by the ship's sensor; The distance return value of the ship sensor; Returns the value of the ship sensor angle; It is the maximum angle detected by the ship sensor.

[0091] After the parent grid with a value of 0.5 is divided again, the position numbers of the 9 sub-grids in this layer are as follows: Figure 3 As shown, the grid number obtained after n subdivisions is ,in According to the grid i The position of the layer is selected from integers 0-8. For the grid to be segmented, the position number of the sub-grid obtained by segmentation is added to the back of its parent grid number position in order from left to right. For example, after three subdivisions, the grid number is 152. If the fourth subdivision is continued, 9 sub-grids are obtained, numbered 1520, 1521, 1522, ..., 1528. The position of the grid in the map can be obtained by the grid number. Assume that the map scale of the ship's navigation waters is a×b, and the number is The center coordinates of the grid corresponding to ,calculate , The expression is:

[0092] ;

[0093] In the formula, , are the length and width of the map of the navigable waters respectively; When 0, 3, and 6 are selected, ,when Take 1, 4, 7, ,when Take 2, 5, 8 o'clock, ;when When 0, 1, or 2 is selected, ,when When taking 3, 4, and 5, ,when Take 6, 7, 8 o'clock, .

[0094] Step S13: Divide the gray grid into nine sub-grids again, and repeat step S12 until the current number of iterations reaches the set maximum number of iterations and the size of the smallest sub-grid reaches the set grid threshold.

[0095] After all the gray grids are divided again, the map is updated:

[0096] ;

[0097] In the formula, Indicates that after updating the map, the ship moves to s The position of the step; Indicates that after the map is updated, the ship moves to s Sensor readings for each step; Indicates that after the map is updated, the ship moves to s- 1 step position; Indicates that after the map is updated, the ship moves to s -1 step of sensor reading.

[0098] When the map is continuously updated, it is necessary to set a threshold for the algorithm to stop running. That is, the algorithm will stop running when the set minimum grid size threshold or the minimum level of nine-tree division is met. This is because with the subdivision of the grid and the refinement of the environmental model, the number of grids will gradually increase. When the number of grids reaches the minimum grid size threshold or the minimum level of nine-tree division, the subdivision of the map is sufficient to meet the needs of path planning, and further subdivision will no longer have a significant impact on the path planning results. Therefore, the algorithm will automatically stop running to avoid unnecessary calculations and resource consumption. Through the convergent nine-tree adaptive grid map method, environmental modeling can be efficiently performed and stopped after the preset conditions are met, thereby improving the efficiency and accuracy of path planning.

[0099] The size of the ship will have an impact on the precision and accuracy of the grid map when determining the grid size. For example, smaller ships may require a higher grid resolution to ensure that the grid map can accurately capture the details and features of the ship. Higher resolution can provide more precise location information, which helps to more accurately assess the risk of ship collision. Areas with larger ship sizes may require a larger grid size to fully consider the safe spacing between ships.

[0100] At the same time, the ship's advance and initial turn diameter have a direct impact on the grid size. The larger the advance, that is, the larger the space occupied by the ship's motion in the grid, the grid size should be increased accordingly to ensure that the ship can be fully represented in the grid. The initial turn diameter determines the space required for the ship to rotate. A smaller initial diameter means that the ship needs a smaller grid to accurately simulate its rotation process, and vice versa. Therefore, the determination of the grid size should comprehensively consider the advance and initial turn diameter to ensure that the ship's motion can be accurately represented and simulated.

[0101] Therefore, the embodiment of the present invention studies the influence of ship size, ship advance and initial turning diameter on the grid size. In step S13, the grid threshold is set by the following steps:

[0102] Step S131: Based on the size of the ship 、Map size of the waters where the ship is sailing and set safe distance Calculate the first grid threshold r .

[0103] When performing map modeling in ship path planning, the grid size is generally determined by the ship size, the size of the study waters map, and the pre-given safety distance. Since the embodiment of the present invention is based on a square grid for environmental modeling, the longer side of the grid is ultimately used as the side length of the square. It can be set according to the following formula:

[0104] ;

[0105] In the formula, , , They are ship size factor, map size factor, and safety distance factor respectively.

[0106] Step S132: Based on the distance of the own ship and initial diameter of the cycle Calculate the second grid threshold d The following is the calculation formula for the ship's advance and initial turning radius:

[0107] ;

[0108] ;

[0109] ;

[0110] ;

[0111] ;

[0112] In the formula, , , B , L They represent the grid size, transverse size, longitudinal size, ship width and ship length respectively; is the ship speed.

[0113] Step S133: selecting the larger one of the first grid threshold and the second grid threshold as the grid threshold.

[0114] Step S2: Use a path planning algorithm to search for a path on the grid map to obtain the initial route of the ship.

[0115] Specifically, among the existing ship path planning algorithms, there are few matching algorithms with the adaptive grid map. In addition, due to the complex navigation environment of ships and the characteristics of the ships themselves, the ship path planning problem is relatively complicated, which requires the algorithm to spend a lot of time and resources to find the optimal solution, thereby reducing the efficiency of path planning; some algorithms do not fully consider risk factors when planning paths, such as avoiding potential collision risks with other ships, resulting in low safety of the planned paths. The embodiment of the present invention optimizes the heuristic function, initial pheromone distribution and pheromone update rules in the ant colony algorithm, and combines the ship navigation adaptive grid map to realize the ship static and environmental path planning, including the following steps:

[0116] Step S21: Initialize the parameters of the ant colony algorithm. In order to improve the optimization ability of the algorithm in the early stage, the embodiment of the present invention improves the initial setting of the information concentration of the ant colony. The main method is to distribute pheromones of different concentrations in different locations to guide the ants to move toward the target point, reduce the convergence time of the algorithm, and improve the efficiency of the algorithm. The specific method is to connect the starting point and the end point in the map, divide the map into two parts with the connecting line, and adjust the distribution area of ​​pheromones according to the set mathematical model, so that the pheromones decrease outward along the connecting line. It includes the following steps:

[0117] Step S211: Connect the starting point and the end point of the own ship to obtain a straight line D, wherein the straight line D divides the grid map into two parts.

[0118] Step S212: Calculate the distance from each grid in the grid map to the straight line D, and initialize the pheromone distribution of the grid map according to the distance:

[0119] ;

[0120] ;

[0121] In the above formula, Q( is the initial pheromone concentration; is the pheromone distribution factor; is the pheromone intensity constant; A, B, C are the parameters of line D; ( x , y ) are the position coordinates of the center point of the grid.

[0122] Step S22: Calculate the state transition probability of the kth ant in the grid map, select the next grid for the kth ant, and record the path of the kth ant. Based on the actual situation of ship navigation, when the planned path has fewer turning points and the path is smoother, the algorithm path can be made shorter, more in line with navigation reality, and the rationality of the planned path is improved. Therefore, the embodiment of the present invention introduces a smoothness heuristic function when calculating the state transition probability. At the same time, in ship path planning, if safety heuristic information is not considered, the planned path may be more dangerous and cause ship collision accidents. Therefore, it is necessary to add safety heuristic information to ship path planning, that is, to add the risk cost value of static obstacles to the heuristic function. Then the expression for calculating the state transition probability in the embodiment of the present invention is:

[0123] ;

[0124] ;

[0125] ;

[0126] In the above formula, is the pheromone concentration index; is the heuristic factor index; for t Time Ant k The next step is to select the set and moving grid; for t Moment ants from the grid i Move to Grid j the concentration of pheromones left behind; is the number of ants from the grid at time t i Move to Grid j The path cost of , They are the weight coefficient of path length and the weight coefficient of static risk value respectively; For Grid j The distance to the target grid o; For Grid j The static risk cost value of is the smoothness heuristic function; is the planning path smoothing coefficient; is the weight coefficient of the straight-ahead mode; The direction of the ship's previous path planning; The direction in which the ship will turn next.

[0127] Step S23: Repeat step S22 until the next grid is selected for all ants in the ant colony.

[0128] Step S24: Update the pheromone of each grid in the grid map according to the path of the ant colony, and set the number of ants in the ant colony to M=M+1.

[0129] In order to improve the convergence ability of the algorithm in the later stage and avoid the algorithm from falling into the local optimum in the early stage, the embodiment of the present invention proposes a rule for updating pheromones according to the length of the iterative path. When the path planned for the ant is longer, less pheromones are given to the ant on the basis of the amount of pheromones obtained by the original update rule, and when the path planned for the ant is shorter, more pheromones are given to the ant on the basis of the amount of pheromones obtained by the original update rule. Then the expression for updating the pheromone of each grid in the grid map is:

[0130] ;

[0131] ;

[0132] In the above formula, For Ant The pheromone concentration at the iteration; ρ is the pheromone concentration volatility factor; is the number of ants from the grid at time t i Move to Grid j the concentration of pheromones left behind; for t The amount of pheromones released by ants on the path at any moment; for t The increment of pheromone concentration at each moment; is the coefficient for increasing pheromone concentration.

[0133] Step S25: Repeat steps S22 to S24 until the current number of iterations reaches the set maximum number of iterations, and output the optimal path.

[0134] Step S3: establishing a collision risk field model of the target ship according to the real-time navigation parameters of the target ship, substituting the coordinates of the center point of each grid in the grid map into the collision risk field model, and obtaining a collision risk value of each grid in the grid map.

[0135] Specifically, the ship domain of the target ship is calculated according to the length, advance, initial turning radius and navigation speed of the target ship. The ship domain is composed of four sectors of different shapes. The difference in the sectors also indicates that the scope and nature of the domain in the four directions are different. The shape of the sector is determined by the parameter k. The boundary expression of the obtained ship domain is:

[0136] ;

[0137] In the formula, are the coordinates of the point within the ship's domain; is the boundary value of the ship field; The controlling quaternion for the shape of the ship's domain; , , , They are the distances in the ship area in the four directions of bow, stern, left and right; Shape control parameters for the ship domain.

[0138] When a ship is planning its path, one of the bases for path selection is the risk value of the grid. It is necessary to assign the dynamic risk field of the ship based on the ship field to the grid map. Therefore, the fuzzy set theory is introduced to divide the area around the ship into multiple danger levels, and the fuzzy four-element ship field is obtained:

[0139] ;

[0140] ;

[0141] ;

[0142] ;

[0143] ;

[0144] In the above formula, The ship area for the target ship; is a symbolic function; is the risk value of the ship area along each semi-axis direction; is the semi-axis length; Control parameters for the shape of the ship domain.

[0145] The final expression of the collision risk field model of the target ship is:

[0146] ;

[0147] ;

[0148] In the formula, is the spatial collision risk value; is the longitudinal collision risk of the target ship; The risk of lateral collision with the target ship; is a scaling factor to adjust the probability of ship collision.

[0149] Step S4: When the ship sails to a grid where the collision risk value is greater than a set threshold, the grid closest to the end point and where the collision risk value is less than the set threshold is taken as the local optimal point, and the ship's route is updated.

[0150] Specifically, in order to realize the dynamic path planning of the ship, the embodiment of the present invention adds the optimal local target point and the dynamic collision risk heuristic factor to the static path planning algorithm based on the improved ant colony algorithm. First, the ship sails along the initial route planned by the static path planning method based on the improved ant colony algorithm, and calculates the risk value of the ship in the target ship collision risk field in real time. When the ship reaches the first position where the collision risk value is greater than 0.05, the ship dynamic path planning method based on the improved ant colony algorithm is used, and the node starting point and the selected local optimal target point are used as the target point to be reached at present. The dynamic path planning is performed using the search-while-walking strategy, and it is judged whether the target point is the end point. If so, the path planning has been completed, and if not, the dynamic path planning continues. During the navigation process of the ship, when the collision risk value of the first target ship is lower than 0.05, the collision risk field of the target ship is ignored, and only the static path planning algorithm is used until the global end point is successfully reached.

[0151] When performing dynamic path planning, the expression of the heuristic function used is:

[0152] ;

[0153] In the formula, is the collision risk value of the target ship in grid j; and are respectively the path length weight coefficient and the dynamic risk value weight coefficient. , The values ​​of are 0.2 and 0.8 respectively.

[0154] In the embodiment of the present invention, the candidate point with the smallest weighted sum of the distance between the candidate point and the end point and the collision risk value of the candidate point is taken as the local optimal point, and the calculation formula is as follows:

[0155] ;

[0156] In the formula, Z The weighted sum of distance and risk, select Z The point with the minimum value is taken as the optimal local target point; is the distance from the local optimal target point to the end point; , is the distance weight coefficient and the risk value coefficient. In the embodiment of the present invention, , 0.2 and 0.8; is the risk value of the target ship at the optimal local target point.

[0157] Step S5: Repeat steps S2 to S4 until the ship reaches the destination.

[0158] Embodiment 1

[0159] The map is rasterized based on the basic nine-tree method to obtain a raster map processed according to the map obstacle density. At the same time, in order to improve the efficiency of ship path planning, this embodiment processes the research sea area when rasterizing the map. The size of the processed map is 6318m 6318m, and three static obstacles are added to the original map. In this embodiment, the grid processing after adding three static obstacles is as follows Figure 4 As shown, Figure 4 The left side shows the result of processing the map using the traditional uniform grid method. Figure 4 The right side is the result of the grid method processing of this embodiment. A total of 4 levels of division are performed, and the minimum grid resolution threshold is set to 4, and the obstacle ratio threshold is set to 0.05.

[0160] Table 1 is a comparison of mapping parameters between a traditional grid map and the grid map of this embodiment, where the unit of storage space is M, and the unit of mapping time is second s.

[0161] Table 1 Comparison of construction parameters of traditional grid map and grid map of the present invention

[0162]

[0163] As can be seen from Table 1, the grid map construction process of this embodiment takes 1.25 seconds. Although it consumes some more time than the traditional uniform grid map, it saves a large part of the storage space. Therefore, although the grid map modeling algorithm of this embodiment sacrifices part of the map construction time, it saves a lot of map construction space and ensures the calculation efficiency of the algorithm. At the same time, the algorithm effectively distinguishes navigable waters and obstacle areas based on the distribution of obstacles, and divides the grid boundaries more carefully. Therefore, the grid map modeling algorithm of this embodiment effectively balances the algorithm calculation efficiency and grid accuracy.

[0164] The optimal grid size is determined based on actual ship data. The first ship selected in this embodiment has a length of 21 m, a width of 5 m, and a speed of 5 m / s. Other parameters of the ship are shown in Table 2.

[0165] Table 2 Ship parameters

[0166]

[0167] Based on the above values ​​and the calculation formula mentioned in the present invention, the optimal grid size value calculated considering the ship size is about 64m 64m; when considering the maneuverability of the ship, the calculated minimum grid size is about 71m 71m. Therefore, the minimum grid size value of the grid map cannot be less than 71m 71m. The map size selected by the present invention is 6318m 6318m, the maximum number of iterations is set to 5, so the size of the minimum grid obtained based on the nine-tree method is 2106m 2106m、702m 702m、234m 234m、78m 78m、26m 26m. Considering the influence of ship size and maneuverability, the optimal grid size for path planning for this ship is 78m. 78m. This grid size value satisfies the constraints of ship size and maneuverability on the grid map, and can refine the ship's navigation trajectory and improve the efficiency of path planning.

[0168] The second ship selected in the embodiment of the present invention has a length of 63 m, a width of 12 m, a speed of 5 m / s, and other parameters of the ship are shown in Table 3.

[0169] Table 3 Ship parameters

[0170]

[0171] Based on the above values ​​and the calculation formula mentioned in the present invention, the optimal grid size value calculated considering the ship size is about 192m 192m; when considering the maneuverability of the ship, the minimum grid size value calculated is about 211m 211m. Therefore, the minimum grid size value of the grid map cannot be less than 211m 211m. If the maximum number of iterations is set to 5, the size of the minimum grid obtained based on the nine-branch tree method is 2106m. 2106m、702m 702m、234m 234m、78m 78m、26m 26m. Considering the influence of ship size and maneuverability, the optimal grid size for the second ship to plan its path is 234m. 234m. This grid size value meets the constraints of ship size and maneuverability on the grid map, and can refine the ship's navigation trajectory and improve the efficiency of path planning.

[0172] By using the parameters of the first ship and the second ship, the grid map can be divided multiple times based on the nine-tree method to obtain grid maps with different minimum grid size values.

[0173] In order to verify the effect of the collision avoidance dynamic path planning method based on the improved ant colony algorithm, this embodiment designs a ship collision avoidance dynamic path planning simulation experiment for target ships with different headings.

[0174] (1) 45° target ship dynamic path planning experiment

[0175] In this experiment, the first ship is selected as the ship, and the optimal grid size calculated using the ship parameters is 78m 78m, assuming that the starting coordinates of the ship are (13.5, 67.5) and the end coordinates are (55.5, 28.5). The target ship is selected as a ship with a length of 40m, a width of 6m, a starting coordinate of (19.5, 28.5), an initial heading of 45°, and a speed of 4m / s. The risk value at the boundary of the ship field is taken as 0.95, and the target ship collision risk field is calculated based on the parameters of the ship, and the target ship collision risk is assigned to the grid map. The schematic diagram of the rasterization of the ship collision risk is shown in the figure below. Figure 5 At the same time, the length distribution of the ship's domain in all directions is calculated according to the ship domain model, and the relative distance between the ships is calculated according to the motion trajectories of the ship and the target ship. The ship dynamic path planning process and the change values ​​of related parameters are shown in Figures 6 and Figure 7 shown.

[0176] From Figure 6(a) and Figure 7 It can be seen that at the beginning of the navigation mission, the ship static path planning algorithm is first used to plan an initial path from the starting point to the end point, and the ship will sail along this path. When the ship travels to the coordinates (44.5, 41.5), according to the ship collision risk model and various parameters of the target ship, the collision risk value of the ship at this point is just greater than 0.05. At this time, the collision risk of the target ship is considered, and the local target point is selected according to the optimal local target point selection rule. The first local optimal target point selected in Figure 6 (b) is (45.5, 40.5), and the dynamic path planning algorithm is used to plan the path with this point as the target point. Repeat the above steps, as shown in Figure 6 (c), when the ship travels to the coordinates (22.5, 45.5), the risk value of this point is calculated to be 0.030, which is the first point less than 0.05. At this time, the collision risk of the target ship is no longer considered, and only the static path planning method is applied to plan the path with the end point as the target point until the ship reaches the end point and the path planning is completed. The result is shown in Figure 6 (d).

[0177] (2) 270° target ship dynamic path planning experiment

[0178] In this experiment, the ship selected is the second ship, and the optimal grid size calculated using the ship parameters is 78m 78m, the starting coordinates of the ship are (16.5, 34.5), and the end coordinates are (58.5, 76.5). The target ship is selected as a ship with a length of 53m, a width of 12m, a starting coordinate of (49.5, 49.5), a heading of 270°, and a speed of 4m / s. The risk value at the boundary of the ship domain is taken as 0.95, and the collision risk field value of the target ship is calculated based on the ship parameters of the target ship, and the collision risk value of the target ship is assigned to the grid map. At the same time, the length distribution of the ship domain of the ship in all directions is calculated according to the ship domain model, and the relative distance between the ships is calculated according to the motion trajectory of the ship and the target ship. The ship dynamic path planning process and the change values ​​of related parameters are shown in Figure 8 Fig. 9 Show.

[0179] From Figure 8(a) and Fig. 9 As we know, at the beginning of the navigation mission, the static path planning algorithm is first used to plan an initial path from the starting point to the end point, and the ship will sail along this path. When the ship travels to the coordinate (25.5, 43.5), according to the ship collision risk model and various parameters of the target ship, the collision risk value of the ship at this point is calculated to be just greater than 0.05. At this time, the collision risk of the target ship is considered, and the local target point is selected according to the optimal local target point selection rule. The first local optimal target point selected in Figure 8 (b) is (26.5, 43.5), and the dynamic path planning algorithm is used to plan the path with this point as the target point. Repeat the above steps, as shown in Figure 8 (c), when the ship travels to the coordinate (31.5, 43.5), the risk value of this point is calculated to be 0.045, which is the first point less than 0.05. At this time, the collision risk of the target ship is no longer considered, and only the static path planning algorithm is applied to plan the path with the end point as the target point until the ship reaches the end point and the path planning is completed. The result is shown in Figure 8 (d). Through the ship field of the second ship established, it can be known that The values ​​are 90m, 55m, 44m, and 34m respectively.

[0180] From Figure 6, Figure 7 Figure 8 Fig. 9 It can be seen that during the entire navigation process, the ship can identify the collision risk of the target ship based on the established ship collision risk field model, and always keep a safe distance with the target ship during the encounter. This shows that the ship collision avoidance dynamic path planning algorithm based on the improved ant colony algorithm proposed in the present invention can complete the path planning task in a dynamic environment.

[0181] The technical features of the above embodiments can be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. Only the preferred embodiments of the present invention are expressed. The description is more specific and detailed, but it cannot be understood as limiting the scope of the patent of the present invention. As long as there is no contradiction in the combination of these technical features, they should be considered as the scope recorded in this specification.

[0182] It should be noted that, for those skilled in the art, several modifications and improvements can be made without departing from the concept of the present invention, and these modifications and improvements all belong to the protection scope of the present invention. Therefore, the protection scope of the patent of the present invention shall be subject to the attached claims.

Claims

1. A ship path dynamic planning method based on collision risk field, characterized in that: The following steps are involved: Step S1: grid-dividing the map of the ship's navigation waters to obtain a grid map of the ship's navigation; Step S2: using a path planning algorithm to search for a path on the grid map to obtain an initial route for the ship; Step S3: establishing a collision risk field model of the target ship according to the real-time navigation parameters of the target ship, substituting the coordinates of the center point of each grid in the grid map into the collision risk field model, and obtaining a collision risk value of each grid in the grid map; The method of establishing a collision risk field model of the target ship according to the real-time navigation parameters of the target ship comprises the following steps: calculating the ship field of the target ship according to the length, advance, initial turning diameter and navigation speed of the target ship, taking the ship field as the collision risk field of the target ship, and establishing the collision risk field model of the target ship according to the collision risk field; Step S4: When the ship sails to a grid where the collision risk value is greater than the set threshold, the grid closest to the end point and where the collision risk value is less than the set threshold is taken as the local optimal point, and the route of the ship is updated; Step S5: Repeat steps S2 to S4 until the ship reaches the destination.

2. A ship path dynamic planning method based on collision risk field according to claim 1, characterized in that: The grid division of the map of the navigation waters of the ship in step S1 includes the following steps: Step S11: Divide the map of the ship's navigation area into nine sub-grids; Step S12: Calculate the probability of each sub-grid having an obstacle, set the sub-grid with a probability of 1 to black, set the sub-grid with a probability of 0 to white, and set the remaining grids to gray; Step S13: Divide the gray grid into nine sub-grids again, and repeat step S12 until the current number of iterations reaches the set maximum number of iterations and the size of the smallest sub-grid reaches the set grid threshold.

3. A ship path dynamic planning method based on collision risk field according to claim 2, characterized in that: In step S13, the grid threshold is set by the following steps: Step S131: Based on the size of the ship 、Map size of the waters where the ship is sailing and set safe distance Calculate the first grid threshold r : ； In the formula, , , They are ship size factor, map size factor, and safety distance factor; Step S132: Based on the distance of the own ship and initial diameter of the cycle Calculate the second grid threshold d : ； ； ； ； ； In the formula, , , B , L They represent the grid size, transverse size, longitudinal size, ship width and ship length respectively; is the ship's speed; Step S133: selecting the larger one of the first grid threshold and the second grid threshold as the grid threshold.

4. The method for dynamic ship path planning based on collision risk field according to claim 1, characterized in that: The expression for calculating the ship field of the target ship is: ； ； ； ； ； In the above formula, The ship area for the target ship; are the coordinates of the point within the ship's domain; is the boundary value of the ship field; Shape control parameters for the ship domain; is a symbolic function; The controlling quaternion for the shape of the ship's domain; , , , They are the distances in the ship area in the four directions of bow, stern, left and right; is the risk value of the ship field along each semi-axis direction; is the semi-axis length; is the control parameter for the shape of the ship domain.

5. The method for dynamic ship path planning based on collision risk field according to claim 1, characterized in that: The expression of the collision risk field model in step S3 is: ； ； In the formula, is the spatial collision risk value; is the longitudinal collision risk of the target ship; The risk of lateral collision with the target ship; are the coordinates of the point within the ship's domain; is a symbolic function; is a scaling factor to adjust the probability of ship collision; is the risk value of the ship field along each semi-axis direction; , is the control parameter for the shape of the ship domain.

6. A ship path dynamic planning method based on collision risk field according to claim 1, characterized in that: In step S2, an ant colony algorithm is used to perform path search on the grid map, which includes the following steps: Step S21: Initialize the parameters of the ant colony algorithm; Step S22: Calculate the state transition probability of the k-th ant in the grid map, select the next grid for the k-th ant, and record the path of the k-th ant; Step S23: repeat step S22 until the next grid is selected for all ants in the ant colony; Step S24: updating the pheromone of each grid in the grid map according to the path of the ant colony, and setting the number of ants in the ant colony to M=M+1; Step S25: Repeat steps S22 to S24 until the current number of iterations reaches the set maximum number of iterations, and output the optimal path.

7. A method for dynamic ship path planning based on collision risk field according to claim 6, characterized in that: Initializing the pheromone distribution of the grid map in step S21 includes the following steps: Step S211: Connect the starting point and the end point of the own ship to obtain a straight line D, wherein the straight line D divides the grid map into two parts; Step S212: Calculate the distance from each grid in the grid map to the straight line D, and initialize the pheromone distribution of the grid map according to the distance: ； ； In the above formula, Q( is the initial pheromone concentration; is the pheromone distribution factor; is the pheromone intensity constant; A, B, C are the parameters of line D; The coordinates of the center point of the grid.

8. A method for dynamic ship path planning based on collision risk field according to claim 6, characterized in that: The expression of the state transition probability in step S22 is: ； ； ； In the above formula, is the pheromone concentration index; is the heuristic factor index; for t Moment ants from the grid i Move to Grid j the concentration of pheromones left behind; for t Moment ants from the grid i Move to Grid j The path cost of for t Time Ant k The next step is to select the set and moving grid; , They are the weight coefficient of path length and the weight coefficient of static risk value respectively; For Grid j To Target Raster o distance; For Grid j The static risk cost value of is the smoothness heuristic function; is the weight coefficient of the straight-ahead mode; is the planning path smoothing coefficient; The direction of the ship's previous path planning; The direction in which the ship will turn next.

9. A method for dynamic ship path planning based on collision risk field according to claim 6, characterized in that: The expression for updating the pheromone of each grid in the grid map in step S24 is: ； ； In the above formula, For Ant The pheromone concentration at the iteration; ρ is the pheromone concentration volatility factor; is the number of ants from the grid at time t i Move to Grid j the concentration of pheromones left behind; is the amount of pheromone released by the ants on the path at time t; is the increment of pheromone concentration at time t; is the coefficient for increasing pheromone concentration.

Images