CN116225032B - Unmanned ship cluster collaborative obstacle avoidance planning method based on known flight path - Google Patents
Unmanned ship cluster collaborative obstacle avoidance planning method based on known flight path Download PDFInfo
- Publication number
- CN116225032B CN116225032B CN202310515184.4A CN202310515184A CN116225032B CN 116225032 B CN116225032 B CN 116225032B CN 202310515184 A CN202310515184 A CN 202310515184A CN 116225032 B CN116225032 B CN 116225032B
- Authority
- CN
- China
- Prior art keywords
- unmanned ship
- unmanned
- ship
- collision
- cluster
- 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.)
- Active
Links
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- 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
Abstract
The embodiment of the invention provides an unmanned ship cluster collaborative obstacle avoidance planning method based on a known track, which comprises the following steps: establishing two-way communication between the unmanned ship cluster and the server; setting global tracks of all unmanned vessels in the unmanned vessel cluster; each unmanned ship in the unmanned ship cluster runs according to the global track and sends respective data information and local route information to the server in real time; the server side makes real-time behavior decision and planning according to the data information and the local route information of the unmanned ship, and sends the decision and planning information to the corresponding unmanned ship respectively; and the unmanned ship executes the decision and planning instructions according to the received decision and planning information. The unmanned ship cluster scene based on the known flight path is based on the collaborative planning thought of target guidance, and the collaborative obstacle avoidance planning method in the unmanned ship cluster is designed and invented, so that networking, informatization and intellectualization of unmanned ship cluster collaborative operation are realized.
Description
Technical Field
The invention relates to the technical field of unmanned ship obstacle avoidance planning, in particular to an unmanned ship cluster collaborative obstacle avoidance planning method based on a known track.
Background
With the gradual maturity of unmanned technology, the unmanned technology of surface of water receives the attention and the attention of vast scholars more and more. The unmanned ship on the water surface is a water surface task platform with high expansibility, and can be used for carrying different devices so as to realize different task demands.
However, as the water area scene is increasingly complex, the operation tasks are increasingly diversified, the operation capability of a single unmanned ship is extremely limited, and the unmanned ship cluster cooperative operation becomes one of important development trends of the future water area operation, and is also a necessary requirement for networking, informatization and intellectualization. How to design a reasonable path for a single agent individual in an unmanned ship cluster and how to handle the problem of mutual avoidance among unmanned ships in the cluster has become an important direction in unmanned ship problem research.
Disclosure of Invention
The embodiment of the invention provides an unmanned ship cluster collaborative obstacle avoidance planning method based on a known track, and aims to solve the technical problem that the operation capacity of the existing single unmanned ship is limited.
In order to achieve the above purpose, the technical scheme provided by the invention is as follows:
the invention provides an unmanned ship cluster collaborative obstacle avoidance planning method based on a known track, which comprises the following steps of:
establishing two-way communication between the unmanned ship cluster and the server;
setting global tracks of all unmanned vessels in the unmanned vessel cluster;
each unmanned ship in the unmanned ship cluster runs according to the global track and sends respective data information and local route information to the server in real time;
the server side makes real-time behavior decision and planning according to the data information and the local route information of the unmanned ship, and sends the decision and planning information to the corresponding unmanned ship respectively;
and the unmanned ship executes the decision and planning instructions according to the received decision and planning information.
The method for setting the global track of each unmanned ship in the unmanned ship cluster comprises the following steps:
sequentially marking cruising task routes of all unmanned ships in the unmanned ship cluster on the electronic map, and recording positions on the marked cruising task routes to obtain a point queue of the cruising task routes;
and carrying out path point compensation on two adjacent points in the point queue by using a difference algorithm to obtain a desired point queue, and taking the desired point queue as a global track.
Wherein, each unmanned ship in the unmanned ship cluster drives according to the global track and sends respective data information and local route information to the server in real time, and the method comprises the following steps:
the unmanned ship acquires the longitude and latitude of the current GPS in real time through a global positioning system carried by the unmanned ship, and sends the longitude and latitude of the current GPS to a server;
the unmanned ship acquires the current running speed in real time through a speed calculation module configured by the unmanned ship, and sends the current running speed to the server side.
Each unmanned ship in the unmanned ship cluster runs according to the global track and sends respective data information and local route information to the server in real time, and the acquisition of the local route in the server comprises the following steps:
by European distance formulaCalculating the distance d between the longitude and latitude coordinates of the unmanned ship at the current moment and the point queue on the cruising task route; wherein d represents the distance between the unmanned ship and the global track point array, (x) 2 ，y 2 ) Representing Cartesian coordinates of an unmanned ship, (x) 1 ,y 1 ) Cartesian coordinates representing a point queue of a global track;
the minimum value in the d value is selected as the position information of the unmanned ship in the global track at the current moment;
taking the current position of the unmanned ship in the global track as the starting point coordinate of the local route, and intercepting the route with a certain length d from the global track as the current local route of the unmanned ship so as to obtain the local route.
The step server side makes real-time behavior decision and plan according to the data information and the local route information of the unmanned ship, and sends the decision and plan information to the corresponding unmanned ship respectively, and the method comprises the following steps:
performing collision calculation on each unmanned ship in the unmanned ship cluster;
performing control area calculation on each unmanned ship in the unmanned ship cluster;
and carrying out decision planning according to the collision calculation result and the control area calculation structure.
Wherein, the step of performing collision calculation on each unmanned ship in the unmanned ship cluster comprises the following steps:
setting collision scenes among all unmanned ships in the unmanned ship cluster as meeting, exceeding, following and crossing;
calculating the intersection point of the local route of the unmanned ship A and the local route of the unmanned ship B as a collision point coordinate, and if no intersection point exists, recording that the unmanned ship A and the unmanned ship B have no collision; if the two are intersected, executing the subsequent steps;
calculating the overall geometric trend of the local route point queues of the unmanned ship A and the unmanned ship B, taking line segment trend angles, namely a1 and a2, as heading angles of the unmanned ship A and the unmanned ship B, calculating heading angle difference a= |a1-a2|, and setting θ as the approximate same threshold value of the heading of the unmanned ship; recording the current speed Va of the unmanned ship A, wherein the current speed of the unmanned ship B is Vb; if the condition a is smaller than theta, calculating the front-back relation of the positions of the unmanned ship A and the unmanned ship B, when the unmanned ship A is behind the unmanned ship B, if Va is smaller than Vb, recording that the collision scene of the unmanned ship A relative to the unmanned ship B is overrun, otherwise, recording that the collision scene of the unmanned ship A relative to the unmanned ship B is followed; otherwise, when the unmanned ship B is behind the unmanned ship A, if Va is smaller than Vb, recording that the collision scene of the unmanned ship A relative to the unmanned ship B is overrun, otherwise, recording that the collision scene of the unmanned ship A relative to the unmanned ship B is followed; if the condition 180-theta is smaller than a < 180, recording collision scenes of the unmanned ship A and the unmanned ship B as a meeting; if the collision scene of the unmanned ship A and the unmanned ship B does not meet the relation, recording that the collision scene of the unmanned ship A and the unmanned ship B is crossed;
executing the steps on every two unmanned vessels in the unmanned vessel cluster to obtain collision information data among the unmanned vessels, wherein the collision information data comprises: the coordinates of the collision points and the collision scene.
Wherein, the step of performing the control region calculation on each unmanned ship in the unmanned ship cluster includes:
judging whether the newly added collision point is inside the existing control area, if so, adding an unmanned ship combination corresponding to the collision point to the original control area, otherwise, generating a new control area N based on the collision point.
Wherein the generating a new regulated area N based on the collision point comprises the following steps:
calculating a circle by taking the collision point as a center and taking the parameter R as a radius as the boundary point coordinate of the control region N;
establishing internal data of the regulated area N, wherein the internal data comprises: each unmanned ship in the control area N is GPS, speed, local route and control area boundary point queue in real time;
if the exceeding scene exists in the controlled area, selecting the unmanned ship to which the scene belongs as a master ship Umaster, otherwise, executing the subsequent steps;
calculating the intersection position Ln of the boundary of the controlled area N and each unmanned ship in the unmanned ship cluster in the controlled area N on a local route to which the unmanned ship belongs;
the calculation formula of the length of the route is as followsCalculating a length set L1, L2 and L3 from a local route starting point position L0 to an intersection position Ln, and setting an unmanned ship with the shortest length as a main ship Umaster; wherein (1)>Representing the distance of every two sample points of the local route, (x) i ，y i ) Representing the cartesian coordinates of the local route track points.
The step server side makes real-time behavior decision and plan according to the data information and the local route information of the unmanned ship, and sends the decision and plan information to the corresponding unmanned ship respectively, and the method comprises the following steps:
setting rules: only the master ship Umaster in the control area has detour decision authority, and the other unmanned ships can only execute braking or sailing decisions;
carrying out decision planning on a master ship Umaster in the control area; if the collision scene of the main ship Umaster is overrun or meet, establishing target ship environment information with collision risk with the main ship Umaster, calling an A-path planning algorithm, and calculating a detour route; if the master ship Umaster collision scene is a cross and follows the scene, the master ship keeps the original track local route.
The method further comprises the step of carrying out decision planning on the rest of unmanned ships in the controlled area, wherein the step of carrying out decision planning on the rest of unmanned ships in the controlled area comprises the following steps:
the collision danger distances of other unmanned ships except the master ship Umaster in the control area are calculated, and decision is made according to the collision danger distances, wherein the judgment mode is as follows:
wherein, the liquid crystal display device comprises a liquid crystal display device,the minimum collision distance of the two ship bodies is set; />For the position length of the collision point on the partial route of the unmanned ship, the calculation formula of the distance between the collision point and the partial route of the unmanned ship is +.>，/>Representing the distance of every two sample points of the local route, (x) i ，y i ) Cartesian coordinates representing local route track points; if the brake decision is 0, the unmanned ship executes the brake decision; otherwise, executing the navigation decision.
Compared with the prior art, the embodiment of the invention provides the unmanned ship cluster collaborative obstacle avoidance planning method based on the known track, which is designed based on the collaborative planning thought of target guidance based on the unmanned ship cluster scene of the known track, and meets the requirements of networking, informatization and intellectualization of unmanned ship cluster collaborative operation.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for the description of the embodiments will be briefly described below, and it is obvious that the drawings in the following description are some embodiments of the present invention, and other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
Fig. 1 is a main flow chart of an unmanned ship cluster collaborative obstacle avoidance planning method based on a known track provided by an embodiment of the invention.
Fig. 2 is a sub-flowchart of an unmanned ship cluster collaborative obstacle avoidance planning method based on a known track according to an embodiment of the present invention.
Fig. 3 is a sub-flowchart of an unmanned ship cluster collaborative obstacle avoidance planning method based on a known track according to an embodiment of the present invention.
Fig. 4 is a sub-flowchart of an unmanned ship cluster collaborative obstacle avoidance planning method based on a known track according to an embodiment of the present invention.
Fig. 5 is a sub-flowchart of an unmanned ship cluster collaborative obstacle avoidance planning method based on a known track according to an embodiment of the present invention.
Fig. 6 is a sub-flowchart of an unmanned ship cluster collaborative obstacle avoidance planning method based on a known track according to an embodiment of the present invention.
Detailed Description
The following description of the embodiments of the present invention will be made clearly and fully with reference to the accompanying drawings, in which it is evident that the embodiments described are some, but not all embodiments of the invention. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
It should be understood that the terms "comprises" and "comprising," when used in this specification and the appended claims, specify the presence of stated features, integers, steps, operations, elements, and/or components, but do not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and/or groups thereof.
It is also to be understood that the terminology used in the description 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 this specification and the appended claims, the singular forms "a," "an," and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise.
It should be further understood that the term "and/or" as used in the present specification and the appended claims refers to any and all possible combinations of one or more of the associated listed items, and includes such combinations.
Referring to fig. 1 to 6, fig. 1 is a main flow chart of an unmanned ship cluster collaborative obstacle avoidance planning method based on a known track, which is provided by an embodiment of the present invention, and is based on an unmanned ship cluster formed by a server side and a plurality of unmanned ships communicatively connected with the server side, and the method includes the following steps:
step S100, establishing two-way communication between the unmanned ship cluster and a server; the unmanned ship cluster communication unit is provided with a server and intelligent body terminals of the unmanned ship cluster; and establishing a network communication interface between the server and each intelligent terminal of the unmanned ship cluster by a 5G network communication mode, so as to realize bidirectional communication between the server and each intelligent terminal of the unmanned ship cluster.
Step 200, setting global tracks of all unmanned ships in the unmanned ship cluster;
referring to fig. 2, the step S200 of setting the global track of each unmanned ship in the unmanned ship cluster includes the following steps:
step S201, sequentially marking the cruising task routes of all unmanned ships in the unmanned ship cluster on the electronic map, and recording the positions on the marked cruising task routes to obtain a point queue of the cruising task routes;
and step S202, carrying out path point compensation on two adjacent points in the point queue by using a difference algorithm to obtain a desired point queue, and taking the desired point queue as a global track.
Step S300, each unmanned ship in the unmanned ship cluster runs according to the global track and sends respective data information and local route information to the server in real time;
referring to fig. 3, the step S300 of each unmanned ship in the unmanned ship cluster traveling according to the global track and sending the respective data information and the local route information to the server in real time includes the following steps:
step S301, acquiring the longitude and latitude of a current GPS (global positioning system) in real time by the unmanned ship through a global positioning system carried by the unmanned ship, and sending the longitude and latitude of the current GPS to a server;
step S302, the unmanned ship acquires the current running speed in real time through a speed calculation module configured by the unmanned ship, and sends the current running speed to a server side.
Wherein, the step S300 of obtaining the local route information in the server according to the global track and sending the data information and the local route information of each unmanned ship in the unmanned ship cluster in real time includes the following steps:
step 303, obtaining the Euclidean distance formula asCalculating the distance d between the longitude and latitude coordinates of the unmanned ship at the current moment and the point queue on the cruising task route; wherein d represents the distance between the unmanned ship and the global track point array, (x) 2 ，y 2 ) Representing Cartesian coordinates of an unmanned ship, (x) 1 ,y 1 ) Cartesian coordinates representing a point queue of a global track;
step S304, selecting the minimum value in the d value as the position information of the unmanned ship in the global track at the current moment;
step S305, taking the current position of the unmanned ship in the global track as the starting point coordinate of the local route, and intercepting the route with a certain length d from the global track as the current local route of the unmanned ship so as to obtain the local route.
Step S400, the server side makes real-time behavior decisions and plans according to the data information and the local route information of the unmanned ship, and sends the decisions and the planning information to the corresponding unmanned ship respectively;
referring to fig. 4 again, the step S400 of the server side performing real-time behavior decision-making and planning according to the data information and the local route information of the unmanned ship and sending the decision-making and planning information to the corresponding unmanned ship respectively includes the following steps:
s401, performing collision calculation on each unmanned ship in the unmanned ship cluster;
step S402, performing control area calculation on each unmanned ship in the unmanned ship cluster;
and S403, performing decision planning according to the collision calculation result and the control region calculation structure.
Referring to fig. 5 again, the step S401 of performing collision calculation on each unmanned ship in the unmanned ship cluster includes the following steps:
step S4011, setting collision scenes among all unmanned ships in the unmanned ship cluster as meeting, exceeding, following and crossing;
s4012, calculating the intersection point of the local route of the unmanned ship A and the local route of the unmanned ship B as a collision point coordinate, and if no intersection point exists, recording that the unmanned ship A and the unmanned ship B have no collision; if the two are intersected, executing the subsequent steps;
s4013, calculating overall geometric trends of the local route point queues of the unmanned ship A and the unmanned ship B, taking a line segment trend angle as a1, a2 as a heading angle of the unmanned ship A and a heading angle of the unmanned ship B, calculating a heading angle difference a= |a1-a2|, and setting θ as a threshold value which is approximately the same as the heading of the unmanned ship; recording the current speed Va of the unmanned ship A, wherein the current speed of the unmanned ship B is Vb; if the condition a is smaller than theta, calculating the front-back relation of the positions of the unmanned ship A and the unmanned ship B, when the unmanned ship A is behind the unmanned ship B, if Va is smaller than Vb, recording that the collision scene of the unmanned ship A relative to the unmanned ship B is overrun, otherwise, recording that the collision scene of the unmanned ship A relative to the unmanned ship B is followed; otherwise, when the unmanned ship B is behind the unmanned ship A, if Va is smaller than Vb, recording that the collision scene of the unmanned ship A relative to the unmanned ship B is overrun, otherwise, recording that the collision scene of the unmanned ship A relative to the unmanned ship B is followed; if the condition 180-theta is smaller than a < 180, recording collision scenes of the unmanned ship A and the unmanned ship B as a meeting; if the collision scene of the unmanned ship A and the unmanned ship B does not meet the relation, recording that the collision scene of the unmanned ship A and the unmanned ship B is crossed;
step S4014, executing the steps on every two unmanned ships in the unmanned ship cluster to obtain collision information data between the unmanned ships, wherein the collision information data comprises: the coordinates of the collision points and the collision scene.
Wherein, the step S402 of performing the control area calculation on each unmanned ship in the unmanned ship cluster includes:
judging whether the newly added collision point is inside the existing control area, if so, adding an unmanned ship combination corresponding to the collision point to the original control area, otherwise, generating a new control area N based on the collision point.
Referring again to fig. 6, the generating a new regulated area N based on the collision point includes the following steps:
step S4021, calculating a circle by taking a collision point as a center and taking a parameter R as a radius, and taking the circle as a boundary point coordinate of a control area N;
step S4022, establishing internal data of the regulated area N, where the internal data includes: each unmanned ship in the control area N is GPS, speed, local route and control area boundary point queue in real time;
step S4023, if an overrun scene exists in the regulated area, selecting an unmanned ship to which the scene belongs as a master ship Umaster, and otherwise, executing the subsequent steps;
step S4024, calculating the intersection position Ln between the local route of each unmanned ship in the unmanned ship cluster in the controlled area N and the boundary of the controlled area N;
the calculation formula of the length of the route is as followsCalculating a length set L1, L2 and L3 from a local route starting point position L0 to an intersection position Ln, and setting an unmanned ship with the shortest length as a main ship Umaster; wherein (1)>Representing the distance of every two sample points of the local route, (x) i ，y i ) Representing the cartesian coordinates of the local route track points.
The step S400 of the server side performing real-time behavior decision and planning according to the data information and the local route information of the unmanned ship and respectively transmitting the decision and planning information to the corresponding unmanned ship further comprises the following steps:
step S404, rule setting: only the master ship Umaster in the control area has detour decision authority, and the other unmanned ships can only execute braking or sailing decisions;
step S405, performing decision planning on a master ship Umaster in a management area; if the collision scene of the main ship Umaster is overrun or meet, establishing target ship environment information with collision risk with the main ship Umaster, calling an A-path planning algorithm, and calculating a detour route; if the collision scene of the main ship Umaster is crossed and follows the scene, the main ship keeps the original track local route, and the target speed of the main ship Umaster is set as the maximum speed of the ship body and is recorded as Vm.
Specifically, the path planning algorithm includes:
the formula is:
where G = a movement cost to move from a starting point to a specified square, along a path length generated to reach the square; h = estimated cost of moving from the specified square to the endpoint;
establishing an Open list and a Close list;
adding the starting point to the Open list;
the following procedure was repeated: traversing the Openlist, searching the node with the minimum F value, moving to the Close list, and taking the node as the node to be processed currently; each of the 8 adjacent tiles of the current tile is determined, and if the current tile is not reachable or is already in the Close list, the current tile is ignored. Otherwise, the following operation is performed; if it is not in the Open list, add it to the Open list and set the current pane as its parent, record the F, G and H values for that pane; if it is already in the Open list, it is checked whether the path (i.e. it arrives there via the current pane) is better, with the G value as reference. A smaller G value indicates that this is a better path. If so, setting its father as the current square, and recalculating its G and F values; until the Open list is empty, starting from the end point, each pane moves along the parent node to the start point, which is the optimal path.
The decision planning comprises braking, sailing and bypassing three decision types; the braking is that the unmanned ship intelligent body control system executes a braking instruction to enable the unmanned ship to be static; the navigation, namely, a decision-making planning unit plans the target speed of the unmanned ship and keeps the original local track navigation; the detour is to say that the decision planning unit plans the detour route of the unmanned ship and plans the target speed to perform the action of detour the target ship;
the method further comprises a decision planning step S406 for the rest of unmanned vessels in the controlled area, wherein the decision planning step comprises the following steps:
the collision danger distances of other unmanned ships except the master ship Umaster in the control area are calculated, and decision is made according to the collision danger distances, wherein the judgment mode is as follows:
wherein, the liquid crystal display device comprises a liquid crystal display device,the minimum collision distance of the two ship bodies is set; />The distance between the local route of the unmanned ship and the collision point is calculated according to a route length calculation formula
， />Representing the distance of every two sample points of the local route, (x) i ，y i ) Cartesian coordinates representing local route track points; if->0, the unmanned ship executes a braking decision; otherwise, executing the navigation decision.
Wherein the speed planning comprises:
calculating the length of the route of the master control area, wherein the length is Lm, and the master target speed Vm is known;
so the length of time of the master Umaster pipe production area is as follows: tm=lm/Vm
Calculating the target speed v=ud/Tm of the unmanned ship;
executing the steps on each unmanned ship (except for a master ship Umaster) in the controlled area, and deleting the controlled area if the number of unmanned ships in the controlled area is less than 1;
steps S405 to S406 are performed for all the regulated areas of the unmanned ship cluster.
And S500, the unmanned ship executes the decision and planning instructions according to the received decision and planning information. And each intelligent agent terminal of the unmanned ship cluster transmits the decision and planning result to the corresponding control module, and the control module performs unmanned ship control according to the corresponding instruction.
Compared with the prior art, the embodiment of the invention provides the unmanned ship cluster collaborative obstacle avoidance planning method based on the known track, which is designed based on the collaborative planning thought of target guidance based on the unmanned ship cluster scene of the known track, and meets the requirements of networking, informatization and intellectualization of unmanned ship cluster collaborative operation.
The foregoing is merely illustrative of the preferred embodiments of the present invention and is not intended to limit the embodiments of the present invention, and those skilled in the art can easily make corresponding variations or modifications according to the main concept and spirit of the present invention, so the protection scope of the present invention shall be defined by the claims.
Claims (6)
1. The unmanned ship cluster collaborative obstacle avoidance planning method based on the known flight path is characterized by comprising the following steps of:
establishing two-way communication between the unmanned ship cluster and the server;
setting global tracks of all unmanned vessels in the unmanned vessel cluster;
each unmanned ship in the unmanned ship cluster runs according to the global track and sends respective data information and local route information to the server in real time;
the server side makes real-time behavior decision and planning according to the data information and the local route information of the unmanned ship, and sends the decision and planning information to the corresponding unmanned ship respectively;
executing a decision and planning instruction by the unmanned ship according to the received decision and planning information;
the step server side carries out real-time behavior decision and planning according to the data information and the local route information of the unmanned ship and respectively sends the decision and the planning information to the corresponding unmanned ship, and the method comprises the following steps:
performing collision calculation on each unmanned ship in the unmanned ship cluster;
performing control area calculation on each unmanned ship in the unmanned ship cluster;
performing decision planning according to the collision calculation result and the control area calculation result;
wherein, the step of performing collision calculation on each unmanned ship in the unmanned ship cluster comprises the following steps:
setting collision scenes among all unmanned ships in the unmanned ship cluster as meeting, exceeding, following and crossing;
calculating the intersection point of the local route of the unmanned ship A and the local route of the unmanned ship B as a collision point coordinate, and if no intersection point exists, recording that the unmanned ship A and the unmanned ship B have no collision; if the two are intersected, executing the subsequent steps;
calculating the overall geometric trend of the local route point queues of the unmanned ship A and the unmanned ship B, taking line segment trend angles, namely a1 and a2, as heading angles of the unmanned ship A and the unmanned ship B, calculating heading angle difference a= |a1-a2|, and setting θ as the approximate same threshold value of the heading of the unmanned ship; recording the current speed Va of the unmanned ship A, wherein the current speed of the unmanned ship B is Vb; if the condition a is smaller than theta, calculating the front-back relation of the positions of the unmanned ship A and the unmanned ship B, when the unmanned ship A is behind the unmanned ship B, if Va is smaller than Vb, recording that the collision scene of the unmanned ship A relative to the unmanned ship B is overtaking, otherwise, recording that the collision scene of the unmanned ship A relative to the unmanned ship B is following; otherwise, when the unmanned ship B is behind the unmanned ship A, if Va is smaller than Vb, the collision scene of the unmanned ship A relative to the unmanned ship B is recorded as the pursuit, otherwise, the collision scene of the unmanned ship A relative to the unmanned ship B is recorded as the follow; if the condition 180-theta is smaller than a < 180, recording collision scenes of the unmanned ship A and the unmanned ship B as a meeting; if the collision scene of the unmanned ship A and the unmanned ship B does not meet the relation, recording that the collision scene of the unmanned ship A and the unmanned ship B is crossed;
executing the steps on every two unmanned vessels in the unmanned vessel cluster to obtain collision information data among the unmanned vessels, wherein the collision information data comprises: coordinates of collision points and collision scene;
the step of performing the control area calculation on each unmanned ship in the unmanned ship cluster comprises the following steps:
judging whether the newly added collision point is inside the existing control area, if so, adding an unmanned ship combination corresponding to the collision point to the original control area, otherwise, generating a new control area N based on the collision point;
the generating of the new regulated area N based on the collision point comprises the following steps:
calculating a circle by taking the collision point as a center and taking the parameter R as a radius as the boundary point coordinate of the control region N;
establishing internal data of the regulated area N, wherein the internal data comprises: each unmanned ship in the control area N is GPS, speed, local route and control area boundary point queue in real time;
if the exceeding scene exists in the controlled area, selecting the unmanned ship to which the scene belongs as a master ship Umaster, otherwise, executing the subsequent steps;
calculating the intersection position Ln of the boundary of the controlled area N and each unmanned ship in the unmanned ship cluster in the controlled area N on a local route to which the unmanned ship belongs;
the calculation formula of the length of the route is as followsCalculating a length set L1, L2 and L3 from a local route starting point position I0 to an intersection position Ln, and setting an unmanned ship with the shortest length as a main ship Umaster; where d represents the distance of every two sample points of the local route and (x, y) represents the Cartesian coordinates of the local route track points.
2. The unmanned ship cluster collaborative obstacle avoidance planning method based on known flight paths according to claim 1, wherein the setting of global flight paths of each unmanned ship in the unmanned ship cluster comprises the following steps:
sequentially marking cruising task routes of all unmanned ships in the unmanned ship cluster on the electronic map, and recording positions on the marked cruising task routes to obtain a point queue of the cruising task routes;
and carrying out path point compensation on two adjacent points in the point queue by using a difference algorithm to obtain a desired point queue, and taking the desired point queue as a global track.
3. The unmanned ship cluster collaborative obstacle avoidance planning method based on a known track according to claim 1, wherein each unmanned ship in the unmanned ship cluster runs according to a global track and sends respective data information and local route information to the server in real time, and the method comprises the following steps:
the unmanned ship acquires the longitude and latitude of the current GPS in real time through a global positioning system carried by the unmanned ship, and sends the longitude and latitude of the current GPS to a server;
the unmanned ship acquires the current running speed in real time through a speed calculation module configured by the unmanned ship, and sends the current running speed to the server side.
4. The unmanned ship cluster collaborative obstacle avoidance planning method based on the known flight path according to claim 3, wherein the step of obtaining the local route in the server by each unmanned ship in the unmanned ship cluster according to the global flight path and transmitting the respective data information and the local route information in real time comprises the following steps:
by European distance formulaCalculating the distance d between the longitude and latitude coordinates of the unmanned ship at the current moment and the point queue on the cruising task route; wherein d represents the distance between the unmanned ship and the global track point array, (x) 2 ，y 2 ) Representing Cartesian coordinates of an unmanned ship, (x) 1 ,y 1 ) Cartesian coordinates representing a point queue of a global track;
the minimum value in the d value is selected as the position information of the unmanned ship in the global track at the current moment;
taking the current position of the unmanned ship in the global track as the starting point coordinate of the local route, and intercepting the route with a certain length d from the global track as the current local route of the unmanned ship so as to obtain the local route.
5. The unmanned ship cluster collaborative obstacle avoidance planning method based on the known flight path according to claim 1, wherein the step of the server side performing real-time behavior decision and planning according to the data information and the local route information of the unmanned ship and respectively transmitting the decision and planning information to the corresponding unmanned ship comprises the following steps:
setting rules: only the master ship Umaster in the control area has detour decision authority, and the other unmanned ships can only execute braking or sailing decisions;
carrying out decision planning on a master ship Umaster in the control area; if the collision scene of the master ship Umaster is overtaking or collision, establishing target ship environment information with collision risk with the master ship Umaster, calling an A-path planning algorithm, and calculating a detour route; if the master ship Umaster collision scene is a cross and follows the scene, the master ship keeps the original track local route.
6. The unmanned ship cluster collaborative obstacle avoidance planning method based on known tracks according to claim 5, further comprising a step of decision-making planning for the remaining unmanned ships within the controlled area, the decision-making planning for the remaining unmanned ships within the controlled area comprising the steps of:
the collision danger distances of other unmanned ships except the master ship Umaster in the control area are calculated, and decision is made according to the collision danger distances, wherein the judgment mode is as follows:
，
wherein, the liquid crystal display device comprises a liquid crystal display device,the minimum collision distance of the two ship bodies is set; />On the local route of unmanned shipDistance collision point position length is +.>，/>Representing the distance of every two sample points of the local route, (x) i ，y i ) Cartesian coordinates representing local route track points; if->0, the unmanned ship executes a braking decision; otherwise, executing the navigation decision.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310515184.4A CN116225032B (en) | 2023-05-09 | 2023-05-09 | Unmanned ship cluster collaborative obstacle avoidance planning method based on known flight path |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310515184.4A CN116225032B (en) | 2023-05-09 | 2023-05-09 | Unmanned ship cluster collaborative obstacle avoidance planning method based on known flight path |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116225032A CN116225032A (en) | 2023-06-06 |
| CN116225032B true CN116225032B (en) | 2023-07-25 |
Family
ID=86575414
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310515184.4A Active CN116225032B (en) | 2023-05-09 | 2023-05-09 | Unmanned ship cluster collaborative obstacle avoidance planning method based on known flight path |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116225032B (en) |
Family Cites Families (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108200175B (en) * | 2018-01-04 | 2020-07-17 | 浙江大学 | Multi-unmanned ship formation cooperative control system and method based on cooperative cloud control |
| CN108681321B (en) * | 2018-04-10 | 2021-05-14 | 华南理工大学 | Underwater detection method for unmanned ship cooperative formation |
| CN111694365B (en) * | 2020-07-01 | 2021-04-20 | 武汉理工大学 | Unmanned ship formation path tracking method based on deep reinforcement learning |
| CN112797987B (en) * | 2021-03-23 | 2021-07-23 | 陕西欧卡电子智能科技有限公司 | Navigation method and device for obstacle avoidance of unmanned ship, computer equipment and storage medium |
| CN113253721B (en) * | 2021-04-23 | 2023-12-05 | 大连海事大学 | Unmanned ship cluster cooperative collision avoidance guidance method and system under time-varying ocean current interference |
| CN114089754A (en) * | 2021-11-11 | 2022-02-25 | 中国船舶重工集团公司第七一九研究所 | Autonomous path planning method and system based on artificial intelligence |
| CN114035587B (en) * | 2021-11-24 | 2024-03-29 | 陕西欧卡电子智能科技有限公司 | Unmanned ship cluster multi-ship collaborative path planning method and device and unmanned ship |
| CN115061468A (en) * | 2022-06-29 | 2022-09-16 | 天津大学 | Unmanned ship formation separating and recovering method |
-
2023
- 2023-05-09 CN CN202310515184.4A patent/CN116225032B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN116225032A (en) | 2023-06-06 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108445879B (en) | Unmanned ship obstacle avoidance method based on collision danger prediction area | |
| CN112650237B (en) | Ship path planning method and device based on clustering processing and artificial potential field | |
| CN108200175B (en) | Multi-unmanned ship formation cooperative control system and method based on cooperative cloud control | |
| KR101831264B1 (en) | Autonomous navigation system and method for a maneuverable platform | |
| CN109460045B (en) | Improved ant colony optimization-based collision avoidance planning method for USV under dynamic obstacle online perception | |
| US9671238B2 (en) | Navigation system with a combined navigation mechanism and method of operation thereof | |
| CN110471426A (en) | Unmanned intelligent vehicle automatic Collision Avoidance method based on quantum wolf pack algorithm | |
| CN111006667B (en) | Automatic driving track generation system under high-speed scene | |
| Yu et al. | A hybrid path planning method for an unmanned cruise ship in water quality sampling | |
| CN112379697B (en) | Track planning method, device, track planner, unmanned aerial vehicle and storage medium | |
| CN114355981B (en) | Method and system for autonomous exploration and mapping of four-rotor unmanned aerial vehicle | |
| CN111665868A (en) | Unmanned ship return method, device, equipment and storage medium based on virtual channel | |
| CN113916234A (en) | Automatic planning method for ship collision avoidance route under complex dynamic condition | |
| Kim et al. | Development of ship collision avoidance system and sea trial test for autonomous ship | |
| CN116225032B (en) | Unmanned ship cluster collaborative obstacle avoidance planning method based on known flight path | |
| CN111256694A (en) | Method for determining path of unmanned surface vehicle | |
| CN115796544A (en) | Dispatching method and device for unmanned horizontal transportation of port | |
| CN115909817A (en) | Distributed autonomous cooperative collision avoidance method for multiple intelligent ships | |
| CN111928870B (en) | Method and device for planning driving route, computing equipment and storage medium | |
| CN109164797B (en) | Track guidance method and system for intelligent control of ship | |
| JP2022123401A (en) | Route planning device, moving object, route planning method and program | |
| Koschorrek et al. | Towards semi-autonomous operation of an over-actuated river ferry | |
| Zhao | Step-by-step multi-ship collision avoidance navigation path planning based on particle swarm optimization | |
| Godhavn et al. | Hybrid control in sea traffic management systems | |
| Du et al. | Hierarchical path planning and obstacle avoidance control for unmanned surface vehicle |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |