An automatic collision avoidance method for unmanned boats based on probabilistic game theory framework

Abstract

The present invention provides an automatic collision avoidance method for unmanned boats based on a probabilistic game theory framework, which relates to the field of unmanned boats and includes the following steps: establishing a probability distribution model, discretizing the probability map, calculating the absolute position of the boat, and designing a collision avoidance plan. The design of the avoidance plan includes the following steps: collecting the navigation state, speed, and heading of the target boat, calculating all possible positions and probabilities of the target boat at the next moment, calculating all possible forward speeds and heading angles, and calculating the cost function Cos[u a (t)] at the minimum value of u a (t), transmitting u a (t) to the unmanned boat controller, and controlling the unmanned boat to navigate according to u a (t). It is assumed that the behavior of the target ship is random, which is more in line with the actual complex scenario. The cost function of local minimum search and the method of discretizing the probability map do not require too much computing power and are easy to apply in practice, and can easily handle the collision avoidance of multiple random target boats.

Description

Technical Field

[0001] The invention relates to the field of unmanned boats, and in particular to an automatic collision avoidance method for unmanned boats based on a probability game theory framework. Background Art

[0002] Unmanned surface vessels are used in various fields because of their flexibility and strong autonomy, especially in busy ports, docks and other waters. Unmanned surface vessels can realize autonomous garbage cleaning, patrol and early warning functions, which are more efficient and safer than manned vessels. When working in busy ports, docks and other waters, autonomous collision avoidance of unmanned surface vessels is one of the key technologies for realizing intelligent navigation.

[0003] In the existing technology, the commonly used automatic collision avoidance methods for unmanned boats include speed obstacle method, dynamic window method, artificial potential field method, etc.

[0004] The basic principle of the speed obstacle method is to generate a conical obstacle area in the speed space. As long as the speed vector of the unmanned boat is outside the VO, it will not collide with the other ship. This method is mostly limited to single-target collision avoidance, and rarely considers multiple random moving targets. It is obviously not suitable for busy ports, docks and other waters.

[0005] The basic idea of ​​the dynamic window method is to discretize the linear velocity and angular velocity of the current velocity space of the unmanned boat, form different linear velocities and angular velocities into a sample point, and estimate all possible trajectories of the unmanned boat in a very short time based on the selected sample points, and then perform collision detection on the estimated trajectory, eliminate undesirable sample points, and finally use the distance from the target point, the distance from the obstacle, the speed change and other characteristics to construct an objective function, thereby evaluating the feasible speed combination, and selecting the optimal speed combination without collision as the decision target of the next stage of the unmanned boat. Although this method optimizes the calculation speed, it has poor flexibility in complex environments.

[0006] The basic principle of the artificial potential field method is to virtualize the geographical space where the unmanned boat navigates into an artificial potential field. The target point generates a gravitational potential field in the entire space, and obstacles generate a repulsive potential field in the surrounding space. The total combined potential field is formed by the superposition of the gravitational potential field and the repulsive potential field. The unmanned boat in the combined potential field moves toward the target point through the gravitational potential field, and avoids obstacles by relying on the repulsive potential field generated by the obstacles, and finally reaches the target point without collision. However, this method has local minima and jitter phenomena.

[0007] There are many moving ships in busy waters, and the collision avoidance behavior between ships often depends on the driver's experience and random judgment. Their navigation behavior has a very large randomness, and traditional collision avoidance strategies are very difficult to deal with. Summary of the invention

[0008] The purpose of the present invention is to provide an automatic collision avoidance method for an unmanned boat based on a probabilistic game theory framework, so as to improve the practicality of the collision avoidance method in busy waters.

[0009] To achieve the above object, the present invention provides the following technical solutions:

[0010] An automatic collision avoidance method for an unmanned boat based on a probabilistic game theory framework comprises the following steps:

[0011] S1. Establish a probability distribution model;

[0012] S2. Discretize the probability map;

[0013] S3. Calculate the absolute position of the vessel;

[0014] S4. Design a collision avoidance plan, including the following steps:

[0015] 1) Collect the navigation status, speed and heading of the target ship;

[0016] 2) Calculate all possible positions and probabilities of the target ship at the next moment. The formula is as follows: p[c ij (t+1)y(t)]=p[c i (t+1)y(t)·p[c j (t+1)y(t)];y cartes (c ij ,t+1)=y cartes (t)+△y cartes (c ij ,t)

[0017] 3) Calculate all possible forward speeds and heading angles,

[0018] 4) Calculate the cost function Cos[u a (t)] when u is minimum a (t), the cost function is as follows

[0019] 5) will u a (t) is transmitted to the unmanned boat controller, which controls the unmanned boat according to u a (t) navigation;

[0020] 6) Return to step 1).

[0021] Preferably, in the probability distribution model established in S1, it is assumed that the heading angle and speed of the target ship are random within a certain range. Due to the existence of physical constraints, the heading angle and speed of the target ship change slowly and no sudden changes are possible. The probability distribution of the heading angle is defined as a normal distribution, and the formula is as follows: in is the heading angle, is the variance of the heading angle;

[0022] The probability distribution of speed is as follows: Where u represents the ship's forward speed, σ u is the forward speed variance, U crs is the cruising speed.

[0023] Preferably, the probability map is discretized in S2. Based on the collision avoidance of probability game theory, the map is discretized and the map discretization is performed only for the probability area. For the unmanned boat or the target ship, there are the following physical constraints. Its speed has an upper limit, which is defined as U max , the heading angle will not change suddenly in a short time Δt, that is

[0024] Based on the above two constraints, assuming that the position of the ship at time t on the map is y(t), then at t+1, the position of the ship y(t+1) must be in the fan-shaped area with y(t) as the center, and its polar coordinate form is: At time t+1, the probability distribution of the ship's position y(t+1) is: Divide the radial distance of the sector area into n equal parts, and divide the span angle into m equal parts, to obtain n×m units. Define the discrete unit as C ij , then the conditional probability distribution of the unit at time t+1 is:

[0025] p[C ij (t+1)y(t)]=p[C i (t+1)y(t)·p[C j (t+1)y(t)]

[0026] where i∈{1...n}, j∈{1...m}, C i (t+1) represents any cell in the i-th row, C j (t+1) represents any cell in the jth column. Combining the above formula, we get the following formula:

[0027]

[0028]

[0029] Multiplying the above equations gives us the conditional probability distribution.

[0030] Preferably, the discretized area is a sector area with the position of the ship at time t as the center. To achieve collision avoidance of the unmanned boat, it is necessary to establish absolute coordinates. In S3, the position of the cell is set to be the polar vector relative to the position at time t with their center as the coordinate: Converting to Cartesian coordinate system gives: Then the ship is in cell C ij Absolute position: y cartes (C ij ,t)=y cartes (t)+Δy cartes (C ij ,t), so the ship position is discretized and obtained t+1 The probability of the position at a given moment.

[0031] Preferably, in S4, the unmanned boat obtains the position and heading of the target vessel through sensors such as radar, and makes a decision based on the collision avoidance strategy to guide the movement of the unmanned boat at the next time t+1.

[0032] Compared with the prior art, the present invention has the following beneficial effects:

[0033] 1. This method assumes that the behavior of the target ship is random, which is more in line with the actual complex scenarios;

[0034] 2. The cost function of local minimum search and the probability map discretization method do not require much computing power and are easy to apply in practice;

[0035] 3. Can easily handle collision avoidance of multiple random target ships. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 The fan-shaped diagram of the position distribution of the unmanned boat at the next moment;

[0037] Figure 2 This is a schematic diagram of simulation results of Example 1 of the present invention;

[0038] Figure 3 This is a schematic diagram of simulation results of Example 2 of the present invention;

[0039] Figure 4 Schematic diagram of simulation results of Example 3 of the present invention. DETAILED DESCRIPTION

[0040] (1) Establish a probability distribution model for the next position of the unmanned boat

[0041] like Figure 1 As shown in the figure, from a probability perspective, the next position of the unmanned boat is located in a certain fan-shaped area in front. Assuming that the heading angle and speed of the target ship are random within a certain range, due to physical constraints, the heading angle and speed of the target ship change slowly and no sudden changes are possible. The probability distribution of the heading angle is defined as a normal distribution, and the formula is as follows: in is the heading angle, is the variance of the heading angle; similarly, the probability distribution of the speed can be defined as follows Where u represents the ship's forward speed, σ u is the forward speed variance, U crs is the cruising speed;

[0042] (2) Discretize the probability map

[0043] Collision avoidance based on probabilistic game theory requires discretization of the map. This method only discretizes the map for the probability area, which can reduce the difficulty of calculation. For unmanned boats or target ships, there are the following physical constraints:

[0044] a. Its speed has an upper limit, defined as U max ;

[0045] b. The heading angle will not produce a sudden change in a short time Δt, that is,

[0046] Based on the above two constraints, assuming that the position of the ship at time t on the map is y(t), then at t+1, the position of the ship y(t+1) must be in the fan-shaped area with y(t) as the center, and its polar coordinate form is: At time t+1, the probability distribution of the ship's position y(t+1) is: Divide the radial distance of the sector area into n equal parts and the span angle into m equal parts to obtain n×m units, such as Figure 1 As shown in the figure, the position of the ship at the next moment y(t+1) is successfully discretized, and the discrete unit is defined as C ij , then the conditional probability distribution of the unit at time t+1 is p[C ij (t+1)|y(t)]=p[C i (t+1)|y(t)·p[C j (t+1)|y(t)], where i∈{1...n}, j∈{1...m}, C i (t+1) represents any cell in the i-th row, C j (t+1) represents any cell in the jth column. Combining the above formula, we get the following formula:

[0047]

[0048] Multiplying the above two formulas together, we can get the conditional probability distribution of any unit.

[0049] (3) Calculate the absolute position of the ship

[0050] The discretized area is a fan-shaped area with the ship position at time t as the center. To achieve collision avoidance of the unmanned boat, it is necessary to establish absolute coordinates. The position of the cell is assumed to be the polar vector relative to the position at time t with their center as the coordinate: Converting to Cartesian coordinate system gives: Then the ship is in cell C ij Absolute position: y cartes (C ij ,t)=y cartes (t)+Δy cartes (C ij ,t), so far the ship position is discretized and the probability of the position at time t+1 is obtained, and then the collision avoidance strategy is designed;

[0051] (4) Collision avoidance strategy

[0052] In S4, the unmanned boat obtains the position and heading of the target ship through sensors such as radar, and makes a decision based on the collision avoidance strategy to guide the movement of the unmanned boat at the next time t+1 to achieve the purpose of collision avoidance. The forward speed and heading angle of the unmanned boat at time t are defined as Define the following cost function:

[0053] Where W1 and W2 represent weights, and D() represents the distance between two points. The first term of the cost function uses the distance between the unmanned boat and the target ship, multiplied by the probability of all discrete points on the target boat's map, taking all possible combinations into account, and then finding the inverse. The second term represents the distance between the unmanned boat's own position and its expected position. Based on the above probability game theory and the cost function, the unmanned boat collision avoidance strategy process is as follows:

[0054] Step 1: Collect the navigation status, speed and heading of the target ship;

[0055] Step 2: Calculate all possible positions and probabilities of the target ship at the next moment;

[0056] Step 3: Calculate all possible u a (t)];

[0057] Step 4: Calculate the cost function Cos[u a (t)] when u is minimum a (t)];

[0058] Step 5: Put u a (t)] is transmitted to the unmanned boat controller, and the unmanned boat is controlled according to u a (t)] navigation;

[0059] Step 6: Go back to step 1

[0060] (5) Simulation verification

[0061] Assume that when the unmanned boat is sailing, there are two randomly sailing boats around it. Their motion parameters are shown in Table 1. The sampling time interval is 2 seconds. Ship C is regarded as the active ship and the other two ships are regarded as passive ships. Three different collision avoidance situations are studied.

[0062]

[0063]

[0064] Table 1

[0065] Example 1

[0066] The unmanned boat and the target ship are sailing in opposite directions

[0067] The unmanned boat sails from the position (0, 0) to the positive direction of X, and the target ship sails randomly from the position (1500, 30) to the negative direction of X. From the simulation results, Figure 2 , the unmanned boat achieved effective collision avoidance.

[0068] Example 2

[0069] The unmanned boat sails in the same direction as the target ship

[0070] The unmanned boat sails from the position (0, 0) to the positive direction of X, and the target ship sails randomly from the position (200, 30) to the positive direction of X. From the simulation results, Figure 3 Although the target ship interfered with the unmanned boat at a distance of about 700 meters, the unmanned boat was able to effectively avoid collision with it.

[0071] Example 3

[0072] The unmanned boat evaded two target ships

[0073] The unmanned boat sails from position (0, 0) to the positive direction of X, the target ship A sails randomly from position (200, 30), and the target ship B sails randomly from position (1000, -20). Figure 4 As shown in the figure, the unmanned boat achieved effective collision avoidance of two random target ships while sailing.

[0074] It can be seen from the simulation results that under the collision avoidance method based on the probabilistic game theory framework involved in the present invention, the unmanned boat can achieve effective collision avoidance of random ships.

Claims

1. An automatic collision avoidance method for unmanned boats based on a probabilistic game theory framework, characterized in that: The steps include: S1. Establish a probability distribution model; Assuming that the heading angle and speed of the target ship are random within a certain range, due to physical constraints, the heading angle and speed of the target ship change slowly and cannot change suddenly. The probability distribution of the heading angle is defined as a normal distribution, and the formula is as follows: in is the heading angle, is the variance of the heading angle; The probability distribution of speed is as follows: Where u represents the ship's forward speed, σ u is the forward speed variance, U crs is the cruising speed; S2. Discretize the probability map; Based on the collision avoidance of probability game theory, the map is discretized and the map discretization is only carried out for the probability area. For the unmanned boat or the target ship, there are the following physical constraints. Its speed has an upper limit, which is defined as U max , the heading angle will not change suddenly in a short time Δt, that is Based on the above two constraints, assuming that the position of the ship at time t on the map is y(t), then at t+1, the position of the ship y(t+1) must be in the fan-shaped area with y(t) as the center, and its polar coordinate form is: At time t+1, the probability distribution of the ship's position y(t+1) is: Divide the radial distance of the sector area into n equal parts, and divide the span angle into m equal parts, to obtain n×m units. Define the discrete unit as C ij , then the conditional probability distribution of the unit at time t+1 is: p[C ij (t+1)|y(t)]=p[C i (t+1)|y(t)]·p[C j (t+1)|y(t)] where i∈{1...n}, j∈{1...m}, C i (t+1) represents any cell in the i-th row, C j (t+1) represents any cell in the jth column. Combining the above formula, we get the following formula: Multiply the above formula to get the conditional probability distribution; S3. Calculate the absolute position of the vessel; The discretized area is a fan-shaped area with the ship position at time t as the center. To achieve collision avoidance of the unmanned boat, it is necessary to establish absolute coordinates. In S3, the position of the cell is set to be the polar vector relative to the position at the time with the center of the circle as the coordinate: Converting to Cartesian coordinate system gives: Then the ship is in cell C ij Absolute position: y cartes (C ij ,t)=y cartes (t)+Δy cartes (C ij ,t), so that the ship position is discretized and the probability of the position at time t+1 is obtained; S4. Design a collision avoidance plan, including the following steps: 1) Collect the navigation status, speed and heading of the target ship; 2) Calculate all possible positions and probabilities of the target ship at the next moment. The formula is as follows: p[c ij (t+1)|y(t)]=p[c i (t+1)|y(t)]·p[c j (t+1)|y(t)];y cartes (c ij ,t+1)=y cartes (t)+△y cartes (c ij ,t) 3) Calculate all possible forward speeds and heading angles, 4) Calculate the cost function Cos[u a (t)] when u is minimum a (t), the unmanned boat in S4 obtains the position and heading of the target ship through the radar sensor, and makes a decision based on the collision avoidance strategy to guide the movement of the unmanned boat at the next time t+1 to achieve the purpose of collision avoidance. The forward speed and heading angle of the unmanned boat at time t are defined as The cost function is as follows Where W1 and W2 represent weights, and D( ) represents the distance between two points; 5) will u a (t) is transmitted to the unmanned boat controller, which controls the unmanned boat according to u a (t) navigation; 6) Return to step 1).

Images