CN117387628A - Underwater robot path planning method and device based on directed particle ant colony - Google Patents
Underwater robot path planning method and device based on directed particle ant colony Download PDFInfo
- Publication number
- CN117387628A CN117387628A CN202311685953.1A CN202311685953A CN117387628A CN 117387628 A CN117387628 A CN 117387628A CN 202311685953 A CN202311685953 A CN 202311685953A CN 117387628 A CN117387628 A CN 117387628A
- Authority
- CN
- China
- Prior art keywords
- path
- ant
- ants
- pheromone
- accessible
- 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
- 238000000034 method Methods 0.000 title claims abstract description 86
- 239000002245 particle Substances 0.000 title claims abstract description 42
- 239000003016 pheromone Substances 0.000 claims abstract description 88
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims abstract description 60
- 230000008569 process Effects 0.000 claims abstract description 27
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 24
- 241000257303 Hymenoptera Species 0.000 claims description 90
- 230000007704 transition Effects 0.000 claims description 28
- 239000011159 matrix material Substances 0.000 claims description 19
- 230000006870 function Effects 0.000 claims description 10
- 238000004590 computer program Methods 0.000 claims description 5
- 238000003491 array Methods 0.000 claims description 2
- 230000007246 mechanism Effects 0.000 claims description 2
- 239000011664 nicotinic acid Substances 0.000 abstract description 25
- 241001481833 Coryphaena hippurus Species 0.000 abstract description 24
- 238000005265 energy consumption Methods 0.000 abstract description 11
- 238000004364 calculation method Methods 0.000 description 7
- 238000010586 diagram Methods 0.000 description 4
- 230000008901 benefit Effects 0.000 description 2
- 230000003592 biomimetic effect Effects 0.000 description 2
- 230000001419 dependent effect Effects 0.000 description 2
- 238000011065 in-situ storage Methods 0.000 description 2
- 241000287196 Asthenes Species 0.000 description 1
- 241001125840 Coryphaenidae Species 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000019637 foraging behavior Effects 0.000 description 1
- 230000006698 induction Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
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/20—Instruments for performing navigational calculations
- G01C21/203—Specially adapted for sailing ships
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
- G06N3/00—Computing arrangements based on biological models
- G06N3/004—Artificial life, i.e. computing arrangements simulating life
- G06N3/006—Artificial life, i.e. computing arrangements simulating life based on simulated virtual individual or collective life forms, e.g. social simulations or particle swarm optimisation [PSO]
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Biophysics (AREA)
- Evolutionary Computation (AREA)
- Artificial Intelligence (AREA)
- Biomedical Technology (AREA)
- Health & Medical Sciences (AREA)
- Computational Linguistics (AREA)
- Data Mining & Analysis (AREA)
- Life Sciences & Earth Sciences (AREA)
- General Health & Medical Sciences (AREA)
- Molecular Biology (AREA)
- Computing Systems (AREA)
- General Engineering & Computer Science (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Automation & Control Theory (AREA)
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
Abstract
The invention discloses an underwater robot path planning method and equipment based on directed particle ant colony, and belongs to the technical field of path planning. According to the method, each ant is regarded as a directional particle, the turning cost of the ant for each accessible node is calculated through the current direction of the ant and the current accessible node, and then the turning cost is added into the roulette flow when the ant selects the next node, so that the probability of selecting each node by the ant is corrected. In addition, after each individual ant completes the searching process, the water resistance of the path is analyzed according to the current path and the average speed, and the water resistance is used as a water resistance pheromone to be left on the path, so that the ant colony algorithm can search a high-quality path and simultaneously consider the energy consumption index of the bionic mechanical dolphin. The invention performs the path planning of the underwater robot in a reasonable, efficient and safe way.
Description
Technical Field
The invention relates to the technical field of path planning, in particular to an underwater robot path planning method and equipment based on directed particle ant colony.
Background
The ant colony algorithm is used as an intelligent planning method in path planning, is very suitable for ground robots, has reference value for path planning of underwater robots, has wide application range and relatively simple understanding, and is widely applied.
However, in general, the ant colony algorithm cannot directly plan a real map, but rather, the map information needs to be rasterized first and then operated by using a grid map. In this process, the carrier is generally regarded as a non-directional mass point in the grid map, that is, the carrier is considered to be able to travel to any non-obstacle direction at any current moment, and no turning cost exists. For a general land robot, the in-situ turning of the advancing direction is a common basic function, and the energy consumption is very little and negligible. In the field of underwater robots, the underwater robot has a certain turning cost due to the influence of water flow resistance, water flow fluctuation and the like. In the field of traditional underwater robots relying on propeller propulsion, a carrier can realize the functions of turning direction or backing by directly controlling the forward and reverse directions of a propeller motor, and even though the turning process is quicker, the carrier still needs to pay a certain turning cost. For some emerging underwater robots, especially bionic underwater robots, taking a bionic dolphin as an example, the bionic dolphin cannot easily complete large deflection and retreating actions like a land robot and a traditional underwater robot due to the influence of the motion mode of the bionic dolphin, which means that the carrier has a non-negligible turning cost. However, the conventional ant colony algorithm path planning does not take this point into consideration, and the planned path may even further increase the energy consumption of the carrier on the premise of not being optimal. In addition, the underwater robot tends to face resistance during traveling, which is also an aspect not considered by the conventional ant colony algorithm. Taking the bionic dolphin as an example, the turning cost and the water resistance of the bionic dolphin facing the movement under water are not negligible, and in the practical engineering project, the environment in the ocean is complicated, the bionic dolphin often has higher energy consumption index when executing tasks, and excessive energy consumption can greatly reduce the efficiency of the bionic dolphin and even cause the serious consequences that the carrier is used up energy prematurely to sink.
The traditional ant colony algorithm thought is derived from simulating ant foraging behaviors. In nature, ants achieve communication between ant colonies through a substance called a "pheromone" and leave the pheromone on the path they have traversed. The shorter the path an individual ant takes to reach the endpoint, the higher the concentration of pheromone it leaves. Through repeated iterative search, the pheromone concentration on the shorter path is continuously accumulated, so that the probability of selecting the path by the next ant is improved, and finally, the method helps to find a high-quality path. In the ant colony algorithm in the prior art, for example, an ant colony algorithm based on elite ants, an ant colony algorithm based on maximum-minimum ants, and the like, thereby realizing different path planning effects. However, the above work does not improve the directivity of the ants, i.e. the ants are all misdirected to a non-directional particle, the direction of the ants does not have turning cost, and the water resistance generated when the underwater robot moves is not optimized and improved.
Therefore, how to perform path planning of an underwater robot in a reasonable, efficient and safe manner is a problem to be solved by those skilled in the art.
Disclosure of Invention
In view of the above, the present invention provides a method and apparatus for planning a path of an underwater robot based on directional mass ant colony, which are used for solving the technical problems in the prior art.
In order to achieve the above object, the present invention provides the following technical solutions:
an underwater robot path planning method based on directed particle ant colony, comprising the following steps:
s100: when the path planning overall process starts, constructing an underwater environment model, and determining a starting point and an ending point; placing all ants at the starting point position at the beginning of each round of iteration; when each ant starts to act, constructing the ants as directed particles, acquiring the current direction of the ants and current accessible nodes, and calculating to obtain a set of turning costs of all accessible nodes;
s200: according to the turning cost, an improved roulette algorithm is used for realizing ant routing;
s300: judging whether ants reach a preset end point, if not, returning to S100, updating the direction of the directed particles, and recalculating the turning cost of all accessible nodes; if yes, then:
s400: calculating the total water resistance cost of the current path-finding path, and synchronously updating the total water resistance cost of the current path-finding path as a water resistance pheromone and a path pheromone;
s500: judging whether all iteration rounds are completed, if not, returning to S100; if yes, outputting the path which is passed by the ant with the minimum path cost in all iteration rounds as a final planning path and outputting the final planning path.
Preferably, the step S100: the ant is constructed as directed particles, the turning cost of all accessible nodes is calculated by the current direction of the ant and the current accessible nodes, and the method comprises the following steps:
s110: defining ant current direction information, and determining all possible directions of the current ants in the grid chart;
s120: determining the ultimate cornering power of the carrier;
s130: defining a turning cost function according to all possible directions of the current ants in the grid chart and the limit turning capacity of the carrier;
s140: acquiring ant current position information and all accessible node information;
s150: calculating turning cost through a turning cost function based on ant current position information and all accessible node information;
s160: and outputting a set of turning costs of all accessible nodes.
Preferably, the step S200: the method for improving the routing of the roulette ants according to the turning cost comprises the following steps:
s210: recording the reciprocal of the distance from all accessible nodes to the end point in the raster image to form a heuristic information four-dimensional matrix, and recording the pheromones among all nodes in the raster image to form a pheromone multi-dimensional matrix;
s220: calculating the transition probability of ants to each accessible node based on the set of turning cost of all the accessible nodes, the heuristic information four-dimensional matrix and the pheromone multi-dimensional matrix, and obtaining a transition probability array set of all the accessible nodes;
s230: selecting a forward node through a roulette mechanism based on a set of transition probability arrays for all accessible nodes;
s240: updating ant position and accessible node information;
s250: and outputting the updated ant position information and the accessible node information.
Preferably, the step S220: calculating the transition probability of ants to each accessible node based on the set of turning cost of all accessible nodes, the heuristic information four-dimensional matrix and the pheromone multi-dimensional matrix, and obtaining a transition probability array set of all accessible nodes, wherein the formula is as follows:
in the method, in the process of the invention,for transfer expression, ++>Indicating that the kth accessible node is to be reached,representing the current grid position of ants->To the accessible node->Is provided with the heuristic information of (a),representing the current grid position +.>To the accessible node->Pheromone content of->Representing the turn cost of ants moving to that node, +.>A transition probability for an ant to transition to a kth node accessible node;
traversing all accessible nodes to obtain a transition probability array set of all accessible nodes。
Preferably, the step S220: calculating the transition probability from the ant to each accessible node based on the set of turning cost of all the accessible nodes, the heuristic information four-dimensional matrix and the pheromone multi-dimensional matrix, and obtaining the transition probability array set of all the accessible nodes, wherein the method further comprises the following steps:
transition probability array set at all accessible nodesIn,taking random number +.>Taking a transition probability array as follows:
wherein if itIf not, the next node representing the ant is。
Preferably, the step S240: updating ant position and accessible node information, thenAnd traversing all accessible nodes of the nodes where the ants are located after moving, including the number of accessible nodes +.>Coordinates with each accessible node:
；
in the method, in the process of the invention,representing the current direction of ants, +.>Indicating the direction in which the selected ant moves behind the o-node.
Preferably, the step S400: calculating the total water resistance cost of the current path-finding path, and synchronously updating the total water resistance cost of the current path-finding path as a water resistance pheromone and a path pheromone, wherein the method comprises the following steps:
s410: acquiring carrier speed information and motion path information;
s420: calculating the water resistance information element related to the speed and the water resistance information element related to the distance, which are left by ants on the current path searching path, based on the carrier speed information and the motion path information;
s430: judging whether all ants complete the iteration task of the current round, if not, executing S440, if so, executing S450;
s440: selecting the next ant, returning to S100, and continuing the iteration task;
s450: updating the pheromone concentration on the node.
Preferably, the step S420: based on the carrier speed information and the motion path information, calculating a speed-related water resistance pheromone and a path-related water resistance pheromone which are left by ants on the current path-finding path, wherein the method comprises the following steps of:
first, theThe incremental expression of the pheromone left by ants on the path is:
；
the water resistance pheromone expression related to the speed in the pheromone increment is as follows:
；
the water resistance pheromone expression related to the journey in the pheromone increment is as follows:
；
in the method, in the process of the invention,total length of path for ants, +.>For the pheromone increment constant,/->For carrier wobble frequency, < >>Is the carrier wobble amplitude.
Preferably, the step S450: updating the pheromone concentration on the node, comprising:
when all ants complete one round of iterative task, nodes are updatedThe pheromone concentration is expressed as follows:
；
in the method, in the process of the invention,for the total number of ants reaching the end point in one iteration, +.>For weight constants, representing the importance of three pheromone increments, respectively, < >>Is the concentration and volatility coefficient of pheromone>For node->To node->Pheromone concentration of (2).
An electronic device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, the processor implementing the steps of a method for path planning of an underwater robot based on directed particle ant colony when the computer program is executed.
Compared with the prior art, the invention discloses the method and the equipment for planning the path of the underwater robot based on the directed particle ant colony, wherein ants are constructed into the directed particles, the real-time turning cost judgment in the ant colony algorithm is realized, the water resistance pheromone is introduced for iterative updating, the energy consumption characteristic and the motion characteristic of the bionic dolphin are combined with the ant colony algorithm, the ant colony algorithm simultaneously gives consideration to three indexes of short path and less energy consumption in the path planning process, and the method and the equipment are suitable for real engineering application, so that the path planning of the underwater robot is realized in a reasonable, efficient and safe mode. The method has the specific beneficial effects that:
1) The invention can make the whole path planning process more intelligent, greatly enhance the adaptability of the ant colony algorithm in the application of the underwater robot, and make it possible to optimize the path according to the turning cost and the water resistance factor.
2) The method has superiority for path planning of the bionic dolphin, considers the turning energy consumption and the movement characteristics of the bionic dolphin, and finally can effectively avoid unnecessary energy loss possibly occurring in the future in the path planning process of the bionic dolphin by taking the swinging frequency and the swinging amplitude of the tail fin of the bionic dolphin as a carrier speed measuring index, so that the tracking path of the bionic dolphin is more efficient and energy-saving.
3) The robot has universality, is not only suitable for bionic dolphins, but also suitable for general underwater robots and land special robots, such as mud exploration robots, and has turning cost during movement and also receives resistance during advancing.
4) Through the underwater robot path planning based on the directed particle ant colony algorithm, ants are regarded as directed particles in a grid chart, transfer possibility is output according to the current direction of the ants and the information of accessible nodes, then the next advancing node of the ants is selected by utilizing a method for improving roulette, and when the ants finish searching tasks, updating iteration is carried out on pheromones according to the influence of water resistance.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings that are required to be used in the embodiments or the description of the prior art will be briefly described below, and it is obvious that the drawings in the following description are only embodiments of the present invention, and that other drawings can be obtained according to the provided drawings without inventive effort for a person skilled in the art.
FIG. 1 is a schematic diagram of the overall process of the method of the present invention;
FIG. 2 is a schematic diagram of a directional particle direction update and turn cost calculation process;
FIG. 3 is a rasterized map used in directed particle direction updating and turn cost calculation;
FIG. 4 is a schematic diagram of all possible directions of ants in a rasterized map;
FIG. 5 is a flow chart of an improved roulette ant routing;
fig. 6 is a schematic diagram of a flow of water resistance cost calculation and pheromone update.
Detailed Description
The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. 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.
The invention aims to provide an underwater robot path planning method and equipment based on directed particle ant colony, wherein the method comprises the following steps: s100: when the path planning overall process starts, constructing an underwater environment model, and determining a starting point and an ending point; placing all ants at the starting point position at the beginning of each round of iteration; when each ant starts to act, constructing the ants as directed particles, acquiring the current direction of the ants and current accessible nodes, and calculating to obtain a set of turning costs of all accessible nodes; s200: according to the turning cost, an improved roulette algorithm is used for realizing ant routing; s300: judging whether ants reach a preset end point, if not, returning to S100, updating the direction of the directed particles, and recalculating the turning cost of all accessible nodes; if yes, then: s400: calculating the total water resistance cost of the current path-finding path, and synchronously updating the total water resistance cost of the current path-finding path as a water resistance pheromone and a path pheromone; s500: judging whether all iteration rounds are completed, if not, returning to S100; if yes, outputting the path which is passed by the ant with the minimum path cost in all iteration rounds as a final planning path and outputting the final planning path. The method provides a solution for realizing reasonable, efficient and safe way of planning the path of the underwater robot.
In order that the above-recited objects, features and advantages of the present invention will become more readily apparent, a more particular description of the invention will be rendered by reference to the appended drawings and appended detailed description.
As shown in fig. 1, the embodiment of the invention discloses an underwater robot path planning method based on directed particle ant colony, which mainly comprises the following steps: the turning cost of all accessible nodes is calculated using the ant current direction and the current accessible nodes, thereby treating the ant as a directed particle. Improved roulette routing is performed based on the turn cost that has been calculated. Judging whether ants reach a preset end point, if not, returning, updating the direction of the directed particles, and recalculating the turning cost of all accessible nodes; if the final ant reaches the end point, calculating the total water resistance cost of the path finding path, and synchronously updating the total water resistance cost as a water resistance pheromone and a path pheromone; judging whether all iteration rounds are completed, if not, returning to recalculate the turning cost of all accessible nodes; if yes, outputting the path which is passed by the ant with the minimum path cost in all iteration rounds as a final planning path.
Specifically, the present embodiment assumes that the rasterized map is an N-order 0/1 matrix, where 0 represents a passable position and 1 represents an obstacle position.
Specifically, S100: when the path planning overall process starts, constructing an underwater environment model, and determining a starting point and an ending point; placing all ants at the starting point position at the beginning of each round of iteration; when each ant starts to act, the ant is constructed as a directed particle, and the current direction of the ant and the current accessible node are obtained to calculate the turning cost set of all accessible nodes.
In one embodiment, to construct the ants as directed particles and obtain the turn cost of each accessible node of the ants, it is necessary to determine the direction each ant may have and determine that the carrier is under waterTrue cornering ability during exercise. In which the direction in which each ant may be present is related to the definition of the raster pattern, and is generallyNeither too fine nor too coarse division has engineering application reference value. The actual turning capacity of the carrier in underwater movement represents the limit of the turning cost, and is usually determined by the limit turning capacity of the carrier, and the calculation formula of the turning cost is related to the index.
In one embodiment, as shown in FIG. 2, the steps of directed particle direction update and turn cost calculation are as follows:
s110: and defining the current direction information of the ants, and determining all possible direction conditions of the ants.
Specifically, in the path planning of the ant colony algorithm based on the raster pattern, too fine direction division can lead ants to be unable to accurately align with the raster, and the algorithm complexity can be greatly increased. Meanwhile, in a real engineering environment, the underwater robot carrier is usually limited by a self structure, and the deflection flow cannot be executed too accurately, so that too fine path division has no practical engineering significance. The case where the yaw angle of the ant based on the inertial coordinate system is 0 is defined as the right direction of the ant, as shown in fig. 3, at which time the ant is assumed to be at the position No. 5. Defining ant current direction information asThen there are eight directions possible for ants in the grid pattern, as shown in fig. 4, then:
；
in the method, in the process of the invention,the yaw angle of ants in the map is expressed, and the included angle between the current direction of ants and the east axis of inertial coordinates is taken as the yaw angle in the embodiment;
s120: the carrier limit turning ability is determined.
Specifically, in this embodiment, the bionic dolphin uses the bionic dolphin as an example, and the bionic dolphin moves forward by using the swing of the tail fin as power, that is, the carrier does not have in-situ turning capability, so that the ant has turning cost when moving to any oblique side and increases along with the increase of the deflection angle. In addition, the bionic dolphin does not have a backward function, so the backward turning cost of ants is maximum;
s130: the turning cost function is defined according to all possible directions in the raster pattern in which the current ant is located and the extreme turning capability of the carrier.
Specifically, a turning cost function is defined according to the formula features determined in S110-S120 and the preset grid map features.
Specifically, a general underwater robot has a maximum deflection angle per unit timeAngle of deflection nominal to unit time->Therefore, the turning cost for defining the deflection of the carrier at different relative positions is +.>，/>，，/>. Wherein (1)>Indicating that the deflection angle of the carrier per unit time is +.>The turning cost is the same as the rest. />Is constant and->；
S140: and acquiring the current position information of ants and all accessible node information.
Specifically, the initial state, the current location information of the ant and all accessible node information are input. Wherein the current position information comprises the coordinates of ants in a geodetic coordinate systemInformation on the current direction of ants->The accessible node information includes the number of accessible nodes +.>Coordinates with each accessible node:
。
s150: and calculating the turning cost.
Specifically, according to the current coordinates of antsCoordinate with accessible node->Then it is predicted from the geometrical relationship that the ant moves to +.>The direction information behind the nodes is:
；
then the firstOnly ants move to +.>The turning cost of the node is as follows:
；
in the method, in the process of the invention,to predict ant moving to +.>Direction information behind node,>is the direction information of the ant current state.
In particular, the method comprises the steps of,wherein P represents ant direction information, prediction is prediction, k is kth node,is the direction information of the ant after the ant is predicted to move to the k node. />In which now represents the current, i.e. ant is at the current node, direction information in the present state.
Repeating the above process until all accessible nodes are traversed, and obtaining turning cost sets of all accessible nodes；
S160: outputting all accessible node turning cost sets；
In one particular embodiment, the turn cost is introduced into the path planning flow by setting ants as directional particles. The cost of turning has the significant advantage that ants will intentionally reduce turning movements, more tending to select those paths for searching that are less cost of turning, thereby reducing energy consumption. The path of each ant searching may be different, so the turning cost of the ants with different direction information for the same searched grid is also different, so the turning cost cannot be directly used for updating the pheromone, but the process of each step of roulette betting and path searching of the ants is needed to be integrated, and the roulette betting process is improved and optimized.
In one embodiment, as shown in FIG. 5, the roulette ant routing steps are as follows:
s210: recording the reciprocal of the distance from all accessible nodes to the end point in the raster image to form a heuristic information four-dimensional matrix, and recording the pheromones among all nodes in the raster image to form a pheromone multi-dimensional matrix;
specifically, the initial state, input all accessible node turn cost setsHeuristic information four-dimensional matrix->Pheromone multidimensional matrix->. Wherein heuristic information matrix->Recording the reciprocal of the distance from all nodes to the end point in the raster patternPheromone matrix->Recording pheromones among all nodes in the raster pattern;
s220: the transfer possibilities of ants to each accessible node are calculated.
Specifically, among others,representing the kth accessible node, the turn may be:
；
wherein,representing the current grid position of ants->To the accessible node->Heuristic information of->Representing the current grid position +.>To the accessible node->Pheromone content of->Representing the turn cost of ants moving to that node, +.>The transition probabilities of nodes are accessible for ants to transition to the kth node.The weight of heuristic information, pheromone induction and transfer cost is respectively calculated. Wherein (1)>Excessive, excessive ++>This will result in ants being prone to fall into local optima. />The bigger the->Smaller indicates that the ants are less likely to migrate to the node, consistent with well established expectations.
S230: roulette selects a forward node.
Specifically, according to S220, the ant is transferred toThe probability of the individual accessible nodes is:
；
traversing all accessible nodes to obtain a transition probability array set of all accessible nodesAnd->。
Taking random numberTaking a transition probability array:
；
if it isIf not, the next node representing the ant is +.>I.e. transfer node +.>。
S240: updating ant position and accessible node information, thenAnd traversing all accessible nodes of the nodes where the ants are located after moving, including the number of accessible nodes +.>With each accessible node coordinates；
S250: outputting the updated ant position information and the accessible node information;
in one embodiment, the primary purpose of considering water resistance is to measure the energy consumption of the carrier, rather than to pay attention to specific values of water resistance, but still to understand key parameters affecting the magnitude of water resistance.
The water resistance calculation formula is:
；
wherein,represents water density->For resistance coefficient>For the cross section of the object->Is the carrier velocity.
In general engineering situations, when the shape of the carrier is fixed, the key factors influencing the water resistance areThe faster the speed, the higher the water resistance. In addition, when the carrier speed is constant and the water resistance is also constant, the energy lost by the carrier due to the water resistance is more or less equal to the path length of the carrierThe positive correlation, i.e. the longer the motion path, the more energy is lost. Therefore, the key factors for judging the water resistance cost of the carrier are the carrier speed and the path length of the carrier.
As shown in fig. 6, the steps of water resistance cost calculation and pheromone update are as follows:
s410: carrier velocity information and motion path information are acquired.
Specifically, in the initial state, carrier speed information and motion path information are input. It should be noted that unlike land robots and conventional underwater robots, the biomimetic robotic dolphin mainly moves forward through the swing of the tail fin, so that the biomimetic robotic dolphin cannot move at a uniform speed, but moves forward at a uniform speed like an oscillating swing speed, and at this time, the speed information of the carrier is not the linear speed of the carrier。
Therefore, the embodiment of the invention provides a frequency for swinging the tail fin of the bionic dolphinAnd amplitude->As a measure of carrier velocity. I.e. < ->And->The larger the indicating the faster the movement speed of the carrier. And the motion path information of ants representing the carrier in this path search is +.>. Including the nodes experienced by ants->Total length of path undergone by ant +.>。
S420: pheromone remains.
Specifically, the firstOnly the fundamental pheromone increment left by ants on the path:
；
the water resistance pheromone increment related to the speed of the underwater robot in the total pheromone increment:
；
the water resistance information increment element related to the walking length of ants in the total pheromone increment:
；
in the method, in the process of the invention,total length of path for ants, +.>For the pheromone increment constant,/->For carrier wobble frequency, < >>Is the carrier wobble amplitude.
Specifically, in the present invention, more than one pheromone is used, which together constitute the total pheromone. That is, in the overall pheromone, there is a speed-dependent water resistance pheromone, and there is a path-dependent water resistance pheromone, that is, the overall path length of the planned path, the specific expression is as described above.
S430: and judging whether all ants finish the iteration task.
Specifically, if all ants have completed the iterative task, S450 is performed. Otherwise, executing S440;
s440: selecting the next ant, returning to S100, and continuously executing the iteration task;
s450: and updating the pheromone. Specifically, when all ants complete a round of iterative task, the nodes are updatedThe pheromone concentration:
；
in the method, in the process of the invention,for the total number of ants reaching the end point in one iteration, +.>For weight constants, representing the importance of three pheromone increments, respectively, < >>Is the concentration and volatility coefficient of pheromone>For node->To node->Pheromone concentration of (2).
In particular, the method comprises the steps of,the pheromone volatilization coefficient, that is, how much the pheromone volatilizes when each new iteration starts, is set to be 0.3, that is, 30% of each round of volatilization.
S500: judging whether all iteration rounds are completed, if not, returning to S100; if yes, outputting the path which is passed by the ant with the minimum path cost in all iteration rounds as a final planning path.
Specifically, it is determined whether all iteration rounds are completed, and if not, S100 is returned to, and linear execution is continued. The ant colony algorithm has two parameters of ant number and iteration number, and the iteration number is 50 assuming 50 ants. Then the 1 st iteration starts, the first ant is selected first, searching is started, after the 1 st ant finishes the task, the 2 nd ant is selected, searching task is continued, and so on until 50 ants are all explored, and the 1 st iteration is considered to be finished. And judging whether 50 iterations are completed according to the flow chart, if not, continuing to start the 2 nd iteration, repeating the steps until all iteration rounds are completed, and outputting a path which is passed by the ant with the minimum path cost in all iteration rounds as a final planning path.
According to the technical scheme, the ant colony algorithm is improved by using the directional particles, the turning cost and the water resistance cost are integrated, the transfer possibility is optimized by the current direction information and the current accessible node information of ants, the intelligence and the adaptability of the ant colony algorithm during path planning can be effectively enhanced, and the ant colony algorithm is more in line with engineering scenes. The invention combines the energy consumption characteristic and the motion characteristic of the bionic robot dolphin during underwater motion, takes the tail fin swinging frequency and the amplitude of the bionic robot dolphin as speed consideration, simultaneously considers the water resistance of the bionic robot dolphin, gives out a corresponding pheromone iteration function, and provides an underwater robot path planning method which combines the turning cost and the water resistance cost on the basis of a basic ant colony algorithm and combines the power consumption limitation of the bionic robot dolphin.
In another aspect, the invention discloses an electronic device comprising a memory, a processor and a computer program stored on the memory and operable on the processor, the processor executing the computer program to perform the steps of a method for path planning of an underwater robot based on directed particle ant colony.
Herein, a carrier is a popular term and may refer to an underwater robot. Since the present invention has a certain versatility, the carrier can be regarded as other kinds of robots in the field of versatility.
In the present specification, each embodiment is described in a progressive manner, and each embodiment is mainly described in a different point from other embodiments, and identical and similar parts between the embodiments are all enough to refer to each other. For the device disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant points refer to the description of the method section.
The previous description of the disclosed embodiments is provided to enable any person skilled in the art to make or use the present invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the generic principles defined herein may be applied to other embodiments without departing from the spirit or scope of the invention. Thus, the present invention is not intended to be limited to the embodiments shown herein but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.
Claims (10)
1. An underwater robot path planning method based on directed particle ant colony is characterized by comprising the following steps:
s100: when the path planning overall process starts, constructing an underwater environment model, and determining a starting point and an ending point; placing all ants at the start of each round of iteration; when each ant starts to act, constructing the ants as directed particles, acquiring the current direction of the ants and current accessible nodes, and calculating to obtain a set of turning costs of all accessible nodes;
s200: according to the turning cost, an improved roulette algorithm is used for realizing ant routing;
s300: judging whether ants reach a preset end point, if not, returning to S100, updating the direction of the directed particles, and recalculating the turning cost of all accessible nodes; if yes, then:
s400: calculating the total water resistance cost of the current path-finding path, and synchronously updating the total water resistance cost of the current path-finding path as a water resistance pheromone and a path pheromone;
s500: judging whether all iteration rounds are completed, if not, returning to S100; if yes, outputting the path which is passed by the ant with the minimum path cost in all iteration rounds as a final planning path and outputting.
2. The method for planning an underwater robot path based on directed particle ant colony of claim 1, wherein S100: the ant is constructed as directed particles, the turning cost of all accessible nodes is calculated by the current direction of the ant and the current accessible nodes, and the method comprises the following steps:
s110: defining ant current direction information, and determining all possible directions of the current ants in the grid chart;
s120: determining the ultimate cornering power of the carrier;
s130: defining a turning cost function according to all possible directions of the current ants in the grid chart and the limit turning capacity of the carrier;
s140: acquiring ant current position information and all accessible node information;
s150: calculating turning cost through a turning cost function based on ant current position information and all accessible node information;
s160: and outputting a set of turning costs of all accessible nodes.
3. The method for planning an underwater robot path based on directed particle ant colony of claim 1, wherein S200: the method for improving the routing of the roulette ants according to the turning cost comprises the following steps:
s210: recording the reciprocal of the distance from all accessible nodes to the end point in the raster image to form a heuristic information four-dimensional matrix, and recording the pheromones among all nodes in the raster image to form a pheromone multi-dimensional matrix;
s220: calculating the transition probability of ants to each accessible node based on the set of turning cost of all the accessible nodes, the heuristic information four-dimensional matrix and the pheromone multi-dimensional matrix, and obtaining a transition probability array set of all the accessible nodes;
s230: selecting a forward node through a roulette mechanism based on a set of transition probability arrays for all accessible nodes;
s240: updating ant position and accessible node information;
s250: and outputting the updated ant position information and the accessible node information.
4. A method for path planning of an underwater robot based on directed particle ant colony according to claim 3, wherein the step S220: calculating the transition probability of ants to each accessible node based on the set of turning cost of all accessible nodes, the heuristic information four-dimensional matrix and the pheromone multi-dimensional matrix, and obtaining a transition probability array set of all accessible nodes, wherein the formula is as follows:
in the method, in the process of the invention,for transfer expression, ++>Indicating that the kth accessible node is to be reached,representing the current grid position of ants->To the accessible node->Is provided with the heuristic information of (a),representing the current grid position +.>To the accessible node->Pheromone content of->Representing the turn cost of ants moving to that node, +.>Transition probability for ants to transition to a kth node accessible node, +.>Is the weight;
traversing all accessible nodes to obtain a transition probability array set of all accessible nodes。
5. The method for path planning of an underwater robot based on directed particle ant colony of claim 4, wherein the step S220: calculating the transition probability from the ant to each accessible node based on the set of turning cost of all the accessible nodes, the heuristic information four-dimensional matrix and the pheromone multi-dimensional matrix, and obtaining the transition probability array set of all the accessible nodes, wherein the method further comprises the following steps:
transition probability array set at all accessible nodesIn (I)>Taking random number +.>Taking a transition probability arrayThe method comprises the following steps:
wherein if itIf not, it means that the ant does not walk into the local optimum node, and the next node for ant transfer is +.>。
6. A method for path planning of an underwater robot based on directed particle ant colony as claimed in claim 3, wherein said S240: updating ant position and accessible node information, thenAnd traversing all accessible nodes of the nodes where the ants are located after moving, including the number of accessible nodes +.>Coordinates with each accessible node:
；
in the method, in the process of the invention,representing the current direction of ants, +.>Indicating the direction in which the selected ant moves behind the o-node.
7. The method for planning an underwater robot path based on directed particle ant colony of claim 1, wherein the step S400: calculating the total water resistance cost of the current path-finding path, and synchronously updating the total water resistance cost of the current path-finding path as a water resistance pheromone and a path pheromone, wherein the method comprises the following steps:
s410: acquiring carrier speed information and motion path information;
s420: calculating the water resistance information element related to the speed and the water resistance information element related to the distance, which are left by ants on the current path searching path, based on the carrier speed information and the motion path information;
s430: judging whether all ants complete the iteration task of the current round, if not, executing S440, if so, executing S450;
s440: selecting the next ant, returning to S100, and continuing the iteration task;
s450: updating the pheromone concentration on the node.
8. The method for path planning of an underwater robot based on directed particle ant colony of claim 7, wherein the step S420: based on the carrier speed information and the motion path information, calculating a speed-related water resistance pheromone and a path-related water resistance pheromone which are left by ants on the current path-finding path, wherein the method comprises the following steps of:
first, theThe incremental expression of the pheromone left by ants on the path is:
；
the water resistance pheromone expression related to the speed in the pheromone increment is as follows:
；
the water resistance pheromone expression related to the journey in the pheromone increment is as follows:
；
in the method, in the process of the invention,total length of path for ants, +.>For the pheromone increment constant,/->For carrier wobble frequency, < >>Is the carrier wobble amplitude.
9. The method for path planning of an underwater robot based on directed particle ant colony of claim 7, wherein the step S450: updating the pheromone concentration on the node, comprising:
when all ants complete one round of iterative task, nodes are updatedThe pheromone concentration is expressed as follows:
；
in the method, in the process of the invention,for the total number of ants reaching the end point in one iteration, +.>For weight constants, representing the importance of three pheromone increments, respectively, < >>Is the concentration and volatility coefficient of pheromone>For node->To node->Pheromone concentration of (2).
10. An electronic device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, wherein the processor performs the steps of the method for directional particle ant colony based underwater robot path planning as claimed in any of claims 1 to 9.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311685953.1A CN117387628B (en) | 2023-12-11 | 2023-12-11 | Underwater robot path planning method and device based on directed particle ant colony |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311685953.1A CN117387628B (en) | 2023-12-11 | 2023-12-11 | Underwater robot path planning method and device based on directed particle ant colony |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117387628A true CN117387628A (en) | 2024-01-12 |
| CN117387628B CN117387628B (en) | 2024-02-23 |
Family
ID=89466967
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311685953.1A Active CN117387628B (en) | 2023-12-11 | 2023-12-11 | Underwater robot path planning method and device based on directed particle ant colony |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117387628B (en) |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103149933A (en) * | 2013-02-27 | 2013-06-12 | 南京邮电大学 | Closed-loop control-based humanoid robot omnidirectional walking method |
| CN107229998A (en) * | 2017-05-31 | 2017-10-03 | 江苏德胜智业信息技术有限公司 | A kind of autonomous pathfinding strategy process of unmanned plane |
| CN114964261A (en) * | 2022-05-20 | 2022-08-30 | 江南大学 | Mobile robot path planning method based on improved ant colony algorithm |
| CN115328118A (en) * | 2022-07-22 | 2022-11-11 | 东软集团股份有限公司 | Path determining method and device, readable storage medium and electronic equipment |
| WO2023029188A1 (en) * | 2021-08-31 | 2023-03-09 | 南京天溯自动化控制系统有限公司 | Path planning method and apparatus, and device and storage medium |
| CN116540738A (en) * | 2023-06-13 | 2023-08-04 | 东北石油大学 | Mobile robot path planning method based on motion constraint improved ant colony algorithm |
| CN116560363A (en) * | 2023-04-25 | 2023-08-08 | 新疆理工学院 | Robot low-energy-consumption path planning method for improving ant colony algorithm in multi-attribute grid environment |
| CN117053793A (en) * | 2023-08-01 | 2023-11-14 | 武汉理工大学 | Water quality monitoring and inspection method, device and equipment based on sea-air cross-domain cooperation |
-
2023
- 2023-12-11 CN CN202311685953.1A patent/CN117387628B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103149933A (en) * | 2013-02-27 | 2013-06-12 | 南京邮电大学 | Closed-loop control-based humanoid robot omnidirectional walking method |
| CN107229998A (en) * | 2017-05-31 | 2017-10-03 | 江苏德胜智业信息技术有限公司 | A kind of autonomous pathfinding strategy process of unmanned plane |
| WO2023029188A1 (en) * | 2021-08-31 | 2023-03-09 | 南京天溯自动化控制系统有限公司 | Path planning method and apparatus, and device and storage medium |
| CN114964261A (en) * | 2022-05-20 | 2022-08-30 | 江南大学 | Mobile robot path planning method based on improved ant colony algorithm |
| CN115328118A (en) * | 2022-07-22 | 2022-11-11 | 东软集团股份有限公司 | Path determining method and device, readable storage medium and electronic equipment |
| CN116560363A (en) * | 2023-04-25 | 2023-08-08 | 新疆理工学院 | Robot low-energy-consumption path planning method for improving ant colony algorithm in multi-attribute grid environment |
| CN116540738A (en) * | 2023-06-13 | 2023-08-04 | 东北石油大学 | Mobile robot path planning method based on motion constraint improved ant colony algorithm |
| CN117053793A (en) * | 2023-08-01 | 2023-11-14 | 武汉理工大学 | Water quality monitoring and inspection method, device and equipment based on sea-air cross-domain cooperation |
Non-Patent Citations (2)
| Title |
|---|
| 周敬东等: "基于改进蚁群算法的路径规划方法", 湖北工业大学学报, no. 05, 15 October 2018 (2018-10-15), pages 51 - 54 * |
| 陈侠等: "基于改进进化算法的无人机航迹规划", 科学技术与工程, no. 03, 28 January 2018 (2018-01-28), pages 84 - 89 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117387628B (en) | 2024-02-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110703766B (en) | Unmanned aerial vehicle path planning method based on transfer learning strategy deep Q network | |
| Liu et al. | Dynamic path planning of mobile robot based on improved ant colony optimization algorithm | |
| WO2021042827A1 (en) | Multi-agv path planning method and system | |
| CN112925315A (en) | Crawler path planning method based on improved ant colony algorithm and A-star algorithm | |
| CN112650229B (en) | Mobile robot path planning method based on improved ant colony algorithm | |
| CN111174798A (en) | Foot type robot path planning method | |
| CN111307153B (en) | Multi-AUV task allocation and path planning method based on hexagonal grid map | |
| CN113703450B (en) | Mobile robot path planning method based on smoothing factor improved ant colony algorithm | |
| CN111552183B (en) | Six-legged robot obstacle avoidance method based on adaptive weight reinforcement learning | |
| CN112666957A (en) | Underwater robot path planning method based on improved ant colony algorithm | |
| Nie et al. | Research on robot path planning based on Dijkstra and Ant colony optimization | |
| CN113848889A (en) | Path planning method based on combination of whale optimization algorithm and artificial potential field method | |
| Bai et al. | Design and Simulation of a Collision-free Path Planning Algorithm for Mobile Robots Based on Improved Ant Colony Optimization. | |
| CN114756027A (en) | Mobile robot path planning method based on improved ant colony algorithm and Bezier curve | |
| CN115373384A (en) | Vehicle dynamic path planning method and system based on improved RRT | |
| CN117387628B (en) | Underwater robot path planning method and device based on directed particle ant colony | |
| CN115248591A (en) | UUV path planning method based on hybrid initialization Hui wolf particle swarm algorithm | |
| CN114815801A (en) | Adaptive environment path planning method based on strategy-value network and MCTS | |
| Han et al. | Multi-uav automatic dynamic obstacle avoidance with experience-shared a2c | |
| CN116642498A (en) | Robot path planning method based on improved goblet sea squirt group model | |
| CN110750095A (en) | Robot cluster motion control optimization method and system based on 5G communication | |
| CN114610024B (en) | Multi-agent collaborative searching energy-saving method for mountain land | |
| Guo et al. | A Path Planning Method for the Spherical Amphibious Robot Based on Improved A-star Algorithm | |
| CN116050682A (en) | Full-coverage field path planning method and device based on improved genetic algorithm | |
| CN114578845A (en) | Unmanned aerial vehicle flight path planning method based on improved ant colony algorithm |
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 |