CN112797999A - Multi-unmanned-boat collaborative traversal path planning method and system - Google Patents
Multi-unmanned-boat collaborative traversal path planning method and system Download PDFInfo
- Publication number
- CN112797999A CN112797999A CN202011549643.3A CN202011549643A CN112797999A CN 112797999 A CN112797999 A CN 112797999A CN 202011549643 A CN202011549643 A CN 202011549643A CN 112797999 A CN112797999 A CN 112797999A
- Authority
- CN
- China
- Prior art keywords
- path
- priority
- unmanned
- point
- grid
- 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.)
- Granted
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
- Y02A90/10—Information and communication technologies [ICT] supporting adaptation to climate change, e.g. for weather forecasting or climate simulation
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention relates to a multi-unmanned-boat collaborative traversal path planning method and a system, which relate to the technical field of multi-robot path planning and comprise the steps of determining a sea area map to be surveyed and drawn; discretizing a sea area map to be mapped to obtain a grid map; determining traversable grid points and obstacle grid points according to the grid map; clustering traversable grid points by adopting a K-means + + algorithm to determine a plurality of mutually communicated path point areas; the number of the mutual communication path point areas is the same as that of the unmanned boats; planning the running path of the unmanned ship in each mutual communication path point area by adopting a priority heuristic path planning constraint condition; an unmanned ship is arranged in a mutual communication path point area; the priority heuristic path planning constraint condition is a condition that the driving paths are planned from high to low according to the priority. The method can efficiently and quickly finish mapping sea area distribution and traversal path planning in the mapping sea area.
Description
Technical Field
The invention relates to the technical field of multi-robot path planning, in particular to a method and a system for planning a path by collaborative traversal of multiple unmanned boats.
Background
Ocean resource development is an important direction of current economic construction and technological development, and one important task is autonomous mapping of geological data of island sea areas. Unmanned sweeping and surveying boat is a novel intelligent autonomous surveying and mapping equipment, and has the advantages of stable path following, strong maneuverability and the like.
In some complex and wide island reef environments, the defects of long mapping time and poor system robustness can exist when a single unmanned scanning and measuring boat is used, and the complex marine environment cannot be responded. Therefore, a technical scheme is urgently needed to be capable of using a plurality of unmanned scanning and measuring boats to conduct collaborative surveying and mapping, the overall surveying and mapping efficiency is improved, the sudden situations such as single unmanned scanning and measuring boat faults can be responded when efficient surveying and mapping is achieved, and the robustness of a multi-unmanned scanning and measuring boat system is reflected.
Disclosure of Invention
The invention aims to provide a method and a system for planning a multi-unmanned-vessel collaborative traversal path, which can efficiently and quickly complete mapping sea area allocation and traversal path planning in a mapping sea area and have the advantages of small calculation amount and strong adaptability.
In order to achieve the purpose, the invention provides the following scheme:
a multi-unmanned-boat collaborative traversal path planning method comprises the following steps:
determining a sea area map to be mapped;
discretizing the sea area map to be mapped to obtain a grid map;
determining traversable grid points and obstacle grid points according to the grid map;
clustering the traversable grid points by adopting a K-means + + algorithm to determine a plurality of mutually communicated path point areas; the number of the mutual communication path point areas is the same as that of the unmanned boats;
planning the running path of the unmanned ship in each mutually communicated path point area by adopting a priority heuristic path planning constraint condition; one unmanned ship is arranged in one area of the mutual communication path point; the priority heuristic path planning constraint condition is a condition that a driving path is planned according to the sequence of the priority from high to low.
Optionally, the determining traversable grid points and obstacle grid points according to the grid map specifically includes:
processing the grid map using a scanline polygon algorithm to determine traversable grid points and obstacle grid points.
Optionally, the planning a driving path of the unmanned ship in each of the mutually communicating path point regions by using a priority heuristic path planning constraint condition specifically includes:
determining a priority heuristic path planning constraint condition; the priority heuristic path planning constraint condition comprises a first priority, a second priority, a third priority and a fourth priority, wherein the priority of the first priority is higher than that of the second priority, the priority of the second priority is higher than that of the third priority, and the priority of the third priority is higher than that of the fourth priority; the first priority corresponding region is above the mutually communicating path point region, the second priority corresponding region is below the mutually communicating path point region, the third priority corresponding region is sequentially above left, above left and below left of the mutually communicating path point region, and the fourth priority corresponding region is sequentially above right, above right and below right of the mutually communicating path point region;
and planning the running path of the unmanned ship in each mutually communicated path point area by adopting the priority heuristic path planning constraint condition.
Optionally, the method further includes:
in the process of traversing the unmanned ship path, dynamically describing the mutual communication path point region by adopting a dynamic grid method so as to determine traversed grid points and non-traversed grid points;
in the unmanned ship path traversal process, when the unmanned ship is in a deadlock state, determining a target grid point and a position point of the unmanned ship in the deadlock state; the deadlock state is a state when eight field points around the unmanned ship are all barrier grid points and/or traversed grid points;
determining the shortest path of the unmanned ship by adopting an A-x heuristic search algorithm according to the target grid points and the position points of the unmanned ship in the deadlock state; the unmanned ship shortest path is a path for the unmanned ship to go out of a deadlock state.
Optionally, in the unmanned ship path traversal process, when the unmanned ship is in a deadlock state, determining a target grid point and a position point of the unmanned ship when the unmanned ship is in the deadlock state specifically includes:
determining a position point when the unmanned ship is in a deadlock state;
determining the leftmost column of non-traversed grid points in the mutual communication path point region;
determining the uppermost non-traversed grid point and the lowermost non-traversed grid point in the leftmost column of non-traversed grid points;
calculating a first distance between the uppermost non-traversed grid point and a position point of the unmanned ship in a deadlock state, and calculating a second distance between the lowermost non-traversed grid point and the position point of the unmanned ship in the deadlock state;
and comparing the first distance with the second distance, and determining the non-traversed grid point corresponding to the minimum distance as a target grid point.
Optionally, the method further includes:
in the unmanned ship path traversing process, when the unmanned ship cannot work continuously, updating the non-traversed area in the grid map, the number of the current unmanned ships capable of working and the position coordinates of the current unmanned ships capable of working;
according to the number of the current workable unmanned ships and the position coordinates of the current workable unmanned ships, clustering traversable grid points in the non-traversable area by adopting a K-means + + algorithm to determine a plurality of updated mutual communication path point areas; the updated number of the mutually communicated path point areas is the same as the number of the current working unmanned boats;
planning the running path of the current workable unmanned ship in each updated mutually communicated path point area by adopting a priority heuristic path planning constraint condition; and configuring one current workable unmanned ship in one updated mutual communication path point area.
A multi-unmanned-boat collaborative traversal path planning system comprises:
the to-be-mapped sea area map determining module is used for determining a to-be-mapped sea area map;
the discretization processing module is used for performing discretization processing on the sea area map to be drawn to obtain a grid map;
the traversable grid point and obstacle grid point determining module is used for determining traversable grid points and obstacle grid points according to the grid map;
the mutual communication path point region determining module is used for clustering the traversable grid points by adopting a K-means + + algorithm so as to determine a plurality of mutual communication path point regions; the number of the mutual communication path point areas is the same as that of the unmanned boats;
the path planning module is used for planning the running path of the unmanned ship in each mutual communication path point area by adopting a priority heuristic path planning constraint condition; one unmanned ship is arranged in one area of the mutual communication path point; the priority heuristic path planning constraint condition is a condition that a driving path is planned according to the sequence of the priority from high to low.
Optionally, the path planning module specifically includes:
the constraint condition determining unit is used for determining a priority heuristic path planning constraint condition; the priority heuristic path planning constraint condition comprises a first priority, a second priority, a third priority and a fourth priority, wherein the priority of the first priority is higher than that of the second priority, the priority of the second priority is higher than that of the third priority, and the priority of the third priority is higher than that of the fourth priority; the first priority corresponding region is above the mutually communicating path point region, the second priority corresponding region is below the mutually communicating path point region, the third priority corresponding region is sequentially above left, above left and below left of the mutually communicating path point region, and the fourth priority corresponding region is sequentially above right, above right and below right of the mutually communicating path point region;
and the path planning unit is used for planning the running path of the unmanned ship in each mutually communicated path point area by adopting the priority heuristic path planning constraint condition.
Optionally, the method further includes:
the traversed grid point and non-traversed grid point determining module is used for dynamically describing the mutual communication path point region by adopting a dynamic grid method in the unmanned boat path traversing process so as to determine traversed grid points and non-traversed grid points;
the target grid point and position point determining module when the unmanned ship is in the deadlock state is used for determining the target grid point and the position point when the unmanned ship is in the deadlock state in the unmanned ship path traversing process; the deadlock state is a state when eight field points around the unmanned ship are all barrier grid points and/or traversed grid points;
the unmanned ship shortest path determining module is used for determining the unmanned ship shortest path by adopting an A-ray heuristic search algorithm according to the target grid points and the position points of the unmanned ship in the deadlock state; the unmanned ship shortest path is a path for the unmanned ship to go out of a deadlock state.
Optionally, the method further includes:
the traversal region updating module is used for updating the number of the unperturbed regions, the current working unmanned ships and the current working unmanned ship position coordinates in the grid map when the unmanned ships cannot work continuously in the path traversal process of the unmanned ships;
the mutual communication path point area updating module is used for clustering traversable grid points in the non-traversable area by adopting a K-means + + algorithm according to the number of the current workable unmanned boats and the position coordinates of the current workable unmanned boats so as to determine a plurality of updated mutual communication path point areas; the updated number of the mutually communicated path point areas is the same as the number of the current working unmanned boats;
the path updating module is used for planning the running path of the current workable unmanned ship in each updated mutually communicated path point area by adopting a priority heuristic path planning constraint condition; and configuring one current workable unmanned ship in one updated mutual communication path point area.
According to the specific embodiment provided by the invention, the invention discloses the following technical effects:
the invention comprehensively considers the task area distribution and the task area traversal path planning as the multi-unmanned-vessel collaborative traversal path planning method and the system, adopts the K-means + + clustering algorithm with better clustering effect and the ordered priority heuristic path planning constraint condition to generate the collaborative traversal planning path, can efficiently and quickly complete the mapping sea area distribution and the mapping sea area traversal path planning, and has the advantages of small calculated amount and strong adaptability.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings needed to be used in the embodiments will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings without inventive exercise.
FIG. 1 is a schematic flow diagram of a collaborative traversal path planning method for multiple unmanned planes according to the present invention;
FIG. 2 is a schematic structural diagram of a collaborative traversal path planning system for multiple unmanned planes according to the present invention;
FIG. 3 is an overall flowchart of the collaborative traversal path planning method for multiple unmanned planes according to the present invention;
FIG. 4 is an illustration of a priority heuristic path planning constraint of the present invention;
FIG. 5 is a diagram of a dynamic re-planning module according to the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
According to task requirements, task area distribution needs to be carried out on the whole surveying and mapping sea area map, the operation path of each unmanned scanning and surveying boat is planned in the distribution area, the cooperative traversal surveying and mapping of the whole surveying and mapping sea area is completed, the whole surveying and mapping sea area is required to be relatively evenly distributed, the planned paths are stable and orderly, collision is avoided, the number of repeated path points is few, meanwhile, mutual coordination among a plurality of unmanned scanning and surveying boats is required, and surveying and mapping operation is completed as quickly as possible. Based on the above, the invention provides a multi-unmanned-vessel collaborative traversal path planning method and system, which can efficiently and rapidly complete mapping sea area allocation and traversal path planning in mapping sea areas, and have the advantages of small calculation amount and strong adaptability.
In order to make the aforementioned objects, features and advantages of the present invention comprehensible, embodiments accompanied with figures are described in further detail below.
Example one
As shown in fig. 1, the present embodiment provides a collaborative traversal path planning method for multiple unmanned planes, which includes the following steps.
Step 101: and determining the sea area map to be mapped.
Step 102: and carrying out discretization processing on the sea area map to be mapped so as to obtain a grid map.
Step 103: determining traversable grid points and obstacle grid points according to the grid map; the method specifically comprises the following steps: processing the grid map using a scanline polygon algorithm to determine traversable grid points and obstacle grid points. The traversable grid points are also called traversable path points.
Step 104: clustering the traversable grid points by adopting a K-means + + algorithm to determine a plurality of mutually communicated path point areas; the number of the mutually communicated path point areas is the same as that of the unmanned boats.
Step 105: planning the running path of the unmanned ship in each mutually communicated path point area by adopting a priority heuristic path planning constraint condition; one unmanned ship is arranged in one area of the mutual communication path point; the priority heuristic path planning constraint condition is a condition that a driving path is planned according to the sequence of the priority from high to low; the method specifically comprises the following steps:
determining a priority heuristic path planning constraint condition; the priority heuristic path planning constraint condition comprises a first priority, a second priority, a third priority and a fourth priority, wherein the priority of the first priority is higher than that of the second priority, the priority of the second priority is higher than that of the third priority, and the priority of the third priority is higher than that of the fourth priority; the first priority corresponding region is above the mutually communicating path point region, the second priority corresponding region is below the mutually communicating path point region, the third priority corresponding region is sequentially above left, above left and below left of the mutually communicating path point region, and the fourth priority corresponding region is sequentially above right, above right and below right of the mutually communicating path point region.
And planning the running path of the unmanned ship in each mutually communicated path point area by adopting the priority heuristic path planning constraint condition.
As a preferred specific implementation manner, the method for planning a collaborative traversal path of multiple unmanned planes according to this embodiment further includes:
and in the path traversing process of the unmanned ship, dynamically describing the mutual communication path point region by adopting a dynamic grid method so as to determine traversed grid points and non-traversed grid points.
In the unmanned ship path traversal process, when the unmanned ship is in a deadlock state, determining a target grid point and a position point of the unmanned ship in the deadlock state; the deadlock state is a state when all eight field points around the unmanned ship are barrier grid points and/or traversed grid points.
Determining the shortest path of the unmanned ship by adopting an A-x heuristic search algorithm according to the target grid points and the position points of the unmanned ship in the deadlock state; the unmanned ship shortest path is a path for the unmanned ship to go out of a deadlock state.
In the unmanned ship path traversal process, when the unmanned ship is in a deadlock state, determining a target grid point and a position point of the unmanned ship in the deadlock state, specifically including:
and determining the position point of the unmanned ship in the deadlock state.
And determining that the leftmost column in the mutual communication path point region does not traverse the grid points.
Determining the uppermost non-traversed grid point and the lowermost non-traversed grid point in the leftmost column of non-traversed grid points.
And calculating a first distance between the uppermost non-traversed grid point and a position point of the unmanned ship in a deadlock state, and calculating a second distance between the lowermost non-traversed grid point and the position point of the unmanned ship in the deadlock state.
And comparing the first distance with the second distance, and determining the non-traversed grid point corresponding to the minimum distance as a target grid point.
As a preferred specific implementation manner, the method for planning a collaborative traversal path of multiple unmanned planes according to this embodiment further includes:
in the unmanned ship path traversing process, when the unmanned ship cannot work continuously, updating the non-traversed area in the grid map, the number of the current unmanned ships which can work and the position coordinates of the current unmanned ships which can work.
According to the number of the current workable unmanned ships and the position coordinates of the current workable unmanned ships, clustering traversable grid points in the non-traversable area by adopting a K-means + + algorithm to determine a plurality of updated mutual communication path point areas; the updated number of the mutually communicated path point areas is the same as the number of the current working unmanned boats.
Planning the running path of the current workable unmanned ship in each updated mutually communicated path point area by adopting a priority heuristic path planning constraint condition; and configuring one current workable unmanned ship in one updated mutual communication path point area.
The embodiment discloses a multi-unmanned-boat collaborative traversal path planning method, which comprises the following steps: selecting a grid map to describe the environment information of the operation chart, performing environment modeling by using a scanning line polygon algorithm, and dividing traversable path points and barrier grid points; distributing task areas to traversable path points based on a K-means + + clustering algorithm; planning a complete traversal path in the distribution area by using a priority heuristic path planning constraint condition; and when the unmanned ship cannot continue the task, activating a dynamic re-planning algorithm, and realizing the quick full coverage of the map through iterative allocation. The algorithm of the invention has small calculation amount and high success rate; the algorithm method is simple and easy to realize. The method can well meet the task of collaborative traversal path planning of the unmanned ships.
Example two
As shown in fig. 2, the system for planning a collaborative traversal path of multiple unmanned planes according to the present embodiment includes:
a to-be-mapped sea area map determining module 201, configured to determine a to-be-mapped sea area map;
the discretization processing module 202 is configured to perform discretization processing on the sea area map to be drawn to obtain a grid map;
a traversable grid point and obstacle grid point determining module 203, configured to determine traversable grid points and obstacle grid points according to the grid map.
A mutual communication path point region determining module 204, configured to perform clustering processing on the traversable grid points by using a K-means + + algorithm to determine a plurality of mutual communication path point regions; the number of the mutually communicated path point areas is the same as that of the unmanned boats.
The path planning module 205 is configured to plan a driving path of the unmanned ship in each of the mutually communicating path point regions by using a priority heuristic path planning constraint condition; one unmanned ship is arranged in one area of the mutual communication path point; the priority heuristic path planning constraint condition is a condition that a driving path is planned according to the sequence of the priority from high to low.
As a preferred specific implementation manner, the path planning module provided in this embodiment specifically includes:
the constraint condition determining unit is used for determining a priority heuristic path planning constraint condition; the priority heuristic path planning constraint condition comprises a first priority, a second priority, a third priority and a fourth priority, wherein the priority of the first priority is higher than that of the second priority, the priority of the second priority is higher than that of the third priority, and the priority of the third priority is higher than that of the fourth priority; the first priority corresponding region is above the mutually communicating path point region, the second priority corresponding region is below the mutually communicating path point region, the third priority corresponding region is sequentially above left, above left and below left of the mutually communicating path point region, and the fourth priority corresponding region is sequentially above right, above right and below right of the mutually communicating path point region.
And the path planning unit is used for planning the running path of the unmanned ship in each mutually communicated path point area by adopting the priority heuristic path planning constraint condition.
As a preferred specific implementation manner, the system for planning a collaborative traversal path of multiple unmanned planes provided in this embodiment further includes:
and the traversed grid point and non-traversed grid point determining module is used for dynamically describing the mutual communication path point region by adopting a dynamic grid method in the unmanned boat path traversing process so as to determine traversed grid points and non-traversed grid points.
The target grid point and position point determining module when the unmanned ship is in the deadlock state is used for determining the target grid point and the position point when the unmanned ship is in the deadlock state in the unmanned ship path traversing process; the deadlock state is a state when all eight field points around the unmanned ship are barrier grid points and/or traversed grid points.
The unmanned ship shortest path determining module is used for determining the unmanned ship shortest path by adopting an A-ray heuristic search algorithm according to the target grid points and the position points of the unmanned ship in the deadlock state; the unmanned ship shortest path is a path for the unmanned ship to go out of a deadlock state.
As a preferred specific implementation manner, the system for planning a collaborative traversal path of multiple unmanned planes provided in this embodiment further includes:
and the traversal area updating module is used for updating the number of the unperturbed areas, the current working unmanned ships and the current working unmanned ship position coordinates in the grid map when the unmanned ships cannot work continuously in the path traversal process of the unmanned ships.
The mutual communication path point area updating module is used for clustering traversable grid points in the non-traversable area by adopting a K-means + + algorithm according to the number of the current workable unmanned boats and the position coordinates of the current workable unmanned boats so as to determine a plurality of updated mutual communication path point areas; the updated number of the mutually communicated path point areas is the same as the number of the current working unmanned boats.
The path updating module is used for planning the running path of the current workable unmanned ship in each updated mutually communicated path point area by adopting a priority heuristic path planning constraint condition; and configuring one current workable unmanned ship in one updated mutual communication path point area.
EXAMPLE III
The embodiment provides a multi-unmanned-vessel collaborative traversal path planning method, which can efficiently and quickly complete mapping sea area allocation, traversal path planning in a mapping sea area and task re-planning when a single unmanned scanning survey vessel fails, and has the advantages of small calculation amount and strong adaptability. As shown in fig. 3, the method for planning the collaborative traversal path of the multiple unmanned planes according to the present embodiment includes the following steps.
The method comprises the following steps: and setting a certain interval to discretize the sea area environment map to be drawn into a grid map.
Step two: determining traversable grid points and obstacle grid points by adopting a scanning line polygon algorithm according to the grid map; the method specifically comprises the following steps: parallel scanning lines are generated in a grid map at the same interval, the parallel scanning lines and the boundary of the obstacle form paired intersection points, the grid where the intersection points are located is marked as obstacle grid points, the rest grids are marked as traversable path points, and the grid points can be traversed.
Step three: clustering the traversable path points x by adopting a K-means + + algorithm to determine a plurality of mutually communicated path point areas; the clustering principle is as follows: setting the number of the unmanned boats capable of working as a k value, and calculating k mass center elements c which are uniformly distributed; and allocating all traversable path points x to the centroid element c with the nearest distance, iteratively updating the centroid element c in each region and reallocating the traversable path points x to the clustering regions until the clustering result is not changed. And obtaining k interconnected path point areas according to the clustering result.
Step four: and traversing the single unmanned ship in each interconnected path point area by using a priority heuristic path planning algorithm, wherein the priorities from high to low are respectively upper, lower, left (upper left, lower left), right (upper right, lower right) and so as to ensure the orderliness of the traversed path.
Step five: in the traversing process of a single unmanned ship, dynamically describing a sea area environment map to be surveyed by using a dynamic grid method, and setting the income value of a grid map into three states: setting the grid profit value of the traversed area to be 0, setting the profit value of the non-traversed grid point to be 1, and setting the profit value of the obstacle grid point to be-1, thereby determining the non-traversed grid point, the traversed grid point and the obstacle grid point.
Step six: in the searching process, when the eight field points around the single unmanned ship are all barrier grid points and/or traversed grid points, a deadlock state is defined; and establishing a cost map based on an A-ray heuristic search algorithm, and setting grid points which are closer to each other in upper and lower vertexes of the left-most non-traversed grid points as temporary target points so that the unmanned ship can conveniently go out of a deadlock state.
Step seven: and when the unmanned ship cannot work continuously, activating a dynamic re-planning algorithm. Updating the current non-traversed area, the current number of the unmanned boats capable of working and the real-time position coordinates of the unmanned boats capable of working; taking the number of the current unmanned boats capable of working as a K value, taking real-time position coordinates of the K value as K initial centroid elements, and clustering the current regions which are not traversed by using a K-means + + clustering algorithm; and after the task areas are redistributed, traversing paths of each redistributing area are planned by using a priority heuristic path planning algorithm, and the traversing paths in each redistributing area are output to the unmanned ship capable of working.
Further, in step three, the specific implementation steps of the K-means + + algorithm are as follows:
(a) setting a k value: and setting a k value for the number of unmanned boats according to the task scene.
(b) Randomly selecting an element x (traversable path point) in the feasible region as an initial centroid element c1。
(c) The shortest Euclidean distance from the element x to the current existing centroid element is D (x), the probability of each element being selected as the next centroid element is y (x), and the probability calculation formula is as follows:
(d) selecting the next centroid element c using the wheel method2。
(e) Repeating steps (c) and (d) until k centroid elements are selected.
(f) Assigning all elements to the centroid element with the smallest Euclidean distance in the centroid set C, such as formula
Shown, further divided into k regions.
(g) Updating the centroid; calculating the arithmetic mean value of the elements in each region C, and updating the arithmetic mean value into a new centroid element CiThe calculation formula is as follows:
(h) and (f) repeating the steps (f) and (g) until the clustering result is not changed any more.
Further, in step four, as shown in fig. 4, in the working process of the unmanned scanning survey boat and under the condition of passing in an eight-neighborhood manner, the feasible regions are 8 adjacent points, which are divided into four equal parts according to the priority level, from high to low: up, down, left side (left up, left down), right side (right up, right down). The priority of the upper and lower 2 directions is defined to be higher so that the vertical direction is the main movement direction, the priority of the left side is defined to be higher than that of the right side so that the traversal path of the unmanned scanning and measuring boat is pushed from left to right, and the orderliness of the planned path is ensured.
Further, in step six, when in the deadlock state, according to the order requirement of the complete traversal path plan, firstly searching the leftmost column L of the non-traversed grid points, and finding the uppermost and lowermost non-traversed grid points in the leftmost column L, which are respectively marked as P1、P2And selecting an unretraversed grid point with a smaller distance from the current unmanned ship position P as a temporary target point.
Further, in the sixth step, a heuristic search A-algorithm is used for walking out the deadlock state; specifically, cost search is carried out from a starting point grid point (current unmanned ship position P) to a target grid point (temporary target point), the grid point with the minimum f (n) is continuously selected as a search node, the search node is recorded until the target grid point, then backtracking is carried out from the target grid point to generate the shortest path, and the calculation formula of the A-x function is as follows: f (n) ═ g (n) + h (n) (4), where g (n) is the actual cost value from the start node to node n, and h (n) is the heuristic function.
Selecting Manhattan distance in the grid map as a heuristic function h (n), wherein the cost of the heuristic function is estimated to be the sum of absolute wheelbases of a current point and a target point in a coordinate system, and a calculation formula is as follows:
h(n)＝D*(abs(n.x-goal.x)+abs(n.y-goal.y))(5)。
further, in step seven, as shown in fig. 5, the specific steps of the dynamic re-planning algorithm are as follows:
(a) and when the situation that the single or a plurality of unmanned boats fail to work continuously is detected, activating the dynamic re-planning module.
(b) And updating the current non-traversed area, the current number of the unmanned ships capable of working and the real-time position coordinates of the unmanned ships capable of working.
(c) And taking the number of the current workable unmanned ships as a K value, taking the real-time positions of the current workable unmanned ships as K initial centroid elements, and clustering the current non-traversed area by using a K-means + + clustering algorithm to enable the divided area to be close to the real-time position of the current workable unmanned ship so as to meet the requirement of path connectivity.
(d) And after the task areas are redistributed, traversing path planning is carried out on each redistribution area by using a priority heuristic path planning algorithm.
(e) And outputting the traversal path in each re-planning area to the workable unmanned ship.
The invention takes the task area allocation, the task area traversal path planning and the dynamic reallocation into comprehensive consideration as the technical scheme for carrying out the collaborative traversal mapping, and adopts a K-means + + clustering algorithm with better clustering effect and an ordered priority heuristic path planning algorithm to generate the collaborative traversal planning path. And for the unmanned ship trapped in the deadlock situation, an A-path planning method is adopted to help the unmanned ship to go out of the deadlock state, and for the unmanned ship with the fault, a dynamic re-planning module is adopted to dynamically re-allocate the non-traversed area. The method has the advantages of small calculated amount, strong robustness, strong adaptability and the like.
The embodiments in the present description are described in a progressive manner, each embodiment focuses on differences from other embodiments, and the same and similar parts among the embodiments are referred to each other. For the system disclosed by the embodiment, the description is relatively simple because the system corresponds to the method disclosed by the embodiment, and the relevant points can be referred to the method part for description.
The principles and embodiments of the present invention have been described herein using specific examples, which are provided only to help understand the method and the core concept of the present invention; meanwhile, for a person skilled in the art, according to the idea of the present invention, the specific embodiments and the application range may be changed. In view of the above, the present disclosure should not be construed as limiting the invention.
Claims (10)
1. A multi-unmanned-boat collaborative traversal path planning method is characterized by comprising the following steps:
determining a sea area map to be mapped;
discretizing the sea area map to be mapped to obtain a grid map;
determining traversable grid points and obstacle grid points according to the grid map;
clustering the traversable grid points by adopting a K-means + + algorithm to determine a plurality of mutually communicated path point areas; the number of the mutual communication path point areas is the same as that of the unmanned boats;
planning the running path of the unmanned ship in each mutually communicated path point area by adopting a priority heuristic path planning constraint condition; one unmanned ship is arranged in one area of the mutual communication path point; the priority heuristic path planning constraint condition is a condition that a driving path is planned according to the sequence of the priority from high to low.
2. The method for planning the collaborative traversal path of the multi-unmanned ship according to claim 1, wherein the determining traversable grid points and obstacle grid points according to the grid map specifically includes:
processing the grid map using a scanline polygon algorithm to determine traversable grid points and obstacle grid points.
3. The method for planning the collaborative traversal path of multiple unmanned planes according to claim 1, wherein the planning of the driving path of the unmanned plane in each of the areas of the mutual communication path points by using a priority heuristic path planning constraint condition specifically comprises:
determining a priority heuristic path planning constraint condition; the priority heuristic path planning constraint condition comprises a first priority, a second priority, a third priority and a fourth priority, wherein the priority of the first priority is higher than that of the second priority, the priority of the second priority is higher than that of the third priority, and the priority of the third priority is higher than that of the fourth priority; the first priority corresponding region is above the mutually communicating path point region, the second priority corresponding region is below the mutually communicating path point region, the third priority corresponding region is sequentially above left, above left and below left of the mutually communicating path point region, and the fourth priority corresponding region is sequentially above right, above right and below right of the mutually communicating path point region;
and planning the running path of the unmanned ship in each mutually communicated path point area by adopting the priority heuristic path planning constraint condition.
4. The method for planning the collaborative traversal path of multiple unmanned planes according to claim 1, further comprising:
in the process of traversing the unmanned ship path, dynamically describing the mutual communication path point region by adopting a dynamic grid method so as to determine traversed grid points and non-traversed grid points;
in the unmanned ship path traversal process, when the unmanned ship is in a deadlock state, determining a target grid point and a position point of the unmanned ship in the deadlock state; the deadlock state is a state when eight field points around the unmanned ship are all barrier grid points and/or traversed grid points;
determining the shortest path of the unmanned ship by adopting an A-x heuristic search algorithm according to the target grid points and the position points of the unmanned ship in the deadlock state; the unmanned ship shortest path is a path for the unmanned ship to go out of a deadlock state.
5. The method for planning the collaborative traversal path of multiple unmanned planes according to claim 4, wherein in the process of the unmanned plane path traversal, when the unmanned plane is in a deadlock state, determining a target grid point and a position point of the unmanned plane in the deadlock state specifically comprises:
determining a position point when the unmanned ship is in a deadlock state;
determining the leftmost column of non-traversed grid points in the mutual communication path point region;
determining the uppermost non-traversed grid point and the lowermost non-traversed grid point in the leftmost column of non-traversed grid points;
calculating a first distance between the uppermost non-traversed grid point and a position point of the unmanned ship in a deadlock state, and calculating a second distance between the lowermost non-traversed grid point and the position point of the unmanned ship in the deadlock state;
and comparing the first distance with the second distance, and determining the non-traversed grid point corresponding to the minimum distance as a target grid point.
6. The method for planning the collaborative traversal path of multiple unmanned planes according to claim 1, further comprising:
in the unmanned ship path traversing process, when the unmanned ship cannot work continuously, updating the non-traversed area in the grid map, the number of the current unmanned ships capable of working and the position coordinates of the current unmanned ships capable of working;
according to the number of the current workable unmanned ships and the position coordinates of the current workable unmanned ships, clustering traversable grid points in the non-traversable area by adopting a K-means + + algorithm to determine a plurality of updated mutual communication path point areas; the updated number of the mutually communicated path point areas is the same as the number of the current working unmanned boats;
planning the running path of the current workable unmanned ship in each updated mutually communicated path point area by adopting a priority heuristic path planning constraint condition; and configuring one current workable unmanned ship in one updated mutual communication path point area.
7. A multi-unmanned-boat collaborative traversal path planning system is characterized by comprising:
the to-be-mapped sea area map determining module is used for determining a to-be-mapped sea area map;
the discretization processing module is used for performing discretization processing on the sea area map to be drawn to obtain a grid map;
the traversable grid point and obstacle grid point determining module is used for determining traversable grid points and obstacle grid points according to the grid map;
the mutual communication path point region determining module is used for clustering the traversable grid points by adopting a K-means + + algorithm so as to determine a plurality of mutual communication path point regions; the number of the mutual communication path point areas is the same as that of the unmanned boats;
the path planning module is used for planning the running path of the unmanned ship in each mutual communication path point area by adopting a priority heuristic path planning constraint condition; one unmanned ship is arranged in one area of the mutual communication path point; the priority heuristic path planning constraint condition is a condition that a driving path is planned according to the sequence of the priority from high to low.
8. The system for planning a path through collaborative traversal by multiple unmanned planes according to claim 7, wherein the path planning module specifically comprises:
the constraint condition determining unit is used for determining a priority heuristic path planning constraint condition; the priority heuristic path planning constraint condition comprises a first priority, a second priority, a third priority and a fourth priority, wherein the priority of the first priority is higher than that of the second priority, the priority of the second priority is higher than that of the third priority, and the priority of the third priority is higher than that of the fourth priority; the first priority corresponding region is above the mutually communicating path point region, the second priority corresponding region is below the mutually communicating path point region, the third priority corresponding region is sequentially above left, above left and below left of the mutually communicating path point region, and the fourth priority corresponding region is sequentially above right, above right and below right of the mutually communicating path point region;
and the path planning unit is used for planning the running path of the unmanned ship in each mutually communicated path point area by adopting the priority heuristic path planning constraint condition.
9. The multi-unmanned-boat collaborative traversal path planning system of claim 7, further comprising:
the traversed grid point and non-traversed grid point determining module is used for dynamically describing the mutual communication path point region by adopting a dynamic grid method in the unmanned boat path traversing process so as to determine traversed grid points and non-traversed grid points;
the target grid point and position point determining module when the unmanned ship is in the deadlock state is used for determining the target grid point and the position point when the unmanned ship is in the deadlock state in the unmanned ship path traversing process; the deadlock state is a state when eight field points around the unmanned ship are all barrier grid points and/or traversed grid points;
the unmanned ship shortest path determining module is used for determining the unmanned ship shortest path by adopting an A-ray heuristic search algorithm according to the target grid points and the position points of the unmanned ship in the deadlock state; the unmanned ship shortest path is a path for the unmanned ship to go out of a deadlock state.
10. The multi-unmanned-boat collaborative traversal path planning system of claim 7, further comprising:
the traversal region updating module is used for updating the number of the unperturbed regions, the current working unmanned ships and the current working unmanned ship position coordinates in the grid map when the unmanned ships cannot work continuously in the path traversal process of the unmanned ships;
the mutual communication path point area updating module is used for clustering traversable grid points in the non-traversable area by adopting a K-means + + algorithm according to the number of the current workable unmanned boats and the position coordinates of the current workable unmanned boats so as to determine a plurality of updated mutual communication path point areas; the updated number of the mutually communicated path point areas is the same as the number of the current working unmanned boats;
the path updating module is used for planning the running path of the current workable unmanned ship in each updated mutually communicated path point area by adopting a priority heuristic path planning constraint condition; and configuring one current workable unmanned ship in one updated mutual communication path point area.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011549643.3A CN112797999B (en) | 2020-12-24 | 2020-12-24 | Multi-unmanned-boat collaborative traversal path planning method and system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011549643.3A CN112797999B (en) | 2020-12-24 | 2020-12-24 | Multi-unmanned-boat collaborative traversal path planning method and system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112797999A true CN112797999A (en) | 2021-05-14 |
| CN112797999B CN112797999B (en) | 2022-06-03 |
Family
ID=75805522
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011549643.3A Active CN112797999B (en) | 2020-12-24 | 2020-12-24 | Multi-unmanned-boat collaborative traversal path planning method and system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112797999B (en) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113467456A (en) * | 2021-07-07 | 2021-10-01 | 中国科学院合肥物质科学研究院 | Path planning method for specific target search in unknown environment |
| CN113641178A (en) * | 2021-09-17 | 2021-11-12 | 广州工业智能研究院 | Planning and surveying method, device and system for unmanned ship |
| CN116205464A (en) * | 2023-03-21 | 2023-06-02 | 哈尔滨工程大学 | Water surface multi-unmanned-ship task allocation method based on expansion consistency beam algorithm under multi-obstacle environment |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20170146999A1 (en) * | 2015-11-19 | 2017-05-25 | Sikorsky Aircraft Corporation | Kinematic motion planning with regional planning constraints |
| CN109540136A (en) * | 2018-10-25 | 2019-03-29 | 广东华中科技大学工业技术研究院 | A kind of more unmanned boat collaboration paths planning methods |
| CN109839935A (en) * | 2019-02-28 | 2019-06-04 | 华东师范大学 | The paths planning method and equipment of more AGV |
| CN109931943A (en) * | 2019-03-25 | 2019-06-25 | 智慧航海（青岛）科技有限公司 | Unmanned ship global path planning method and electronic equipment |
| CN110058613A (en) * | 2019-05-13 | 2019-07-26 | 大连海事大学 | Multi-unmanned-aerial-vehicle multi-ant-colony collaborative target searching method |
| CN110196598A (en) * | 2019-06-25 | 2019-09-03 | 北京航天控制仪器研究所 | A kind of dynamic collision prevention method of unmanned boat |
| CN110530388A (en) * | 2019-09-05 | 2019-12-03 | 苏宁云计算有限公司 | The paths planning method and system of more AGV |
| CN111176281A (en) * | 2019-12-31 | 2020-05-19 | 大连民族大学 | Multi-surface unmanned ship coverage type collaborative search method and system based on quadrant method |
| CN111256697A (en) * | 2020-02-24 | 2020-06-09 | 哈尔滨工业大学 | Unmanned aerial vehicle flight path planning method aiming at path point clustering machine learning |
| US20200284596A1 (en) * | 2018-06-01 | 2020-09-10 | Zhejiang Yat Electrical Appliance Co., Ltd | Route planning method for mobile vehicle |
-
2020
- 2020-12-24 CN CN202011549643.3A patent/CN112797999B/en active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20170146999A1 (en) * | 2015-11-19 | 2017-05-25 | Sikorsky Aircraft Corporation | Kinematic motion planning with regional planning constraints |
| US20200284596A1 (en) * | 2018-06-01 | 2020-09-10 | Zhejiang Yat Electrical Appliance Co., Ltd | Route planning method for mobile vehicle |
| CN109540136A (en) * | 2018-10-25 | 2019-03-29 | 广东华中科技大学工业技术研究院 | A kind of more unmanned boat collaboration paths planning methods |
| CN109839935A (en) * | 2019-02-28 | 2019-06-04 | 华东师范大学 | The paths planning method and equipment of more AGV |
| CN109931943A (en) * | 2019-03-25 | 2019-06-25 | 智慧航海（青岛）科技有限公司 | Unmanned ship global path planning method and electronic equipment |
| CN110058613A (en) * | 2019-05-13 | 2019-07-26 | 大连海事大学 | Multi-unmanned-aerial-vehicle multi-ant-colony collaborative target searching method |
| CN110196598A (en) * | 2019-06-25 | 2019-09-03 | 北京航天控制仪器研究所 | A kind of dynamic collision prevention method of unmanned boat |
| CN110530388A (en) * | 2019-09-05 | 2019-12-03 | 苏宁云计算有限公司 | The paths planning method and system of more AGV |
| CN111176281A (en) * | 2019-12-31 | 2020-05-19 | 大连民族大学 | Multi-surface unmanned ship coverage type collaborative search method and system based on quadrant method |
| CN111256697A (en) * | 2020-02-24 | 2020-06-09 | 哈尔滨工业大学 | Unmanned aerial vehicle flight path planning method aiming at path point clustering machine learning |
Non-Patent Citations (2)
| Title |
|---|
| DEHUI ZHANG,等: "Dynamic Multi-Role Adaptive Collaborative Ant Colony Optimization for Robot Path Planning", 《PROCEEDINGS OF THE 41ST IEEE CONFERENCE ON DECISION AND CONTROL》, 10 March 2003 (2003-03-10) * |
| 谢坤霖，等: "基于启发式搜索算法的扫地机器人路径规划", 《西华大学学报( 自然科学版)》, vol. 38, no. 04, 31 July 2019 (2019-07-31) * |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113467456A (en) * | 2021-07-07 | 2021-10-01 | 中国科学院合肥物质科学研究院 | Path planning method for specific target search in unknown environment |
| CN113467456B (en) * | 2021-07-07 | 2023-10-27 | 中国科学院合肥物质科学研究院 | Path planning method for specific target search under unknown environment |
| CN113641178A (en) * | 2021-09-17 | 2021-11-12 | 广州工业智能研究院 | Planning and surveying method, device and system for unmanned ship |
| CN116205464A (en) * | 2023-03-21 | 2023-06-02 | 哈尔滨工程大学 | Water surface multi-unmanned-ship task allocation method based on expansion consistency beam algorithm under multi-obstacle environment |
| CN116205464B (en) * | 2023-03-21 | 2023-11-24 | 哈尔滨工程大学 | Water surface multi-unmanned-ship task allocation method based on expansion consistency beam algorithm under multi-obstacle environment |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112797999B (en) | 2022-06-03 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112797999B (en) | Multi-unmanned-boat collaborative traversal path planning method and system | |
| CN108444482B (en) | Unmanned aerial vehicle autonomous road finding and obstacle avoiding method and system | |
| CN111060109B (en) | Unmanned ship global path planning method based on improved A-star algorithm | |
| CN107436148B (en) | Robot navigation method and device based on multiple maps | |
| CN108680163B (en) | Unmanned ship path searching system and method based on topological map | |
| JP6360193B2 (en) | Modeling cross faults and complex wells in reservoir simulation | |
| Portugal et al. | Msp algorithm: multi-robot patrolling based on territory allocation using balanced graph partitioning | |
| CN107228673A (en) | Route planner and device | |
| Ando et al. | An automatic piping algorithm including elbows and bends | |
| CN113671985A (en) | Staged multi-base unmanned aerial vehicle task allocation and flight path planning method | |
| CN107092978B (en) | Shortest path layered planning method for virtual earth | |
| CN103900573A (en) | Underwater vehicle multi-constrained path planning method based on S57 standard electronic chart | |
| Ortolf et al. | Online multi-robot exploration of grid graphs with rectangular obstacles | |
| Rodenberg et al. | Indoor A* pathfinding through an octree representation of a point cloud | |
| CN109374005B (en) | Ship internal path planning method based on ship VR model | |
| CN111080786A (en) | BIM-based indoor map model construction method and device | |
| CN110909961A (en) | BIM-based indoor path query method and device | |
| CN111412918B (en) | Unmanned ship global safety path planning method | |
| Xu et al. | An efficient algorithm for environmental coverage with multiple robots | |
| Lee et al. | Generation of Ship’s passage plan using data-driven shortest path algorithms | |
| CN105138711A (en) | Primitive indexing method, primitive detecting method, primitive indexing device and a primitive detecting device | |
| CN113110530B (en) | Underwater robot path planning method for three-dimensional environment | |
| CN113124873B (en) | UUV multi-index constraint three-dimensional route planning method based on marine environment information | |
| CN115993817A (en) | Autonomous exploration method, device and medium for tensor field driven hierarchical path planning | |
| CN115031736A (en) | Multi-unmanned aerial vehicle area coverage track planning method and system |
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 |