Ship course and speed collaborative collision avoidance method based on international maritime collision avoidance rules
The method addresses the lack of coordinated heading and speed control in ship collision avoidance by employing artificial potential field theory and linear search optimization, ensuring timely and accurate collision avoidance decisions in complex navigation environments.
Patent Information
- Application Number
- CN202411227897.1
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-09-03
- Publication Date
- 2024-12-06
AI Technical Summary
Existing collision avoidance strategies for large marine transport ships in complex navigation environments, such as restricted waters or emergency situations, lack effective methods for coordinated control of ship heading and speed, particularly under international maritime collision regulations, leading to inefficiencies and safety risks.
A method for coordinated collision avoidance based on International Maritime Collision Regulations, utilizing artificial potential field theory and linear search optimization to determine right of way, maneuvering timing, and dynamic collision avoidance strategies, incorporating PID controllers for heading and speed adjustments, and fast local path planning to ensure timely and accurate collision avoidance decisions.
Enhances navigation safety by enabling timely and accurate collision avoidance decisions in complex environments, dynamically adjusting between heading, speed, and coordinated collision avoidance modes to respond effectively to varying navigation scenarios.
Smart Images

Figure CN119088028A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to, in particular to, a method for coordinated collision avoidance of ship headings and speeds based on international regulations for avoiding collisions at sea. Background Art
[0002] Due to the maneuvering characteristics of large marine transport ships and the common practices of crew members, a heading collision avoidance strategy is usually adopted in open waters. However, in restricted waters or emergency situations, heading and speed coordinated collision and obstacle avoidance are inevitable requirements for advanced autonomous navigation. In recent years, researchers have achieved certain results in the study of speed control and heading and speed coordinated collision and obstacle avoidance control of large ships, but there are still many shortcomings.
[0003] The timing, amplitude and rate of collision and obstacle avoidance of large commercial ships, as well as the ship's own maneuvering characteristics, have a great impact on the collision avoidance results. At present, there are few studies on autonomous collision avoidance strategies that rely solely on changing speed. The research on coordinated control of heading and speed is even more difficult, and there are fewer advanced and effective theoretical studies and application cases. Therefore, the research on collision avoidance path optimization and strategy selection based on changing speed cannot be ignored.
[0004] At present, the research on autonomous collision and obstacle avoidance of large merchant ships based on artificial potential field theory has not considered the control of speed, nor has it achieved the coordinated control of heading and speed. The coordinated control of heading and speed of large merchant ships requires the establishment of an interactive coupling model of the main control equipment, and the establishment of a hierarchical decision-making and control model by combining artificial intelligence methods such as multi-objective particle swarm optimization and deep reinforcement learning. Summary of the invention
[0005] In view of the above-mentioned defects or deficiencies in the prior art, it is desired to provide a method for coordinated collision avoidance of ship heading and speed based on the International Maritime Collision Regulations, so as to ensure that large ships can make timely collision avoidance decisions in complex navigation environments, thereby significantly improving the safety of navigation.
[0006] The present invention provides a method for ship heading and speed coordinated collision avoidance based on the International Regulations for Preventing Collisions at Sea, comprising the following steps:
[0007] 1) Obtain collision avoidance parameters of own ship;
[0008] 2) Get the ship's position P OS 、Speed v OS and heading ψ c , and the target ship position P TS and speed v TS ; Calculate the distance d between the own ship and the target ship, and the minimum encounter distance d CPA , minimum encounter time t CPA , relative speed v OT , relative position P OT, the angle θ between the relative position line and the relative speed line; where v OT =v OS -v TS , P OT =P OS -P TS ;
[0009] 3) Determine the right of way WR of the own ship, which includes the right of way WR of the give-way ship and the straight-line ship;
[0010] 4) Determine the collision avoidance maneuvering position P of own ship min and collision avoidance maneuver timing t min ;
[0011] 5) Selecting an applicable collision avoidance mode M1, wherein the collision avoidance mode M1 includes a heading collision avoidance mode, a speed collision avoidance mode, and a heading and speed coordinated collision avoidance mode;
[0012] 6) According to the selected collision avoidance mode M1, run the corresponding collision avoidance program and obtain the recommended heading ψ desired and / or recommended speed v desired ;
[0013] 7) Using PID heading controller, according to the recommended heading ψ desired Get the corresponding collision avoidance rudder angle δ desired ; According to the recommended speed v desired Get the corresponding collision avoidance speed RPM desired ;
[0014] 8) Execute the fast local path planning program FLPP to obtain a collision avoidance path, wherein the collision avoidance path includes a collision avoidance turning point WP desired List and speed v of collision avoidance turning point WP-desired List;
[0015] 9) Output collision avoidance maneuvering position P min and collision avoidance maneuver timing t min , recommended heading ψ desired and collision avoidance rudder angle δ desired 、Recommended speed v desired and collision avoidance speed RPM desired , collision avoidance turning point WP desired and the speed v at the collision avoidance turning point WP-desired , the output information is executed after confirmation by the ship's driver;
[0016] 10) Repeat the above steps until the ship reaches the end of the planned route and the algorithm execution ends.
[0017] Furthermore, the collision avoidance parameters include:
[0018] The planned route of the ship, including the planned turning point WP and the speed v at the planned turning point WP ;
[0019] Negotiate collision avoidance repulsion calculation factor η n and the emergency collision avoidance repulsion factor η e ;
[0020] Negotiate collision avoidance distance condition d emg and emergency collision avoidance distance condition d neg ;
[0021] DCPA-TCPA threshold conditions, including the minimum encounter distance d for negotiation collision avoidance nCPA and the minimum encounter time t nCPA , the minimum encounter distance d for emergency collision avoidance eCPA and the minimum encounter time t eCPA ; The distance threshold condition d between the own ship and the target ship TOL ;
[0022] The ship's steering angle limit θ within the sampling period max ;
[0023] Target gravity calculation factor ε1, yaw repair gravity calculation factors ε2 and ε3;
[0024] The alarm threshold d of the ship's deviation distance XTL and set the threshold d min ;
[0025] Maximum speed max and the minimum speed v to maintain rudder effectiveness min , from the maximum speed v max Slow down to minimum speed v min Time t max-min and distance d max-min , from the minimum speed v min Accelerate to maximum speed v max Time t min-max and distance d min-max ;
[0026] The sampling period Δt and number of steps N for fast local path planning;
[0027] The initial collision avoidance mode M0 includes heading collision avoidance mode and speed collision avoidance mode.
[0028] Further, the step 3) comprises the following steps:
[0029] 3.1) When the distance d between the own ship and the target ship is less than d TOL When the target ship is in collision, the ship's right of way WR is determined according to the International Regulations for Preventing Collisions at Sea; otherwise, the ship's right of way WR is determined between the ship and the next target ship;
[0030] The judgment of the right of way WR is as follows: if the own ship is a give-way ship to the target ship, the right of way WR of the own ship is judged as a give-way ship, and the judgment ends; if the own ship is a straight-line ship to the target ship, proceed to the next step;
[0031] 3.2) When 0≤d CPA ≤d eCPA And 0≤t CPA ≤t eCPA When the ship is 1, the ship's right of way is cancelled, and the ship's right of way WR is judged as the give-way ship, and the judgment ends; otherwise, the ship is still a straight-line ship to the target ship, and the right of way WR judgment between the ship and the next target ship begins;
[0032] 3.3) After the right of way WR of own ship and all target ships is determined, if own ship is a give-way ship to any target ship, then the right of way WR of own ship is determined to be a give-way ship; if own ship is a direct-passing ship to all target ships, then the right of way WR of own ship is determined to be a direct-passing ship.
[0033] Further, the step 4) comprises the following steps:
[0034] 4.1) If the ship's right of way WR is the give-way ship, and d CPA >d nCPA or CPA <0; or the ship's right of way WR is a straight-line ship, and d CPA >d eCPA or CPA <0; it is determined that the target ship and the own ship will not pose a collision risk, and the calculation of the next target ship will be entered; otherwise, the next step will be entered;
[0035] 4.2) If the ship's right of way WR is the give-way ship, and 0≤d CPA ≤d nCPA , 0≤t CPA ≤t nCPA ; or the ship's right of way WR is a straight-line ship, and 0≤d CPA ≤d eCPA , 0≤t CPA ≤t eCPA ; then the ship needs to take immediate action to avoid collision, and the maneuvering time is t min =0, that is, the current time, the operating position P min =P OS , that is, the current position; otherwise, go to the next step;
[0036] 4.3) If the ship's right of way WR is the give-way ship, and 0≤d CPA ≤d nCPA , t CPA >t nCPA , then record the maneuvering timing t for the current target ship j =tCPA -t nCPA ; or the ship's right of way WR is a straight-line ship, and 0≤d CPA ≤d eCPA , t CPA >t eCPA , then record the maneuvering timing t for the current target ship j =t CPA -t eCPA ;
[0037] 4.4) After completing the calculation for all target ships, if the ship's right of way WR is the give-way ship, and the ship has d CPA >d nCPA or CPA <0; or the ship's right of way WR is a straight-line ship, and the ship is d to any target ship CPA >d eCPA or CPA <0; then the ship and all target ships do not pose a collision risk, and the collision avoidance maneuver timing t min = -∞;
[0038] Otherwise, select the recorded operation timing list {t0, t1, t2, …, t n} to find the most recent manipulation time t i , collision avoidance maneuver time t min =t i , collision avoidance maneuvering position P min =P OS +v OS ×t min .
[0039] Further, the step 5) comprises the following steps:
[0040] 5.1) If the ship's right of way WR is the give-way ship, and the ship has d CPA >d nCPA or CPA <0; or the ship's right of way WR is a straight-line ship, and the ship has d CPA >d eCPA or CPA <0; then the ship and all target ships do not pose a collision risk, the ship continues to maintain the direction and speed, and the calculation ends; otherwise, proceed to the next step;
[0041] 5.2) If 0≤d CPA ≤d eCPA And 0≤ tCPA ≤t eCPA Regardless of whether the ship's right of way WR is for a give-way ship or a straight-line ship, the ship and at least one target ship are already in the emergency collision avoidance stage. Select the heading and speed coordinated collision avoidance mode and end the calculation; otherwise, proceed to the next step;
[0042] 5.3) The right of way WR of this ship is the give-way ship. This ship and at least one target ship are in the negotiation and collision avoidance phase, that is, 0≤d CPA ≤d nCPA And 0≤t CPA ≤t nCPA ; and the ship and all target ships do not constitute emergency collision avoidance conditions, that is, they do not meet 0≤d CPA ≤d eCPA And 0≤t CPA ≤t eCPA ; If the initial collision avoidance mode M0 of the own ship is heading collision avoidance, then proceed to step 5.4); If the initial collision avoidance mode M0 of the own ship is speed collision avoidance, then proceed to step 5.5);
[0043] 5.4) Perform fast local path planning; if the planned path can ensure that the ship and all target ships are within d eCPA If the ship passes the other ship, the ship will still maintain the heading collision avoidance mode; otherwise, the ship will switch to the heading and speed coordinated collision avoidance mode and end the calculation;
[0044] 5.5) Perform fast local path planning; if the planned path can ensure that the ship and all target ships are within d eCPA If the other party passes and gives way, own ship still maintains speed collision avoidance mode; otherwise, own ship switches to heading and speed coordinated collision avoidance mode and ends the calculation.
[0045] Further, the step 6) comprises the following steps:
[0046] 6.1) If the collision avoidance mode is the heading collision avoidance mode, the heading collision avoidance program based on the artificial potential field theory is run to obtain the recommended heading ψ desired ;
[0047] 6.2) If the collision avoidance mode is selected as the speed collision avoidance mode, the speed collision avoidance program based on the linear search optimization method is run to obtain the recommended speed v desired ;
[0048] 6.3) If the collision avoidance mode is the heading and speed coordinated collision avoidance mode, the heading collision avoidance program based on the artificial potential field theory and the speed collision avoidance program based on the linear search optimization method are run simultaneously to obtain the recommended heading ψ desired and recommended speed v desired .
[0049] Further, the step 6.2) comprises the following steps:
[0050] 6.2.1) Calculate the minimum target collision avoidance speed of own ship for any target ship: minSpd = min{v min ,||v OS|| / 2}; Calculate the maximum target collision avoidance speed: maxSpd = max{v min ,||v OS || / 2};
[0051] Select the speed search step size as Δv; establish the speed cost function for own ship collision avoidance:
[0052]
[0053] Among them, d CPAj (v i ) is the speed v i Next, the closest approach distance between own ship and target ship;
[0054] 6.2.2) Use the linear search optimization method from {minSpd, maxSpd} to obtain the optimal collision avoidance speed optSpd for the ship against the target ship i ; If the optimal collision avoidance speed is not found, optSpdi = minSpd; If the optimal collision avoidance speed is found, optSpd is recorded i ; The number of ships participating in collision avoidance is nShip+1; Calculate the next target ship;
[0055] 6.2.3) After all target ships are calculated, if nShip ≥ 1, select the optimal collision avoidance speed from the list {optSpd0, optSpd1, optSpd2, …, optSpd n Find the minimum collision avoidance speed optSpd in min , which is the expected collision avoidance speed of the ship: v opt =optSpd min Otherwise, there will be no collision risk between the ship and any target ship, and the expected collision avoidance speed of the ship is the design speed of this section, that is, v opt =v WP-i ;
[0056] 6.2.4) According to the ship's speed v OS and target speed v opt , using PID controller to obtain the recommended speed v desired :
[0057]
[0058] Among them, K p >0 is the proportional constant; K d >0 is the differential constant; K i >0 is the integration constant, t is the execution time of the algorithm;
[0059] 6.2.5) According to the recommended speed v desiredThe corresponding relationship between the ship's rotation speed and the speed is used to obtain the recommended rotation speed RPM desired .
[0060] Furthermore, the collision avoidance rudder angle δ in step 7) is desired for:
[0061]
[0062] Among them, K p >0 is the proportionality constant; K d >0 is the differential constant; K i >0 is the integration constant;
[0063] Δψ c is the recommended heading change;
[0064] is Δψ c The rate of change of is in radians per second;
[0065] t is the execution time of the algorithm.
[0066] Furthermore, the fast local path planning includes the following steps:
[0067] 8.1) Based on the current own ship position P OS 、Speed v OS , rudder angle δ, speed RPM, and collision avoidance rudder angle δ desired and collision avoidance speed RPM desired , driving the solution of high-precision mathematical models of ship motion;
[0068] 8.2) Based on the high-precision ship motion mathematical model, calculate the ship's position P in the next sampling period OS 、Speed v OS , rudder angle δ and speed RPM; the estimated own ship position P OS and speed v OS The turning points and speeds of the turning points as planned paths are stored in the turning point list WP. desired and the speed list v of the turning point WP-desired ;
[0069] 8.3) Calculate the target ship positions P of all target ships in the next sampling period TS 、Speed v TS ; Calculate the distance d between the ship and all target ships, and the minimum encounter distance d CPA , minimum encounter time t CPA ;
[0070] 8.4) According to the selected collision avoidance mode M1, adopt the corresponding collision avoidance procedure to obtain the recommended course ψ of the ship in the next sampling period desiredand recommended speed v desired ;
[0071] 8.5) Use PID controller to obtain the collision avoidance rudder angle δ of the ship in the next sampling period desired and collision avoidance speed RP Mdesired ;
[0072] 8.6) Repeat the above steps until the number of iterations j = N, and obtain the local path planning result, that is, the turning point WP desired List and turn point speed v WP-desired List, end calculation.
[0073] Compared with the prior art, the beneficial effects of the present invention are:
[0074] (1) The ship heading and speed coordinated collision avoidance method of the present invention is based on artificial potential field theory and linear search optimization method. Under the international maritime collision avoidance rules, it performs right of way calculation, maneuvering timing and maneuvering position calculation, dynamic collision avoidance strategy adjustment, heading and speed coordinated collision avoidance program execution, heading recovery and speed recovery. This method provides an effective heading and speed coordinated collision avoidance decision-making method for large commercial ships, ensuring that ships can make collision avoidance decisions in a complex navigation environment in a timely manner, significantly improving the safety of navigation.
[0075] (2) The ship heading and speed coordinated collision avoidance method of the present invention flexibly selects heading collision avoidance mode, speed collision avoidance mode or heading and speed coordinated collision avoidance mode according to the initial collision avoidance mode and the relative dynamics between the ships. This follows the navigation habit of ships usually using heading collision avoidance mode or speed collision avoidance mode as the main mode during navigation. When the initial heading collision avoidance mode / speed collision avoidance mode cannot guarantee that the collision is within the threshold range d emg When an unexpected vehicle passes by and gives way, the course and speed coordinated collision avoidance mode is activated. This dynamic adjustment mechanism makes collision avoidance more accurate and effective, especially enabling quick response in emergency situations.
[0076] It should be understood that the contents described in the summary of the invention are not intended to limit the key or important features of the embodiments of the present invention, nor are they intended to limit the scope of the present invention. Other features of the present invention will become easily understood through the following description. BRIEF DESCRIPTION OF THE DRAWINGS
[0077] Other features, objects and advantages of the present invention will become more apparent from the detailed description of non-limiting embodiments made with reference to the following drawings:
[0078] Figure 1 This is a flow chart of the method for ship heading and speed coordinated collision avoidance;
[0079] Figure 2 Flow chart for determining own ship's right of way WR;
[0080] Figure 3 To determine the collision avoidance maneuvering position P of own ship min and collision avoidance maneuver timing t min Flowchart of
[0081] Figure 4 Flow chart for selection and switching of own ship's collision avoidance model;
[0082] Figure 5 This is a flow chart of the speed collision avoidance procedure based on the linear search optimization method;
[0083] Figure 6 Flowchart for fast local path planning. DETAILED DESCRIPTION
[0084] The present invention will be further described in detail below in conjunction with the accompanying drawings and embodiments. It is to be understood that the specific embodiments described herein are only used to explain the relevant invention, rather than to limit the invention. It should also be noted that, for ease of description, only the parts related to the invention are shown in the accompanying drawings.
[0085] It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments can be combined with each other. The present invention will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.
[0086] Please refer to Figure 1 to Figure 6 The embodiment of the present invention provides a method for ship heading and speed coordinated collision avoidance based on the International Regulations for Preventing Collisions at Sea, comprising the following steps:
[0087] 1) Obtain collision avoidance parameters of own ship;
[0088] Collision avoidance parameters include:
[0089] The planned route of the ship, including the planned turning point WP and the speed v at the planned turning point WP ;
[0090] Negotiate collision avoidance repulsion calculation factor η n and the emergency collision avoidance repulsion factor η e ;
[0091] Negotiate collision avoidance distance condition d emg and emergency collision avoidance distance condition d neg ;
[0092] DCPA-TCPA threshold conditions, including the minimum encounter distance d for negotiation collision avoidance nCPA and the minimum encounter time t nCPA , the minimum encounter distance d for emergency collision avoidance eCPA and the minimum encounter time t eCPA; The distance threshold condition d between the own ship and the target ship TOL ;
[0093] The ship's steering angle limit θ within the sampling period max ;
[0094] Target gravity calculation factor ε1, yaw repair gravity calculation factors ε2 and ε3;
[0095] The alarm threshold d of the ship's deviation distance XTL and set the threshold d min ;
[0096] Maximum speed max and the minimum speed v to maintain rudder effectiveness min , from the maximum speed v max Slow down to minimum speed v min Time t max-min and distance d max-min , from the minimum speed v min Accelerate to maximum speed v max Time t min-max and distance d min-max ;
[0097] The sampling period Δt and number of steps N for fast local path planning;
[0098] The initial collision avoidance mode M0 includes the heading collision avoidance mode and the speed collision avoidance mode;
[0099] 2) Get the ship's position P OS 、Speed v OS and heading ψ c , and the target ship position P TS and speed v TS ; Calculate the distance d between the own ship and the target ship, and the minimum encounter distance d CPA , minimum encounter time t CPA , relative speed v OT , relative position P OT , the angle θ between the relative position line and the relative speed line; where v OT =v OS -v TS , P OT =P OS -P TS ;
[0100] 3) Determine the right of way WR of own ship, which includes the right of way WR of give-way ship and direct-flowing ship;
[0101] 3.1) When the distance d between the own ship and the target ship is less than d TOL, determine the right of way WR of the own ship to the target ship according to the International Regulations for Preventing Collisions at Sea; otherwise, proceed to the right of way WR determination between the own ship and the next target ship;
[0102] The judgment of the right of way WR is as follows: if the own ship is a give-way ship to the target ship, the right of way WR of the own ship is judged as a give-way ship, and the judgment ends; if the own ship is a straight-line ship to the target ship, proceed to the next step;
[0103] 3.2) When 0≤d CPA ≤d eCPA And 0≤t CPA ≤t eCPA When the ship is 1, the ship's right of way is cancelled, and the ship's right of way WR is judged as the give-way ship, and the judgment ends; otherwise, the ship is still a straight-line ship to the target ship, and the right of way WR judgment between the ship and the next target ship begins;
[0104] 3.3) After the right of way WR of the own ship and all target ships is determined, if the own ship is a give-way ship to any target ship, the right of way WR of the own ship is determined to be a give-way ship; if the own ship is a direct-passing ship to all target ships, the right of way WR of the own ship is determined to be a direct-passing ship;
[0105] 4) Determine the collision avoidance maneuvering position P of own ship min and collision avoidance maneuver timing t min ;
[0106] 4.1) If the ship's right of way WR is the give-way ship, and d CPA >d nCPA or CPA <0; or the ship's right of way WR is a straight-line ship, and d CPA >d eCPA or CPA <0; it is determined that the target ship and the own ship will not pose a collision risk, and the calculation of the next target ship will be entered; otherwise, the next step will be entered;
[0107] 4.2) If the ship's right of way WR is the give-way ship, and 0≤d CPA ≤d nCPA , 0≤t CPA ≤t nCPA ; or the ship's right of way WR is a straight-line ship, and 0≤d CPA ≤d eCPA , 0≤t CPA ≤t eCPA ; then the ship needs to take immediate action to avoid collision, and the maneuvering time is t min =0, that is, the current time, the operating position P min =P OS , that is, the current position; otherwise, go to the next step;
[0108] 4.3) If the ship's right of way WR is the give-way ship, and 0≤dCPA ≤d nCPA , t CPA >t nCPA , then record the maneuvering timing t for the current target ship j =t CPA -t nCPA ; or the ship's right of way WR is a straight-line ship, and 0≤d CPA ≤d eCPA , t CPA >t eCPA , then record the maneuvering timing t for the current target ship j =t CPA -t eCPA ;
[0109] 4.4) After completing the calculation for all target ships, if the ship's right of way WR is the give-way ship, and the ship has d CPA >d nCPA or CPA <0; or the ship's right of way WR is a straight-line ship, and the ship is d to any target ship CPA >d eCPA or CPA <0; then the ship and all target ships do not pose a collision risk, and the collision avoidance maneuver timing t min = -∞;
[0110] Otherwise, select the recorded operation timing list {t0, t1, t2, …, t n} to find the most recent manipulation time t i , collision avoidance maneuver time t min =t i , collision avoidance maneuvering position P min =P OS +v OS ×t min ;
[0111] 5) Select the applicable collision avoidance mode M1, which includes heading collision avoidance mode, speed collision avoidance mode and heading and speed coordinated collision avoidance mode;
[0112] 5.1) If the ship's right of way WR is the give-way ship, and the ship has d CPA >d nCPA or CPA <0; or the ship's right of way WR is a straight-line ship, and the ship has d CPA >d eCPA or CPA <0; then the ship and all target ships do not pose a collision risk, the ship continues to maintain the direction and speed, and the calculation ends; otherwise, proceed to the next step;
[0113] 5.2) If 0≤dCPA ≤d eCPA And 0≤ tCPA ≤t eCPA Regardless of whether the ship's right of way WR is for a give-way ship or a straight-line ship, the ship and at least one target ship are already in the emergency collision avoidance stage. Select the heading and speed coordinated collision avoidance mode and end the calculation; otherwise, proceed to the next step;
[0114] 5.3) The right of way WR of this ship is the give-way ship. This ship and at least one target ship are in the negotiation and collision avoidance phase, that is, 0≤d CPA ≤d nCPA And 0≤t CPA ≤t nCPA ; and the ship and all target ships do not constitute emergency collision avoidance conditions, that is, they do not meet 0≤d CPA ≤d eCPA And 0≤t CPA ≤t eCPA ; If the initial collision avoidance mode M0 of the own ship is heading collision avoidance, then proceed to step 5.4); If the initial collision avoidance mode M0 of the own ship is speed collision avoidance, then proceed to step 5.5);
[0115] 5.4) Perform fast local path planning; if the planned path can ensure that the ship and all target ships are within d eCPA If the ship passes the other ship, the ship will still maintain the heading collision avoidance mode; otherwise, the ship will switch to the heading and speed coordinated collision avoidance mode and end the calculation;
[0116] 5.5) Perform fast local path planning; if the planned path can ensure that the ship and all target ships are within d eCPA If the ship passes the other ship, the ship will still maintain the speed collision avoidance mode; otherwise, the ship will switch to the heading and speed coordinated collision avoidance mode and end the calculation;
[0117] 6) According to the selected collision avoidance mode M1, run the corresponding collision avoidance program and obtain the recommended heading ψ desired and recommended speed v desired ;
[0118] 6.1) If the collision avoidance mode is the heading collision avoidance mode, the heading collision avoidance program based on the artificial potential field theory is run to obtain the recommended heading ψ desired ;
[0119] 6.2) If the collision avoidance mode is selected as the speed collision avoidance mode, the speed collision avoidance program based on the linear search optimization method is run to obtain the recommended speed v desired ;
[0120] 6.2.1) Calculate the minimum target collision avoidance speed of own ship for any target ship: minSpd = min{v min ,||v OS|| / 2}; Calculate the maximum target collision avoidance speed: maxSpd = max{v min ,||v OS || / 2};
[0121] Select the speed search step size as Δv; establish the speed cost function for own ship collision avoidance:
[0122]
[0123] Among them, d CPAj (v i ) is the speed v i Next, the closest approach distance between own ship and target ship;
[0124] 6.2.2) Use the linear search optimization method from {minSpd, maxSpd} to obtain the optimal collision avoidance speed optSpd for the ship against the target ship i ; If the optimal collision avoidance speed is not found, optSpdi = minSpd; If the optimal collision avoidance speed is found, optSpd is recorded i ; The number of ships participating in collision avoidance is nShip+1; Calculate the next target ship;
[0125] 6.2.3) After all target ships are calculated, if nShip ≥ 1, select the optimal collision avoidance speed from the list {optSpd0, optSpd1, optSpd2, …, optSpd n Find the minimum collision avoidance speed optSpd in min , which is the expected collision avoidance speed of the ship: v opt =optSpd min Otherwise, there will be no collision risk between the ship and any target ship, and the expected collision avoidance speed of the ship is the design speed of this section, that is, v opt =v WP-i ;
[0126] 6.2.4) According to the ship's speed v OS and target speed v opt , using PID controller to obtain the recommended speed v desired :
[0127]
[0128] Among them, K p >0 is the proportional constant; K d >0 is the differential constant; K i >0 is the integration constant, t is the execution time of the algorithm;
[0129] 6.2.5) According to the recommended speed v desiredThe corresponding relationship between the ship's rotation speed and the speed is used to obtain the recommended rotation speed RPM desired ;
[0130] 6.3) If the collision avoidance mode is the heading and speed coordinated collision avoidance mode, the heading collision avoidance program based on the artificial potential field theory and the speed collision avoidance program based on the linear search optimization method are run simultaneously to obtain the recommended heading ψ desired and recommended speed v desired ;
[0131] 7) Using PID heading controller, according to the recommended heading ψ desired Get the corresponding collision avoidance rudder angle δ desired ; According to the recommended speed v desired Get the corresponding collision avoidance speed RPM desired ;
[0132]
[0133] Among them, K p >0 is the proportionality constant; K d >0 is the differential constant; K i >0 is the integration constant;
[0134] Δψ c is the recommended heading change;
[0135] is Δψ c The rate of change of is in radians per second;
[0136] t is the execution time of the algorithm;
[0137] 8) Execute the fast local path planning program FLPP to obtain the collision avoidance path, which includes the collision avoidance turning point WP desired List and speed v of collision avoidance turning point WP-desired List;
[0138] Fast local path planning includes the following steps:
[0139] 8.1) Based on the current own ship position P OS 、Speed v OS , rudder angle δ, speed RPM, and collision avoidance rudder angle δ desired and collision avoidance speed RPM desired , driving the solution of high-precision mathematical models of ship motion;
[0140] 8.2) Based on the high-precision ship motion mathematical model, calculate the ship's position P in the next sampling period OS 、Speed v OS , rudder angle δ and speed RPM; the estimated own ship position P OS and speed vOS The turning points and speeds of the turning points as planned paths are stored in the turning point list WP. desired and the speed list v of the turning point WP-desired ;
[0141] 8.3) Calculate the target ship positions P of all target ships in the next sampling period TS 、Speed v TS ; Calculate the distance d between the ship and all target ships, and the minimum encounter distance d CPA , minimum encounter time t CPA ;
[0142] 8.4) According to the selected collision avoidance mode M1, adopt the corresponding collision avoidance procedure to obtain the recommended course ψ of the ship in the next sampling period desired and recommended speed v desired ;
[0143] 8.5) Use PID controller to obtain the collision avoidance rudder angle δ of the ship in the next sampling period desired and collision avoidance speed RP Mdesired ;
[0144] 8.6) Repeat the above steps until the number of iterations j = N, and obtain the local path planning result, that is, the turning point WP desired List and turn point speed v WP-desired List, end calculation;
[0145] 9) Output collision avoidance maneuvering position P min and collision avoidance maneuver timing t min , recommended heading ψ desired and collision avoidance rudder angle δ desired 、Recommended speed v desired and collision avoidance speed RPM desired , collision avoidance turning point WP desired and the speed v at the collision avoidance turning point WP-desired , the output information is executed after confirmation by the ship's driver;
[0146] 10) Repeat the above steps until the ship reaches the end of the planned route and the algorithm execution ends.
[0147] In this embodiment, based on artificial potential field theory and linear search optimization method, under the international maritime collision avoidance regulations, right of way calculation, maneuvering timing and maneuvering position calculation, dynamic collision avoidance strategy adjustment, heading and speed coordinated collision avoidance program execution, heading recovery and speed recovery are performed. This method provides an effective heading and speed coordinated collision avoidance decision method for large commercial ships, ensuring that ships can make collision avoidance decisions in a complex navigation environment in a timely manner, significantly improving the safety of navigation.
[0148] According to the initial collision avoidance mode and the relative dynamics between ships, the heading collision avoidance mode, speed collision avoidance mode or heading and speed coordinated collision avoidance mode is flexibly selected. This follows the navigation habit of ships usually using the heading collision avoidance mode or speed collision avoidance mode as the main mode during navigation. When the initial heading collision avoidance mode / speed collision avoidance mode cannot guarantee that the collision will be within the threshold range d emg When an unexpected vehicle passes by and gives way, the course and speed coordinated collision avoidance mode is activated. This dynamic adjustment mechanism makes collision avoidance more accurate and effective, especially enabling quick response in emergency situations.
[0149] In the description of this specification, the description of the terms "one embodiment", "some embodiments", etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present application. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any one or more embodiments or examples in a suitable manner.
[0150] The above are only preferred embodiments of the present application and are not intended to limit the present application. For those skilled in the art, the present application may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included in the protection scope of the present application.
Claims
1. A method for ship heading and speed coordinated collision avoidance based on the International Regulations for Preventing Collisions at Sea, characterized in that: The steps include: 1) Obtain collision avoidance parameters of own ship; 2) Get the ship's position P OS 、Speed v OS and heading ψ c , and the target ship position P TS and speed v TS ; Calculate the distance d between the own ship and the target ship, and the minimum encounter distance d CPA , minimum encounter time t CPA , relative speed v OT , relative position P OT , the angle θ between the relative position line and the relative speed line; Among them, v OT =v OS -v TS , P OT =P OS -P TS ; 3) Determine the right of way WR of the own ship, which includes the right of way WR of the give-way ship and the straight-line ship; 4) Determine the collision avoidance maneuvering position P of own ship min and collision avoidance maneuver timing t min ; 5) Selecting an applicable collision avoidance mode M1, wherein the collision avoidance mode M1 includes a heading collision avoidance mode, a speed collision avoidance mode, and a heading and speed coordinated collision avoidance mode; 6) According to the selected collision avoidance mode M1, run the corresponding collision avoidance program and obtain the recommended heading ψ desired and / or recommended speed v desired ; 7) Using PID heading controller, according to the recommended heading ψ desired Get the corresponding collision avoidance rudder angle δ desired ; According to the recommended speed v desired Get the corresponding collision avoidance speed RPM desired ; 8) Execute the fast local path planning program FLPP to obtain a collision avoidance path, wherein the collision avoidance path includes a collision avoidance turning point WP desired List and speed v of collision avoidance turning point WP-desired List; 9) Output collision avoidance maneuvering position P min and collision avoidance maneuver timing t min , recommended heading ψ desired and collision avoidance rudder angle δ desired 、Recommended speed v desired and collision avoidance speed RPM desired , collision avoidance turning point WP desired and the speed v at the collision avoidance turning point WP-desired , the output information is executed after confirmation by the ship's driver; 10) Repeat the above steps until the ship reaches the end of the planned route and the algorithm execution ends.
2. A method for ship heading and speed coordinated collision avoidance based on the International Regulations for Preventing Collisions at Sea according to claim 1, characterized in that: The collision avoidance parameters include: The planned route of the ship, including the planned turning point WP and the speed v at the planned turning point WP ; Negotiate collision avoidance repulsion calculation factor η n and the emergency collision avoidance repulsion factor η e ; Negotiate collision avoidance distance condition d emg and emergency collision avoidance distance condition d neg ; DCPA-TCPA threshold conditions, including the minimum encounter distance d for negotiation collision avoidance nCPA and the minimum encounter time t nCPA , the minimum encounter distance d for emergency collision avoidance eCPA and the minimum encounter time t eCPA ; The distance threshold condition d between the own ship and the target ship TOL ; The ship's steering angle limit θ within the sampling period max ; Target gravity calculation factor ε1, yaw repair gravity calculation factors ε2 and ε3; The alarm threshold d of the ship's deviation distance XTL and set the threshold d min ; Maximum speed max and the minimum speed v to maintain rudder effectiveness min , from the maximum speed v max Slow down to minimum speed v min Time t max-min and distance d max-min , from the minimum speed v min Accelerate to maximum speed v max Time t min-max and distance d min-max ; The sampling period Δt and number of steps N for fast local path planning; The initial collision avoidance mode M0 includes heading collision avoidance mode and speed collision avoidance mode.
3. The method for ship heading and speed coordinated collision avoidance based on the International Regulations for Preventing Collisions at Sea according to claim 2 is characterized in that: The step 3) comprises the following steps: 3.1) When the distance d between the own ship and the target ship is less than d TOL When the target ship is in collision, the ship's right of way WR is determined according to the International Regulations for Preventing Collisions at Sea; otherwise, the ship's right of way WR is determined between the ship and the next target ship; The judgment of the right of way WR is as follows: if the own ship is a give-way ship to the target ship, the right of way WR of the own ship is judged as a give-way ship, and the judgment ends; if the own ship is a straight-line ship to the target ship, proceed to the next step; 3.2) When 0≤d CPA ≤d eCPA And 0≤t CPA ≤t eCPA When the ship is 1, the ship's right of way is cancelled, and the ship's right of way WR is judged as the give-way ship, and the judgment ends; otherwise, the ship is still a straight-line ship to the target ship, and the right of way WR judgment between the ship and the next target ship begins; 3.3) After the right of way WR of own ship and all target ships is determined, if own ship is a give-way ship to any target ship, then the right of way WR of own ship is determined to be a give-way ship; if own ship is a direct-passing ship to all target ships, then the right of way WR of own ship is determined to be a direct-passing ship.
4. The method for ship heading and speed coordinated collision avoidance based on the International Regulations for Preventing Collisions at Sea according to claim 2 is characterized in that: The step 4) comprises the following steps: 4.1) If the ship's right of way WR is the give-way ship, and d CPA >d nCPA or CPA <0; or the ship's right of way WR is a straight-line ship, and d CPA >d eCPA or CPA <0; it is determined that the target ship and the own ship will not pose a collision risk, and the calculation of the next target ship will be entered; otherwise, the next step will be entered; 4.2) If the ship's right of way WR is the give-way ship, and 0≤d CPA ≤d nCPA , 0≤t CPA ≤t nCPA ; or the ship's right of way WR is a straight-line ship, and 0≤d CPA ≤d eCPA , 0≤t CPA ≤t eCPA ; then the ship needs to take immediate action to avoid collision, and the maneuvering time is t min =0, that is, the current time, the operating position P min =P OS , that is, the current position; otherwise, go to the next step; 4.3) If the ship's right of way WR is the give-way ship, and 0≤d CPA ≤d nCPA , t CPA >t nCPA , then record the maneuvering timing t for the current target ship j =t CPA -t nCPA ; or the ship's right of way WR is a straight-line ship, and 0≤d CPA ≤d eCPA , t CPA >t eCPA , then record the maneuvering timing t for the current target ship j =t CPA -t eCPA ; 4.4) After completing the calculation for all target ships, if the ship's right of way WR is the give-way ship, and the ship has d CPA >d nCPA or CPA <0; or the ship's right of way WR is a straight-line ship, and the ship is d to any target ship CPA >d eCPA or CPA <0; then the ship and all target ships do not pose a collision risk, and the collision avoidance maneuver timing t min = -∞; Otherwise, select the recorded operation timing list {t0, t1, t2, …, t n } to find the most recent manipulation time t i , collision avoidance maneuver time t min =t i , collision avoidance maneuvering position P min =P OS +v OS ×t min .
5. The method for ship heading and speed coordinated collision avoidance based on the International Regulations for Preventing Collisions at Sea according to claim 2 is characterized in that: The step 5) comprises the following steps: 5.1) If the ship's right of way WR is the give-way ship, and the ship has d CPA >d nCPA or CPA <0; or the ship's right of way WR is a straight-line ship, and the ship has d CPA >d eCPA or CPA <0; then the ship and all target ships do not pose a collision risk, the ship continues to maintain the direction and speed, and the calculation ends; otherwise, proceed to the next step; 5.2) If 0≤d CPA ≤d eCPA And 0≤ tCPA ≤t eCPA Regardless of whether the ship's right of way WR is for a give-way ship or a straight-line ship, the ship and at least one target ship are already in the emergency collision avoidance stage. Select the heading and speed coordinated collision avoidance mode and end the calculation; otherwise, proceed to the next step; 5.3) The right of way WR of this ship is the give-way ship. This ship and at least one target ship are in the negotiation and collision avoidance phase, that is, 0≤d CPA ≤d nCPA And 0≤t CPA ≤t nCPA ; and the ship and all target ships do not constitute emergency collision avoidance conditions, that is, they do not meet 0≤d CPA ≤d eCPA And 0≤t CPA ≤t eCPA ; If the initial collision avoidance mode M0 of the own ship is heading collision avoidance, then proceed to step 5.4); If the initial collision avoidance mode M0 of the own ship is speed collision avoidance, then proceed to step 5.5); 5.4) Perform fast local path planning; if the planned path can ensure that the ship and all target ships are within d eCPA If the ship passes the other ship, the ship will still maintain the heading collision avoidance mode; otherwise, the ship will switch to the heading and speed coordinated collision avoidance mode and end the calculation; 5.5) Perform fast local path planning; if the planned path can ensure that the ship and all target ships are within d eCPA If the other party passes and gives way, own ship still maintains speed collision avoidance mode; otherwise, own ship switches to heading and speed coordinated collision avoidance mode and ends the calculation.
6. The method for ship heading and speed coordinated collision avoidance based on the International Regulations for Preventing Collisions at Sea according to claim 2 is characterized in that: The step 6) comprises the following steps: 6.1) If the collision avoidance mode is the heading collision avoidance mode, the heading collision avoidance program based on the artificial potential field theory is run to obtain the recommended heading ψ desired ; 6.2) If the collision avoidance mode is selected as the speed collision avoidance mode, the speed collision avoidance program based on the linear search optimization method is run to obtain the recommended speed v desired ; 6.3) If the collision avoidance mode is the heading and speed coordinated collision avoidance mode, the heading collision avoidance program based on the artificial potential field theory and the speed collision avoidance program based on the linear search optimization method are run simultaneously to obtain the recommended heading ψ desired and recommended speed v desired .
7. The method for ship heading and speed coordinated collision avoidance based on the International Regulations for Preventing Collisions at Sea according to claim 6 is characterized in that: The step 6.2) comprises the following steps: 6.2.1) Calculate the minimum target collision avoidance speed of own ship for any target ship: minSpd = min{v min ,||v OS || / 2}; Calculate the maximum target collision avoidance speed: maxSpd = max{v min ,||v OS || / 2}; Select the speed search step size as Δv; establish the speed cost function for own ship collision avoidance: Among them, d CPAj (v i ) is the speed v i Next, the closest approach distance between own ship and target ship; 6.2.2) Use the linear search optimization method from {minSpd, maxSpd} to obtain the optimal collision avoidance speed optSpd for the ship against the target ship i ; If the optimal collision avoidance speed is not found, optSpdi = minSpd; If the optimal collision avoidance speed is found, optSpd is recorded i ; The number of ships participating in collision avoidance is nShip+1; Calculate the next target ship; 6.2.3) After all target ships are calculated, if nShip ≥ 1, select the optimal collision avoidance speed from the list {optSpd0, optSpd1, optSpd2, …, optSpd n Find the minimum collision avoidance speed optSpd in min , which is the expected collision avoidance speed of the ship: v opt =optSpd min Otherwise, there will be no collision risk between the ship and any target ship, and the expected collision avoidance speed of the ship is the design speed of this section, that is, v opt =v WP-i ; 6.2.4) According to the ship's speed v OS and target speed v opt , using PID controller to obtain the recommended speed v desired : Among them, K p >0 is the proportional constant; K d >0 is the differential constant; K i >0 is the integration constant, t is the execution time of the algorithm; 6.2.5) According to the recommended speed v desired The corresponding relationship between the ship's rotation speed and the speed is used to obtain the recommended rotation speed RPM desired .
8. The method for ship heading and speed coordinated collision avoidance based on the International Regulations for Preventing Collisions at Sea according to claim 1 is characterized in that: The collision avoidance rudder angle δ in step 7) desired for: Among them, K p >0 is the proportionality constant; K d >0 is the differential constant; K i >0 is the integration constant; Δψ c is the recommended heading change; is Δψ c The rate of change of is in radians / s; t is the execution time of the algorithm.
9. The method for ship heading and speed coordinated collision avoidance based on the International Regulations for Preventing Collisions at Sea according to claim 5, characterized in that: The fast local path planning comprises the following steps: 8.1) Based on the current own ship position P OS 、Speed v OS , rudder angle δ, speed RPM, and collision avoidance rudder angle δ desired and collision avoidance speed RPM desired , driving the solution of high-precision mathematical models of ship motion; 8.2) Based on the high-precision ship motion mathematical model, calculate the ship's position P in the next sampling period OS 、Speed v OS , rudder angle δ and speed RPM; the estimated own ship position P OS and speed v OS The turning points and speeds of the turning points as planned paths are stored in the turning point list WP. desired and the speed list v of the turning point WP-desired ; 8.3) Calculate the target ship positions P of all target ships in the next sampling period TS 、Speed v TS ; Calculate the distance d between the ship and all target ships, and the minimum encounter distance d CPA , minimum encounter time t CPA ; 8.4) According to the selected collision avoidance mode M1, adopt the corresponding collision avoidance procedure to obtain the recommended course ψ of the ship in the next sampling period desired and recommended speed v desired ; 8.5) Use PID controller to obtain the collision avoidance rudder angle δ of the ship in the next sampling period desired and collision avoidance speed RP Mdesired ; 8.6) Repeat the above steps until the number of iterations j = N, and obtain the local path planning result, that is, the turning point WP desired List and turn point speed v WP-desired List, end calculation.
Citation Information
Cited By
COLREG rule-based autonomous ship meeting collision avoidance method
CN120066047A
