A collision avoidance path planning method for intelligent ships based on uncertain speed obstacles

Abstract

The present invention proposes a rule-aware dynamic path planning method based on uncertain speed obstacles. The proposed method solves the priority problem in the decision-making process of multi-ship encounter scenarios. The present invention calculates the collision risk of ships in real time to activate the collision avoidance strategy. In addition, considering the encounter scenarios of crossing, overtaking and encountering, the idea of ​​robust pipeline is used to deal with the uncertainty in the target ship information. In the presence of uncertainty, situations that comply with COLREGs are also achieved in various encounter scenarios with multiple ships. Simulation experiments and AIS data verification show that this method enables intelligent ships to make safe decisions and avoid collisions in complex marine environments.

Description

Technical Field

[0001] The invention belongs to the field of intelligent ship path planning, and relates to an intelligent ship collision avoidance path planning method based on uncertain speed obstacles. Background Art

[0002] The collision-free local path planning of intelligent ships has real-time requirements. The potential field method based on attractive and repulsive forces used in the prior art for local path planning is at risk of falling into a local minimum. When there are obstacles near the target point, a vector field histogram algorithm is proposed in the prior art. However, the vector field histogram method is difficult to achieve robustness in complex environments. Velocity space methods such as dynamic window methods, velocity obstacles and their variants are also proposed in the prior art for online processing of collision avoidance problems. These methods are suitable for real and complex environments, and can avoid possible collisions in velocity space by selecting feasible speeds within dynamic constraints.

[0003] However, most of the above-mentioned prior art works only focus on the efficiency and feasibility of the path, and do not consider path planning in combination with the International Regulations for Preventing Collisions at Sea (COLREGs). Some researchers have proposed some methods to improve the above-mentioned work so that ship maneuvers obey COLREGs, such as fuzzy logic, interval programming, and trajectory algorithms based on local normal distribution. However, these methods are difficult to apply to multi-ship encounter scenarios where multiple rules need to be met simultaneously. To this end, an improved artificial potential field method is also proposed in the prior art to deal with situations involving multiple ships, and the behavior constrained by COLREGs is realized by adding appropriate functions and safety requirements to the corresponding virtual forces. The prior art also proposes a ship collision avoidance method based on model predictive control to meet the requirements of COLREGs by constraining optional control behaviors. However, in complex marine environments, the acquired information used by the decision-making system is often uncertain and noisy. This brings further challenges to the design of navigation systems.

[0004] Although COLREGs establish rules for how to maneuver to avoid ship collisions in general, they do not provide quantitative standards for when to initiate procedures and how to maneuver at the operational level. At the same time, in multi-ship encounter scenarios, the collision risks of the own ship and multiple target ships are different. In most studies, real-time ship collision risks are usually monitored by calculating the closest point of approach (CPA), such as the distance to the closest point of approach (DCPA), and / or the time to the closest point of approach (TCPA). Some methods define collision risks by setting ship safety areas and conflict between ships. A funnel library method is proposed by considering the uncertainty of collision risk, which enables robots to perform real-time planning in an environment with complex geometric constraints. Fuzzy logic methods are also widely used for risk assessment between ships, extending rule-based methods by considering different influencing parameters as input membership functions of risk. However, in previous methods, most studies did not explicitly and effectively consider the dynamic collision risk changes between the own ship and the target ship. Summary of the invention

[0005] In order to overcome the problem that there is no good unification of rules and risks in motion planning in existing intelligent ship research, the present invention proposes a collision avoidance path planning method for intelligent ships based on uncertain speed obstacles. The present invention takes into account collision risks, rule requirements and sensor uncertainties at the same time, and first proposes a dynamic risk assessment step based on input fuzzy membership functions to monitor the risks of complex scenes. The risk value is then used to activate the collision avoidance operation. Then, a dynamic path planning step based on uncertain VO that complies with the COLREGs rule is proposed. In addition, the relative cone angle is systematically expanded using the robust funnel idea to deal with the probabilistic uncertainty in the target ship being measured. The method of the present invention can not only deal with the risks and uncertainties of intelligent ship motion planning, but also perform path planning and obstacle avoidance on the basis of meeting the International Regulations for Preventing Collisions at Sea.

[0006] The technical solution of the present invention is as follows:

[0007] The present invention provides a method for intelligent ship collision avoidance path planning based on uncertain speed obstacles, which comprises the following steps:

[0008] S1.Collision risk assessment of multi-ship encounter scenarios;

[0009] Select the time of the most recent meeting T CPA 、The closest encounter distance D CPA , relative distance, relative orientation and ship speed ratio are taken as the most relevant parameters for the collision risk between the own ship and the target ship, and the collision risk index CRI between the own ship and any target ship i is calculated through the fuzzy membership function of each parameter i , the maximum CRI iThe value is compared with the safety CRI threshold. If it is higher than the threshold, S2 is executed, otherwise the collision risk assessment continues;

[0010] S2. Perform rule-aware dynamic collision-free path planning, including S21-S23;

[0011] S21. Collision-free path planning using VO

[0012] Let subscript o represent the own ship, i = 1, 2, ..., n represent n target ships; let p o =[x o ,y o ] T , p i =[x i ,y i ] T Indicates the positions of own ship and target ship on the two-dimensional ocean surface, Respectively represent the velocity vectors of the own ship and the target ship on the two-dimensional ocean surface,

[0013] Defining VO o|i is the speed set that causes the ship o to collide with the target ship i. The condition for the ship to avoid collision with the target ship i is

[0014]

[0015] D(p i , r o +r i ) is a i The center is r and the radius is o +r i The open disk, r o and r i are the radii of the own ship and the target ship respectively; for all target ships i=1, 2, ..., n, the speed obstacle set in the multi-ship encounter scenario is The ship must sail at a speed outside this setting range to ensure safety;

[0016] S22. Robust collision-free path planning with uncertain VO

[0017] Considering the uncertainty of the speed and position of the target ship, it is assumed that the radius, speed and position of the target ship have uncertainties that obey the Gaussian distribution. in is the corresponding mean value; is the corresponding variance or covariance matrix; δ· represents the corresponding standard value; diag represents the diagonal matrix; the deterministic VO in S21 o|i Expanded to uncertain VO(UVO):

[0018]

[0019] in:

[0020]

[0021]

[0022]

[0023] For all target ships i=1, 2, ..., n, the uncertain speed obstacle set in the multi-ship encounter scenario is The ship sails at a speed not within the joint assembly to ensure navigation safety;

[0024] S23. Rule-aware robust collision-free path planning

[0025] Plan the path to avoid collision with other ships while complying with COLREGs rules.

[0026] Compared with the prior art, the beneficial effects of the present invention include:

[0027] The rule-aware dynamic path planning method based on uncertain speed obstacles proposed in the present invention solves the priority problem in the decision-making process of multi-ship encounter scenarios. The present invention calculates the collision risk of ships in real time to activate the collision avoidance strategy. In addition, considering the crossing, overtaking and head-on encounter scenarios, the idea of ​​robust pipeline is used to deal with the uncertainty in the target ship information. In the presence of uncertainty, safe path planning in accordance with COLREGs is also implemented in various encounter scenarios with multiple ships. This method enables intelligent ships to make safe decisions and avoid collisions in complex marine environments. In various complex scenarios following COLREGs, the collision risk and safety of path planning are guaranteed, which means that there is great potential for practical application in intelligent ships. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] Figure 1 It is a schematic diagram of deterministic speed barrier;

[0029] Figure 1 In the equation, 1-the position of target ship 1, 2-the speed of target ship 1, 3-the sum of the radius of target ship 1 and own ship, 4-the determined speed obstacle of target ship 1, 5-the position of target ship 2, 6-the speed of target ship 2, 7-the determined speed obstacle of target ship 2, 8-the position of own ship, 9-the speed of own ship, 10-the translation speed of target ship 1, 11-the translation speed of target ship 2.

[0030] Figure 2 It is the principle diagram of the uncertainty speed barrier of the present invention.

[0031] Figure 2 In it, 1-the position of target ship 1, 2-the uncertainty of the position of target ship 1, 3-the uncertainty variance of the radius of target ship 1, 4-the sum of the uncertain radius of target ship 1 and the radius of own ship, 5-the uncertainty of the speed of target ship 1, 6-the uncertain speed obstacle between target ship 1 and own ship, 7-the position of target ship 2, 8-the uncertainty of the position of target ship 2, 9-the uncertainty variance of the radius of target ship 2, 10-the sum of the uncertain radius of target ship 2 and the radius of own ship, 11-the uncertainty of the speed of target ship 2, 12-the uncertain speed obstacle between target ship 2 and own ship, 13-the translation speed of target ship 1, 14-the translation speed of target ship 2, 15-the cone point of the uncertain speed obstacle between target ship 2 and own ship, 16-the cone point of the uncertain speed obstacle between target ship 1 and own ship, 17-the reachable set of left turn speeds corresponding to the uncertain speed obstacle, 18-the reachable set of right turn speeds corresponding to the uncertain speed obstacle, 19-the reachable set of speeds away from the target corresponding to the uncertain speed obstacle.

[0032] Figure 3 This is the USV path planning under COLREGs and the uncertain VO algorithm in the overtaking scenario. The left side of the figure shows the uncertain path of the target ship, and the right side shows the planned path of the own ship.

[0033] Figure 4 is a relative distance diagram of USV path planning in the coordinate system. (a) The distance between the own ship and the target ship without considering the uncertainty. (b) Considering the uncertainty of the target ship, the distance between the own ship and the target ship is calculated using the uncertainty speed barrier method.

[0034] Figure 5 Multi-target ship encounter scenario 1 under COLREGs and uncertain speed obstacle algorithm. The three target ships have uncertainties in radius, speed and position. From top to bottom, left to right: (a) the initial stage of the four ships; (b) maneuvering in the overtaking situation; (c) maneuvering in the overtaking and encounter situations; (d) the overall maneuvering path of the own ship and the target ship.

[0035] Figure 6 Real-time collision risk index in scenario 1.

[0036] Fig. 7 Relative distance of USV path planning under COLREGs in scenario 1. (a) The distance between own ship and target ship without considering the uncertainty. (b) The distance between own ship and target ship is calculated using the uncertain VO method considering the uncertainty of the target ship.

[0037] Figure 8Scenario 2 of multi-target ship encounters under COLREGs and the uncertain speed obstacle algorithm. From top to bottom, left to right: (a) the initial stage of the four ships; (b) maneuvering in the overtaking situation; (c) maneuvering in the overtaking, crossing and encounter situations; (d) the overall maneuvering paths of the own ship and the target ship.

[0038] Fig. 9 Real-time collision risk index in scenario 2.

[0039] Fig. 10 Relative distance of USV path planning under COLREGs in scenario 2. (a) The distance between the own ship and the target ship without considering the uncertainty. (b) The distance between the own ship and the target ship is calculated using the uncertain VO method considering the uncertainty of the target ship.

[0040] Fig.11 The obstacle avoidance trajectory of the ship in the Wuhan section of the Yangtze River simulated based on AIS data. From left to right: (a) the initial stage of the ship; (b) two maneuvers on the starboard side; (c) the last maneuver in the overtaking situation. DETAILED DESCRIPTION

[0041] The present invention is further described and illustrated below in conjunction with specific embodiments. The embodiments are merely exemplary of the present disclosure and do not define the scope of limitation. The technical features of each embodiment of the present invention may be combined accordingly without conflicting with each other.

[0042] This embodiment proposes a rule-aware dynamic path planning method for intelligent ships based on uncertain speed obstacles. In particular, we first propose a dynamic risk assessment method based on input fuzzy membership functions to monitor the risks of complex scenarios. The risk value is then used to activate collision avoidance operations. Then, a dynamic path planning method based on uncertain VO that complies with COLREGs rules is proposed. Considering different encounter scenarios, different subsets of the reachable speed set in traditional VO are designed to meet the different requirements of COLREGs. In addition, the relative cone angle is systematically expanded using a robust funnel idea to handle the probabilistic uncertainty in the target ship under test. It specifically includes the following steps:

[0043] S1 Collision risk assessment for multi-ship encounter scenarios

[0044] The dynamic collision risk between the own ship and the target ship needs to be evaluated in real time to determine how to maneuver the ship. CPA 、The closest encounter distance D CPA, relative distance, relative orientation and ship speed ratio are considered to be most relevant to determine the collision risk. Then, a fuzzy membership function is proposed to calculate the collision risk index (CRI) between the own ship and all encountered target ships. The maximum CRI value is compared with the safe CRI threshold as an activation condition in regulating the perceptual dynamic motion planning method.

[0045] Let subscript o represent the own ship, i=1, 2, ..., n represent the n target ships. o =[x o ,y o ] T , p i =[x i ,y i ] T and Denote the position and velocity vectors of own ship and target ship on the two-dimensional ocean surface respectively. Then, the CPAs between own ship and target ship are calculated as follows:

[0046]

[0047]

[0048] where ||·|| is the Euclidean norm. Then calculate The risk membership function is:

[0049]

[0050] Among them, T0 is the shortest safe time to the nearest approach point, and T1 is the sufficient safe time to the nearest approach point. It has a great impact on the risk of ship collision. According to (3), The larger it is, the less dangerous other ships are to your ship. Parameters T0 and T1 can be set according to the size of your ship.

[0051] The risk membership function of is calculated as follows:

[0052]

[0053] Where D0 is the minimum safe distance to the nearest approach point, and D1 is the sufficient safe distance to the nearest approach point. It has a great impact on the risk of ship collision. According to formula (4), The larger it is, the less dangerous the ship is to the own ship. Parameters D0 and D1 are also set according to the actual size of the own ship.

[0054] In some scenarios, the use of the CPA method can effectively assess the risk of ship collision. However, in complex multi-ship encounter scenarios considering COLREGs, it is unreliable to use only the CPA value for pre-collision checks. For example, for the previous encounter of the ship, the own ship and the target ship are close to each other. The target ship on the right side of the own ship's centerline is more dangerous than the target ship on the left side, and the relative azimuth needs to be considered when calculating the collision risk. Generally speaking, the situation of cross encounter is more dangerous than the situation of head-on encounter. Therefore, in addition to the conventional TCPA and DCPA, the relative distance, relative azimuth and ship speed ratio are also considered when calculating the collision risk.

[0055] Specifically, the risk membership function of the relative distance is calculated as:

[0056]

[0057] Where S i is the relative distance between the own ship and the target ship i. According to formula (5), the closer the relative positions of the two ships are, the higher the risk of collision between the two ships. The values ​​of S0 and S1 are set differently according to the actual situation encountered.

[0058] Target ships in different directions pose different degrees of danger to the ship. According to collision avoidance experience, when the approaching ship is at about 19°, the collision risk of the ship is the greatest. When the approaching ship is at about 199°, the collision risk is the lowest. Therefore, the membership function of the relative orientation of the target ship is established as follows:

[0059]

[0060] Where B0 is the threshold of the target ship's parameter bearing. If B0 = 19°, when B i =19°, relative azimuth risk This is the most dangerous relative position. Generally speaking, the risk on the starboard side is greater than the risk on the port side, and the risk on the front side is greater than the risk on the rear side.

[0061] Similarly, the ship speed ratio membership function is established as follows:

[0062]

[0063] In the formula, the ship speed ratio is defined as K0 is usually set to 1. i = 1, the two ships have the same speed. It is the dividing line between safety and danger.

[0064] Then, for a specific target ship i, we define the collision risk vector as

[0065]

[0066] To balance the impact of each of the five factors, define the weight vector

[0067] W T =[W TCPA , W DCPA , W S , W B , W K ] T , (9)

[0068] in Then, the CRI value of target ship i is calculated as:

[0069] CRI i =W T R i (10)

[0070] Indicates that the range of CRI values ​​is between [0-1]. In a multi-ship encounter scenario, the overall collision risk is determined by the most dangerous target ship, i.e.

[0071] CRI=max{CRI i}, i = 1, ..., n (11)

[0072] This CRI value is then compared to a safety threshold to activate the vessel’s collision avoidance maneuvers.

[0073] S2. Rule-aware dynamic collision-free path planning:

[0074] In S1, the collision risk of the ship's heading is evaluated. When the CRI value is higher than the safety threshold, the collision avoidance operation needs to be activated. This step proposes a collision-free path planning method in a multi-ship encounter scenario based on VO. The VO method generates a cone obstacle in the velocity space based on the speeds of multiple moving ships for dynamic collision avoidance. In addition, considering the uncertainty that is prevalent in the measurement information, the collision cone in the traditional VO is expanded. An uncertain VO path planning method based on the idea of ​​a robust pipeline is proposed. At a certain confidence level (p r ), the probability impact is uniformly considered to achieve robust collision-free path planning that complies with COLREGs. S2 specifically includes S2.1-S2.3.

[0075] S2.1 Collision-free path planning using VO

[0076] The basic idea of ​​VO is to transform the general dynamic obstacle avoidance problem into a static velocity reachability problem by defining a cone obstacle in velocity space. Figure 1Specifically, the encounters are observed for the head-on encounter (target ship 1) and the crossing from the right (target ship 2). Both the own ship and the target ship are defined as disks. The diameter of the disk is set to the length of the ship.

[0077] Figure 1 In the example, the determined speed obstacle of target ship 1 is the speed set that causes the own ship to collide with target ship 1. Therefore, the condition for the own ship to avoid collision with target ship 1 is the speed of the own ship. Determined speed obstacle for target ship 1. For each target ship, the radius of the dashed circle is the sum of the radii of own ship and target ship. The solid line forms a collision cone of relative speed between own ship and target ship. The dashed line forms a collision cone of absolute speed by taking into account the target ship speed translation. Therefore, a collision will result when the own ship's speed is inside the dashed cone, or equivalently when the relative speed is inside the solid cone.

[0078] More specifically, in Figure 1 In, VO o｜i , i = 1, 2 is the speed set that causes the own ship o to collide with the target ship i. Therefore, the condition for the own ship to avoid collision with the target ship i is For each target ship, the radius of the dotted circle is the sum of the radii of the own ship and the target ship. The solid line forms the collision cone of relative speed between the own ship and the target ship. The dotted line forms the collision cone of absolute speed by taking into account the translation cone of the ship speed. Therefore, when the speed v o When inside the dotted cone, or equivalently, when the relative velocity v o -v i When inside the solid cone, it causes a collision.

[0079] Mathematically, let λ(p, v) = {p + tv | t ≥ 0} be a ray starting from p and heading towards v. Defined as represents the Minkowski sum of the radii of the own ship and the target ship. Define -O = {-o|o∈O} as the reflection. Then,

[0080]

[0081] is the set of speed obstacle solutions for the own ship in the multi-ship encounter scenario. Intuitively, (12) means that if the own ship is traveling at v o ∈VO o|i If they travel at a speed of , a collision will occur in the future.

[0082] Since both the host ship and the target ship are simplified as disks, equation (12) can be simplified as

[0083]

[0084] It starts at p o , and D(p i , r o +r i ) is a i The center is r and the radius is o +r i Open disk; r o and r i are the radii of the own ship and the target ship respectively. For all target ships i=1, 2, ..., n, the speed obstacle set in the multi-ship encounter scenario is The ship must sail at a speed outside this setting range to ensure safety.

[0085] S2.2 Robust collision-free path planning with uncertain VO

[0086] It can be seen from formula (13) that in order to calculate the speed obstacle, the speed and position of the target ship need to be known. However, in a complex marine environment, this measurement information is usually uncertain and probabilistic. On the one hand, the information measured by on-board sensors such as lidar and cameras may be noisy; on the other hand, the movement of the target ship may be affected by environmental interference such as wind, waves and currents. The uncertainty of the speed and position of the target ship may make the algorithm infeasible, thereby causing serious safety issues in multi-ship encounter scenarios. Therefore, we next propose a robust collision-free path planning method based on uncertain VO

[0087] Assume that the radius, speed and position of the target ship have uncertainties that follow Gaussian distribution. Specifically, in is the corresponding mean value; is the corresponding variance or covariance matrix, δ· represents the corresponding standard value; diag represents the diagonal matrix. Then, the deterministic VO in (13) is expanded to

[0088]

[0089] in:

[0090]

[0091]

[0092]

[0093] Since these random uncertainties have infinite cases, it is impossible and unnecessary to consider these extreme conditions. Therefore, the present invention only considers r Collision avoidance.

[0094] like Figure 2 As shown, the uncertainty of the position of the target ship 1, the sum of the uncertain radius of the target ship 1 and the radius of the own ship, and the uncertainty of the speed of the target ship 1 together constitute the uncertain speed obstacle between the target ship 1 and the own ship; the same is true for the own ship and the target ship 2. When the own ship and the target ship are in a situation of encounter, overtaking, or crossing under the International Regulations for Preventing Collisions at Sea, COLREG requires that the own ship is responsible for operating to avoid obstacles and avoid collision. Therefore, for example, assuming that the own ship and the target ship 1 and the target ship 2 are in a situation of encountering each other at the same time, the rules stipulate that both the own ship and the target ship have the responsibility to turn right to avoid the passing ships. At this time, the present invention can take one of the concentrated right turn speeds corresponding to the uncertain speed obstacle to take collision avoidance measures.

[0095] Specifically, Figure 2 As shown, for each target ship, the smallest dashed circle is considered to be the position uncertainty of the target ship, and the slightly larger dashed circle is considered to be the speed uncertainty. The larger dashed circle is D cone Minkowski and, where D cone Represents the extended circle of the collision cone. Specifically, in, Therefore, D cone It can be expressed as:

[0096]

[0097] It is a is the center and the radius is ||Δp i ||'s open loop radius.

[0098] Considering the uncertainty between the own ship and the target ship, the solid line forms a collision cone. The collision cone is represented by C(p o , D cone ). o is the cone point, D cone is the uncertainty radius of the expansion. cone Yes o Linear transformation of cone C(p cone , D cone ) is the dotted line that separates the uncertainty collision cone C(p o , D cone ) is moved vertically outward by a sufficient amount, including through the covariance matrix The linear transformation of the disk, and p cone It's UVO o|i This is achieved by assuming that robustness is only for some probability p of infinite support random uncertainty.r to satisfy. We consider 2 standard deviations around the mean. The uncertain VO can then be designed to be robust to 95% measurement uncertainty.

[0099] Uncertain VO can effectively handle the existing uncertainty. When the velocity v o When it is within the dotted cone, that is, v o ∈UVO o|i , or equivalently, when the relative velocity v o -v i When it is within the solid line cone, it will cause a collision. For all target ships i = 1, 2, ..., n, the uncertain speed obstacle set in the multi-ship encounter scenario is In this case, even if there is uncertainty, the ship can ensure navigation safety by sailing at a speed that is not within the joint set.

[0100] S2.3 Rule-aware robust collision-free path planning

[0101] It is necessary to consider the environment shared by smart ships and traditional ships on the water. This requires smart ships to comply with COLREGs and be able to safely plan paths to avoid collisions with other smart ships or traditional ships. This paper focuses on the three main situations considered in COLREGs, namely overtaking, crossing and head-on, and COLREGs have different views on the definition of scene encounter situations. For the head-on encounter scenario of traditional ships, the angle is generally defined as 5-6 degrees to the left and right in front of the ship. Taking into account the perception distance and accuracy limitations of smart ships, this paper expands the angle to 15 degrees to the left and right in front of the ship, which is more in line with the actual situation. Based on the previous section, we combine the uncertain VO algorithm with COLREGs and propose a dynamic collision-free path planning method based on rule perception.

[0102] Each speed reachable set of the target ship is divided into three subsets according to COLREGs, namely The division of the three subsets is achieved by comparing the uncertain speed barrier cone C(p cone , D cone ) are obtained by taking the angle bisectors tangent to the cone, and the subset is perpendicular to the cone.

[0103] When the own ship and the target ship are in a head-on situation, COLREGs require both ships to turn right to avoid obstacles. Therefore, the USV needs to follow One of the speeds, that is,

[0104]

[0105] Then, the corresponding subsets of all target ships are merged in the multi-ship encounter scenario as As attached Figure 2 shown.

[0106] These merged subsets are calculated as:

[0107]

[0108] Rule 8 of COLREGs also suggests that if there is enough space available, changing heading should be prioritized over speed change, and the maneuver must be performed in sufficient time. To better comply with Rule 8, when the speed magnitude or direction of the achievable speed set can be changed simultaneously, we usually choose to change the speed direction. Let α i , β i They are defined as C(p o , D cone ) and C(p cone , D cone ) is half of the cone angle, then

[0109]

[0110] Then, let α i =β i To calculate the new v o When the collision risk values ​​of multiple target ships exceed the set threshold at the same time, the maximum angle β needs to be selected i Another benefit of selecting the maximum angle is that the target ship can better perceive the movement intention of the own ship, so that the target ship can also perform corresponding maneuvers in advance.

[0111] It should be noted that when own ship acts as a straight-line ship in various encounter situations, own ship has no COLREGs constraints. Then the target ship needs to take corresponding maneuvers to avoid collision with own ship. However, in real encounter scenarios, the target ship may not take any measures, or the measures taken may not meet the requirements of COLREGs. Even if this happens, own ship can still adopt the speed of the achievable speed concentration to safely avoid collision with the target ship.

[0112] The present invention provides simulation results to prove the effectiveness of the proposed dynamic collision-free path planning method. Three situations in the marine environment are considered, namely, overtaking, overtaking and encountering. The method of the present invention is mainly used to simulate the planning decision-making process of the own ship and the target ship, and mainly considers the uncertainty of the position, speed and radius of the target ship.

[0113] During the path planning process, the ship evaluates the collision risk of each target ship in real time (i.e., S1 mentioned above). When one of the target ships reaches the safety threshold, the ship starts to perform path planning and collision avoidance according to the uncertain VO algorithm (i.e., S2 mentioned above). When the risk index of multiple target ships exceeds the set threshold at the same time, the ship with the highest collision risk index needs to be considered first. When calculating CRI, it should be noted that the weights of the five risk factors are different. Among them, we set W T =[0.32, 0.36, 0.14, 0.10, 0.08] T , then, according to the membership function of the influencing factors, the collision risk assessment matrix R of the target ship to the own ship can be established. We consider 2 standard deviations around the mean value. Then, the uncertain VO can be designed to be r = 95% measurement uncertainty. In our simulations, we set the length of the vessel to 4 meters.

[0114] Scenario 1: Single target ship

[0115] As attached Figure 3 As shown in the figure, the simulation shows the situation of overtaking a single target ship under COLREGs. The initial position of the own ship is [0, -25] m, and the target ship is at [0, 0] m. The own ship and the target ship are heading north at a speed of 1 m / s and 0.5 m / s respectively. Among them, the position and speed of the target ship (left track) are processed with a binary Gaussian distribution to represent the random uncertainty of the target ship. The collision avoidance decision of the own ship (right track) is based on the extended uncertain VO signed by the present invention. The Gaussian distribution of the uncertain parameters of the present invention selects two standard deviations to ensure the safe motion planning and collision avoidance of the intelligent ship with a confidence level of 95% (p r =95%). Since the target ship is uncertain, the trajectory of each simulation is random, and Figure 3 One of the simulations that goes beyond the scenario is shown.

[0116] Among them, CRI is calculated based on fuzzy set theory, and a certain threshold is set. In the present invention, the threshold is taken as 0.6. When the set threshold is exceeded, the ship begins to make planning decisions and changes the speed direction according to Rule 8 of COLREGs. Figure 4(a) shows the distance between the ship and the target ship obtained by using the deterministic speed barrier method without considering the uncertainty of the target ship. The straight line is the set safety line. It should be noted that the safety line distance set by the uncertain speed barrier is usually greater than the determined speed barrier. This is because it needs to consider the uncertainty of the radius and position of the target ship. Figure 4(b) shows the distance between the ship and the target ship obtained by using the present invention taking into account the uncertainty of the target ship. The simulation results verify the effectiveness of the algorithm.

[0117] Scenario 2: Multiple Target Ships

[0118] This simulation shows the encounter between the own ship and multiple target ships under COLREGs. Considering the uncertainty of radius, position and speed of the three target ships, the size of the uncertain VO collision cone is expanded. Using the expanded collision cone, that is, at 95% confidence level (p r =95%), the reliability of path planning is guaranteed.

[0119] Scenario 1: Overtaking and encounter. Figure 5 The simulation process of the own ship and the target ship is shown to verify the effectiveness of collision avoidance using the expanded uncertain VO collision cone. The gray dashed lines in the figure represent the left and right tangents of the expanded VO cone during the collision avoidance process. The target ships 1, 2 and 3 (the 2nd, 3rd, and 4th tracks from bottom to top) and the own ship (the bottom track) use the uncertain VO method to decide maneuvers. Figure 5 In (a), the host ship is located at [0, -50]m and sails straight north at a speed of 1m / s; two target ships are located at [0, -25]m and [0, -15]m, both sailing straight north at a speed of 0.5m / s, and the third target ship is located at [5, 45]m and sails at a speed of [0.05, -0.5]m / s. In the initial stage, for target ship 1 and target ship 2, the host ship is in a surpassing encounter scenario, and is in an encounter situation with target ship 3.

[0120] Figure 5 (b) shows that the own ship recognizes the overtaking situation and applies the COLREGS constraint. The collision risk of target ship 1 exceeds the set threshold, and the COLREGs constraint encourages the overtaking evasive maneuver to turn right. The real-time collision risk between the own ship and the target ship is shown in Figure 6 shown.

[0121] exist Figure 5 In (c), after the own ship has moved for a while, the target ship 3 has gradually approached, the own ship and the target ship 1 have completed the collision avoidance process, and are still in the situation of overtaking the target ship 2 and the situation of encountering the target ship 3. At this time, the collision risk values ​​of the target ships 2 and 3 exceed the set threshold at the same time.

[0122] Then, according to the collision avoidance rule, β is selected i The maximum angle of

[0123] Figure 5(d) shows the successful avoidance of collision with the target ship. When the collision risk value of each target ship is lower than the set threshold, the ship sails at the initial speed. Figure 7(a) shows the distance between the ship and the target ship by using the deterministic VO method without considering the uncertainty of the target ship, where the straight line is the set safety line. Figure 7(b) shows the distance between the ship and the target ship obtained by using the uncertain VO method considering the uncertainty of the three target ships.

[0124] Scenario 2: Overtaking, encountering, and crossing. In this case, the own ship first overtakes target ship 1 (the second track from the bottom). While maneuvering around it, the other two target ships (the top and rightmost tracks) begin to approach the own ship, forming an encounter and crossing situation at the same time, such as Figure 8 shown.

[0125] Figure 8 (a) shows the initial state of the ship, and the ship forms three collision cones. Figure 8 In (b), the CRI of target ship 1 is greater than the safety CRI, and the own ship recognizes the overtaking situation. Then, the own ship moves to the starboard side, which complies with the navigation rules. The real-time collision risk between the own ship and the target ship is as follows: Fig. 9 shown.

[0126] exist Figure 8 In (c), while avoiding target ship 1, the own ship detects other target ships. Then, the own ship realizes that it is in the overtaking, meeting, and crossing situations at the same time. The CRI of target ship 1 and target ship 3 exceeds the set safe CRI. The COLREGs constraint forces the own ship to turn to starboard to avoid the passing target ship 3.

[0127] Figure 8 (d) shows that all target ships have passed safely. Note that when the own ship successfully avoids target ship 3, the own ship is still in the state of overtaking target ship 1. At this time, target ship 1 is far enough away and the CRI is relatively small. The own ship does not consider this situation as overtaking. Finally, the own ship starts the turning maneuver at the initial speed. Figures 10(a) and 10(b) show the distance between the own ship and the target ship by using the traditional VO or uncertain VO method, respectively.

[0128] 3.3 Verification with AIS data

[0129] Based on the real Automatic Identification System (AIS) data of the Wuhan section of the Yangtze River, the effectiveness and application potential of the present invention are further verified.

[0130] Fig.11 (a) shows the simulation at the beginning. The initial direction and length represent the speed and direction of each ship, respectively. Fig.11(b), during the process, the own ship is in an encounter situation and a risk of collision with the target ship 1 is detected. According to COLREGs, the own ship has the responsibility to turn right to avoid the collision. In the process of avoiding target ship 1, target ship 2 may also collide with the own ship and be in an overtaking situation with the own ship. This leads to a complex multi-ship encounter scenario. Then, according to the method proposed in the present invention, the own ship calculates the turning angle with the target ship 2. The calculated angle is smaller than the angle for avoiding target ship 1, and the speed direction remains unchanged, as shown in the trajectory in the figure. After completely avoiding target ship 1, the own ship sails at a speed to avoid target ship 2.

[0131] While avoiding target ship 2, the own ship detects target ship 3 and is also in a situation of overtaking target ship 3. Then, the own ship chooses a larger angle to avoid collision. The final trajectory represents the collision avoidance route between the own ship and target ship 3, and returns to the initial speed after the collision avoidance process is completed. Fig.11 (c) shows that the own ship successfully avoids all target ships.

[0132] 4 Conclusion

[0133] The present invention proposes a rule-aware dynamic path planning method based on uncertain speed obstacles. The proposed method solves the priority problem in the decision-making process of multi-ship encounter scenarios. The present invention calculates the collision risk of ships in real time to activate the collision avoidance strategy. In addition, considering the encounter scenarios of crossing, overtaking and encountering, the idea of ​​robust pipeline is used to deal with the uncertainty in the target ship information. In the presence of uncertainty, situations that comply with COLREGs are also achieved in various encounter scenarios with multiple ships. Simulation experiments and AIS data verification show that this method enables intelligent ships to make safe decisions and avoid collisions in complex marine environments.

[0134] The above-mentioned embodiments only express several implementation methods of the present invention, and the description is relatively specific and detailed, but it cannot be understood as limiting the scope of the present invention. For ordinary technicians in this field, several modifications and improvements can be made without departing from the concept of the present invention, which all belong to the protection scope of the present invention.

Claims

1. A method for intelligent ship collision avoidance path planning based on uncertain speed obstacles, characterized in that: The steps include: S1.Collision risk assessment of multi-ship encounter scenarios; Select the time of the most recent meeting T CPA 、The closest encounter distance D CPA , relative distance, relative orientation and ship speed ratio are taken as the most relevant parameters for the collision risk between the own ship and the target ship, and the collision risk index CRI between the own ship and any target ship i is calculated through the fuzzy membership function of each parameter i , the maximum CRI i The value is compared with the safety CRI threshold. If it is higher than the threshold, S2 is executed, otherwise the collision risk assessment continues; In S1, represents the T between the own ship and the target ship i CPA , D CPA , fuzzy membership functions of relative distance, relative orientation and ship speed ratio, The collision risk vector between the own ship and the target ship i is defined as: The weight vector is defined as: in W S , W B , W K is the weight of the corresponding parameter, The CRI values ​​of own ship and target ship i are calculated as: CRI i =W T R i ; The maximum CRI i The value is max{CRI i },i=1,…,n; T CPA , D CPA The calculation is as follows: where ∥·∥ is the Euclidean norm; T CPA Risk membership function for Among them, T0 is the shortest safe time to the nearest approach point, and T1 is the sufficient safe time to the nearest approach point; D CPA Risk membership function for: Where D0 is the minimum safe distance to the nearest approach point, and D1 is the sufficient safe distance to the nearest approach point; The risk membership function of relative distance is calculated as: Where S i is the relative distance between own ship and target ship i; The membership function of relative orientation is as follows: Among them B o is the threshold of the target ship's parameter bearing; The ship speed ratio membership function is as follows: In the formula, the ship speed ratio is defined as S2. Perform rule-aware dynamic collision-free path planning, including S21-S23; S21. Collision-free path planning using VO Let subscript o represent the own ship, i = 1, 2, ..., n represent n target ships; let p o =[x o ,y o ] T ,p i =[x i ,y i ] T Indicates the positions of own ship and target ship on the two-dimensional ocean surface, Respectively represent the velocity vectors of the own ship and the target ship on the two-dimensional ocean surface, Defining VO o∣i is the speed set that causes the ship o to collide with the target ship i. The condition for the ship to avoid collision with the target ship i is D(p i ,r o +r i ) is a i The center is r and the radius is o +r i The open disk, r o and r i are the radii of the own ship and the target ship respectively; for all target ships i=1,2,…,n, the speed obstacle set in the multi-ship encounter scenario is The ship must sail at a speed outside this setting range to ensure safety; S22. Robust collision-free path planning with uncertain VO Considering the uncertainty of the speed and position of the target ship, it is assumed that the radius, speed and position of the target ship have uncertainties that obey the Gaussian distribution. in is the corresponding mean value; is the corresponding variance or covariance matrix; δ· represents the corresponding standard value; diag represents the diagonal matrix; the deterministic VO in S21 o∣i Expanded to uncertain VO(UVO): in: For all target ships i=1,2,…,n, the uncertain speed obstacle set in the multi-ship encounter scenario is The ship sails at a speed not within the joint assembly to ensure navigation safety; In S22, since there are infinite cases of uncertainty that obey the Gaussian distribution, it is impossible and unnecessary to consider all extreme conditions. Therefore, only those with a certain probability p are considered. r Collision avoidance; D cone Indicates the uncertain extension circle radius of the collision cone, in, Then D cone It is expressed as: It is a is the center and the radius is ||Δp i ||'s open loop radius; represents the Minkowski sum; Considering the uncertainty between the own ship and the target ship, two rays from the own ship to the target uncertainty circle form a collision cone, which is expressed as C(p o ,D cone ), where p o is the cone point, D cone is the uncertainty radius of expansion; p cone Yes o The linear transformation is UVO o∣i The cone point, cone C(p cone ,D cone ) is the collision cone of uncertainty collision cone C(p o ,D cone ) is moved vertically outward by a sufficient amount, including through the covariance matrix Linear transformations of the disk; When the speed v o When it is within the dotted cone, that is, v o ∈UVO o∣i , or equivalently, when the relative velocity v o -v i When it is within the solid line cone, it will cause a collision. For all target ships i=1,2,…,n, the uncertain speed obstacle set in the multi-ship encounter scenario is In this case, even if there is uncertainty, own ship can ensure navigation safety by sailing at a speed that is not within the joint set; S23. Rule-aware robust collision-free path planning Plan the path to avoid collision with other ships in compliance with COLREGs rules; S23 is specifically: Each speed reachable set of the target ship is divided into three subsets according to COLREGs, namely They represent the speed sets of the ship turning left, turning right, and moving away from the target ship respectively; the division of the three subsets is based on the uncertainty speed obstacle cone C(p cone ,D cone ) obtained by the tangent angle bisector, the subset is perpendicular to the cone; When own ship and target ship are in a head-on situation, COLREGs require both ships to turn right to avoid obstacles; therefore, own ship needs to follow One of the speeds, that is, Then, the corresponding subsets of all target ships are merged in the multi-ship encounter scenario as These merged subsets are calculated as: In a multi-vessel encounter scenario, the ship adopts a subset of the combined actions to take avoidance measures against multiple vessels simultaneously, which better complies with the requirements of the rules; When the speed magnitude or direction of the achievable speed set can be changed simultaneously, choose to change the speed direction; let α i , β i They are defined as C(p o ,D cone ) and C(p cone ,D cone ) is half of the cone angle, then Then, let α i =β i To calculate the new v o ; When the collision risk values ​​of multiple target ships exceed the set threshold at the same time, the largest angle β needs to be selected i To avoid.

Images