CN117270522A - Artificial potential field-based obstacle avoidance method for double-boat cooperative system - Google Patents
Artificial potential field-based obstacle avoidance method for double-boat cooperative system Download PDFInfo
- Publication number
- CN117270522A CN117270522A CN202310977887.9A CN202310977887A CN117270522A CN 117270522 A CN117270522 A CN 117270522A CN 202310977887 A CN202310977887 A CN 202310977887A CN 117270522 A CN117270522 A CN 117270522A
- Authority
- CN
- China
- Prior art keywords
- virtual
- virtual pilot
- obstacle
- pilot
- defect
- 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 73
- 230000007547 defect Effects 0.000 claims abstract description 64
- 230000015572 biosynthetic process Effects 0.000 claims abstract description 7
- 238000005755 formation reaction Methods 0.000 claims abstract description 6
- 230000008569 process Effects 0.000 claims description 10
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 5
- 230000008859 change Effects 0.000 claims description 5
- 238000011084 recovery Methods 0.000 description 10
- 238000010586 diagram Methods 0.000 description 6
- 239000003305 oil spill Substances 0.000 description 3
- 238000002922 simulated annealing Methods 0.000 description 3
- 238000000137 annealing Methods 0.000 description 2
- 208000027418 Wounds and injury Diseases 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005574 cross-species transmission Effects 0.000 description 1
- 230000006378 damage Effects 0.000 description 1
- 230000002950 deficient Effects 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 230000002068 genetic effect Effects 0.000 description 1
- 230000036541 health Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 208000014674 injury Diseases 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000010355 oscillation Effects 0.000 description 1
- 230000002265 prevention Effects 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
An obstacle avoidance method of a double-boat cooperative system based on an artificial potential field relates to the technical field of unmanned boat obstacle avoidance. The invention aims to solve the problems that the unmanned ship can not simultaneously collide with an obstacle, and the target is unreachable and the local minimum value exists in the unmanned ship obstacle avoidance method. The invention comprises the following steps: setting the central position of the double unmanned boat formations as a virtual pilot, and acquiring virtual pilot information; acquiring resultant force F borne by a virtual pilot, and navigating the virtual pilot along the F direction; judging whether the virtual pilot falls into a defect at the current position, and if the virtual pilot falls into the defect, adjusting the course of the virtual pilot; if the virtual pilot does not have a defect at the current position, the virtual pilot sails along the F direction; and the virtual navigator advances to the next position, the current position of the virtual navigator is updated, whether the virtual navigator reaches a target point is judged according to the updated current position of the virtual navigator, and if the virtual navigator reaches the target point, the virtual navigator is ended. The invention is used for unmanned boats to avoid the obstacle.
Description
Technical Field
The invention relates to the technical field of unmanned boats obstacle avoidance, in particular to a double-boat cooperative system obstacle avoidance method based on an artificial potential field.
Background
The oil spill accident at sea has the characteristics of long hazard time, large influence degree and the like, and can cause serious hazard to marine environment and biological health. The oil spill is caught and recovered by the manned ship carrying the oil containment boom, so that a large amount of manpower and material resources are consumed, and serious injury is caused to the personnel on the ship. The unmanned ship is utilized to carry out the offshore spilled oil recovery task, and the unmanned ship has a plurality of advantages, including avoiding human participation, reducing recovery cost, improving task execution efficiency, avoiding further volatilization and expansion of spilled oil caused by overlong recovery time, and the like. The double-boat spilled oil recovery system consists of two unmanned boats towing an oil containment boom, and is often used for handling offshore spilled oil accidents. The path of the unmanned ship in the oil spill recovery process is reasonably planned, so that the unmanned ship can be ensured to safely and efficiently finish tasks, and the unmanned ship has practical application value.
The existing artificial potential field method is widely applied to path planning because of the advantages of simplicity in calculation and easiness in implementation. The traditional artificial potential field method is a virtual force method which is proposed by Khatib and simulates a natural field. The method treats an obstacle, a target point, a robot and the like as one point, and the robot moves in a virtual force field consisting of a gravitational potential field and a repulsive potential field. Wherein, there is the gravitation potential field that has gravitation to the robot around the target point, there is the repulsion potential field that has repulsive force to the robot around the obstacle. The method is simple in principle, convenient to calculate and easy to implement, and therefore is widely applied. But also have some drawbacks: 1. when the robot is closer to the obstacle and the target point is farther away, the attraction force borne by the robot is far greater than the repulsive force, and the robot is likely to collide with the obstacle; 2. when the target point is within the influence distance of the obstacle and is close to the obstacle, the repulsive force of the obstacle to the robot is larger than the gravitation born by the robot, and the robot cannot reach the target point at the moment; 3. when the resultant force of repulsive force and attractive force applied to the robot is zero at a certain position, the robot falls into a local minimum point to stop the movement.
The path planning based on the improved artificial potential field method is mainly realized by adopting a method with the patent number of CN 111176272B and the name of 'an artificial potential field track planning method and system based on motion constraint', and a vehicle track planning method based on an artificial potential field is provided. According to the method, the problem that the track of the vehicle is not smooth is solved by adding motion constraint; and setting the minimum gravitation, and when the calculated gravitation is smaller than the minimum gravitation, using the set minimum gravitation to solve the problem that the target is not reachable. However, the method does not solve the problems that the robot stops moving and possibly collides with an obstacle due to the local minimum value existing in the traditional artificial potential field method, and the planned track cannot completely meet the actual requirements. Patent number is CN112099501B, the name is "unmanned ship path planning method based on potential field parameter optimization", and an unmanned ship path planning method based on an artificial potential field is provided. According to the method, path planning is carried out through a plurality of attractive force gain coefficients and repulsive force gain coefficients, and three evaluation coefficients including a collision prevention coefficient, a length coefficient and a corner coefficient are considered, so that a classical genetic algorithm is used for reference, the planned path is free of local minimum points, and the motion constraint of the unmanned ship can be met. However, the method does not solve the problems of unreachable targets and possibility of collision with obstacles in the conventional artificial potential field method, and still has room for improvement. The patent number is CN113110604B, the name is 'a path dynamic planning method based on an artificial potential field', and an improved artificial potential field method combined with an analog annealing method is provided. According to the method, the neighborhood function of the simulated annealing method is optimized based on the target position, the problem that the path planning task fails after the unmanned aerial vehicle falls into a local optimal state is solved, and the overall operation efficiency and the success rate of path planning are improved. However, the method is only improved aiming at the defect of local minimum value existing in the traditional artificial potential field method, and the problem that an obstacle is easy to collide with an unmanned ship and the unmanned ship cannot reach a target point is not solved. Therefore, the unmanned ship obstacle avoidance method based on the improved artificial potential field method in the prior art also cannot solve the problems that the robot is easy to collide with an obstacle when the attraction force of the robot is far greater than the repulsive force, the robot cannot reach a target point when the repulsive force of the robot is far greater than the attraction force, and the robot is easy to sink into a local minimum value and stop when the resultant force of the attraction force and the repulsive force of the robot is zero in the traditional artificial potential field method.
Disclosure of Invention
The invention aims to solve the problems that the existing unmanned ship obstacle avoidance method cannot solve the problems that the unmanned ship is easy to collide with an obstacle, a target cannot reach a local minimum value, and provides a double-ship cooperative system obstacle avoidance method based on an artificial potential field.
The obstacle avoidance method of the double-boat cooperative system based on the artificial potential field comprises the following specific processes:
setting the central position of the double unmanned boat formations as a virtual pilot, and acquiring information of the virtual pilot;
the virtual pilot information includes: coordinates V (x) of virtual pilot v ,y v ) Heading psi v And velocity v v ；
Step two, acquiring resultant force F borne by the virtual pilot at the current position, so that the virtual pilot sails along the direction of the resultant force F;
judging whether the virtual pilot falls into a defect at the current position, if so, adjusting the course of the virtual pilot by utilizing the information of the virtual pilot and the resultant force of the virtual pilot obtained in the step two, and then executing the step four; if the virtual pilot does not have a defect at the current position, the virtual pilot navigates along the direction of resultant force F, and then step four is executed;
and step four, the virtual navigator proceeds to the next position, the current position of the virtual navigator is updated, whether the virtual navigator reaches a target point is judged according to the updated current position of the virtual navigator, if the virtual navigator reaches the target point, the process is ended, and if the virtual navigator does not reach the target point, the process returns to the step two.
Further, the step one of obtaining virtual navigator information includes the following formula:
wherein the method comprises the steps of，(x 1 ,y 1 )、(x 2 ,y 2 ) The coordinates of two unmanned boats on the water surface are respectively, and psi 1 、ψ 2 The heading, v, of two unmanned boats on the water surface 1 、v 2 The speeds of two unmanned boats on the water surface are respectively (x) v ,y v ) Is the coordinates of the virtual pilot V.
Further, the resultant force F suffered by the virtual pilot in the second step at the current position is as follows:
F＝F att +F rep
F att ＝-k att (V-T)
wherein F is att Is the attraction of the target point to the virtual navigator, F rep Is the repulsive force of the obstacle to the virtual pilot, k att Is the gravitational gain constant, T is the position of the target point, k rep Is the repulsive force gain constant ρ obs Is the distance ρ between the virtual pilot and the obstacle 0 Is the radius of influence of the obstacle.
Further, in the third step, whether the virtual navigator falls into a defect at the current position is determined, specifically:
when ρ is ot ＞ρ 0 And is also provided withAnd l < r obs +r s +Γ, then represents that the virtual pilot falls into defect 1 at the current location;
wherein μ is the angle between VR and F, VR is the connection between the virtual pilot and the obstacle, l is the vertical distance from the obstacle to the line where F is located, r obs Is the actual radius of the obstacle, Γ is the safety compensation distance for defect 1, r s Is the safety compensation radius ρ aiming at the formation of double unmanned boats ot Is the distance between the target point and the obstacle;
when ρ is ot ＜ρ 0 Then represent the virtual collarThe navigator falls into defect 2 at the current position;
if f=0, it indicates that the virtual pilot is trapped in defect 3 at the current location.
Further, if the virtual pilot falls into a defect in the third step, the course of the virtual pilot is adjusted by using the information of the virtual pilot and the resultant force of the virtual pilot obtained in the second step, specifically:
if the virtual pilot falls into defect 1, makeSailing the virtual pilot in a direction away from the obstacle;
wherein,is the included angle between the navigation direction of the virtual navigator and the x direction of the geodetic coordinate system, beta is the included angle between VR and the x direction of the geodetic coordinate system, gamma is the included angle formed by VR and VS, VS is the connecting line of the virtual navigator and the point S, S is the equivalent radius r of the obstacle along the vertical direction of the straight line from the obstacle to the F o Extending the point at which Γ is obtained;
the equivalent radius r of the obstacle o The following formula:
r o ＝r obs +r s
if the virtual pilot falls into defect 2, when d is greater than or equal to r obs +r s +Ω orWhen in use, let->When d < r obs +r s +Ω and->When in use, let->
Wherein η is the angle between VT and the direction x of the geodetic coordinate system, (x) t ,y t ) Is the position coordinate of the target point T, d is the vertical distance from the obstacle to VT, VT is the connection line of the virtual pilot and the target point, Ω is the safety compensation distance for the defect 2, λ is the angle formed by VR and VT, β is the angle formed by VR and the direction x of the geodetic coordinate system, γ1 is the angle formed by VR and VQ, VQ is the connection line of the virtual pilot and Q, Q is the equivalent radius r of the obstacle along the direction from the obstacle to VT o Extending the point of omega acquisition;
if the virtual pilot falls into the defect 3, the next position of the virtual pilot is acquired, so that the resultant force F born by the virtual pilot at the next position is not 0.
Further, the vertical distance of the obstacle to the straight line where F is located is obtained by the following formula:
l＝ρ obs cosε
wherein epsilon is an included angle formed by VR and RS, RS is a connecting line of an obstacle and an S point, theta is an included angle between the F direction and the x direction of a geodetic coordinate system, (x) obs ,y obs ) Is the position coordinates of the obstacle R, (x) o ,y o ) Is the origin of the geodetic coordinate system.
Further, the angle formed by VR and VT is obtained by the following formula:
wherein ρ is vt Is the distance of the virtual pilot to the target point.
Further, the included angle between VR and VS is obtained by the following formula:
wherein, gamma is the included angle formed by VR and VS.
Further, the angle formed by VR and VQ is as follows:
wherein σ is the included angle formed by VR and RQ, RQ is the connecting line between the obstacle and Q.
Further, if the virtual pilot falls into defect 3, the next position of the virtual pilot is obtained, so that the resultant force F suffered by the virtual pilot at the next position is not 0, specifically:
s1, for initial temperature T 0 Termination temperature T f Maximum course change angle of random probability threshold value xi and double-boat cooperative systemInitializing;
wherein Δt is the time step, R s The turning radius of the unmanned boat positioned on the outer side in the double-boat cooperative system at the current moment;
s2, selecting a random point X 1 ；
S3, acquiring a potential field U (V) of a position V and a position X 1 Potential field U (X) 1 ) If U (X) 1 ) -U (V) < 0, then execute S4If U (X) 1 ) U (V) is ≡0, then T is used according to Metropolis criterion 0 And T f Calculation of acceptance X 1 The probability P of the next position of the virtual pilot is obtained, and if P is more than xi, S4 is executed; if P is less than or equal to xi, returning to S2 to reselect the random point;
potential field U (V) at position V and position X 1 Potential field U (X) 1 ) Obtained by the following formula:
U＝U att +U rep
wherein U is the combined potential field, U att Is the gravitational potential field, U rep Is a repulsive potential field;
s4, acquiring the difference between the current position course and the next position course of the virtual pilotWill->And->Comparing ifThen accept X 1 As the virtual pilot next location; if->Returning to S2, and reselecting the random point;
s5, virtual pilot arrives at position X 1 Then, judging whether the resultant force of the attraction force and the repulsion force applied to the current position of the virtual pilot is 0, if the resultant force is not 0, the local pole is removedA small value point, then tracking the target point continuously; and if the resultant force of the attraction force and the repulsion force of the current virtual pilot is still 0, returning to S2 to reselect the random point.
The beneficial effects of the invention are as follows:
aiming at the problems of collision with obstacles, unreachable targets and local minimum value existing in the conventional artificial potential field method based unmanned ship path planning, the invention introduces a safety compensation distance, improves an analog annealing method, combines the maximum course change angle of a double-ship spillover oil recovery system, screens the next position of a virtual pilot selected by an improved simulated annealing algorithm, ensures that the unmanned ship path does not generate larger oscillation, firstly judges whether the unmanned ship path is at risk of trapping three defects, and then provides a solution for the three defects, and the invention provides APF-1, APF-2 and APF-3 which can avoid the problems that the unmanned ship is easy to collide with the obstacles due to the fact that the attractive force of the unmanned ship is far greater than the repulsive force, the unmanned ship cannot reach the target point due to the attractive force, and the robot is easy to stop due to the fact that the resultant force of the repulsive force of the unmanned ship is zero.
Drawings
FIG. 1 is a flow chart of the present invention;
FIG. 2 is a schematic diagram of defect 1;
FIG. 3 is a schematic diagram of defect 2;
FIG. 4 is a schematic diagram of defect 3;
FIG. 5 is a schematic view of a virtual pilot according to the present invention;
FIG. 6 is a schematic view of an obstacle according to the present invention;
FIG. 7 is a schematic diagram of an improved artificial potential field method for defect 1 in the present invention;
FIG. 8 is a schematic diagram of an improved artificial potential field method for defect 2 in the present invention;
figure 9 is a flow chart of the APF-3 algorithm off local minima points.
Detailed Description
The first embodiment is as follows: as shown in fig. 1, the specific process of the obstacle avoidance method of the double-boat cooperative system based on the artificial potential field in this embodiment is as follows:
setting the formation center of a double unmanned ship in the spilled oil recovery system as a virtual pilot, and acquiring information of the virtual pilot;
the information of the virtual navigator comprises: coordinates V (x) of virtual pilot v ,y v ) Heading psi v And velocity v v ：
Wherein, (x) 1 ,y 1 ) Is the coordinates of USV1 (unmanned surface vehicle 1), (x) 2 ,y 2 ) Is USV1 (unmanned surface vehicle 2), ψ 1 Is the heading of USV1, psi 2 Is the heading of USV2, v 1 Is the speed of USV1, v 2 Is the speed of USV2, as shown in fig. 5;
step two, as shown in fig. 6, setting a safety compensation radius r for the double unmanned ship formation s And obtains the virtual pilot at the current position V (x v ,y v ) The resultant force f=f att +F rep Enabling the virtual pilot to navigate along the direction of the resultant force F;
F att ＝-k att (V-T)
wherein F is att Is the attraction of the target point to the virtual navigator, the direction is the target point pointed by the navigator, F rep Is the repulsive force of the obstacle to the virtual navigator, the direction is pointed by the obstacle to the navigator, k att Is the gravitational gain constant, T is the position of the target point, k rep To repulsive force gain constant ρ obs ρ is the distance between the virtual pilot and the obstacle 0 Is the radius of influence of the obstacle.
Judging whether the virtual pilot falls into a defect at the current position, if so, adopting an improved artificial potential field method to adjust the course of the virtual pilot by utilizing the information of the virtual pilot and the resultant force of the virtual pilot obtained in the step two, and executing the step four after adjusting the course of the virtual pilot; if the virtual navigator does not have a defect at the current position, executing the fourth step;
step three, judging whether the virtual navigator is defective at the current position or not:
firstly, judging whether a virtual pilot falls into any one of defect 1 (the virtual pilot is closer to an obstacle and is farther from a target point, the attraction force born by the virtual pilot is far greater than repulsive force, the virtual pilot collides with the obstacle), defect 2 (when the target point is within the influence distance of the obstacle and is closer to the obstacle, repulsive force of the obstacle to the virtual pilot is greater than the attraction force born by the virtual pilot, at the moment, the virtual pilot cannot reach the target point), defect 3 (the resultant force of repulsive force and attractive force born by a robot at a certain position is zero, the virtual pilot falls into a local minimum point and stops moving), if no defect is sunk into, executing step four, if no defect 1 falls into any defect, executing step three, if a defect 2 falls into a defect, and executing step three, if a defect 3 falls into a defect; three defect schematic diagrams are shown in fig. 2-4;
when the target point is out of the range of influence of the obstacle (ρ ot ＞ρ 0 ) At the same timeAnd l < r obs +r s +Γ, then represents trapping defect 1; if any condition is not satisfied, it indicates that defect 1 is not trapped, let +.>The virtual pilot sails along the resultant force direction;
l＝ρ obs cosε
wherein ρ is ot For the distance between the target point and the obstacle, μ is the angle between VR and F, and l is the straight line from the obstacle to FVertical distance of line, r obs Is the actual radius of the obstacle, Γ is the safety compensation distance introduced for the defect 1, ε is the included angle formed by VR and RS, S is the equivalent radius r of the obstacle along the l direction o Extending the point obtained by Γ, r o ＝r obs +r s RS is the connection between the obstacle and the S point, VR is the connection between the virtual pilot and the obstacle,is the included angle between the navigation direction of the virtual pilot and the x axis, and θ is the included angle between the F direction and the x axis;
when ρ is ot ＜ρ 0 The virtual pilot is trapped in the defect 2, or else the virtual pilot is not trapped in the defect 2, and the virtual pilot navigates along the F direction;
if f=0, it indicates that defect 3 is trapped, and if the virtual pilot does not trap defect 3, the virtual pilot navigates along direction F;
step three, adopting an improved artificial potential field method (APF-1) to adjust the course of the virtual pilot by utilizing the information of the virtual pilot and the resultant force born by the virtual pilot obtained in the step two, and then executing the step four, wherein the method specifically comprises the following steps:
when defect 1 is trapped, letThe virtual pilot sails away from the obstacle, so that the defect 1 can be effectively avoided;
as shown in fig. 7, R (x obs ,y obs ) Is the position coordinate of the obstacle, r obs Is the actual radius of the obstacle, r o ＝r obs +r s Is the equivalent radius of the obstacle, l is the vertical distance from the obstacle to the straight line where F is located, Γ is the safety compensation distance introduced for defect 1, β is the angle between VR and the x-axis, μ is the angle between VR and F, θ is the angle between F and the x-axis, γ is the clip corresponding to edge RS in ΔVRSAngle (angle RVS formed by VR and VS, VR is the connection between the virtual pilot and the obstacle, VS is the connection between the virtual pilot and the S point), epsilon is the angle corresponding to the edge VS in DeltaVRS (angle VRS formed by RS and RV, RS is the connection between the obstacle and the S point),is the angle between the navigation direction of the virtual pilot and the x axis, (x) o ,y o ) Is the origin of the geodetic coordinate system;
the coordinate system in the invention is a geodetic coordinate system, and the situation that the center point of the obstacle is positioned below the connecting line of the virtual navigator and the target point is similar to the situation, so that the description is omitted.
Step three, the heading of the virtual pilot is adjusted by adopting an improved artificial potential field method (APF-2) aiming at the defect 2 by utilizing the information of the virtual pilot and the resultant force born by the virtual pilot obtained in the step two, and then the step four is executed, as shown in fig. 8:
when d is greater than or equal to r obs +r s +Ω orWhen in use, let->The virtual navigator directly trends to the target point along the connecting line direction with the target point; otherwise (d < r) obs +r s +Ω and->) Let->The virtual pilot approaches the target point while avoiding the obstacle, so that the occurrence of the defect 2 can be effectively avoided.
Wherein d is the vertical distance from the obstacle to VT, which is the virtual pilot and destinationPunctuation line, Ω is the newly introduced safety compensation distance for defect 2, λ is the included angle formed by RV and VT, RV is the line of obstacle and virtual pilot, ρ vt Is the distance of the virtual pilot to the target point ρ ot Is the distance between the target point and the obstacle; sigma is the included angle corresponding to the edge VQ in DeltaVRQ (the included angle formed by RV and RQ is VRQ, RV is the connecting line of the obstacle and the virtual pilot, RQ is the connecting line between the obstacle and Q), Q is the equivalent radius r of the obstacle along the d direction o Extending the point obtained by omega, wherein eta is the included angle between VT and x-axis, gamma 1 is the included angle corresponding to the edge RQ in DeltaVRQ (the included angle formed by RV and VQ is RVQ, VQ is the connecting line of the virtual pilot and Q), T (x t ,y t ) Is the location of the target point as shown in fig. 6.
The central point of the obstacle is located below the connection line between the virtual pilot and the target point, so that the description is omitted.
Step three, adopting an improved artificial potential field method (APF-3) aiming at the defect 3 to adjust the course of the virtual pilot by utilizing the information of the virtual pilot and the resultant force born by the virtual pilot obtained in the step two, and then executing the step four, as shown in fig. 9, specifically comprising the following steps:
step three, four and one, initializing parameters: determining an initial temperature T 0 Termination temperature T f Maximum course change angle of random probability threshold value xi and double-boat spilled oil recovery system
Wherein,is the maximum course change angle of the double-boat spilled oil recovery system, delta t is the time step, v v For virtual pilot speed, R s The radius of gyration of the unmanned ship positioned on the outer side in the double-ship spilled oil recovery system at the current moment;
step three, four and two, selecting a random point X near the virtual pilot position V 1 ；
Step three, four and three, calculating position V and position X 1 Potential fields U (V), U (X) 1 ) If U (X) 1 ) -U (V) < 0, then the virtual pilot's next position is X 1 Then executing the third step, the fourth step and the fourth step; if U (X) 1 ) -U (V) is ≡0, then the acceptance X is calculated according to Metropolis criterion 1 If P > ζ, then the virtual pilot's next position is X 1 Then executing the third step, the fourth step and the fourth step; if P is less than or equal to xi, returning to the third and fourth steps to reselect the random points;
the potential field calculation formula is as follows:
U＝U att +U rep
wherein U is the combined potential field, U att Is the gravitational potential field, U rep Is the repulsive potential field.
Calculation of acceptance X according to Metropolis criterion 1 Is represented by the following formula:
wherein, T1 is the temperature in the simulated annealing algorithm, and is reduced in a certain way, as shown in the following formula:
T1(t)＝αT1(t-1)
wherein alpha is any positive constant of the interval (0.85,1), selected according to actual conditions, and t is the iteration number;
t1 (T) from T 0 Start to stop temperature T f And if P > xi is obtained in the iteration process, executing the third step, the fourth step and the fourth step; if T is reached f If P > ζ is not found, returning to the stepStep III, step IV;
step three, four and four, receiving X 1 Before the virtual pilot is at the next position, calculating the difference between the current position course of the virtual pilot and the course of the next positionWill->And->A comparison is made. If->Then accept the virtual navigator next location as X 1 The method comprises the steps of carrying out a first treatment on the surface of the If->Returning to the third and fourth steps to reselect the random point.
Step three, four and five, virtual pilot arrives at position X 1 Then judging whether the resultant force of the gravitation and the repulsive force applied to the current position of the virtual pilot is 0, if the resultant force is not 0, getting rid of a local minimum point, and continuing to track the target point; and if the resultant force of the attraction force and the repulsion force of the current virtual pilot is still 0, returning to the step three and four to reselect the random point.
And step four, the virtual navigator proceeds to the next position, the current position of the virtual navigator is updated, whether the virtual navigator reaches a target point is judged according to the updated current position of the virtual navigator, if the virtual navigator reaches the target point, the process is ended, and if the virtual navigator does not reach the target point, the process returns to the step two.
Claims (10)
1. The obstacle avoidance method of the double-boat cooperative system based on the artificial potential field is characterized by comprising the following specific processes of:
setting the central position of the double unmanned boat formations as a virtual pilot, and acquiring information of the virtual pilot;
the virtual pilot information includes: coordinates V (x) of virtual pilot v ,y v ) Heading psi v And velocity v v ；
Step two, acquiring resultant force F borne by the virtual pilot at the current position, so that the virtual pilot sails along the direction of the resultant force F;
judging whether the virtual pilot falls into a defect at the current position, if so, adjusting the course of the virtual pilot by utilizing the information of the virtual pilot and the resultant force of the virtual pilot obtained in the step two, and then executing the step four; if the virtual pilot does not have a defect at the current position, the virtual pilot navigates along the direction of resultant force F, and then step four is executed;
and step four, the virtual navigator proceeds to the next position, the current position of the virtual navigator is updated, whether the virtual navigator reaches a target point is judged according to the updated current position of the virtual navigator, if the virtual navigator reaches the target point, the process is ended, and if the virtual navigator does not reach the target point, the process returns to the step two.
2. The method for avoiding the obstacle of the double-boat collaboration system based on the artificial potential field according to claim 1, wherein the method comprises the following steps of: the virtual navigator information is acquired in the first step, and the following formula is as follows:
wherein, (x) 1 ,y 1 )、(x 2 ,y 2 ) The coordinates of two unmanned boats on the water surface are respectively, and psi 1 、ψ 2 The heading, v, of two unmanned boats on the water surface 1 、v 2 The speeds of two unmanned boats on the water surface are respectively (x) v ,y v ) Is the coordinates of the virtual pilot V.
3. The method for avoiding the obstacle of the double-boat collaboration system based on the artificial potential field as claimed in claim 2, wherein the method comprises the following steps of: and F, the virtual navigator receives a resultant force F at the current position in the second step, wherein the resultant force F is represented by the following formula:
F＝F att +F rep
F att ＝-k att (V-T)
wherein F is att Is the attraction of the target point to the virtual navigator, F rep Is the repulsive force of the obstacle to the virtual pilot, k att Is the gravitational gain constant, T is the position of the target point, k rep Is the repulsive force gain constant ρ obs Is the distance ρ between the virtual pilot and the obstacle 0 Is the radius of influence of the obstacle and V is the position of the virtual pilot.
4. A method for obstacle avoidance of a dual-boat collaboration system based on an artificial potential field as claimed in claim 3, wherein: in the third step, whether the virtual navigator falls into a defect at the current position is judged, specifically:
when ρ is ot ＞ρ 0 And is also provided withAnd l < r obs +r s +Γ, then represents that the virtual pilot falls into defect 1 at the current location;
wherein μ is the angle between VR and F, VR is the connection between the virtual pilot and the obstacle, l is the vertical distance from the obstacle to the line where F is located, r obs Is the actual radius of the obstacle, Γ is the safety compensation distance for defect 1, r s Is the safety compensation radius ρ aiming at the formation of double unmanned boats ot Is the distance between the target point and the obstacle;
when ρ is ot ＜ρ 0 Then the virtual pilot falls into defect 2 at the current position;
if f=0, it indicates that the virtual pilot is trapped in defect 3 at the current location.
5. The method for avoiding the obstacle of the double-boat collaboration system based on the artificial potential field as claimed in claim 4, wherein the method comprises the following steps of: in the third step, if the virtual pilot falls into a defect, the course of the virtual pilot is adjusted by using the information of the virtual pilot and the resultant force of the virtual pilot obtained in the second step, specifically:
if the virtual pilot falls into defect 1, makeSailing the virtual pilot in a direction away from the obstacle;
wherein,is the included angle between the navigation direction of the virtual navigator and the x direction of the geodetic coordinate system, beta is the included angle between VR and the x direction of the geodetic coordinate system, gamma is the included angle formed by VR and VS, VS is the connecting line of the virtual navigator and the point S, S is the equivalent radius r of the obstacle along the vertical direction of the straight line from the obstacle to the F o Extending the point at which Γ is obtained;
the equivalent radius r of the obstacle o The following formula:
r o ＝r obs +r s
if the virtual pilot falls into defect 2, when d is greater than or equal to r obs +r s +Ω orWhen in use, let->When d < r obs +r s +Ω andwhen in use, let->
Wherein η is the angle between VT and the direction x of the geodetic coordinate system, (x) t ,y t ) Is the position coordinate of the target point T, d is the vertical distance from the obstacle to VT, VT is the connection line of the virtual pilot and the target point, Ω is the safety compensation distance for the defect 2, λ is the angle formed by VR and VT, β is the angle formed by VR and the direction x of the geodetic coordinate system, γ1 is the angle formed by VR and VQ, VQ is the connection line of the virtual pilot and Q, Q is the equivalent radius r of the obstacle along the direction from the obstacle to VT o Extending the point of omega acquisition;
if the virtual pilot falls into the defect 3, the next position of the virtual pilot is acquired, so that the resultant force F born by the virtual pilot at the next position is not 0.
6. The artificial potential field-based obstacle avoidance method of the double-boat collaboration system, as claimed in claim 5, is characterized in that: the vertical distance of the obstacle to the line in which F is located is obtained by the following formula:
l＝ρ obs cosε
wherein epsilon is an included angle formed by VR and RS, RS is a connecting line of an obstacle and an S point, theta is an included angle between the F direction and the x direction of a geodetic coordinate system, (x) obs ,y obs ) Is the position coordinates of the obstacle R, (x) o ,y o ) Is the origin of the geodetic coordinate system.
7. The artificial potential field-based obstacle avoidance method of the double-boat collaboration system, as claimed in claim 6, wherein: the angle formed by VR and VT is obtained by the following formula:
wherein ρ is vt Is the distance of the virtual pilot to the target point.
8. The method for avoiding the obstacle of the double-boat collaboration system based on the artificial potential field as claimed in claim 7, wherein the method comprises the following steps of: the angle between VR and VS is obtained by the following formula:
wherein, gamma is the included angle formed by VR and VS.
9. The method for avoiding the obstacle of the double-boat collaboration system based on the artificial potential field as claimed in claim 8, wherein the method comprises the following steps of: the angle formed by VR and VQ is as follows:
wherein σ is the included angle formed by VR and RQ, RQ is the connecting line between the obstacle and Q.
10. The method for avoiding the obstacle of the double-boat collaboration system based on the artificial potential field as claimed in claim 9, wherein the method comprises the following steps of: if the virtual pilot falls into the defect 3, the next position of the virtual pilot is obtained, so that the resultant force F suffered by the virtual pilot at the next position is not 0, specifically:
s1, for initial temperature T 0 Termination temperature T f Probability of randomnessThreshold value xi, maximum course change angle of double-boat cooperative systemInitializing;
wherein Δt is the time step, R s The turning radius of the unmanned boat positioned on the outer side in the double-boat cooperative system at the current moment;
s2, selecting a random point X 1 ；
S3, acquiring a potential field U (V) of a position V and a position X 1 Potential field U (X) 1 ) If U (X) 1 ) -U (V) < 0, then S4 is performed, if U (X) 1 ) U (V) is ≡0, then T is used according to Metropolis criterion 0 And T f Calculation of acceptance X 1 The probability P of the next position of the virtual pilot is obtained, and if P is more than xi, S4 is executed; if P is less than or equal to xi, returning to S2 to reselect the random point;
potential field U (V) at position V and position X 1 Potential field U (X) 1 ) Obtained by the following formula:
U＝U att +U rep
wherein U is the combined potential field, U att Is the gravitational potential field, U rep Is a repulsive potential field;
s4, acquiring the difference between the current position course and the next position course of the virtual pilotWill->And->Comparing ifThen accept X 1 As the virtual pilot next location; if->Returning to S2, and reselecting the random point;
s5, virtual pilot arrives at position X 1 Then judging whether the resultant force of the gravitation and the repulsive force applied to the current position of the virtual pilot is 0, if the resultant force is not 0, indicating that the local minimum point is eliminated, continuing tracking the target point; and if the resultant force of the attraction force and the repulsion force of the current virtual pilot is still 0, returning to S2 to reselect the random point.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310977887.9A CN117270522A (en) | 2023-08-04 | 2023-08-04 | Artificial potential field-based obstacle avoidance method for double-boat cooperative system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310977887.9A CN117270522A (en) | 2023-08-04 | 2023-08-04 | Artificial potential field-based obstacle avoidance method for double-boat cooperative system |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117270522A true CN117270522A (en) | 2023-12-22 |
Family
ID=89207069
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310977887.9A Pending CN117270522A (en) | 2023-08-04 | 2023-08-04 | Artificial potential field-based obstacle avoidance method for double-boat cooperative system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117270522A (en) |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20160209849A1 (en) * | 2015-01-15 | 2016-07-21 | William Dale Arbogast | System and method for decentralized, multi-agent unmanned vehicle navigation and formation control |
| CN108469828A (en) * | 2018-03-23 | 2018-08-31 | 哈尔滨工程大学 | A kind of AUV Route planners improving artificial potential field optimization algorithm |
| CN111176272A (en) * | 2019-11-28 | 2020-05-19 | 的卢技术有限公司 | Artificial potential field trajectory planning method and system based on motion constraint |
| CN112099501A (en) * | 2020-09-15 | 2020-12-18 | 哈尔滨工程大学 | Unmanned ship path planning method based on potential field parameter optimization |
| CN113110604A (en) * | 2021-04-28 | 2021-07-13 | 江苏方天电力技术有限公司 | Path dynamic planning method based on artificial potential field |
| CN114019984A (en) * | 2021-12-13 | 2022-02-08 | 大连民族大学 | Unmanned ship and unmanned ship formation online track planning method and system |
| CN114859945A (en) * | 2022-05-26 | 2022-08-05 | 厦门大学 | Underwater formation control method, system and medium based on artificial potential field method |
| CN115268497A (en) * | 2022-08-05 | 2022-11-01 | 南京揽星邮通科技有限公司 | Unmanned aerial vehicle cluster formation obstacle avoidance method based on virtual sub-target combined boundary force |
-
2023
- 2023-08-04 CN CN202310977887.9A patent/CN117270522A/en active Pending
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20160209849A1 (en) * | 2015-01-15 | 2016-07-21 | William Dale Arbogast | System and method for decentralized, multi-agent unmanned vehicle navigation and formation control |
| CN108469828A (en) * | 2018-03-23 | 2018-08-31 | 哈尔滨工程大学 | A kind of AUV Route planners improving artificial potential field optimization algorithm |
| CN111176272A (en) * | 2019-11-28 | 2020-05-19 | 的卢技术有限公司 | Artificial potential field trajectory planning method and system based on motion constraint |
| CN112099501A (en) * | 2020-09-15 | 2020-12-18 | 哈尔滨工程大学 | Unmanned ship path planning method based on potential field parameter optimization |
| CN113110604A (en) * | 2021-04-28 | 2021-07-13 | 江苏方天电力技术有限公司 | Path dynamic planning method based on artificial potential field |
| CN114019984A (en) * | 2021-12-13 | 2022-02-08 | 大连民族大学 | Unmanned ship and unmanned ship formation online track planning method and system |
| CN114859945A (en) * | 2022-05-26 | 2022-08-05 | 厦门大学 | Underwater formation control method, system and medium based on artificial potential field method |
| CN115268497A (en) * | 2022-08-05 | 2022-11-01 | 南京揽星邮通科技有限公司 | Unmanned aerial vehicle cluster formation obstacle avoidance method based on virtual sub-target combined boundary force |
Non-Patent Citations (5)
| Title |
|---|
| KE CHEN等: "Improved Multi-UUV Formation Control for Artificial Potential Fields and Virtual Navigators", 《2021 IEEE 7TH INTERNATIONAL CONFERENCE ON CONTROL SCIENCE AND SYSTEMS ENGINEERING (ICCSSE)》, 24 September 2021 (2021-09-24) * |
| 周伟江: "基于虚拟领航者的UUV空间编队方法", 应用科技, no. 11, 15 November 2011 (2011-11-15) * |
| 潘无为: "人工势场和虚拟结构相结合的多水下机器人编队控制", 兵工学报, no. 02, 15 February 2017 (2017-02-15) * |
| 王子静: "水面无人艇编队路径规划与避障技术研究", 《中国优秀硕士学位论文电子全文库 工程科技II辑》, 15 June 2022 (2022-06-15) * |
| 王钦钊: "基于多虚拟领航者的多机器人编队控制方法", 装甲兵工程学院学报, no. 05, 22 March 2018 (2018-03-22) * |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110262492B (en) | Real-time collision avoidance and target tracking method for unmanned ship | |
| CN112327885B (en) | Unmanned ship self-adaptive global-local mixed path planning method | |
| CN112379672B (en) | Intelligent unmanned ship path planning method based on improved artificial potential field | |
| CN111399506B (en) | Global-local hybrid unmanned ship path planning method based on dynamic constraint | |
| CN108762264A (en) | The dynamic obstacle avoidance method of robot based on Artificial Potential Field and rolling window | |
| CN107544500B (en) | Unmanned ship berthing behavior trajectory planning method considering constraint | |
| CN110906934B (en) | Unmanned ship obstacle avoidance method and system based on collision risk coefficient | |
| KR101831264B1 (en) | Autonomous navigation system and method for a maneuverable platform | |
| CN112212872B (en) | End-to-end automatic driving method and system based on laser radar and navigation map | |
| CN108981716B (en) | Path planning method suitable for inland and offshore unmanned ship | |
| CN109318890A (en) | A kind of unmanned vehicle dynamic obstacle avoidance method based on dynamic window and barrier potential energy field | |
| CN109871031B (en) | Trajectory planning method for fixed-wing unmanned aerial vehicle | |
| CN109784201B (en) | AUV dynamic obstacle avoidance method based on four-dimensional risk assessment | |
| CN105589464A (en) | UUV dynamic obstacle avoidance method based on speed obstruction method | |
| CN111123923B (en) | Unmanned ship local path dynamic optimization method | |
| CN113419428B (en) | Machine/ship cooperative path tracking controller design method based on 3D mapping guidance | |
| CN110908386A (en) | Layered path planning method for unmanned vehicle | |
| CN108829134A (en) | A kind of real-time automatic obstacle avoiding method of deepwater robot | |
| CN111522351A (en) | Three-dimensional formation and obstacle avoidance method for underwater robot | |
| CN113916234A (en) | Automatic planning method for ship collision avoidance route under complex dynamic condition | |
| CN110262473B (en) | Unmanned ship automatic collision avoidance method based on improved Bi-RRT algorithm | |
| CN114527744A (en) | Unmanned sailing ship path tracking guidance method based on longicorn whisker search optimization | |
| CN114564008A (en) | Mobile robot path planning method based on improved A-Star algorithm | |
| CN117369441A (en) | Self-adaptive intelligent ship path planning method considering ship kinematics and COLLEGs | |
| CN117270522A (en) | Artificial potential field-based obstacle avoidance method for double-boat cooperative system |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |