Ship formation algorithm combining fast marching method with virtual obstacle potential field method

Abstract

The application provides a ship formation algorithm combining a fast marching method with a virtual obstacle potential field method, adopts the fast marching method to perform global static path planning, then realizes path tracking of a follower ship to a leader ship through a leader-following formation control method, finally, adopts an improved virtual obstacle potential field method to perform local collision avoidance, and improves the situation that the ship may enter a local minimum point position through the way of increasing virtual obstacles; experimental results show that the algorithm plans a route with time, smoothness and safety advantages for the leader in the ship, and quickly forms a stable formation; compared with the running speed of the Dijkstra algorithm and the A* algorithm, the running speed is significantly improved; when the ship encounters an obstacle with a collision risk, collision avoidance operation can be performed in time, and the problem of being unable to solve the problem of being separated from a local minimum point by the traditional artificial potential field method is solved.

Description

TECHNICAL FIELD

[0001] The application relates to a ship formation algorithm capable of effectively establishing a formation structure and guaranteeing the safety of all ships, in particular to a ship formation algorithm combining a fast marching method and a virtual obstacle potential field method, and belongs to the technical field of intelligent safe navigation of ships. BACKGROUND

[0002] Ship formation is of great significance to maintaining the stability of multi-ship coordinated operation. The leader-following formation control method is favored by scholars due to its simple principle and easy implementation. In order to realize multi-ship coordinated operation at sea through the method, path planning needs to be performed in advance for the leader in the team. Among the commonly used global path planning algorithms, Dijkstra algorithm and A* algorithm are algorithms for solving optimal paths, but Dijkstra algorithm and A* algorithm have the following disadvantages: first, Dijkstra algorithm is simple in principle, but the calculation process is too complex and occupies a large amount of memory, and is only suitable for small-scale path planning; A* algorithm is faster than Dijkstra algorithm, but it is very dependent on the heuristic function, which leads to a huge amount of calculation. Secondly, since the main goal of both is to plan the shortest route, the safety during navigation cannot be guaranteed.

[0003] And when the follower ship tracks the path of the leader ship through the leader-following formation control method, there may be static obstacles in the navigation route, so it is urgent to propose an algorithm to solve the problem of collision that may occur during tracking. In the traditional collision avoidance method, the artificial potential field method has the advantages of strong real-time performance, simple mathematical calculation, smooth planned path, and easy programming, and is therefore widely used in solving local collision avoidance problems, but it may fall into a local minimum point and cause the ship to stop. SUMMARY

[0004] The application discloses a ship formation algorithm combining a fast marching method and a virtual obstacle potential field method, which combines the path planning method of the leader-following formation control method and the local collision avoidance method of the virtual obstacle potential field method, and realizes an algorithm capable of effectively establishing a formation structure and guaranteeing the safety of all ships.

[0005] The ship formation algorithm combining the fast marching method and the virtual obstacle potential field method has the following technical solutions:

[0006] 1) Path planning based on the fast marching method

[0007] The steps of the fast marching method are as follows:

[0008] Step1: Model the navigation environment and convert it into a binary grid map; mark the cells belonging to obstacles and the cells corresponding to the passable area respectively;

[0009] Step2: The fast marching method treats each cell marked as an obstacle in the map as a wave source, and expands multiple waves at the same time. The result value of each cell in the map represents the time that the wave needs to reach the nearest obstacle, which is proportional to the distance to the obstacle. The potential map obtained in this step is the velocity potential, denoted as W(x). Each grid in the velocity potential map has a value representing its distance to the nearest obstacle, with a value range of 0-1;

[0010] Step3: Based on the potential map W(x), treat the target point as the only wave source to ensure a global minimum value, and expand the wave on the map until it reaches the initial point. The expansion speed of the wave at each cell in the feasible area is extracted from the velocity potential map W(x) calculated in the previous step;

[0011] Step4: Finally, start from the starting point of the ship and perform gradient descent on the entire arrival time map to move towards the target point (global minimum value of the result map), obtaining a path that considers arrival time, smoothness, and safety;

[0012] 2) Formation control based on Laplace matrix

[0013] Consider a leader-follower multi-ship formation system, where each ship in the system can be regarded as a node in the network, and the information transmission link between ships can be regarded as an edge connecting different nodes. Thus, the multi-ship system and its communication network can be modeled as a graph. In the formation, there is information flow, so the ship formation system is constructed as a directed graph, as shown in Figure 1 , where the arrow direction represents the direction of information transmission.

[0014] Let G = {V, E} represent the directed graph, where V = {v1, v2,..., v n} represents the N vertices in the graph, i.e. the N ships in the system, and E represents the set of edges composed of vertices. Its Laplace matrix L is defined as: L = D - A, where D is the in-degree matrix of the graph, and the elements on the diagonal are the sum of the in-degree of the nodes; A is the weight matrix of the graph, if vertex v is the head of edge E, then the element a ij = 1 in the A matrix, otherwise a ij = 0.

[0015] Thus, the weight matrix of the system can be obtained as:

[0016]

[0017] The incidence matrix is:

[0018]

[0019] The Laplace matrix is:

[0020]

[0021] Let x = [x1, x2,... x5] T is the current coordinate value of each ship, and η is the expected relative position between two adjacent points.

[0022] Thus, for each edge on the topological graph, we have:

[0023] Point x j Relative to point x i The difference from the expected value is e ij = x i - x j - η

[0024] Point x j The difference from the expected value for all points is:

[0025] where α is the relative position gain coefficient.

[0026] In order to enable the follower to track the leader's trajectory, we need to reduce the value of e j to 0, so the speed of each follower will be affected by e j , and the formation will be stabilized by eliminating the error.

[0027] 3) Local obstacle avoidance based on virtual obstacle potential field method

[0028] In the process of tracking the leader's path by the followers in the formation, if the distance between the tracked path and the obstacle is too close, there is a high risk of collision. Therefore, it is necessary for each follower to independently achieve local collision avoidance to deal with such situations. The traditional artificial potential field method has the advantages of strong real-time performance, simple mathematical calculation, smooth planned path, and easy programming, so it is widely used in solving local collision avoidance problems. However, it can cause the ship to be trapped in a local minimum point, resulting in stagnation. To solve this problem, a virtual obstacle potential field method is proposed. When the follower ship enters the position of the local minimum point, a virtual obstacle is first added by judging the distribution of the obstacle. At this time, through the added virtual obstacle, the resultant force on the ship will change, and the ship will be provided with additional escape force by the virtual obstacle to escape the local minimum point. Moreover, due to the existence of this repulsive force, the ship can be prevented from being trapped in the minimum point again.

[0029] Assume that the real-time coordinate position of the ship is P = (x, y), and the position of the target point is P g =(x g ,y g ), then the gravitational field function is:

[0030]

[0031] wherein the coefficient k is the gain coefficient of the gravitational field, and a suitable value is selected through experiment.

[0032] Through the gradient relationship between gravity and the gravitational field, the expression of the gravity is:

[0033] F a =-grad(U a )=-k(P-P g ) (5)

[0034] The mathematical expression of the repulsive field is:

[0035]

[0036] wherein the coefficient β is the gain coefficient of the repulsive field, ρ is the distance between the ship and the obstacle, and ρ0 is the influence radius of the repulsive field, and the repulsive force is zero beyond the range.

[0037] The expression of the repulsive force is:

[0038]

[0039] When the ship enters the position of the local minimum point, a virtual obstacle is introduced, and the virtual repulsive field function is:

[0040]

[0041] wherein β' is a virtual repulsive potential field constant greater than zero, ρ' is the distance between the ship and the virtual obstacle, and ρ0' is the influence distance of the virtual obstacle on the ship.

[0042] The virtual repulsive force is:

[0043]

[0044] Thus, the expression of the resultant force is:

[0045]

[0046] The application steps of the virtual obstacle potential field method in ship formation navigation are as follows:

[0047] Step 1: input the current position, navigation speed, direction and other parameters of each ship;

[0048] Step 2: Determine whether the follower ship has entered the influence range of the obstacle, if it has entered the influence range of the obstacle, calculate the repulsive force and attractive force, otherwise continue sailing;

[0049] Step 3: When the follower has been affected by the obstacle, judge whether the resultant force is 0. If it is 0, add a virtual obstacle and then recalculate the resultant force to judge the motion direction; if it is not 0, directly calculate the motion direction. Repeat this step until it leaves the influence range of the obstacle;

[0050] Step 4: When the follower ship leaves the collision risk, continue sailing according to the original trajectory.

[0051] The positive effect of the present application is to provide a ship formation algorithm combining the fast marching method and the virtual obstacle potential field method. Firstly, the fast marching method is used for global static path planning, then the leader-following formation control method is used to realize path tracking of the follower ship to the leader ship, finally, the improved virtual obstacle potential field method is used for local collision avoidance, and the situation that the ship may enter the local minimum point position is improved by adding virtual obstacles; the experimental results show that the algorithm plans a route for the leader in the ship, which has advantages in time, smoothness and safety, and quickly forms a stable formation; for an N*N grid, the total arithmetic complexity of this method is O(NlogN), the total arithmetic complexity of Dijkstra algorithm and A* algorithm is O(N 2 ), so the running speed of this method is significantly improved compared with Dijkstra algorithm and A* algorithm; when the ship encounters an obstacle with collision risk, it can perform collision avoidance operation in time, and solves the problem of leaving the local minimum point which cannot be solved by the traditional artificial potential field method. BRIEF DESCRIPTION OF DRAWINGS

[0052] Figure 1 is a ship formation system diagram;

[0053] Figure 2 is an initial map constructed;

[0054] Figure 3 is a velocity potential map obtained by using the fast marching method;

[0055] Figure 4 is a route map obtained by using the fast marching method;

[0056] Figure 5 is a formation sailing route map obtained by using the present application;

[0057] Figure 6 is an initial map in experimental example 2;

[0058] Figure 7Error curve graph in Experimental Example 2;

[0059] Figure 8 Trajectory graph in Experimental Example 2;

[0060] Figure 9 Error curve graph of follower-3 in Experimental Example 2;

[0061] Figure 10 Error curve graph of follower-2 in Experimental Example 2;

[0062] Figure 11 Collision avoidance route graph in Experimental Example 2 with virtual obstacles introduced;

[0063] Figure 12 Initial map in Experimental Example 3;

[0064] Figure 13 Error curve graph in Experimental Example 3;

[0065] Figure 14 Collision avoidance route graph in Experimental Example 3. DETAILED DESCRIPTION

[0066] The present application is further illustrated by the following examples, which in no way limit the present application, and any modification or change made by those skilled in the art without departing from the technical solutions of the present application will fall within the scope of the claims of the present application.

[0067] Example 1

[0068] In Figure 2 , an initial map was generated by MATLAB, with the size of 200x 200 pixels, and the path planning step of the fast marching method was first entered:

[0069] Step 1: The environment was converted into a binary grid map, in which the black area represented obstacles such as land or islands, and the white area represented free space;

[0070] Step 2: Iteration was performed from the propagation surface of all cells considered as obstacles in the map, and a velocity potential map was obtained as shown in Figure 3 , the value in the grid represented the distance from the nearest obstacle, and the value range was 0-1, and the point with low value indicated that the current position might be too close to the obstacle, so the ship should sail in the area with high value;

[0071] Step 3: Iteration was again performed from the propagation surface of the end point in the map based on the velocity potential map W(x), and a time of arrival map was obtained;

[0072] Step4: Gradient descent is performed on the whole time of arrival map to obtain a path that takes into account the time of arrival, smoothness and safety, as shown in Figure 4

[0073] Then the fast marching method is combined with the formation control based on the Laplace matrix, and the formation is arranged to move towards the end point with a 60° angle between the two wings, and the follower keeps consistent with the leader. At this time, the formation control step based on the Laplace matrix is entered.

[0074] Step1: Calculate the in-degree matrix, weight matrix and Laplace matrix of the multi-ship formation system graph;

[0075] Step2: Calculate the relative error value of each ship relative to other ships;

[0076] Step3: The stability of the formation is achieved by eliminating the error; finally, the path curve in Figure 5 can be obtained.

[0077] Therefore, this method can realize the stability and consistency of the formation.

[0078] Example 2

[0079] When the leader-following method is used for the follower to track the path of the leader, if the distance between the tracked path and the obstacle is too close, the follower may have a collision risk, so the local collision avoidance problem needs to be solved.

[0080] As shown in Table 1, the initial information of the leader and each follower is given.

[0081] Table 1 Ship initial information

[0082]

[0083] The preset obstacle is indicated by a cross in Figure 6 , the coordinates of the obstacle point are (0.4, 0.8), (-0.5, 1), (3.1, 3.4), the collision radius of the obstacle is 50 meters, and the influence distance is 200 meters. The leader is represented by a red circle, and the followers are represented by circles of other colors. The leader travels according to the pre-planned path, which is specified as traveling to the target point (5, 5) in this experiment, and the remaining followers track the leader's movement in a pre-set formation (60° angle between the two wings, distance between adjacent ships on the same side is 0.2km).

[0084] From the relative error curve graph ( Figure 7 ) and the navigation route graph ( Figure 8 ), it can be seen that the follower has been tracking the leader's trajectory for the first 2 minutes and 30 seconds.​

[0085] As Figure 9 shown, at 2.7 min, follower-3 perceived the existence of the obstacle, at this time into the virtual obstacle potential field method of local obstacle avoidance steps:

[0086] Step1: input the current position of the ship, navigation speed, direction and other parameters and perceived obstacle information;

[0087] Step2: calculate the real-time repulsion and attraction of the ship through formula (4) (5) (6) (7);

[0088] Step3: determine whether the resultant force is 0, as shown in the figure, because the obstacle and the path tracking point are not on the same straight line, so the resultant force of the ship is always not 0, the motion direction is calculated through the real-time resultant force, repeat this step until it leaves the influence range of the obstacle;

[0089] Step4: at 3.2 min, the follower ship escapes from the collision risk and continues to sail according to the original trajectory.

[0090] Similarly, as Figure 10 shown, follower-2, at 4.6 min, felt the existence of the obstacle, and completed the collision avoidance operation at 5.2 min.

[0091] As Figure 11 shown, in the following sailing process, at about 12 min, follower-4 appeared in the same straight line with the obstacle and the path tracking point, at this time, the virtual obstacle potential field method of local obstacle avoidance Step3: at this time, a virtual obstacle is introduced in its left direction, which is represented by a red cross in the figure, the resultant force and motion direction of the ship are changed by increasing a virtual repulsive force, repeat this step until it leaves the influence range of the obstacle; Step4: at 12.4 min, the follower ship escapes from the collision risk and continues to sail according to the original trajectory. Finally, at about 14 min, all the followers reached the preset position and could maintain a stable structure with the leader after that.

[0092] Example 3

[0093] This experiment verifies the effectiveness when multiple obstacles appear on the tracking path when the follower tracks the path of the leader.

[0094] As shown in Table 2, the initial information of the leader and each follower is given.

[0095] As Figure 12 shown, the leader is represented by a black circle, the follower is represented by a circle of other colors, and the preset obstacle is represented by a cross.

[0096] Table 2 initial information of the ship

[0097]

[0098] The leader travels along a pre-planned path, which in this experiment is defined as traveling to the target point (9, 9), and the rest of the followers track the movement of the leader in a pre-set formation (two wings at an angle of 60°, and the distance between the two adjacent ships on the same side is 0.5 km).

[0099] From the relative error curve ( Figure 13 ) and the sailing route map ( Figure 14 ), it can be seen that the follower enters the multi-obstacle area at about 7.5 min and enters the collision avoidance state. Finally, the formation is restored at about 17 min.

[0100] Example 1 of the experiment

[0101] The results of the present application are accurate, which is proved by the following experiment;

[0102] As can be seen from Example 1, the path planned using the fast marching method takes into account time, smoothness and safety, so the route planned by the present application is more conducive to the sailing of the ship. The running time using the present application is 3 minutes and 40 seconds, and the running time using the A* algorithm is 16 minutes and 24 seconds. The present application reduces the running time by 12 minutes and 44 seconds.

[0103] As can be seen from Example 2, the virtual obstacle potential field method effectively solves the problem that the traditional artificial potential field method cannot solve. When the follower appears to be in a force balance, i.e., in a local minimum point, this method can effectively help it get out of the local minimum point by adding a virtual repulsive force, and the added virtual obstacle can ensure that the ship will not fall into this position again in the future.

[0104] Conclusion:

[0105] By using the fast marching method, a collision-free and smooth path is planned for the ship to sail when referring to, and the leader-following control method is used to achieve the effect of the follower tracking the trajectory of the leader and maintaining the formation. In view of the situation that the follower may encounter obstacles during sailing, a virtual obstacle potential field method is proposed to solve the problem, so that the follower can independently take collision avoidance behavior, and this method can effectively avoid the ship entering the position of the local minimum point, and finally return to the team using the consistency theory after completing independent collision avoidance.

Claims

1. A ship formation algorithm combining fast marching method and virtual obstacle potential field method, comprising the following steps: 1) path planning based on fast marching method The steps of fast marching method are as follows: Step1: model the sailing environment, convert the environment into a binary grid map, and mark the cells belonging to obstacles and the cells corresponding to passable areas respectively; Step2: Fast Marching Method, each cell marked as obstacle in the map is regarded as a wave source, and multiple waves are expanded, the result value of each cell in the map represents the time that the wave needs to reach the nearest obstacle, which is proportional to the distance from the obstacle; the potential map obtained in this step is the velocity potential, denoted as , each grid in the velocity potential map has a value representing the distance from the nearest obstacle, with a value range of 0-1; Step 3: In the potential diagram Based on this, the target point is treated as the sole wave source to ensure a global minimum is achieved as the wave expands across the map until it reaches the initial point; within each cell of the feasible region, the wave expansion velocity is calculated from the velocity potential map of the previous step. Extract from; Step4: start from the starting point of the ship, perform gradient descent on the entire arrival time map, and move to the global minimum value of the result map, to obtain a path considering arrival time, smoothness and safety; 2) formation control based on Laplace matrix For a leader-follower multi-ship formation system, each ship in the system can be viewed as a node in the network, and the information transmission links between ships can be viewed as edges connecting different nodes. The multi-ship system and its communication network can be modeled as follows: The directed graph represented, where This represents the N ships in the system. This indicates the information transmission process between ships; directed graph laplacian matrix of a directed graph is defined as: where is the in-degree matrix of the graph, whose diagonal elements are the sum of the in-degrees of the nodes; is the weight matrix of the graph, whose element is the weight of the edge if vertex is the head of the edge , and otherwise; Definitions is the current coordinate value for each vessel, is the desired relative position of two adjacent points; For each edge on the topological graph, we have: Point Relative to point The difference from the distance expectation value is: ; The difference of a point from the expected value for all points is: ; wherein, is a relative position gain coefficient; To enable the followers to achieve tracking of the leader trajectory, it is necessary to reduce the value of to 0 by eliminating the error amount achieving stability of the formation; 3) local obstacle avoidance based on virtual obstacle potential field method The real-time coordinate position of the ship is , the position of the target point is , and the gravitational field function is where the coefficient is the gain coefficient of the gravitational field, and the appropriate value is selected through experiments; Through the gradient relationship of gravity and gravity field, we have: The mathematical expression of the repulsive field is: wherein the coefficient is a gain coefficient of the repulsive field, is the distance of the ship to the obstacle, is the influence radius of the repulsive field, beyond which the repulsive force is zero; The expression of the repulsive force is: When the ship enters the position of the local minimum point, the repulsive force field function generated by the introduction of the virtual obstacle is: wherein, is a virtual repulsive potential field constant greater than zero, is a distance between the ship and the virtual obstacle, is an influence distance of the virtual obstacle on the ship; The virtual repulsive force is: Thus, the expression of the resultant force is: ； The application steps of the virtual obstacle potential field method in ship formation navigation are as follows: Step1: input the current position, sailing speed and direction parameters of each ship; Step2: determine whether the follower ship has entered the influence range of the obstacle, if it has entered the influence range of the obstacle, calculate the repulsive force and the attractive force, otherwise continue sailing; Step3: when the follower has been affected by the obstacle, judge whether the resultant force is 0; if it is 0, add a virtual obstacle and then recalculate the resultant force, judge the motion direction; if it is not 0, directly calculate the motion direction; repeat this step until it leaves the influence range of the obstacle; Step4: when the follower ship is out of the collision risk, continue sailing according to the original trajectory.

Images