CN114077256A - Overwater unmanned ship path planning method - Google Patents
Overwater unmanned ship path planning method Download PDFInfo
- Publication number
- CN114077256A CN114077256A CN202111507580.XA CN202111507580A CN114077256A CN 114077256 A CN114077256 A CN 114077256A CN 202111507580 A CN202111507580 A CN 202111507580A CN 114077256 A CN114077256 A CN 114077256A
- Authority
- CN
- China
- Prior art keywords
- robot
- unmanned ship
- formula
- obstacle
- water surface
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 54
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 94
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims abstract description 56
- 238000005457 optimization Methods 0.000 claims abstract description 20
- 238000011160 research Methods 0.000 claims abstract description 20
- 238000004088 simulation Methods 0.000 claims abstract description 11
- 238000013178 mathematical model Methods 0.000 claims abstract description 9
- 238000012795 verification Methods 0.000 claims abstract description 5
- 239000002245 particle Substances 0.000 claims description 75
- 238000002922 simulated annealing Methods 0.000 claims description 26
- 239000011159 matrix material Substances 0.000 claims description 15
- 239000013598 vector Substances 0.000 claims description 12
- 238000000137 annealing Methods 0.000 claims description 10
- 238000013459 approach Methods 0.000 claims description 9
- 238000004364 calculation method Methods 0.000 claims description 8
- 230000006835 compression Effects 0.000 claims description 3
- 238000007906 compression Methods 0.000 claims description 3
- 238000013016 damping Methods 0.000 claims description 3
- 238000010408 sweeping Methods 0.000 claims description 3
- 238000012360 testing method Methods 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 230000006870 function Effects 0.000 description 20
- 238000010586 diagram Methods 0.000 description 13
- 230000010355 oscillation Effects 0.000 description 6
- 230000009471 action Effects 0.000 description 4
- 230000008859 change Effects 0.000 description 4
- 230000007547 defect Effects 0.000 description 4
- 230000008569 process Effects 0.000 description 4
- 238000004458 analytical method Methods 0.000 description 3
- 230000008901 benefit Effects 0.000 description 3
- 230000002068 genetic effect Effects 0.000 description 3
- 230000004888 barrier function Effects 0.000 description 2
- 230000007246 mechanism Effects 0.000 description 2
- 241000282414 Homo sapiens Species 0.000 description 1
- 230000003044 adaptive effect Effects 0.000 description 1
- 238000007792 addition Methods 0.000 description 1
- 230000004075 alteration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 210000000349 chromosome Anatomy 0.000 description 1
- 230000019771 cognition Effects 0.000 description 1
- 238000001816 cooling Methods 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 230000007812 deficiency Effects 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000005265 energy consumption Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 238000004880 explosion Methods 0.000 description 1
- 239000004459 forage Substances 0.000 description 1
- 230000008303 genetic mechanism Effects 0.000 description 1
- 230000013011 mating Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000011664 nicotinic acid Substances 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000010845 search algorithm Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/0206—Control of position or course in two dimensions specially adapted to water vehicles
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a path planning method for an unmanned ship on water, and relates to the field of path planning. The method for planning the path of the unmanned ship on water comprises the following steps: constructing a mathematical model of the motion of the unmanned ship on the water surface; analyzing an optimization target of the unmanned ship on the water surface in path planning; obtaining a dynamics and kinematics model of the unmanned ship according to the established mathematical model, and describing a ship motion rule; researching an autonomous obstacle avoidance strategy in a local area of the unmanned ship on the water surface; carrying out global path planning optimal control research on the unmanned surface vehicle; the global path planning refers to path planning made after all the relevant surrounding environments are known, and the global planning needs to master all environment information in the motion space and arrange the path planning according to the information. And researching various global path planning methods, seeking an optimal control algorithm, and finally performing simulation verification on the obstacle avoidance strategy and the path planning algorithm.
Description
Technical Field
The invention relates to the field of path planning, in particular to a path planning method for an unmanned ship on water.
Background
How an unmanned ship on the water surface can safely and effectively run in a complex and changeable sea area environment, potential risks are avoided according to effective means, and a technical route of local autonomous obstacle avoidance and global route planning is mainly researched in the research of reaching a terminal point by an optimal route. In the global path planning, the environment information is completely known, including the position and shape of the obstacle, and the environment information model is built based on the environment information model, and then the optimal solution is found by using the related path searching algorithm. Global planning is planning based on known information in the environment, while local planning is real-time dynamic planning based on changes in the external environment. Local planning is to plan a path from a starting point or a sub-target point to a next sub-target point after a study object senses the current position and the change condition of the external environment through equipment such as a sensor.
The current research mainly comprises the following methods:
visual Graph method (Visibility Graph) was proposed and used first in 1979 by Michael a.wesley. The algorithm connects the starting point with the top points of all barriers and the end point by straight line combination, ensures that each connecting line cannot pass through the barriers, and then searches for the optimal path from the starting point to the end point. The algorithm has the advantages that the algorithm is simple, the search time is long, and when the number of obstacles is large, large combined explosion is easy to occur.
② the free space method was introduced in the 80 s of the last century, proposed by professor lozano-Perez of the institute of technology and technology of Ma province, which is totally called free space method based on C space. The algorithm divides an environment space into a free space and an obstacle space, and then searches a path in the divided free space by utilizing a search algorithm. The free space method has the greatest characteristic of flexibility, but the complexity of the free space method is improved along with the increase of the number of obstacles.
③ the unit decomposition method is proposed by Brooks. The algorithm divides the space into sub-modules, treats each sub-module as a unit, and connects each adjacent unit into a connected graph, so that the path search is simplified into the graph search.
The topology method is to divide the space to be planned into sub-spaces with topology characteristics, because the spaces have connectivity, a topology network can be built on the basis, the built topology network search paths are utilized, and the geometric paths are solved according to the searched topology paths. The main idea of the topology is dimension reduction, which has the advantages of greatly reducing the searching space and only determining the complexity of the algorithm by the number of obstacles, and in addition, because the method does not need the accurate position of the robot, the method also has better robustness, but has the defects that the establishment of the topology network is complicated, and especially when the number of the obstacles is increased, the correction of the existing topology network has certain difficulty.
In the last 90 th century, Eberhart and Kennedy two-position doctors were inspired by the predation of birds in nature, and a Particle Swarm Optimization (PSO) algorithm was proposed. The particle swarm algorithm is one of swarm intelligence algorithms and can be regarded as a modern optimization algorithm. When birds forage for food, the positions of the birds cannot be known, the distance between the food and the birds cannot be known, the most effective strategy is to approach the position of the bird closest to the food, and therefore the probability of finding the food is greatly increased. The mechanism is a shared mechanism, in continuous cognition, an individual gives consideration to the beliefs of the individual and other individuals, and when the beliefs of the surrounding individuals are perceived to be better, the individual self can make adaptive adjustment so as to achieve population optimization. The particle swarm algorithm individuals cooperate and compete with one another, and then an optimal solution is sought in a complex space.
The genetic skimming method is a bionic algorithm for simulating Darwinian biological evolution theory, utilizes a calculation model of natural selection and genetic mechanism in the evolution theory, and is a method for searching an optimal solution by simulating the natural evolution process. Genetic algorithms include several problems including chromosome coding, population initialization, fitness evaluation, population selection, population mating and variation. When the method is applied to path planning of a mobile robot, the search direction is adjusted in a self-adaptive manner by adopting an uncertain search rule, and the genetic algorithm is mainly characterized in that a structural object is directly operated, the derivation and the limitation of function continuity do not exist, and the method has the inherent hidden parallelism and better global optimization performance; by adopting a probabilistic optimization method, the search space can be automatically acquired and optimized, the search direction can be adaptively adjusted, and a determined rule is not needed.
The simulated annealing algorithm is derived from the physical annealing process and is proposed by Kirkpatrick in 1983. The basic idea of the simulated annealing algorithm is that according to the solving process of the random optimization problem and the similar principle of the thermal balance problem in the aspect of statistical mechanics, the initial temperature, the initial situation and the cooling rate of the system are set manually to control the temperature to be reduced continuously, and the neighborhood range of the learning space is fully utilized by combining the snap-through characteristic so as to carry out random search. The advantage of applying the simulated annealing algorithm to the local path planning is that the occurrence of local extremum can be avoided, but the defect of harsh convergence condition exists, so that the simulated annealing algorithm cannot be satisfied in practical application. Under the condition of limited calculation amount, the convergence of the simulated annealing algorithm depends on the parameters of the simulated annealing algorithm, and based on the condition, how to set the parameters becomes an important link in the application of the algorithm.
The fuzzy logic algorithm simulates the natural thinking mode of human beings, so that the trouble of modeling the fuzzy logic algorithm is avoided. The algorithm is not very precise for the sensor and is not sensitive to the uncertainty of the surrounding environment of the object under study, so that the object under study can show good continuity, consistency and stability. However, since the fuzzy membership and the control rule are set as they are, it is difficult to summarize the rule, and once the control rule is established, it is difficult to adjust the rule on line, and the algorithm cannot meet the change of the situation. Therefore, how to acquire the membership function, how to reasonably formulate the control rule, and how to adjust the control rule are important problems to be faced by the fuzzy logic algorithm. In the face of the problems, the idea of combining the fuzzy logic algorithm with other algorithms can be realized, and the controller can be adjusted, so that the algorithm has learning capability and self-adaptive capability.
Ninthly, an Artificial Potential Field (APF) (artificial Potential field) algorithm is proposed by Khatib in 1986 and is introduced into the robot obstacle avoidance problem first. The basic idea of the artificial potential field method is that the motion of a research object in an environment space is regarded as the motion generated by the action of a virtual potential field, an attraction force is generated on the research object by a target point, and a repulsive force is generated on the research object by an obstacle, so that the research object moves towards the target point under the combined action of a mass force. Most of the problems exposed by the artificial potential field method are caused by the fact that in relatively complex environment information, a robot forms a special position relation between a target point and an obstacle in a special motion state. For example, the robot is difficult to find a path when walking to a nearby obstacle, is easy to swing back and forth when walking in a narrow passage, cannot find a target point when an obstacle exists near the target point, and the like.
Disclosure of Invention
The invention aims to overcome the defects, and provides a method for planning the path of the unmanned ship on water, which aims to adopt an improved artificial potential field and fuzzy logic algorithm for local autonomous obstacle avoidance and adopts an optimized particle swarm algorithm for global path planning.
The invention specifically adopts the following technical scheme:
a method for planning the path of an unmanned ship on water comprises the following steps:
(1) constructing a mathematical model of the motion of the unmanned ship on the water surface; the motion model of the unmanned ship on the water surface comprises a ground coordinate system eta ═ x, y, psi]TAnd a motion coordinate system v ═ u, v, r]TEstablishing a corresponding relation for the two coordinate systems, and obtaining the relation between the two coordinate systems of the unmanned ship on the water surface on the horizontal plane through coordinate transformation as shown in formulas (1) and (2):
η＝R(ψ)ν (1)
wherein x and y are position coordinates of the unmanned ship on the water surface; u is the surging velocity, v is the surging velocity, and r is the heading angular velocity; r (4) is a rotation matrix from a geodetic coordinate system to a motion coordinate system; psi is the heading angle, the range is 4 epsilon (-pi, m), and the motion model of the unmanned ship on the water surface is shown as the formula (3):
M(ν)ν+C(ν)ν+D(ν)ν＝τ (3)
the expression of each term in formula (3) is as follows:
τ＝[τ u 0 τr]T (4)
C23＝-C32＝m11u (8)
wherein tau is a control force, R is a yaw rotation matrix, M is an inertia coefficient matrix, C is a Coriolis force and centripetal force matrix, and D is a damping coefficient matrix.
(2) Analyzing optimization target of unmanned ship on water surface in path planning
Obtaining a dynamics and kinematics model of the unmanned ship according to the established mathematical model of the unmanned ship on the water surface, describing the motion rule of the ship, and using an improved LOS method to track the path, wherein the control model is shown in formulas (10) to (13):
wherein x isc,yc，ψcIs the tracking error, xsf,ysf,ψsfRepresenting pose information of target point, s ═ vr,νtV and vrThe vertical and tangential velocity of the target path are resolved, K(s) is the curvature of a reference point, and the method comprises the following steps:
ψ″ff＝ψ″B+χ″ (16)
in the formula psiBIs the ship heading;
the unmanned ship on the water surface is aligned to x in navigationc,yc，ψcThree tracking errors are controlled to control the navigational speed to approach the desired navigational speed u, so the objective of the path tracking control of the unmanned ship on the water surface is to make the error PcMinimum, as shown in formula (20);
Pc＝(xc,yc,ψc,νc)T→0 (20)
in order to facilitate the tracking of the planned sea-sweeping path, when the unmanned ship on the water keeps stable course and makes straight-line navigation, the angular speed of the heading angle is 0, namely, the formula (20) is simplified into the formula (21):
Pc＝(xc,yc,νc)T→0 (21)
when the unmanned ship on the water surface keeps constant-speed straight line navigation, the navigation speed V is not changed, and the method is further simplified into the formula (22)
Pc＝(xc,yc)T→0 (22)；
(3) Research on autonomous obstacle avoidance strategy of unmanned surface vessel in local area
The method is improved on the basis of a classical repulsion potential field function, the repulsion can be reduced when the improved repulsion potential field function approaches a target point along with the robot, and the formula (23) is shown as follows:
wherein, p-pgoalIs a vector representing the euclidean distance between the robot and the target point, the direction being on the line connecting the robot and the target point, the target point being pointed to by the robot; compared with the repulsive force potential field function in the classical potential function, the formula increases p-pgoalThe multiplier is used for ensuring that the potential field value of the robot when the robot reaches the target point is the global minimum;
(4) water surface unmanned ship global path planning optimal control research
Performing global path planning optimal control on the unmanned surface ship based on a hybrid particle swarm algorithm for simulated annealing, wherein the hybrid particle swarm algorithm based on the simulated annealing is an operation of adding the simulated annealing algorithm into a basic particle swarm algorithm, and the factors added into the particle swarm algorithm mainly comprise an initial temperature, a temperature annealing mode and the capability of accepting a poor solution with a certain probability;
(5) and (5) performing simulation verification test.
Preferably, in the research of the autonomous obstacle avoidance strategy of the unmanned surface vessel in the local area, a dynamic attraction force potential field model and a dynamic repulsion force potential field model are adopted;
when the target point is moving, the robot keeps away from the obstacle while tracking the obstacle, a gravitational potential field model of the target point is established according to a formula (24), the model consists of two parts, and the potential field function part based on relative position potential and relative speed:
where v denotes the current speed of movement of the robot, vgoalRepresenting the current velocity of the target point, both vectors, v-vgoalBy adjusting k for the relative velocity between the robot and the target pointattpAnd kattvThe relative position and the proportion of the relative speed in the gravitational total potential field are adjusted when k isattvWhen the value of zero is taken, the system is degraded into a traditional gravitational field function;
a relative speed term is introduced into the repulsive force potential field function, as shown in the formula (26), and a dynamic repulsive force potential field model is selected as shown in the formula (27) by integrating the repulsive force potential field function of the relative position;
Preferably, in the research on the global path planning optimal control of the unmanned surface ship, the hybrid particle swarm algorithm based on simulated annealing has the following characteristics:
1) increasing the "initial temperature" t0，t0The calculation formula is shown as (28):
t0＝Fit(pg)/ln 5 (28)
where fit (x) denotes the fitness function, pgIs a global extremum;
2) increasing the annealing coefficient lambda to control the annealing mode as shown in formula (29)
tk+1＝λtk (29)
3) The probability calculation formula that the particle is selected as the global optimum at the current temperature is increased, and the formula is as follows (30):
wherein t represents the current temperature, and N represents the number of particles in the particle swarm;
4) selecting a p from all individuals by simulating the jump characteristic of a simulated annealing algorithmiAs global optimum p'gTo replace the true global optimum pgI.e. selecting the poor solution as the global optimum value, p ', with a certain probability'gThe selection of (1) is by roulette, wherein the probability that each individual is selected as the global optimum is calculated by equation (30);
5) the velocity and position of each particle are updated according to equations (31) to (33):
νi,j(t+1)＝φ{νi,j(t)+c1r1[pi,j-xi,j(t)]+c2r2[pg,j-xi,j(t)]} (31)
xi,j(t+1)＝xi,j(t)+νi,j(t+1),j＝1,2,...,d (32)
where φ represents the compression factor and d represents the dimension of the solution corresponding to the particle.
The invention has the following beneficial effects:
the path planning method is solved by utilizing the snap-through characteristic of the simulated annealing algorithm, and the snap-through characteristic of the simulated annealing algorithm can jump out of local optimum when the particle swarm optimization falls into a local optimum solution, so that the global search capability of the particle swarm optimization is enhanced. Meanwhile, the rules of the simulated annealing algorithm and the particle swarm algorithm are simple and easy to realize, and the algorithm efficiency of the simulated annealing algorithm and the particle swarm algorithm is high.
Drawings
Fig. 1 is a schematic diagram of a route analysis of a local autonomous obstacle avoidance technique;
FIG. 2 is a schematic diagram of a global path planning technique route analysis;
FIG. 3 is a force analysis diagram of a target unreachable problem;
FIG. 4 is a diagram of an example simulation of a target unreachable problem;
fig. 5 is a schematic diagram of step 43 of the dynamic obstacle avoidance simulation example 2;
fig. 6 is a schematic diagram of step 103 of the dynamic obstacle avoidance simulation example 2;
FIG. 7 is a particle swarm algorithm flow based on simulated annealing;
FIG. 8 is a schematic diagram of a general particle swarm algorithm planning;
FIG. 9 is a schematic diagram of an improved general particle swarm algorithm planning;
FIG. 10 is a diagram illustrating the iteration times of a conventional particle swarm algorithm;
FIG. 11 is a diagram illustrating the number of iterations of an improved conventional particle swarm algorithm;
FIG. 12 is a general particle swarm algorithm planning;
FIG. 13 is a modified generic particle swarm algorithm plan;
FIG. 14 is a diagram of the number of iterations of a conventional particle swarm algorithm;
fig. 15 shows the number of iterations of the modified general particle swarm algorithm.
Detailed Description
The following description of the embodiments of the present invention will be made with reference to the accompanying drawings:
with reference to fig. 1 to 7, a method for planning a path of an unmanned ship on water includes the following steps:
(1) constructing a mathematical model of the motion of the unmanned ship on the water surface (the mathematical model comprises a power model and a motion model); the motion model of the unmanned ship on the water surface comprises a ground coordinate system eta ═ x, y, psi]TAnd a motion coordinate system v ═ u, v, r]TEstablishing a corresponding relation for the two coordinate systems, and obtaining the relation between the two coordinate systems of the unmanned ship on the water surface on the horizontal plane through coordinate transformation as shown in formulas (1) and (2):
η＝R(ψ)ν (1)
wherein x and y are position coordinates of the unmanned ship on the water surface; u is the surging velocity, v is the surging velocity, and r is the heading angular velocity; r (4) is a rotation matrix from a geodetic coordinate system to a motion coordinate system; psi is the heading angle, the range is 4 epsilon (-pi, m), and the motion model of the unmanned ship on the water surface is shown as the formula (3):
M(ν)ν+C(ν)ν+D(ν)ν＝τ (3)
the expression of each term in formula (3) is as follows:
τ＝[τ u 0 τr]T (4)
C23＝-C32＝m11u (8)
wherein tau is a control force, R is a yaw rotation matrix, M is an inertia coefficient matrix, C is a Coriolis force and centripetal force matrix, and D is a damping coefficient matrix.
(2) Analyzing optimization target of unmanned ship on water surface in path planning
Obtaining a dynamics and kinematics model of the unmanned ship according to the established mathematical model of the motion of the unmanned ship on the water surface, describing a ship motion rule, simulating an environment variable in path planning as truly as possible to ensure the organic balance of stable course, stable navigational speed and path length of the unmanned ship on the water surface in a path considering energy consumption, determining an optimization target, and using an improved LOS method to track the path, wherein the control model is shown as formulas (10) to (13):
wherein x isc,yc，ψcIs the tracking error, xsf,ysf,ψsfRepresenting pose information of target point, s ═ vr,νtV and vrThe vertical and tangential velocity of the target path are resolved, K(s) is the curvature of a reference point, and the method comprises the following steps:
ψ″ff＝ψ″B+χ″ (16)
in the formula psiBIs the ship heading;
the unmanned ship on the water surface is aligned to x in navigationc,yc，ψcThree tracking errors are controlled to control the navigational speed to approach the desired navigational speed u, so the objective of the path tracking control of the unmanned ship on the water surface is to make the error PcMinimum, as shown in formula (20);
Pc＝(xc,yc,ψc,νc)T→0 (20)
in order to facilitate the tracking of the planned sea-sweeping path, when the unmanned ship on the water keeps stable course and makes straight-line navigation, the angular speed of the heading angle is 0, namely, the formula (20) is simplified into the formula (21):
Pc＝(xc,yc,νc)T→0 (21)
when the unmanned ship on the water surface keeps constant-speed straight line navigation, the navigation speed V is not changed, and the method is further simplified into the formula (22)
Pc＝(xc,yc)T→0 (22)。
(3) Research on autonomous obstacle avoidance strategy of unmanned surface vessel in local area
The local planning is real-time dynamic planning according to the change of the external environment. Local planning is to plan a path from a starting point or a sub-target point to a next sub-target point after a study object senses the current position and the change condition of the external environment through equipment such as a sensor. Obstacle avoidance is the key content of local planning research, and when the unmanned ship on the water surface normally runs, the unmanned ship suddenly encounters an unknown static or dynamic obstacle to interfere the air line, and at the moment, the unmanned ship is adjusted so as to avoid danger in time.
Defect of classical artificial potential field method
Most of the problems exposed by the artificial potential field method are caused by the fact that in relatively complex environment information, a robot forms a special position relation between a target point and an obstacle in a special motion state. For example, the robot is difficult to find a path when walking to a nearby obstacle, is easy to swing back and forth when walking in a narrow passage, cannot find a target point when an obstacle exists near the target point, and the like. The following is a study discussion of several of the most prominent problems of the potential field method.
(1) Problem of unreachable target
If an obstacle exists near the target point and the robot is also within the influence range of the obstacle, the repulsion force is increased when the robot approaches the obstacle, the repulsion force is increased sharply as the robot approaches the obstacle, and the attraction force is small relative to the repulsion force, so that the robot cannot reach the target point finally. This is the problem that the target of the artificial potential field method is not reachable, and is also called global minimum problem in some documents, which means that the target point is not the global total potential field lowest point. FIG. 2 shows that both the robot and the target point are within the influence of the obstacle, p-pobsThe value is decreased, then
The value is increased, thereby
Increasing; and p-pgoalIs reduced thereby
Decrease, eventually resulting in the robot being repelled by the obstacle away from the target point.
FIG. 4 is a simulation diagram of a target unreachable problem of the classical potential field method. In this simulation 4 obstacles were set, as graphically shown in the figure, with two obstacles near the target point. It is obvious from the simulation diagram that when the robot is about to reach a target point, the robot enters the influence range of two obstacles, and the repulsion force is too large, so that the robot loops back and forth at the point and cannot reach the target point.
(2) Local minima problem
In a multi-obstacle environment, there are many possibilities of distribution of obstacles, and at a point before the robot reaches the target, the virtual attraction of the target point to the robot and the virtual repulsion of the obstacle to the robot are just equal in magnitude and opposite in direction, and the resultant force is zero, so that the robot mistakenly thinks that the robot reaches the target point. The robot cannot reach a target point without stopping or wandering at the position and without knowing the moving direction of the next step, which is a local minimum problem of the artificial potential field method.
(3) Problem of path oscillation
In addition, when the robot passes through a position with more obstacles, local oscillation occurs. The reason for the oscillation is that the attraction force applied to the robot at the position is zero, but the speed is not zero, so that the robot continues to advance under the action of inertia and leaves the point, and meanwhile, the robot is subjected to an opposite repulsive force, so that the speed of the robot is reduced to zero, and the robot moves backwards under the action of the repulsive force, and the operation is repeated in a cycle. It follows that the root cause of the oscillation is also from the local minima problem. In the process of oscillation, the robot can sometimes walk out local minimum values by itself, much time is wasted during oscillation, the speed of the robot is obviously slowed down, and the local minimum values cannot be walked out independently under most conditions.
The method is improved on the basis of a classical repulsion potential field function, the repulsion can be reduced when the improved repulsion potential field function approaches a target point along with the robot, and the formula (23) is shown as follows:
wherein, p-pgoalIs a vector representing the euclidean distance between the robot and the target point, the direction being on the line connecting the robot and the target point, the target point being pointed to by the robot; compared with the repulsive force potential field function in the classical potential function, the formula increases p-pgoalAnd the multiplier is used for ensuring that the potential field value of the robot when the robot reaches the target point is the global minimum.
In the research of the autonomous obstacle avoidance strategy of the unmanned ship local area on the water surface, a dynamic attraction potential field model and a dynamic repulsion potential field model are adopted;
when the target point is moving, the robot keeps away from the obstacle while tracking the obstacle, a gravitational potential field model of the target point is established according to a formula (24), the model consists of two parts, and the potential field function part based on relative position potential and relative speed:
where v denotes the current speed of movement of the robot, vgoalRepresenting the current velocity of the target point, both vectors, v-vgoalBy adjusting k for the relative velocity between the robot and the target pointattpAnd kattvThe relative position and the proportion of the relative speed in the gravitational total potential field are adjusted when k isattvWhen the value of zero is taken, the system is degraded into a traditional gravitational field function;
a relative speed term is introduced into the repulsive force potential field function, as shown in the formula (26), and a dynamic repulsive force potential field model is selected as shown in the formula (27) by integrating the repulsive force potential field function of the relative position;
(4) Water surface unmanned ship global path planning optimal control research
The hybrid particle swarm algorithm based on simulated annealing is used for carrying out global path planning optimal control on the unmanned ship on the water surface, the hybrid particle swarm algorithm based on simulated annealing is an operation of adding the simulated annealing algorithm into a basic particle swarm algorithm, and factors added into the particle swarm algorithm mainly comprise initial temperature, a temperature annealing mode and the capability of accepting a poor solution with a certain probability.
In the research of global path planning optimal control of the unmanned ship on the water surface, a hybrid particle swarm algorithm based on simulated annealing has the following characteristics:
1) increasing the "initial temperature" t0，t0The calculation formula is shown as (28):
t0＝Fit(pg)/ln 5 (28)
where fit (x) denotes the fitness function, pgIs a global extremum;
2) increasing the annealing coefficient lambda to control the annealing mode as shown in formula (29)
tk+1＝λtk (29)
3) The probability calculation formula that the particle is selected as the global optimum at the current temperature is increased, and the formula is as follows (30):
wherein t represents the current temperature, and N represents the number of particles in the particle swarm;
4) selecting a p from all individuals by simulating the jump characteristic of a simulated annealing algorithmiAs global optimum p'gTo replace the true global optimum pgI.e. selecting the poor solution as the global optimum value, p ', with a certain probability'gThe selection of (1) is by roulette, wherein the probability that each individual is selected as the global optimum is calculated by equation (30);
5) the velocity and position of each particle are updated according to equations (31) to (33):
νi,j(t+1)＝φ{νi,j(t)+c1r1[pi,j-xi,j(t)]+c2r2[pg,j-xi,j(t)]} (31)
xi,j(t+1)＝xi,j(t)+νi,j(t+1),j＝1,2,...,d (32)
where φ represents the compression factor and d represents the dimension of the solution corresponding to the particle.
(5) Simulation verification tests are combined with fig. 8-15, in fig. 8, an environment map example 1 is set to have an environment size of 20 × 20 under a general particle swarm algorithm, and a path planning situation is set. The upper left square box is the starting point, and the lower right star is the destination point. And (4) planning a path by using a common particle swarm algorithm, and reaching a destination point from a starting point.
In fig. 9, in the environment map example 1, the environment size is set to 20 × 20 under the improved particle swarm optimization, and the route is planned. The upper left square box is the starting point, and the lower right star is the destination point. Through the improved particle swarm algorithm path planning, the starting point can reach the destination point, and the path of the improved particle swarm algorithm path planning is shorter than that of a common particle swarm algorithm.
Fig. 10 and fig. 11 show the number of iterations of path planning in the environment map example 1 under the normal particle swarm algorithm and the improved particle swarm algorithm, respectively. As shown in fig. 10 and 11, after 5 iterations, both the normal particle swarm algorithm (fig. 10) and the improved particle swarm algorithm (fig. 11) reach respective local optimal solutions, and after that, the improved particle swarm algorithm finds a better solution than before after 7 iterations, and stabilizes at the optimal solution until the search is finished; after 54 iterations, the ordinary particle swarm algorithm reaches a better solution until the search is finished. Therefore, in the aspect of convergence speed, according to experimental results, the improved particle swarm optimization is obviously found to be faster than the common particle swarm optimization; in the aspect of planning the path length, the path length of the improved particle swarm algorithm is smaller than that of the common particle swarm algorithm.
Fig. 12 shows an environment map example 2, in which the environment size is set to 20 × 20 by the ordinary particle swarm optimization, and a route is planned. The upper left square box is the starting point, and the lower right star is the destination point. Through the path planning of the common particle swarm algorithm, a path from a starting point to a destination is not planned, but a local optimal value is trapped, and the destination cannot be reached.
Fig. 13 shows an environment map example 2, in which the environment size is set to 20 × 20 under the modified particle swarm optimization, and the path planning is performed. The upper left square box is the starting point, and the lower right star is the destination point. Through the improved particle swarm algorithm path planning, the starting point can reach the destination point, and the local optimal value in the graph 12 is obtained.
Fig. 14 and 15 show the number of iterations of path planning in the general particle swarm algorithm and the improved particle swarm algorithm, respectively, in the environment map example 2. As shown in fig. 14 and fig. 15, after 3 iterations, both the normal particle swarm algorithm (fig. 14) and the improved particle swarm algorithm (fig. 15) reach respective local optimal solutions, and after that, the improved particle swarm algorithm finds a better solution than before after 7 iterations, and stabilizes at the optimal solution until the search is finished; after 10 iterations, the ordinary particle swarm algorithm reaches the local optimal position, but cannot break away from the local optimal position to continue searching, so that the final search result is left near the local value 7100. Therefore, the improved particle swarm optimization can overcome the inherent deficiency of the common particle swarm optimization, break through the local optimal value obstacle and finally enable the planning result to reach the global optimal solution.
And (3) applying MATLAB simulation software to carry out simulation verification on the obstacle avoidance strategy and the path planning algorithm.
It is to be understood that the above description is not intended to limit the present invention, and the present invention is not limited to the above examples, and those skilled in the art may make modifications, alterations, additions or substitutions within the spirit and scope of the present invention.
Claims (3)
1. A method for planning the path of an unmanned ship on water is characterized by comprising the following steps:
(1) constructing a mathematical model of the motion of the unmanned ship on the water surface; the motion model of the unmanned ship on the water surface comprises a ground coordinate system eta ═ x, y, psi]TAnd a motion coordinate system v ═ u, v, r]TEstablishing a corresponding relation for the two coordinate systems, and obtaining the relation between the two coordinate systems of the unmanned ship on the water surface on the horizontal plane through coordinate transformation as shown in formulas (1) and (2):
η＝R(ψ)ν (1)
wherein x and y are position coordinates of the unmanned ship on the water surface; u is the surging velocity, v is the surging velocity, and r is the heading angular velocity; r (psi) is a rotation matrix from the geodetic coordinate system to the motion coordinate system; psi is the heading angle, the range is 4 epsilon (-pi, m), and the motion model of the unmanned ship on the water surface is shown as the formula (3):
M(ν)ν+C(ν)ν+D(ν)ν＝τ (3)
the expression of each term in formula (3) is as follows:
τ＝[τu 0 τr]T (4)
C23＝-C32＝m11u (8)
wherein tau is a control force, R is a yaw rotation matrix, M is an inertia coefficient matrix, C is a Coriolis force and centripetal force matrix, and D is a damping coefficient matrix;
(2) analyzing optimization target of unmanned ship on water surface in path planning
Obtaining a dynamics and kinematics model of the unmanned ship according to the established mathematical model of the unmanned ship on the water surface, describing the motion rule of the ship, and using an improved LOS method to track the path, wherein the control model is shown in formulas (10) to (13):
wherein x isc,yc，ψcIs the tracking error, xsf,ysf,ψsfRepresenting pose information of target point, s ═ vr,νtV and vrThe vertical and tangential velocity of the target path are resolved, K(s) is the curvature of a reference point, and the method comprises the following steps:
ψ″ff＝ψ″B+χ” (16)
in the formula psiBIs the ship heading;
the unmanned ship on the water surface is aligned to x in navigationc,yc，ψcThree tracking errors are controlled to control the navigational speed to approach the desired navigational speed u, so the objective of the path tracking control of the unmanned ship on the water surface is to make the error PcMinimum, as shown in formula (20);
Pc＝(xc,yc,ψc,νc)T→0 (20)
in order to facilitate the tracking of the planned sea-sweeping path, when the unmanned ship on the water keeps stable course and makes straight-line navigation, the angular speed of the heading angle is 0, namely, the formula (20) is simplified into the formula (21):
Pc＝(xc,yc,νc)T→0 (21)
when the unmanned ship on the water surface keeps constant-speed straight line navigation, the navigation speed V is not changed, and the method is further simplified into the formula (22)
Pc＝(xc,yc)T→0 (22)；
(3) Research on autonomous obstacle avoidance strategy of unmanned surface vessel in local area
The method is improved on the basis of a classical repulsion potential field function, the repulsion can be reduced when the improved repulsion potential field function approaches a target point along with the robot, and the formula (23) is shown as follows:
wherein, p-pgoalIs a vector representing the euclidean distance between the robot and the target point, the direction being on the line connecting the robot and the target point, the target point being pointed to by the robot; compared with the repulsive force potential field function in the classical potential function, the formula increases p-pgoalThe multiplier is used for ensuring that the potential field value of the robot when the robot reaches the target point is the global minimum;
(4) water surface unmanned ship global path planning optimal control research
Performing global path planning optimal control on the unmanned surface ship based on a hybrid particle swarm algorithm for simulated annealing, wherein the hybrid particle swarm algorithm based on the simulated annealing is an operation of adding the simulated annealing algorithm into a basic particle swarm algorithm, and the factors added into the particle swarm algorithm mainly comprise an initial temperature, a temperature annealing mode and the capability of accepting a poor solution with a certain probability;
(5) carrying out simulation verification test;
2. the method for planning the path of the unmanned ship on water as claimed in claim 1, wherein a dynamic gravitational potential field model and a dynamic repulsive potential field model are adopted in the research of the autonomous obstacle avoidance strategy of the unmanned ship on water in the local area;
when the target point is moving, the robot keeps away from the obstacle while tracking the obstacle, a gravitational potential field model of the target point is established according to a formula (24), the model consists of two parts, and the potential field function part based on relative position potential and relative speed:
where v denotes the current speed of movement of the robot, vgoalRepresenting the current velocity of the target point, both vectors, v-vgoalBy adjusting k for the relative velocity between the robot and the target pointattpAnd kattvThe relative position and the proportion of the relative speed in the gravitational total potential field are adjusted when k isattvWhen the value of zero is taken, the system is degraded into a traditional gravitational field function;
a relative speed term is introduced into the repulsive force potential field function, as shown in the formula (26), and a dynamic repulsive force potential field model is selected as shown in the formula (27) by integrating the repulsive force potential field function of the relative position;
3. The method for planning the path of the unmanned ship on water according to claim 1, wherein in the research on the global path planning optimal control of the unmanned ship on water, the hybrid particle swarm algorithm based on simulated annealing has the following characteristics:
1) increasing the "initial temperature" t0，t0The calculation formula is shown as (28):
t0＝Fit(pg)/ln 5 (28)
where fit (x) denotes the fitness function, pgIs a global extremum;
2) increasing the annealing coefficient lambda to control the annealing mode as shown in formula (29)
tk+1＝λtk (29)
3) The probability calculation formula that the particle is selected as the global optimum at the current temperature is increased, and the formula is as follows (30):
wherein t represents the current temperature, and N represents the number of particles in the particle swarm;
4) selecting a p from all individuals by simulating the jump characteristic of a simulated annealing algorithmiAs global optimum p'gTo replace the true global optimum pgI.e. selecting the poor solution as the global optimum value, p ', with a certain probability'gThe selection of (1) is by roulette, wherein the probability that each individual is selected as the global optimum is calculated by equation (30);
5) the velocity and position of each particle are updated according to equations (31) to (33):
νi,j(t+1)＝φ{νi,j(t)+c1r1[pi,j-xi,j(t)]+c2r2[pg,j-xi,j(t)]} (31)
xi,j(t+1)＝xi,j(t)+νi,j(t+1),j＝1,2,...,d (32)
where φ represents the compression factor and d represents the dimension of the solution corresponding to the particle.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111507580.XA CN114077256A (en) | 2021-12-10 | 2021-12-10 | Overwater unmanned ship path planning method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111507580.XA CN114077256A (en) | 2021-12-10 | 2021-12-10 | Overwater unmanned ship path planning method |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114077256A true CN114077256A (en) | 2022-02-22 |
Family
ID=80284494
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111507580.XA Pending CN114077256A (en) | 2021-12-10 | 2021-12-10 | Overwater unmanned ship path planning method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114077256A (en) |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114527768A (en) * | 2022-03-07 | 2022-05-24 | 上海海事大学 | Unmanned ship optimal obstacle avoidance path planning method based on HJB equation data-driven solution |
| CN114610027A (en) * | 2022-03-07 | 2022-06-10 | 吉林大学 | Ship navigation path planning method |
| CN115357019A (en) * | 2022-08-23 | 2022-11-18 | 福州大学 | Unmanned ship multi-parameter particle swarm identification system and method |
| CN115494848A (en) * | 2022-09-30 | 2022-12-20 | 大连海事大学 | Unmanned ship path tracking sight distance guidance method and system for avoiding dynamic and static obstacles |
| CN116520834A (en) * | 2023-04-24 | 2023-08-01 | 安徽建筑大学 | Low-energy-consumption unmanned ship cruising method |
-
2021
- 2021-12-10 CN CN202111507580.XA patent/CN114077256A/en active Pending
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114527768A (en) * | 2022-03-07 | 2022-05-24 | 上海海事大学 | Unmanned ship optimal obstacle avoidance path planning method based on HJB equation data-driven solution |
| CN114610027A (en) * | 2022-03-07 | 2022-06-10 | 吉林大学 | Ship navigation path planning method |
| CN115357019A (en) * | 2022-08-23 | 2022-11-18 | 福州大学 | Unmanned ship multi-parameter particle swarm identification system and method |
| CN115494848A (en) * | 2022-09-30 | 2022-12-20 | 大连海事大学 | Unmanned ship path tracking sight distance guidance method and system for avoiding dynamic and static obstacles |
| CN115494848B (en) * | 2022-09-30 | 2024-05-03 | 大连海事大学 | Unmanned ship path tracking and viewing distance guidance method and system for avoiding dynamic and static obstacles |
| CN116520834A (en) * | 2023-04-24 | 2023-08-01 | 安徽建筑大学 | Low-energy-consumption unmanned ship cruising method |
| CN116520834B (en) * | 2023-04-24 | 2024-05-28 | 安徽建筑大学 | Low-energy-consumption unmanned ship cruising method |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN114077256A (en) | Overwater unmanned ship path planning method | |
| CN107145161B (en) | Flight path planning method and device for unmanned aerial vehicle to access multiple target points | |
| WO2016045615A1 (en) | Robot static path planning method | |
| CN110750096A (en) | Mobile robot collision avoidance planning method based on deep reinforcement learning in static environment | |
| CN116804879B (en) | Robot path planning framework method for improving dung beetle algorithm and fusing DWA algorithm | |
| CN113534668A (en) | Maximum entropy based AUV (autonomous Underwater vehicle) motion planning method for actor-critic framework | |
| CN114489040A (en) | Hybrid path planning method based on improved A-star algorithm and artificial potential field algorithm | |
| CN107856035A (en) | A kind of robustness dynamic motion method based on intensified learning and whole body controller | |
| CN108445894A (en) | A kind of secondary paths planning method considering unmanned boat movenent performance | |
| Zhao et al. | Autonomous exploration method for fast unknown environment mapping by using UAV equipped with limited FOV sensor | |
| Short et al. | Abio-inspiredalgorithminimage-based pathplanning and localization using visual features and maps | |
| Wu et al. | Real-time three-dimensional smooth path planning for unmanned aerial vehicles in completely unknown cluttered environments | |
| CN111829528B (en) | Real-time path planning method and system for bionic gliding machine dolphin | |
| Shen et al. | Ship route planning based on particle swarm optimization | |
| Wang et al. | Efficient reinforcement learning for autonomous ship collision avoidance under learning experience reuse | |
| CN114609925B (en) | Training method of underwater exploration strategy model and underwater exploration method of bionic machine fish | |
| CN116954062A (en) | Space mining robot motion control method based on African bald-Convergence optimization algorithm | |
| Qin et al. | A path planning algorithm based on deep reinforcement learning for mobile robots in unknown environment | |
| CN116466701A (en) | Unmanned surface vessel energy consumption minimum track planning method | |
| Leng et al. | UAV path planning in 3D complex environments using genetic algorithms | |
| Yao | Path Planning Algorithm of Indoor Mobile Robot Based on ROS System | |
| Zhang et al. | A deep reinforcement learning method for mobile robot path planning in unknown environments | |
| Das et al. | Improved real time A*-fuzzy controller for improving multi-robot navigation and its performance analysis | |
| Novikov et al. | Navigation Method in Non-Stationary Environment for Embedded Control System of Mobile Robots | |
| Zhang et al. | Path planning of underwater vehicle based on improved particle swarm 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 |