CN112965475A - Obstacle collision prevention method based on dynamic navigation ship domain and collision prevention rule - Google Patents
Obstacle collision prevention method based on dynamic navigation ship domain and collision prevention rule Download PDFInfo
- Publication number
- CN112965475A CN112965475A CN202110088455.3A CN202110088455A CN112965475A CN 112965475 A CN112965475 A CN 112965475A CN 202110088455 A CN202110088455 A CN 202110088455A CN 112965475 A CN112965475 A CN 112965475A
- Authority
- CN
- China
- Prior art keywords
- ship
- unmanned
- interference
- collision avoidance
- domain
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Withdrawn
Links
- 238000000034 method Methods 0.000 title claims abstract description 32
- 230000002265 prevention Effects 0.000 title claims abstract description 25
- 230000002452 interceptive effect Effects 0.000 claims description 31
- 230000005484 gravity Effects 0.000 claims description 12
- 230000033001 locomotion Effects 0.000 claims description 8
- 238000004364 calculation method Methods 0.000 claims description 3
- 239000011159 matrix material Substances 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 6
- 230000008859 change Effects 0.000 description 5
- LFULEKSKNZEWOE-UHFFFAOYSA-N propanil Chemical compound CCC(=O)NC1=CC=C(Cl)C(Cl)=C1 LFULEKSKNZEWOE-UHFFFAOYSA-N 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000004422 calculation algorithm Methods 0.000 description 2
- 150000001875 compounds Chemical class 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000005265 energy consumption Methods 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 241000282412 Homo Species 0.000 description 1
- 230000003044 adaptive effect Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000033228 biological regulation Effects 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/0206—Control of position or course in two dimensions specially adapted to water vehicles
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Traffic Control Systems (AREA)
Abstract
The invention relates to an obstacle collision prevention method based on a dynamic navigation ship domain and international maritime collision prevention rules, which comprises the following steps: acquiring basic information of the unmanned ship and the interference ship according to the unmanned ship sensor and the ship automatic identification system, determining the relative speed and the relative azimuth angle beta of the unmanned ship and the interference ship, and determining the meeting situation between the ships based on the collision avoidance rule; according to the size, maneuverability, relative speed and relative azimuth of the meeting ship, establishing a dynamic navigation ship domain by respectively taking the unmanned ship and the interference ship as centers; judging whether the ship domains of the unmanned ship and the interference ship are intersected, if so, having collision risk, carrying out collision avoidance operation, determining a collision avoidance operation mode when encountering a single interference ship and a plurality of interference ships according to a collision avoidance rule, determining local collision avoidance waypoints, planning a collision avoidance path and judging when to finish collision avoidance. The unmanned ship can navigate along the pre-planned path and avoid the obstacles, so that a foundation is provided for safe navigation of the ship.
Description
Technical Field
The invention relates to an autonomous dynamic collision avoidance method for an unmanned ship, in particular to an obstacle collision avoidance method based on a dynamic navigation ship domain and collision avoidance rules.
Background
Unmanned ships can be applied in various fields of science, civil use and military, have the advantages of low energy consumption and low labor cost, and are vigorously developed in many countries. It has been studied that about 50% of accidents at sea are caused by human error, and the other 30% are discovered and prevented by humans. By using the intelligent anti-collision system, the unmanned ship can avoid human errors and reduce loss. The unmanned ship path planning is divided into global path planning and local path planning, wherein the local path planning determines a local collision avoidance path through a collision avoidance algorithm according to obstacle information dynamically detected by a sensor, and the local path planning is the core of an unmanned ship collision avoidance system.
Current unmanned ship collision avoidance generally determines the risk of collision based on the ship domain or minimum distance encountered (DCPA), determining when to begin collision avoidance. The DCPA method is equivalent to determining a collision risk by using a circular ship domain, and the collision risk of the ship cannot be fully estimated and a collision avoidance decision cannot be made because the meeting state of the ship cannot be considered. The ship domain is a safe area that must be maintained around the ship while it is underway. There are four standards for using the ship domain to perform safety judgment on the ship and the interfering ship in different encounter states, including: (a) the ship area of the ship is not invaded; (b) does not infringe the other ship domain; (c) the ship areas of the two parties are not infringed with each other; (d) the two ship domains do not intersect. Based on these criteria, with the domain of vessels, it is possible to judge the risk of collision and to determine the time for collision avoidance operations. However, existing research on ship domains answers less questions about how to perform local collision avoidance path planning.
Early statistical ship domains were generally established based on captain determinations, ignoring factors such as ship speed and encounter patterns, and were static models, not considering the impact of ships on collision hazards when sailing at different ship speeds. In addition, the ship maneuvering characteristics have a great influence on the ship collision avoidance time, and ships with poor maneuvering characteristics should maintain a greater safety distance. Thus, an analytic dynamic ship domain, such as a quaternary ship domain, is provided, the ship domain is a combined ellipse defined by four radii, and the sizes of the radii comprehensively consider ship maneuverability, ship speed, course and other information. However, the ship domain is determined by four radii, and the planning of the collision avoidance path directly is complex.
In addition, the conventional collision avoidance algorithm ends collision avoidance when the distance between the unmanned ship and the target ship exceeds a fixed distance, which results in an excessively long collision avoidance track and increases the operation cost. In addition, in order to achieve a consistent understanding of collision avoidance operations among ships, all ships sailing at sea should comply with the regulations of international rules for collision avoidance at sea (abbreviated as "collision avoidance rules"), otherwise collision risks will be caused due to the inability to understand the collision avoidance behavior of the other party.
Disclosure of Invention
Technical problem to be solved
In order to solve the above problems in the prior art, the present invention provides an obstacle collision avoidance method based on Dynamic Navigation Ship Domain (DNSD) and collision avoidance rules. The design of the ship domain fully considers the influence of factors such as the unmanned ship control characteristic, the ship size, the relative speed and the relative azimuth angle between meeting ships and the like and the difference of safe meeting distances between the bow and the stern, so that the dynamic navigation ship domain with variable size is formed, and the risk of ship collision avoidance is reflected in real time. In addition, the collision prevention operation is determined according to different meeting situations based on the collision prevention rule, and the time when the collision prevention operation is finished is judged, so that a decision basis is provided for dynamic collision prevention among ships, and the navigation cost is reduced.
(II) technical scheme
In order to achieve the purpose, the invention adopts the main technical scheme that:
101. acquiring basic information of the unmanned ship and the interference ship through an unmanned ship sensor and an automatic ship identification system, determining the relative speed and the relative azimuth angle beta of the unmanned ship and the interference ship, and determining the meeting situation between the ships based on a collision avoidance rule;
102. according to the size, maneuverability, relative speed and relative azimuth of the meeting ship, establishing a dynamic navigation ship domain by respectively taking the unmanned ship and the interference ship as centers;
103. judging whether the ship domains of the unmanned ship and the interference ship are intersected, if so, having collision risks, carrying out collision avoidance operation, determining a collision avoidance operation mode when encountering a single interference ship and a plurality of interference ships according to a collision avoidance rule, determining local collision avoidance waypoints, planning a collision avoidance path and judging when to finish collision avoidance.
As a refinement of the method of the invention, 101 comprises: the information obtained by the ship sensor and the automatic identification system comprises the captain, the ship speed, the course, the position and the like of the interference ship, and the self ship speed, the course, the position, the captain and the unmanned ship control performance indexes of the unmanned ship: dead center, center of gravity lateral movement distance, center of gravity longitudinal movement distance, and the like.
As an improvement of the method of the present invention, 101 further comprises: determining meeting situations among ships based on collision avoidance rules, wherein the meeting situations include pursuing situations, confrontation situations and crossing situations. To determine the encounter, a relative azimuth β between the unmanned and interfering vessels is defined. The encounter situation is selected to be 30 degrees with the heading of the interfering ship as the center, the cross situation is 97.5 degrees on each side, and the rest ranges are regarded as the pursuing situation. The relative azimuth angle β is calculated as follows:
wherein
The course of the most dangerous interference ship determined by the unmanned ship; (x)TS,yTS) Determining the most dangerous interference ship position for the unmanned ship; (x)OS,yOS) Is the self position of the unmanned ship.
As an improvement of the method of the present invention, 102 comprises: according to the dimension, maneuverability, relative speed and relative azimuth angle of the meeting ship, a dynamic navigation ship domain is respectively established by taking the interference ship and the unmanned ship as centers, the dynamic navigation ship domain is composed of a semi-ellipse and a semi-circle, and the dynamic navigation ship domain is only composed of two radiuses (including R)fAnd Rs) And (4) determining. The model is easy to realize local obstacle avoidance path planning. In order to facilitate the determination of the dynamic navigation vessel domain, at the origin of the terrestrial coordinate system
A dynamic navigation vessel domain is established in the coordinate system.
And
the axes are oriented in the same direction as the x and y axes, respectively. The dynamic navigation vessel domain may be expressed as follows:
where sgn (·) is a sign function defined as:
the determination of the ellipse and circle radius takes into account the encountered vessel dimensions, maneuverability and relative speed, relative azimuth and encounter. The dynamic radius is described as follows:
in the formula (I), the compound is shown in the specification,
is the relative speed of the unmanned ship and the interfering ship, whereinvOS is the velocity of the unmanned ship;vTS is the speed of the interfering ship; i | · | purple wind2Is a two-norm. C is a constant; re is the dead distance of the unmanned ship, and when the distance between the two ships is smaller than the sum of the dead distances, the unmanned ship is considered to be unable to avoid, and is about 1-2 times of the length of the ship; b is the width of the ship; DT is the transverse distance of the gravity center when the unmanned ship turns 180 degrees, and is about 3-6 times of the ship length; AD is a short distance, namely a longitudinal distance, which is a longitudinal advancing distance of the gravity center when the unmanned ship rotates 90 degrees from the turning, and is about 0.6-1.2 times of DT;
is a dynamic safety domain coefficient, the value of which depends on the unmanned ship and theThe relative azimuth angle beta of the interference ship is 0 degree, the relative speed is the largest at the moment, the influence of AD on the dynamic security domain is larger, so ki is larger at the moment, the relative speed is the smallest at the moment when the relative azimuth angle is 180 degrees, the influence of DT on the dynamic security domain is larger, and therefore the coefficient of ki is smaller at the moment. And s is a constant with the value range of 0.8-1.2, the value of s depends on the main scale of the ship, and the coefficient of ki can be finely adjusted.
As a refinement of the method of the invention, 103 comprises: when the dynamic navigation ship domain of the unmanned ship and the dynamic navigation ship domain of the interference ship are intersected and the distance is gradually shortened along with time, the two ships are considered to have collision risks, at the moment, collision prevention operation is executed, and collision prevention points are planned according to meeting situations and collision prevention rules; otherwise, the path tracing is continuously executed.
In order to judge whether the ship domains intersect, the dynamic navigation ship domains of the unmanned ship and the interference ship need to be operated. First, it should be rotated around the origin
Dynamic navigation zones defined in coordinates such that
The axis points to the course of the vessel. Then, the ship domain is converted into a ship body coordinate system obxbybzbOf the origin. The coordinate rotation matrix is defined as:
a ship domain curve can then be obtained:
wherein (x)DNSD,yDNSD) Is the coordinate of the dynamic navigation ship domain in the terrestrial coordinate system.
According to the collision prevention rule, the unmanned ship should turn right to avoid collision in the meeting and crossing situations; for the overtaking situation, the unmanned ship can turn to the right or the left to avoid collision.
The method for determining the collision avoidance control steering in the overtaking situation comprises the following steps: dividing a collision avoidance operation area through a connecting line of the position of the unmanned ship and the position of the interference ship, and if the next global target point is on the right side of the connecting line, the unmanned ship passes through the starboard; otherwise, pass from port.
As a refinement of the method of the invention, 103 further comprises: calculating the position of a collision avoidance point according to the position of the interference ship and the DNSD; the calculation formula of the collision avoidance point of the unmanned ship is as follows:
Wherein R ═ R1×Rs(OS)+r2×Rs(TS)Is a parameter for adjusting waypoints, which is represented by r1And r2And adjusting the range of the content to be (0-1).
As a refinement of the method of the invention, 103 further comprises: and calculating when the unmanned ship finishes collision avoidance so as to switch to the path tracking mode. When the unmanned ship has passed the boundary line of the semi-ellipse and the semi-circle of the interference ship dynamic navigation ship domain, and the current distance between the unmanned ship and the interference ship is larger than the distance (d) at the previous momentt>dt-1) And switching the obstacle avoidance mode to a path following mode so that the unmanned ship tracks the next target point.
(III) the beneficial effects are as follows:
the invention has the beneficial effects that:
(1) whether the dynamic navigation domains of the unmanned ship and the interference ship are intersected or not is used for determining collision risk and when collision avoidance begins, factors such as ship speed, course, position and ship length of the unmanned ship and the interference ship, factors such as operating performance indexes (lag, center-of-gravity transverse moving distance and center-of-gravity longitudinal moving distance) of the unmanned ship and the interference ship are considered, the collision risk can be changed according to the change of a meeting state, the problem that cost is increased due to the fact that an invalid collision avoiding path is too long due to too early collision avoidance operation is effectively avoided, and collision avoidance failure is caused due to the fact that the collision avoidance operation is too late is effectively avoided.
(2) The collision prevention points are determined based on collision prevention rules and the dynamic navigation ship domain of the unmanned ship and the interference ship, the method is simple, and the collision risk of the unmanned ship and the interference ship is effectively avoided;
(3) the unmanned ship is far away from the dynamic navigation ship domain of the interference ship instead of driving to the collision avoidance point to judge when collision avoidance is finished, so that the problem that collision avoidance energy consumption is increased due to the fact that an invalid collision avoidance path is too long as collision avoidance is finished too late is effectively solved.
Drawings
FIG. 1 is a flow chart of a method for obstacle avoidance based on dynamic navigation domains and avoidance rules;
FIG. 2 is a meeting state based on the international maritime collision avoidance rule;
FIG. 3 is a schematic view of a dynamic navigation boat domain;
FIG. 4 is a collision avoidance rule based unmanned ship and interfering ship avoidance scenario;
FIG. 5 is a schematic view of collision avoidance waypoints of the unmanned ship;
FIG. 6 is a schematic diagram of ending obstacle avoidance;
FIG. 7 is a schematic view of unmanned ship collision avoidance in the case of a single interfering ship;
FIG. 8 is a schematic view of unmanned ship collision avoidance in the case of multiple interfering ships;
FIG. 9 is a schematic view of a dynamic navigation vessel domain change for a single interfering vessel;
FIG. 10 is a schematic view of dynamic navigation vessel domain changes for a plurality of interfering vessels;
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention will be described in further detail below with reference to the accompanying drawings and examples.
All technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention belongs. The terminology used in the description of the embodiments of the invention herein is for the purpose of describing particular embodiments only and is not intended to be limiting of the invention. As used in connection with the examples of the invention, the term "and/or" includes any and all combinations of one or more of the associated listed items.
The invention provides an obstacle collision avoidance method based on a dynamic navigation ship domain and collision avoidance rules, on the basis of collision avoidance of an unmanned ship, the influence of an interference ship on the unmanned ship is comprehensively considered. The ship domain adaptive to the current navigation environment is established in real time aiming at the interference ship encountered by the unmanned ship in the marine environment navigation, and the established ship domain model can change in real time according to the meeting state of the unmanned ship and the interference ship and the difference of the operating characteristics of the unmanned ship, so that a foundation can be provided for the safe navigation of the unmanned ship.
Fig. 1 is a flowchart of an obstacle collision avoidance method based on a dynamic navigation ship domain and collision avoidance rules according to an embodiment of the present invention, as shown in fig. 1, the method includes the following steps:
the method comprises the following steps: acquiring basic information of the unmanned ship and the interference ship through an unmanned ship sensor and an automatic ship identification system, determining relative speed and relative azimuth angle beta between the unmanned ship and the interference ship, and determining a meeting situation between the ships based on a collision avoidance rule;
step two: according to the size, maneuverability, relative speed and relative azimuth of the meeting ship, establishing a dynamic navigation ship domain by respectively taking the unmanned ship and the interference ship as centers;
step three: judging whether the dynamic navigation ship domains of the unmanned ship and the interference ship are intersected or not according to the determined dynamic navigation ship domain, if so, executing the step four, and if not, executing the step six;
step four: calculating collision points according to the collision prevention rule and the dynamic navigation ship domain, and selecting proper collision prevention points according to the relative direction angle beta and the collision prevention rule so as to obtain a collision prevention path;
step five: judging whether collision avoidance is finished, if not, continuing to move towards a collision avoidance point, otherwise, turning to the sixth step;
step six: and executing the path tracking mode, and judging whether the target point is reached. If not, the step one is shifted, otherwise, the whole process is ended.
The specific implementation of the steps of the embodiment shown in fig. 1 is described in detail below:
and step one, acquiring basic information of the unmanned ship and the interference ship through the unmanned ship sensor and the ship automatic identification system.
In one example of the present invention, the information obtained by the ship sensor and the automatic identification system includes ship speed, course, position, ship length, etc. of the interfering ship, and also includes ship speed, course, position, ship length of the unmanned ship and steering performance indexes (dead distance, center of gravity lateral movement distance, center of gravity longitudinal movement distance) determined by a convolution experiment.
In the first step, the relative speed and the relative azimuth angle beta between the unmanned ship and the target ship are determined, and the meeting situation between the ships is determined based on the collision avoidance rule.
In one embodiment of the invention, the meeting situation determined according to the collision avoidance rule comprises a pursuit situation, an encounter situation and a cross situation. To determine the encounter, a relative azimuth β between the unmanned and interfering vessels is defined. The meeting situation is selected to be 30 degrees with the unmanned ship course as the center, the intersection situation is 97.5 degrees on each side, and the rest angles are regarded as the overtaking situation, and fig. 2 shows different meeting situations based on the collision avoidance rule. The relative azimuth angle β is calculated as follows:
wherein
The course of the most dangerous interference ship determined by the unmanned ship; (x)TS,yTS) Determining the most dangerous interference ship position for the unmanned ship; (x)OS,yOS) Is the self position of the unmanned ship.
And in the second step, according to the dimension, maneuverability, relative speed and relative azimuth angle of the meeting ship, respectively establishing a dynamic navigation ship domain by taking the unmanned ship and the interference ship as centers. In one embodiment of the invention, the selection of the most dangerous interference ship is crucial to the unmanned ship, so the invention determines the most dangerous interference ship in the unmanned ships by using the nearest meeting distance method, and if a plurality of the most dangerous interference ships in the unmanned ships exist, the unmanned ships randomly select the most dangerous interference ship.
In one embodiment of the invention, FIG. 3 is a schematic view of a dynamic navigation boat domain. The ship domain refers to the sailing safety range of the unmanned ship, and once the unmanned ship enters the ship domain of the interference ship, the unmanned ship has a great collision risk. Because the ship length, the ship speed, the course and the position of the ships of both parties are met, the collision avoidance response distance is influenced by the factors such as the self-control characteristic of the unmanned ship, and the like, thereby influencing the size of the security domain. For example, the larger the relative ship speed, the larger the collision avoidance response distance, and the larger the ship domain. The safe meeting distance of the bow and the stern is different under the same meeting condition, the safe meeting distance of the bow is long, and the safe meeting distance of the stern is short. The ship domain cannot be defined as a fixed circular area or an elliptical area but can be changed in real time according to the meeting state and the handling characteristics of the unmanned ship itself. The design of the dynamic navigation ship domain of the invention fully considers the influence of factors such as the unmanned ship control characteristic, the ship size, the relative speed and the relative azimuth angle between meeting ships and the like and the difference of safe meeting distances between the bow and the stern under the same meeting condition, thereby forming the dynamic navigation ship domain with variable size range.
The dynamic navigation ship domain consists of a semi-ellipse and a semi-circle, and the dynamic navigation ship domain is established by respectively taking an unmanned ship and an interference ship as centers according to the dimension, maneuverability, relative speed and relative azimuth angle of a meeting ship. The model is easy to realize local obstacle avoidance path planning. In order to facilitate the determination of the dynamic navigation vessel domain, at the origin of the terrestrial coordinate system
A dynamic navigation vessel domain is established in the coordinate system.
And
the axes are oriented in the same direction as the x and y axes, respectively. The specific dynamic navigation vessel domain formula can be written as follows:
where sgn (·) is a sign function defined as:
the determination of the ellipse and circle radius takes into account the encountered vessel dimensions, maneuverability and relative speed, relative azimuth and encounter. The dynamic boat domain radius is described as follows:
in the formula (I), the compound is shown in the specification,
is the relative speed of the unmanned ship and the interfering ship, whereinvOS is the velocity of the unmanned ship;vTS is the speed of the interfering ship; i | · | purple wind2Is a two-norm. C is a constant; re is the dead distance of the unmanned ship, and when the distance between the two ships is smaller than the sum of the dead distances, the unmanned ship is considered to be unable to avoid, and is about 1-2 times of the length of the ship; b is the width of the ship; DT is the transverse distance of the gravity center when the unmanned ship turns 180 degrees, and is about 3-6 times of the ship length; the AD is a short distance, namely a longitudinal distance, which is the longitudinal advancing distance of the gravity center when the unmanned ship rotates 90 degrees from the turning, and is about 0.6-1.2 times of the DT.
Is a dynamic safety domain coefficient, the value of which depends on the relative azimuth angle beta of the unmanned ship and the interference shipWhen the azimuth angle is 0 degrees, the relative speed is the largest at this time, the influence of AD on the dynamic security domain is larger, so ki is larger at this time, when the relative azimuth angle is 180 degrees, the relative speed is the smallest at this time, the influence of DT on the dynamic security domain is larger, so the coefficient of ki is smaller at this time. And s is a constant with the value range of 0.8-1.2, the value of s depends on the main scale of the ship, and the coefficient of ki can be finely adjusted.
And in the third step, judging whether the dynamic navigation ship domain of the unmanned ship is intersected with the dynamic navigation ship domain of the interference ship or not according to the determined dynamic navigation ship domain, if so, executing the fourth step, and if not, executing the sixth step.
In one example of the present invention, when the dynamic navigation ship domains of the unmanned ship and the interfering ship intersect and the distance gradually decreases with time, it is considered that collision avoidance should be performed at this time, and the unmanned ship switches from the path tracking mode to the collision avoidance mode; otherwise, the unmanned ship keeps a path tracking mode and sails to the next path point at a fixed speed. Whether the unmanned ship should avoid collision can be described by the following formula:
when the dynamic navigation ship domains of the unmanned ship and the interference ship do not intersect, the RSK is 0, at the moment, a path tracking mode should be executed, otherwise, a collision avoidance mode should be executed, and a collision avoidance point is determined after the collision avoidance mode is executed.
In order to judge whether the ship domains intersect, the dynamic navigation ship domains of the unmanned ship and the interference ship need to be operated. First, it should be rotated around the origin
Dynamic navigation zones defined in coordinates such that
The axis points to the course of the vessel. Then, the ship domain is converted into a ship body coordinate system obxbybzbOf the origin. The coordinate rotation matrix is defined as:
a ship domain curve can then be obtained:
wherein (x)DNSD,yDNSD) Is the coordinate of the dynamic navigation ship domain in the terrestrial coordinate system.
In the fourth step, calculating collision avoidance points according to a collision avoidance rule and a dynamic navigation ship domain, and selecting proper collision avoidance points according to a relative direction angle beta and the collision avoidance rule so as to obtain a collision avoidance path;
in one embodiment of the present invention, fig. 4 is a collision avoidance rule-based avoidance scenario of an unmanned ship and an interfering ship; for the encounter and crossing situation, the unmanned ship should turn right to avoid collision; for the overtaking situation, the unmanned ship can turn to the right or the left to avoid collision.
The method for determining the collision avoidance control steering in the overtaking situation comprises the following steps: dividing a collision avoidance operation area through a connecting line of the position of the unmanned ship and the position of the interference ship, and if the next global target point is on the right side of the connecting line, the unmanned ship passes through the starboard; otherwise, pass from port.
FIG. 5 is a schematic diagram of an embodiment of the invention for calculating a collision avoidance point location based on the location of an interfering vessel and the dynamic navigation territory of the interfering vessel;
the calculation formula of the collision avoidance point of the unmanned ship is as follows:
Wherein R ═ R1×Rs(OS)+r2×Rs(TS)Is a parameter for adjusting waypoints, which is represented by r1And r2Adjustment of the range thereofBetween (0-1), wherein r1And r2The value of (c) can be determined by experiment. In the present invention, we have demonstrated that in the cross and head-on cases, when r is10.8 and r20.9, and in the case of overtaking, r10.5 and r2When the value is 0.6, the collision avoidance performance is the best. (x)wp1,ywp1) And (x)wp2,ywp2) And selecting a proper collision prevention point according to the collision prevention rule and the relative azimuth angle between the unmanned ship and the interference ship for determining the collision prevention point.
And step five, judging whether the collision avoidance is finished, if not, continuing to move towards the collision avoidance point, and otherwise, turning to step six.
In the obstacle avoidance mode, the unmanned ship carries out obstacle avoidance operation according to the collision avoidance rule and the re-planned path track, so that the collision risk is gradually reduced. Therefore, it is necessary to determine when the obstacle avoidance mode is completed and switch the loop path tracking mode. Obstacle avoidance operations are performed when the dynamic navigation vessel domains of the unmanned and interfering vessels intersect, and if this mode is switched out without the vessel domains overlapping, a long and unnecessary obstacle avoidance path results. Therefore, we use a more reasonable approach to decide when to complete the obstacle avoidance mode. As shown in fig. 6, when the unmanned ship has passed through the boundary line of the semi-ellipse and the semi-circle of the dynamic navigation ship domain determined by the interfering ship, and the current distance between the unmanned ship and the interfering ship is greater than the distance (d) at the previous time (d)t>dt-1) And switching the obstacle avoidance mode to a path tracking mode so that the unmanned ship can track the next target point.
Specifically, in one embodiment of the present invention, the motion parameters of a single interfering vessel and a plurality of interfering vessels, as well as the unmanned vessel, are set as shown in table 1: OS stands for unmanned ship and TS stands for interfering ship.
TABLE 1 unmanned and interfering vessel motion parameter settings
Fig. 7 is a diagram showing the collision avoidance effect of a single interfering vessel, and the simulation result of the cross meeting situation is shown in fig. 7 (a). It can be seen that when t is 6s, the unmanned ship and the dynamic navigation ship domain of the interference ship intersect, the unmanned ship switches to the obstacle avoidance mode, plans an avoidance path to determine a new path point, and the unmanned ship performs avoidance maneuver according to the avoidance rule. Furthermore, we can see that at t 11.5s the drone is assessed as safe and then switches back to the original path tracking mode. This greatly speeds up the avoidance efficiency. And finally, when t is 31.5s, the unmanned ship reaches the target point. Similarly, the simulation results for the chase and encounter scenarios are shown in fig. 7(b) and (c), respectively.
Fig. 8 is a diagram illustrating collision avoidance effects of a plurality of interfering ships, where an unmanned ship starts to switch to an obstacle avoidance mode when t is 7s, and successfully avoids TS1 and switches to a path tracking mode when t is 12.5 s. The unmanned ship meets TS2 at t 18s and starts collision avoidance, and successfully avoids TS2 at t 24 s. Finally, the unmanned ship meets TS3 at t 28s and starts collision avoidance, successfully avoids TS3 at t 41s, and reaches the destination by route tracking at t 47 s. When a plurality of interference ships are encountered, the unmanned ship detects the most dangerous interference ship by using a DCPA method, if collision danger exists, the interference ship starts to be avoided, and after the avoidance is finished, the re-navigation is started. And when the collision danger with a third interfering ship is detected, the third interfering ship is avoided.
Fig. 9 and 10 are schematic diagrams of the dynamic navigation ship domain change of a single interference ship and a plurality of interference ships respectively, and it can be seen from the diagrams that the dynamic navigation ship domain changes continuously along with the change of time. When the unmanned ship and the interference ship are at high collision risk, in order to reduce the collision risk and execute more reliable collision avoidance maneuver, the R of the dynamic navigation ship domain is increased according to the corresponding meeting situationfAnd Rs. When the unmanned ship is at a relatively low collision risk, the ship domain size will be reduced to improve collision avoidance efficiency.
Claims (3)
1. A method for preventing collision of obstacles based on a dynamic navigation ship domain and collision prevention rules is characterized by comprising the following steps:
101. acquiring basic information of the unmanned ship and the interference ship through an unmanned ship sensor and a ship automatic identification system, determining relative speed and relative azimuth angle beta of the unmanned ship and the interference ship, and determining a meeting situation between the ships based on a collision avoidance rule, wherein the step 101 comprises the following steps:
the basic information of the ship comprises the captain, the ship speed, the course, the position and the like of the interference ship, and the self-speed, the course, the position, the captain and the operation performance indexes of the unmanned ship, such as: lag, center of gravity lateral movement distance, center of gravity longitudinal movement distance, etc.; determining meeting situations among ships according to collision avoidance rules, wherein the meeting situations comprise a pursuing situation, an advection situation and a cross situation;
102. according to the size, maneuverability, relative speed and relative azimuth of the meeting ship, establishing a dynamic navigation ship domain by respectively taking the unmanned ship and the interference ship as centers; if a plurality of interference ships exist near the unmanned ship, determining the interference ship which is the most dangerous of the interference ships by using a nearest meeting distance method, and then determining a dynamic navigation ship domain; if a plurality of interference ships with the most danger exist in the unmanned ships, the unmanned ships can randomly select one interference ship with the most danger;
103. judging whether the dynamic navigation ship domains of the unmanned ship and the interference ship are intersected, if so, having collision risk, carrying out collision avoidance operation, determining a collision avoidance operation mode when encountering a single interference ship and a multi-interference ship according to a collision avoidance rule, determining local collision avoidance waypoints, planning a collision avoidance path and judging when to finish collision avoidance, wherein the step 103 comprises the following steps of:
if the dynamic navigation ship domain of the unmanned ship is intersected with the dynamic navigation ship domain of the interference ship, and the distance is gradually shortened along with time, the two ships are considered to have collision risks, at the moment, collision avoidance operation is executed, and collision avoidance points are planned according to meeting situations and collision avoidance rules; otherwise, continuing to execute the path tracking; according to the collision prevention rule, the unmanned ship should turn right to avoid collision in the meeting and crossing situations; for the overtaking situation, the unmanned ship can turn to the right or the left to avoid collision;
the method for determining the collision avoidance control steering in the overtaking situation comprises the following steps: dividing a collision avoidance operation area through a connecting line of the position of the unmanned ship and the position of the interference ship, and if the next global target point is on the right side of the connecting line, the unmanned ship passes through the starboard; otherwise, passing from port;
calculating the position of the collision avoidance point according to the position of the interference ship and the dynamic navigation ship domain, wherein the calculation formula of the collision avoidance point of the unmanned ship is as follows:
Wherein R ═ R1×Rs(OS)+r2×Rs(TS)The parameters for adjusting waypoints.
The method for calculating when the unmanned ship finishes collision avoidance and switching to the path tracking mode comprises the following steps: when the unmanned ship has passed the boundary line of the semi-ellipse and the semi-circle of the interference ship dynamic navigation ship domain, and the current distance between the unmanned ship and the interference ship is larger than the distance (d) at the previous momentt>dt-1) And switching the obstacle avoidance mode to a path following mode so that the unmanned ship tracks the next target point.
2. The method of claim 1, wherein the method comprises the following steps: the 102 further comprises:
according to the size, maneuverability, relative speed and relative azimuth of the meeting ship, a dynamic navigation ship domain is respectively established by taking the interference ship and the unmanned ship as centers, and the dynamic navigation ship domain can be expressed as follows:
where sgn (·) is a sign function defined as:
the determination of the dynamic navigation ship domain takes the ship size, maneuverability, relative speed, relative azimuth and meeting situation into consideration; the dynamic radius is described as follows:
in the formula, RfAnd RsTwo radii of the dynamic navigation ship domain are respectively;
is the relative speed of the unmanned ship and the interfering ship, whereinvOS is the velocity of the unmanned ship;vTS is the speed of the interfering ship; i | · | purple wind2Is a two-norm; c is a constant; re is the dead distance of the unmanned ship; b is the width of the ship; DT is the transverse distance of the gravity center when the unmanned ship turns 180 degrees; AD is a short distance and also called a longitudinal distance, which is the longitudinal advancing distance of the gravity center when the unmanned ship rotates 90 degrees from the turning;
is the dynamic security domain coefficient.
3. The method of claim 1, wherein the method comprises the following steps: the 103 further comprises:
and judging whether the ship domains intersect according to the determined ship domains, wherein the following operations are carried out: first, it should be rotated around the origin
Dynamic navigation vessel domain defined in a coordinate system
The shaft points to the course of the ship; then, the ship domain is converted into a ship body coordinate system obxbybzbThe origin of (a); the coordinate rotation matrix is defined as:
a ship domain curve can then be obtained:
wherein (x)DNSD,yDNSD) Is the coordinate of the dynamic navigation ship domain in the terrestrial coordinate system.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110088455.3A CN112965475A (en) | 2021-01-22 | 2021-01-22 | Obstacle collision prevention method based on dynamic navigation ship domain and collision prevention rule |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110088455.3A CN112965475A (en) | 2021-01-22 | 2021-01-22 | Obstacle collision prevention method based on dynamic navigation ship domain and collision prevention rule |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN112965475A true CN112965475A (en) | 2021-06-15 |
Family
ID=76271414
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110088455.3A Withdrawn CN112965475A (en) | 2021-01-22 | 2021-01-22 | Obstacle collision prevention method based on dynamic navigation ship domain and collision prevention rule |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112965475A (en) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114379718A (en) * | 2021-12-22 | 2022-04-22 | 南京联迪信息系统股份有限公司 | Early warning and rescue method and system for distress of pleasure boat |
| CN115410420A (en) * | 2022-10-31 | 2022-11-29 | 河北东来工程技术服务有限公司 | Ship safe navigation management method and system |
| CN116610125A (en) * | 2023-05-26 | 2023-08-18 | 北鲲睿航科技(上海)有限公司 | Collision prevention method and system for intelligent ship active collision avoidance system |
| CN117826824A (en) * | 2024-01-18 | 2024-04-05 | 交通运输部水运科学研究所 | Ship self-adaptive autonomous navigation decision-making method and device and electronic equipment |
-
2021
- 2021-01-22 CN CN202110088455.3A patent/CN112965475A/en not_active Withdrawn
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114379718A (en) * | 2021-12-22 | 2022-04-22 | 南京联迪信息系统股份有限公司 | Early warning and rescue method and system for distress of pleasure boat |
| CN114379718B (en) * | 2021-12-22 | 2023-05-30 | 南京联迪信息系统股份有限公司 | Method and system for early warning and rescuing in distress of pleasure boat |
| CN115410420A (en) * | 2022-10-31 | 2022-11-29 | 河北东来工程技术服务有限公司 | Ship safe navigation management method and system |
| CN116610125A (en) * | 2023-05-26 | 2023-08-18 | 北鲲睿航科技(上海)有限公司 | Collision prevention method and system for intelligent ship active collision avoidance system |
| CN116610125B (en) * | 2023-05-26 | 2024-01-30 | 北鲲睿航科技(上海)有限公司 | Collision prevention method and system for intelligent ship active collision avoidance system |
| CN117826824A (en) * | 2024-01-18 | 2024-04-05 | 交通运输部水运科学研究所 | Ship self-adaptive autonomous navigation decision-making method and device and electronic equipment |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112965475A (en) | Obstacle collision prevention method based on dynamic navigation ship domain and collision prevention rule | |
| CN107168335B (en) | Water surface unmanned ship path tracking guidance method considering hybrid multi-target obstacle avoidance | |
| CN109597417B (en) | Multi-USV group collaborative collision avoidance planning method based on collision avoidance criterion | |
| Svec et al. | Dynamics-aware target following for an autonomous surface vehicle operating under COLREGs in civilian traffic | |
| CN110083159B (en) | Unmanned ship autonomous dynamic collision avoidance method based on SBG and dynamic window constraint | |
| CN112965477A (en) | Obstacle collision prevention method based on dynamic security domain and collision prevention rule | |
| CN113759939B (en) | Intelligent navigation method and device for limited water area | |
| CN113096446B (en) | Multi-ship collision avoidance decision-making method under hybrid navigation scene, storage medium and processor | |
| Shi et al. | Study on intelligent collision avoidance and recovery path planning system for the waterjet-propelled unmanned surface vehicle | |
| CN112578793B (en) | Obstacle avoiding method for fast moving surface boat | |
| CN114355899A (en) | Unmanned ship collision avoidance method based on dynamic navigation ship domain and speed obstacle | |
| CN111580518A (en) | Unmanned ship layered obstacle avoidance method based on improved drosophila optimization and dynamic window method | |
| Han et al. | A COLREGs-compliant guidance strategy for an underactuated unmanned surface vehicle combining potential field with grid map | |
| CN114895673A (en) | Ship collision avoidance decision method based on deep reinforcement learning under rule constraint | |
| Liu et al. | COLREGS-based collision avoidance algorithm for unmanned surface vehicles using modified artificial potential fields | |
| CN116360434A (en) | Ship path planning method based on improved CSAC-APF algorithm | |
| Yang et al. | Parallel trajectory planning for shipborne Autonomous collision avoidance system | |
| Mei et al. | COLREGs based navigation of riverine Autonomous Surface Vehicle | |
| Wu et al. | Multi-vessels collision avoidance strategy for autonomous surface vehicles based on genetic algorithm in congested port environment | |
| Li et al. | Modelling and simulation of intelligent collision avoidance based on ship domain | |
| Yu et al. | A time dimension-added multiple obstacles avoidance approach for unmanned surface vehicles | |
| CN115755907A (en) | Autonomous collision avoidance method for unmanned inland river ship | |
| Wu et al. | Deliberative collision avoidance for unmanned surface vehicle based on the directional weight | |
| Mahipala et al. | Model Predictive Control for Path Following and Collision-Avoidance of Autonomous Ships in Inland Waterways | |
| CN115373394A (en) | Narrow water channel unmanned ship collision avoidance local path planning method |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| WW01 | Invention patent application withdrawn after publication |
Application publication date: 20210615 |
|
| WW01 | Invention patent application withdrawn after publication |