CN109933067A - A Collision Avoidance Method for Unmanned Vehicles Based on Genetic Algorithm and Particle Swarm Optimization - Google Patents

Abstract

The invention relates to the field of collision avoidance of unmanned boats of maritime intelligent transportation technology, in particular to a collision avoidance method of unmanned boats based on genetic algorithm and particle swarm algorithm. Firstly, the relevant parameters and collision avoidance constraint rules in the collision avoidance path planning of the unmanned vehicle are studied, then the path planning of the surface unmanned vehicle to avoid static obstacles based on the genetic algorithm is carried out, and finally the combination of the genetic algorithm and the particle swarm algorithm is carried out. Dynamic collision avoidance path planning, completes path optimization, outputs a feasible collision avoidance path and resumes sailing; compared with the traditional collision avoidance technology for unmanned boats, the present invention can obtain optimal path planning, accurately avoid collisions, and ensure unmanned boats Reach the target point safely.

Description

A Collision Avoidance Method for Unmanned Vehicles Based on Genetic Algorithm and Particle Swarm Optimization

technical field

The invention relates to the field of collision avoidance of unmanned boats of maritime intelligent transportation technology, in particular to a collision avoidance method of unmanned boats based on genetic algorithm and particle swarm algorithm.

Background technique

Unmanned boat (USV) is a small surface motion platform that integrates functions such as autonomous planning, autonomous navigation, autonomous environmental perception, and target detection. It has become an indispensable device for exploring marine resources.

Because path planning and collision avoidance are the keys to autonomous navigation of unmanned boats. Therefore, before the unmanned boat enters the water area to perform the task, it is necessary to plan a full navigation path according to the known hydrological data of the sea area. Due to the complex and changeable marine environment, it is impossible to predict what will happen during the voyage, such as encountering strong winds and waves, passing ships, etc. The unmanned boat needs to detect the surrounding conditions at all times, obtain environmental information, accurately and quickly adjust the navigation state, avoid obstacles, reach the designated target point according to the mission requirements, perform the mission and resume the voyage.

The collision avoidance technology of unmanned boats reflects the intelligence level of maritime unmanned boats to a certain extent, and is one of the important research contents in the key technology field of unmanned boats. The patent application number is CN201610942213.5, a collision avoidance method for unmanned boats based on improved ant colony algorithm. The improved ant colony algorithm is used to plan the safe route of unmanned boats. This method has certain limitations and does not separately target Avoidance path planning calculation for static obstacles and dynamic obstacles.

SUMMARY OF THE INVENTION

The purpose of the present invention is to provide a collision avoidance method for the unmanned boat based on genetic algorithm and particle swarm algorithm, so as to obtain the optimal path planning, accurately avoid collision, and ensure that the unmanned boat reaches the target point safely.

An embodiment of the present invention provides a method for collision avoidance of an unmanned boat based on a genetic algorithm and a particle swarm algorithm, including:

Step 1: Research on the relevant parameters and collision avoidance constraint rules in the planning of the collision avoidance path of the unmanned boat: According to the position and speed of the unmanned boat and the position and speed of the obstacles involved in the parameters of the mathematical model of the unmanned boat navigation, get the difference between the two. The relative velocity, relative position, and relative azimuth motion parameters between the UAV and the obstacle are summed up in four different collision situations: frontal obstacle avoidance, overtaking obstacle avoidance, right chord crossing and left chord crossing obstacle avoidance. Collision constraint rules and spatial and temporal collision risk;

Step 2: Path planning for the surface unmanned vehicle to avoid static obstacles based on the genetic algorithm: through the above-mentioned parameter information related to the navigation water environment of the unmanned vehicle, initialize the relevant parameters of the genetic algorithm, generate the initial population of the path, and enter the genetic algorithm. The iterative loop of the algorithm calculates the fitness function; according to the obtained fitness function value, the next generation of chromosomes are selected through the "roulette" method to perform crossover, mutation, and repair operations to optimize the population, and after the iteration is completed, it can avoid static obstacles. the optimal path;

Step 3: Dynamic collision avoidance path planning based on the combination of genetic algorithm and particle swarm algorithm: real-time monitoring of whether there are dynamic obstacles around the marine environment, if there are dynamic obstacles, determine the distance between the UAV and the dynamic obstacles, and calculate Collision risk, if a collision is possible, determine whether the motion state of the dynamic obstacle is measurable; if it is measurable, the conventional collision avoidance in the genetic algorithm is used; if it is not measurable, the particle swarm algorithm is used to avoid the dynamic obstacle; finally Complete path optimization and output a feasible collision avoidance path for re-navigation;

The first step includes:

Research on the relevant parameters and collision avoidance constraint rules in the planning of the collision avoidance path of the unmanned boat: According to the position and speed of the unmanned boat and the position and speed of the obstacles involved in the mathematical model parameters of the unmanned boat navigation, the relative relationship between the two is obtained. Speed, relative position, and relative azimuth motion parameters. By summarizing four different collision situations between the UAV and the obstacle: frontal obstacle avoidance, overtaking obstacle avoidance, right chord crossing and left chord crossing obstacle avoidance, the maritime collision avoidance constraints of the unmanned vehicle are obtained. Rules and spatial and temporal collision risk;

Among them, let DCPA represent the shortest encounter distance between the unmanned boat and the obstacle, and TCPA represent the shortest encounter time, and the calculation method of the space collision risk is:

In the above formula, _RT , α _T represents the relative distance between the UAV and the obstacle, the relative speed direction and the course of the obstacle,

In the above formula, θ _T represents the relative angle between the UAV and the obstacle course, d ₂ =2×d ₁ ;

The calculation method of the time collision risk is:

In the above formula, And when TCPA>0, the second item of u _tT takes the negative sign, otherwise takes the positive sign;

The calculation results of DCPA and TCPA are analyzed and studied, and the comprehensive evaluation index of the possibility of collision between unmanned boats and moving objects is obtained:

The second step includes:

Path planning for UAV to avoid static obstacles based on genetic algorithm: through the above-mentioned parameter information related to the navigation water environment of the UAV, initialize the relevant parameters of the genetic algorithm, generate the initial population of the path, and enter the iteration of the genetic algorithm Loop, calculate the fitness function; according to the obtained fitness function value, select the next generation of chromosomes through the "roulette" method to perform crossover, mutation, and repair operations to optimize the population, and after the iteration is completed, the optimal population that can avoid static obstacles is obtained. path;

where the initial population of the path is:

According to the requirements of the task and the relevant information of the initial position and target point of the UAV, the original motion path is generated, that is, each chromosome:

(x _i1 , y _i1 )→(x _i2 , y _i2 )→…→(x _ili , y _ili )

In the above formula, i (1=0, 1, 2...n) represents a path of the unmanned boat, n represents the initial population number, and encodes the chromosome into a real number type, where if the unmanned boat is at the initial position There is no obstacle between the target point and the original path, and the original path is a smooth shortest path. If there is an obstacle between the initial position of the UAV and the target point, it will be iteratively updated based on the original path. Generate multiple new paths. For each path, it can be divided into multiple sections, and calculate and analyze the different sections of the path to obtain the relevant information of the path, and use delta to represent the division of any path. The length value, k _δ is used as a positive proportional parameter value, and d _num represents the total number of segments that the path is divided into:

In the above formula, (x _max , y _max ) and (x _min , y _min ) are the coordinate ranges of the simulation window, s(x _s , y _s ) and e(x _e , y _e ) are the initial positions of the UAV and target point;

After the calculation and division of the number of path segments above, the population will be initialized by the method of heuristic genetic population initialization to generate the corresponding dyed genes. Assuming that there are n total original paths, the i-th path is denoted as P _i , Let d _num = 3, and then randomly select two points p _i(2j-1) and p _i(2j) on the jth (j=1, 2, 3) segment, then within the range of these two points Arbitrary spawn point The abscissa of this point is limited to Within the range, the ordinate is limited to the initial position of the path s ( x _s , y _s ) and the target point e (x _e , y _e ), that is Then p _i(2j-1) , in order, p _i(2j) are connected, and finally the path P _i is generated:

get the set of feasible paths

Wherein, the fitness function is:

Set the individual fitness function as Value(P ^* )=min[f ₁ (P),f ₂ (P),f ₃ (P)]:

(1) represents the length of the path, and the path length of the i-th chromosome is:

In the above formula, m _i is the number of infeasible paths in the P _i path, and C ₁ is a suitable positive number;

(2) Represents the smoothness of the path. When the number of loci divided by the path of the chromosome is greater than 2, the average corner value of the _Pi path of the surface drone is:

Among them, a _ij (j=2,...,li -1) is the angle between p _{i (j-1)} p _ij and p _ij p _i(j+1) (0≤a _ij ≤π),m _i and _ki are not less than a _ij , that is, if a corner is not less than When , the penalty calculation should be performed on the target value. C ₂ is a suitable positive number. When _{li = 2, the path P i is} the connection from the initial point to the target point, Turning(P _i )=m _i ×C ₂ ;

(3) Represents the safety of the path, if the path _Pi is feasible, danger(di) = 1/di, where di>0 represents the minimum distance between the sailing route of the unmanned boat and the static obstacle; if the path Pi is infeasible, danger(P _i )=m _i ×C ₃ , where m _i is the number that the distance between the path segment of the individual path and the obstacle is less than the safety distance, and C ₃ is an appropriate positive number;

The third step includes:

Dynamic collision avoidance path planning based on the combination of genetic algorithm and particle swarm algorithm: real-time monitoring of whether there are dynamic obstacles around the marine environment, if there are dynamic obstacles, determine the distance between the UAV and the dynamic obstacles, and calculate the collision risk , if a collision may occur, judge whether the motion state of the dynamic obstacle is measurable; if it is measurable, use the conventional collision avoidance in the genetic algorithm; if it is not measurable, use the particle swarm algorithm to avoid the dynamic obstacle; finally complete the path optimization , output a feasible collision avoidance path for re-navigation;

Wherein, the dynamic collision avoidance method based on the combination of genetic algorithm and particle swarm algorithm is:

Decomposing Δv into Δv _o and Δv _r ,

Assuming that the speed v _obs and direction of the dynamic moving object do not change much in a short period of time, it can be ignored, that is,

dv _obs = 0, dβ = 0

In the above formula, is the angle between v _USV and v _obs , Δγ must satisfy the following conditions:

The dynamic information of the surrounding marine environment is monitored by relevant equipment. If a dynamic obstacle is found, it can be judged whether the motion state of the dynamic obstacle can be measured; If the global optimal solution is obtained, the dynamic collision avoidance based on the genetic algorithm is carried out; if it is unmeasurable, the parameters of the system can be effectively optimized because the particle swarm algorithm has a relatively fast speed of approaching the optimal solution; Algorithmic dynamic collision avoidance;

Collision avoidance based on genetic algorithm can be regarded as an objective optimization problem under multiple conditions:

f(Δv _USV ,Δα) is the fitness function in the genetic algorithm. The optimal solution obtained will not collide with known static obstacles, but also needs to comply with the constraints of the unmanned ship collision avoidance rules at sea. According to the unmanned ship and dynamic obstacle information, calculate the distance between them, DCPA and TCPA values, if there is a danger of collision, perform collision avoidance processing, which can be achieved by changing the navigation direction:

In the above formula, R _T is the distance between the UAV and the dynamic obstacle; θ _T is the relative orientation of the dynamic obstacle; β is the rotation angle of the relative motion line; is the speed ratio between the dynamic obstacle and the surface UAV; ΔC is the avoidance angle;

The steering angle ΔC is obtained by an iterative method:

Through the relevant AIS radar and other equipment to monitor the movement state of the surrounding dynamic obstacles at all times, predict their movement direction, and then set the current position of the unmanned boat as the new initial position, and take the length of safeD as the distance of the path segment. For a sub-target point, through the iterative optimization of the genetic algorithm, plan a path that can avoid nearby obstacles to reach the sub-target point, and then take the sub-target as the new initial position, repeat the above process, and successfully reach the specified target point. Task;

In the collision avoidance process of the unpredictable dynamic obstacle motion state, every advance of the unmanned boat must be pre-judged, and the polar coordinates are used to describe the position of the unmanned boat and the dynamic obstacle. The length of the unmanned boat is r , the moving radius is ρ, the current position is (A ₁ , B ₁ ), the next target point is (A ₂ , B ₂ ), and the current position of the dynamic obstacle is (C ₁ , D ₁ ); when Indicates that there are obstacles within the safe movement range of the unmanned boat, and collision avoidance processing based on particle swarm algorithm is required at this time;

The maximum value of inertia weight is ω _max , the minimum value is ω _min , the learning factors are C ₁ and C ₂ , the population is D, and the maximum number of iterations is D _max . Take the current point as the origin, ρ as the radius as the polar coordinate, and make n equal parts. m particles, randomly generate m×n particle swarm, and position X and velocity V, set the current optimal position Form the initial population t ₀ ; let Pbest _i represent the optimal value searched by the ith particle, Gbest _i represent the optimal value searched by the entire cluster, calculate the fitness value of the initial population, and update the global optimal value Gbest _i , The fitness function includes the path length and security, and the objective function of the shortest path length:

In the above formula, (x _ji , y _ji ) is the coordinate of point i on the path j, (x _ji-1 , y _ji-1 ) is the coordinate of a point (x _ji , y _ji ) on the path j, at this time, The coordinates of the next point (x _jn , y _jn ) predicted by the path planning of the unmanned boat need to be connected with the target point (g ₁ , g ₂ ) to calculate the path. The entire path length function is:

Y=Y _fit1 +Y _fit2

Safety function:

In the above formula, x _k is the position coordinate of the obstacle;

Based on the above, the fitness function: F=A×Y+B×OB(i), where the weighting factors of the A and B functions are any real numbers greater than or equal to zero. In order to completely avoid collisions, B<A, according to the speed, position The iteration formula is updated to generate a new particle swarm t ₁ , and the iteration formula of particle swarm velocity is:

V _i,j (t+1)=ωV _i,j (t)+c ₁ r ₁ (P _i,j (t)-x _i,j (t))+c ₂ r ₂ (P _g,j ( t)-x _i,j (t))

ω _k+1 =ω _min +(ω _k -ω _min )×((K _max -k)/K _max ) ⁿ

In the above formula, r ₁ , r ₂ are random real numbers greater than 0 and less than 1, P _i,j is the optimal position searched by particle i so far, P _g,i is the global optimal position, ω _k is the result obtained by the current iteration value, its initial value is ω _max , k represents the current number of iterations; K _max represents the maximum number of iterations;

Calculate the fitness value of the new population t ₁ , if it is better than the previous generation, generate a new population t ₂ , otherwise convert it to Cartesian coordinates, and iteratively generate a new population until the optimal collision avoidance path is found;

The beneficial effects of the present invention are:

1. The present invention introduces a genetic algorithm to plan an optimal path for the unmanned boat navigating in the complex and changeable marine environment, thereby saving various resources and reaching the designated target point according to the task requirements;

2. In the present invention, while introducing the genetic algorithm, it is improved. In order to take into account the influence of wind and waves at sea, deletion, repair and smoothing operators will be added, so that the unmanned boat can plan the optimal motion path under the genetic algorithm. ;

3. The present invention introduces an algorithm combining genetic algorithm and particle swarm algorithm, and for dynamic obstacles that suddenly appear in the ocean, the unmanned boat can respond quickly and accurately, plan the optimal collision avoidance path, and ensure the safety of the unmanned boat reach the target point.

Description of drawings

Fig. 1 is the unmanned boat path planning collision avoidance flow chart of the present invention;

Fig. 2 is the flow chart of unmanned boat static global path planning of the present invention;

3 is a schematic diagram of the distance from an obstacle to a path of the present invention;

4 is a schematic diagram of an obstacle avoidance model of the present invention;

Detailed ways

In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the present invention will be further described below in conjunction with the accompanying drawings:

The technical scheme of the present invention is realized as follows:

1. Research on relevant parameters and collision avoidance constraint rules in UAV path planning and collision avoidance:

The mathematical model parameters of the motion of the unmanned boat involve the position and speed of the unmanned boat and the position and speed of the obstacle. The relative velocity, relative position and relative azimuth motion parameters of the two are derived from this. Four different collision situations between the UAV and the obstacle are summarized: frontal obstacle avoidance, overtaking obstacle avoidance, right chord crossing and left chord crossing obstacle avoidance. The constraint rules for collision avoidance of unmanned boats at sea are introduced, and the concept of danger is introduced from the perspectives of time and space, where DCPA represents the shortest encounter distance between the unmanned boat and obstacles, and TCPA represents the shortest encounter time.

(1) Space collision risk

in the formula _RT , α _T represents the relative distance between the UAV and the obstacle, the relative speed direction and the course of the obstacle;

where θ _T represents the relative angle between the UAV and the course of the obstacle, d ₂ =2×d ₁ .

(2) Time collision risk

in And when TCPA>0, the second item of u _tT takes the negative sign, otherwise takes the positive sign.

The calculation results of DCPA and TCPA are analyzed and studied, and the comprehensive evaluation index of the possibility of collision between unmanned boats and moving objects is obtained:

2. Path Planning of Surface Unmanned Vehicle Based on Genetic Algorithm

The algorithm process of the unmanned boat path planning is to first obtain the environmental parameters of the unmanned boat navigation waters, mainly obstacle information, wind and wave level, etc. Then define the fitness function of the individual population, set the iteration times of the genetic algorithm, the number of chromosomes, the probability of crossover and mutation, and finally obtain the global path of unmanned boat navigation through the genetic operation of the selection algorithm.

First, according to the requirements of the task and the relevant information of the initial position and target point of the UAV, the original motion path is generated, that is, each chromosome:

(x _i1 ,y _i1 )→(x _i2 ,y _i2 )→…→(x _ili ,y _ili )

where i (1=0, 1, 2...n) represents a path for the unmanned boat to navigate, n represents the initial population number, and encodes the chromosome as a real number type.

Among them, if there is no obstacle between the initial position of the UAV and the target point, the original path obtained is a smooth shortest path. If there are obstacles between the initial position of the drone and the target point, this will iteratively update the original path to generate multiple new paths. For each path, it can be divided into multiple sections, and different sections of the path can be calculated and analyzed to obtain the relevant information of the path. Use delta to represent the length of the segment of any path, k _δ as a positive proportional parameter value, and d _num to represent the total number of segments that the path is divided into.

Among them, (x _max , y _max ) and (x _min , y _min ) are the coordinate range of the simulation window, s(x _s , y _s ) and e(x _e , y _e ) are the initial position and target of the UAV point.

After the above calculation and division of the number of path segments, the population initialization will be performed by the method of heuristic genetic population initialization, and the corresponding dyeing genes will be generated. Suppose there are n total original paths, denote the i-th path as P _i , set d _num =3, and then randomly select two points p _{i ( 2j-1)} and p _i(2j-1) , then any point is generated within the range of these two points The abscissa of this point. The coordinate is limited to Within the range, the ordinate is limited to the initial position of the path s (x _s , y _s ) and the target point e (x _e , y _e ), that is Then p _i(2j-1) , in order, p _i(2j) are connected, and finally the path P _i is generated:

get the set of feasible paths

After the initial population is set, set the individual fitness function as Value(P ^* )=min[f ₁ (P), f ₂ (P), f ₃ (P)].

in:

(1) Represents the length of the path. The path length of the i-th chromosome is

where m _i is the number of infeasible paths in the P _i path, and C ₁ is a suitable positive number.

(2) Indicates the smoothness of the path. When the number of loci for the path division of the chromosome is greater than 2, the average corner value of the _Pi path of the surface drone is:

Among them, a _ij (j=2,...,li-1) is the angle between p _i(j-1) p _ij and p _ij p _i(j+1) (0≤a _ij ≤π),m _i and k _i is a number not less than π/2 in a _ij , that is, if a certain corner is not less than π/2, the penalty calculation should be performed on the target value, C ₂ is a suitable positive number, when _li = 2 , the path P _i is the connection from the initial point to the target point, Turning(P _i )=mi×C ₂

(3) Indicates the security of the path

If the path P _i is feasible, danger(d _i )=1/d _i , where d _i > 0 represents the minimum distance between the sailing route of the UAV and the static obstacle; if the path P _i is infeasible, danger( P _i )=mi×C ₃ , mi is the number that the distance between the path segment of the individual path and the obstacle is less than the safety distance, and C ₃ is a suitable positive number. According to the value of the fitness function, the next generation of chromosomes is selected by the proportional algorithm to perform genetic operations such as crossover, mutation, and repair, so as to optimize the population. Finally, through iterative optimization, a global path without obstacles is planned.

3. To carry out dynamic collision avoidance based on the combination of genetic algorithm and particle swarm algorithm, the main steps are as follows:

As shown in the schematic diagram of the obstacle avoidance model in Fig. 4, Δv is decomposed into Δv _o and Δv _r .

Assuming that the speed v _obs and direction of the dynamic moving object do not change much in a short period of time, it can be ignored, that is,

dv _Obs = 0, dβ = 0

in is the angle between v _USV and v _obs . Δγ must satisfy the following conditions:

Monitor the dynamic information of the surrounding marine environment through related equipment. If a dynamic obstacle is found, determine whether the motion state of the dynamic obstacle is measurable; if it is measurable, since the genetic algorithm is suitable for solving complex optimization problems, the global optimal solution of the optimization problem can be obtained, and then the genetic algorithm-based method can be carried out. Dynamic collision avoidance; if it is unmeasurable, because the particle swarm algorithm has a fairly fast speed of approaching the optimal solution, the parameters of the system can be effectively optimized; then the dynamic collision avoidance based on the particle swarm algorithm is carried out.

Collision avoidance based on genetic algorithm can be regarded as an objective optimization problem under multiple conditions:

f(Δv _USV ,Δα) is the fitness function in the genetic algorithm, and the optimal solution obtained will not collide with known static obstacles, but also needs to comply with the constraints of the unmanned ship collision avoidance rules at sea. According to the information of the unmanned boat and dynamic obstacles, calculate the distance, DCPA, TCPA value between them, and if there is a danger of collision, perform collision avoidance processing. Collision avoidance processing can be achieved by changing the sailing direction.

where R _T is the distance between the UAV and the dynamic obstacle; r

θ _T is the relative orientation of the dynamic obstacle;

β is the rotation angle of the relative motion line;

is the speed ratio of the dynamic obstacle to the surface UAV;

ΔC is the avoidance angle.

The steering angle ΔC is obtained by an iterative method:

Through the relevant AIS radar and other equipment to monitor the movement state of the surrounding dynamic obstacles at all times, predict their movement direction, and then set the current position of the unmanned boat as the new initial position, and take the length of safeD as the distance of the path segment. For a sub-target point, through the iterative optimization of the genetic algorithm, plan a path that can avoid nearby obstacles to reach the sub-target point, and then take the sub-target as the new initial position, repeat the above process, and successfully reach the specified target point. Task.

In the collision avoidance process of the unpredictable dynamic obstacle motion state, every advance of the unmanned boat must be pre-judged. Using polar coordinates, describe the position of the UAV and dynamic obstacles. The length of the unmanned boat is r, the moving radius is ρ, the current position is (A ₁ , B ₁ ), the next target point is (A ₂ , B ₂ ), and the current position of the dynamic obstacle is (C ₁ , D ₁ ); when Indicates that there are obstacles within the safe movement range of the unmanned boat, and collision avoidance processing based on particle swarm algorithm is required at this time.

The maximum value of inertia weight is ω _max , the minimum value is ω _min , the learning factors are C ₁ , C ₂ respectively, the population is D, and the maximum number of iterations is D _max . Take the current point as the origin, ρ as the radius as the polar coordinate, and make n equal parts. m particles, randomly generate m×n particle swarm, as well as position X and velocity V, set the current optimal position P _i =(p _i1 ,p ₂ ... p _iD ) to form an initial population t ₀ . Let Pbest _i represent the optimal value searched by the ith particle, and Gbest _i represent the optimal value searched by the entire cluster. Calculate the fitness value of the initial population and update the global optimal value Gbest _i .

The fitness function includes path length and security.

The objective function for the shortest path length:

where (x _ji , y _ji ) is the coordinate of point i on the path j, and (x _ji-1 , y _ji-1 ) is the coordinate of a point (x _ji , y _ji ) on the path j. At this point, the next point coordinates (x _jn , y _jn ) predicted by the path planning of the unmanned boat need to be connected with the target point (g ₁ , g ₂ ) to calculate the path. The entire path length function is:

Y=Y _fit1 +Y _fit2

Safety function:

where x _k is the position coordinate of the obstacle.

Combining the above, the fitness function: F=A×Y+B×OB(i)

Among them, the weighting factors of the two functions A and B are any real numbers greater than or equal to zero. In order to avoid collision completely, B<A.

According to the velocity, the position iterative formula is updated to generate a new particle swarm t ₁ .

The iterative formula of particle swarm velocity is:

V _i,j (t+1)=ωV _i,j (t)+c ₁ r ₁ (P _i,j (t)-x _i,j (t))+c ₂ r ₂ (P _g,j ( t)-x _i,j (t))

ω _k+1 =ω _min +(ω _k -ω _min )×((K _max -k)/K _max ) ⁿ

Among them, r ₁ , r ₂ are random real numbers greater than 0 and less than 1, P _i,j is the optimal position searched by particle i so far, and P _g,i is the global optimal position. ω _k is the value obtained from the current iteration, and its initial value is ω _max ; k represents the current number of iterations; K _max represents the maximum number of iterations

Calculate the fitness value of the new population t ₁ , if it is better than the previous generation, generate a new population t ₂ , otherwise convert it to Cartesian coordinates. The loop iteratively generates new populations until the optimal collision avoidance path is found.

Claims (4)

1. an unmanned boat collision avoidance method based on genetic algorithm and particle swarm algorithm, is characterized in that, comprises: Step 1: Research on the relevant parameters and collision avoidance constraint rules in the planning of the collision avoidance path of the unmanned boat: According to the position and speed of the unmanned boat and the position and speed of the obstacles involved in the parameters of the mathematical model of the unmanned boat navigation, get the difference between the two. The relative velocity, relative position, and relative azimuth motion parameters between the UAV and the obstacle are summed up in four different collision situations: frontal obstacle avoidance, overtaking obstacle avoidance, right chord crossing and left chord crossing obstacle avoidance. Collision constraint rules and spatial and temporal collision risk; Step 2: Path planning for the surface unmanned vehicle to avoid static obstacles based on the genetic algorithm: through the above-mentioned parameter information related to the navigation water environment of the unmanned vehicle, initialize the relevant parameters of the genetic algorithm, generate the initial population of the path, and enter the genetic algorithm. The iterative loop of the algorithm calculates the fitness function; according to the obtained fitness function value, the next generation of chromosomes are selected through the "roulette" method to perform crossover, mutation, and repair operations to optimize the population, and after the iteration is completed, it can avoid static obstacles. the optimal path; Step 3: Dynamic collision avoidance path planning based on the combination of genetic algorithm and particle swarm algorithm: real-time monitoring of whether there are dynamic obstacles around the marine environment, if there are dynamic obstacles, determine the distance between the UAV and the dynamic obstacles, and calculate Collision risk, if a collision is possible, determine whether the motion state of the dynamic obstacle is measurable; if it is measurable, the conventional collision avoidance in the genetic algorithm is used; if it is not measurable, the particle swarm algorithm is used to avoid the dynamic obstacle; finally Complete path optimization and output a feasible collision avoidance path for re-navigation. 2. a kind of unmanned boat collision avoidance method based on genetic algorithm and particle swarm algorithm according to claim 1, is characterized in that, described step 1, comprises: Research on the relevant parameters and collision avoidance constraint rules in the planning of the collision avoidance path of the unmanned boat: According to the position and speed of the unmanned boat and the position and speed of the obstacles involved in the mathematical model parameters of the unmanned boat navigation, the relative relationship between the two is obtained. Speed, relative position, and relative azimuth motion parameters. By summarizing four different collision situations between the UAV and the obstacle: frontal obstacle avoidance, overtaking obstacle avoidance, right chord crossing and left chord crossing obstacle avoidance, the maritime collision avoidance constraints of the unmanned vehicle are obtained. Rules and spatial and temporal collision risk; Among them, let DCPA represent the shortest encounter distance between the unmanned boat and the obstacle, and TCPA represent the shortest encounter time, and the calculation method of the space collision risk is: In the above formula, _RT , α _T represents the relative distance between the UAV and the obstacle, the relative speed direction and the course of the obstacle, In the above formula, θ _T represents the relative angle between the UAV and the obstacle course, d ₂ =2×d ₁ ; The calculation method of the time collision risk is: In the above formula, And when TCPA>0, the second item of u _tT takes the negative sign, otherwise takes the positive sign; The calculation results of DCPA and TCPA are analyzed and studied, and the comprehensive evaluation index of the possibility of collision between unmanned boats and moving objects is obtained: 3. a kind of unmanned boat collision avoidance method based on genetic algorithm and particle swarm algorithm according to claim 1, is characterized in that: described step 2, comprises: Path planning for UAV to avoid static obstacles based on genetic algorithm: through the above-mentioned parameter information related to the navigation water environment of the UAV, initialize the relevant parameters of the genetic algorithm, generate the initial population of the path, and enter the iteration of the genetic algorithm Loop, calculate the fitness function; according to the obtained fitness function value, select the next generation of chromosomes through the "roulette" method to perform crossover, mutation, and repair operations to optimize the population, and after the iteration is completed, the optimal population that can avoid static obstacles is obtained. path; where the initial population of the path is: According to the requirements of the task and the relevant information of the initial position and target point of the UAV, the original motion path is generated, that is, each chromosome: (x _i1 , y _i1 )→(x _i2 , y _i2 )→…→(x _ili , y _ili ) In the above formula, i(1=0, 1, 2...n) represents a path of the unmanned boat, n represents the initial population number, and encodes the chromosome into a real number type, where if the unmanned boat is at the initial position There is no obstacle between the target point and the original path, and the original path is a smooth shortest path. If there is an obstacle between the initial position of the UAV and the target point, it will be iteratively updated based on the original path. Generate multiple new paths. For each path, it can be divided into multiple sections, and calculate and analyze the different sections of the path to obtain the relevant information of the path, and use delta to represent the division of any path. The length value, k _δ is used as a positive proportional parameter value, and d _num represents the total number of segments that the path is divided into: In the above formula, (x _max , y _max ) and (x _min , y _min ) are the coordinate ranges of the simulation window, s(x _s , y _s ) and e(x _e , y _e ) are the initial positions of the UAV and target point; After the calculation and division of the number of path segments above, the population will be initialized by the method of heuristic genetic population initialization to generate the corresponding dyed genes. Assuming that there are n total original paths, the i-th path is denoted as P _i , Let d _num = 3, and then randomly select two points p _i(2j-1) and p _i(2j) on the jth (j=1, 2, 3) segment, then within the range of these two points Arbitrary spawn point The abscissa of this point is limited to Within the range, the ordinate is limited to the initial position of the path s (x _s , y _s ) and the target point e (x _e , y _e ), that is Then p _i(2j-1) , in order, p _i(2j) are connected, and finally the path P _i is generated: get the set of feasible paths Wherein, the fitness function is: Set the individual fitness function as Value(P ^* )=min[f ₁ (P), f ₂ (P), f ₃ (P)]: (1) represents the length of the path, the path length of the i-th chromosome is: In the above formula, m _i is the number of infeasible paths in the P _i path, and C ₁ is a suitable positive number; (2) Represents the smoothness of the path. When the number of loci divided by the path of the chromosome is greater than 2, the average corner value of the _Pi path of the surface drone is: Among them, a _ij (j=2, ..., li -1) is the angle between p _{i (j-1)} p _ij and p _ij p _i(j+1) (0≤a _ij ≤π),m _i and _ki are not less than a _ij , that is, if a corner is not less than When , the penalty calculation should be performed on the target value, C ₂ is a suitable positive number, when _{li = 2, the path P i is} the connection between the initial point and the target point, Turning(P _i )=m _i ×C ₂ ; (3) Represents the safety of the path, if the path _Pi is feasible, danger(di)=1/di, where di>0 represents the minimum distance between the navigation route of the unmanned boat and the static obstacle; if the path _Pi is infeasible , danger(P _i )=m _i ×C ₃ , m _i is the number that the distance between the path segment of the individual path and the obstacle is less than the safety distance, and C ₃ is an appropriate positive number. 4. a kind of unmanned boat collision avoidance method based on genetic algorithm and particle swarm algorithm according to claim 1, is characterized in that: described step 3, comprises: Dynamic collision avoidance path planning based on the combination of genetic algorithm and particle swarm algorithm: real-time monitoring of whether there are dynamic obstacles around the marine environment, if there are dynamic obstacles, determine the distance between the UAV and the dynamic obstacles, and calculate the collision risk , if a collision may occur, judge whether the motion state of the dynamic obstacle is measurable; if it is measurable, use the conventional collision avoidance in the genetic algorithm; if it is not measurable, use the particle swarm algorithm to avoid the dynamic obstacle; finally complete the path optimization , output a feasible collision avoidance path for re-navigation; Wherein, the dynamic collision avoidance method based on the combination of genetic algorithm and particle swarm algorithm is: Decomposing Δv into Δv _o and Δv _r , Assuming that the speed v _obs and direction of the dynamic moving object do not change much in a short period of time, it can be ignored, that is, dv _obs = 0, dβ = 0 In the above formula, is the angle between v _USV and v _obs , Δγ must satisfy the following conditions: The dynamic information of the surrounding marine environment is monitored by relevant equipment. If a dynamic obstacle is found, it can be judged whether the motion state of the dynamic obstacle can be measured; If the global optimal solution is obtained, the dynamic collision avoidance based on the genetic algorithm is carried out; if it is unmeasurable, the parameters of the system can be effectively optimized because the particle swarm algorithm has a relatively fast speed of approaching the optimal solution; Algorithmic dynamic collision avoidance; Collision avoidance based on genetic algorithm can be regarded as an objective optimization problem under multiple conditions: f(Δv _USV , Δα) is the fitness function in the genetic algorithm. The optimal solution obtained will not collide with known static obstacles, but also needs to comply with the constraints of the unmanned ship collision avoidance rules at sea. According to the unmanned ship and dynamic obstacle information, calculate the distance between them, DCPA and TCPA values, if there is a danger of collision, perform collision avoidance processing, which can be achieved by changing the navigation direction: In the above formula, R _T is the distance between the UAV and the dynamic obstacle; θ _T is the relative orientation of the dynamic obstacle; β is the rotation angle of the relative motion line; is the speed ratio between the dynamic obstacle and the surface UAV; ΔC is the avoidance angle; The steering angle ΔC is obtained by an iterative method: Through the relevant AIS radar and other equipment to monitor the movement state of the surrounding dynamic obstacles at all times, predict their movement direction, and then set the current position of the unmanned boat as the new initial position, and take the length of safeD as the distance of the path segment. For a sub-target point, through the iterative optimization of the genetic algorithm, plan a path that can avoid nearby obstacles to reach the sub-target point, and then take the sub-target as the new initial position, repeat the above process, and successfully reach the specified target point. Task; In the collision avoidance process of the unpredictable dynamic obstacle motion state, every advance of the unmanned boat must be pre-judged, and the polar coordinates are used to describe the position of the unmanned boat and the dynamic obstacle. The length of the unmanned boat is r , the moving radius is ρ, the current position is (A ₁ , B ₁ ), the next target point is (A ₂ , B ₂ ), and the current position of the dynamic obstacle is (C ₁ , D ₁ ); when Indicates that there are obstacles within the safe movement range of the unmanned boat, and collision avoidance processing based on particle swarm algorithm is required at this time; Set the maximum value of inertia weight as ω _max , the minimum value as ω _min , the learning factors as C ₁ , C ₂ , the population as D, the maximum number of iterations as D _max , the current point as the origin, ρ as the radius as the polar coordinate, Carry out n equal parts, m particles, randomly generate m×n particle swarm, and position X and speed V, set the current optimal position Form the initial population t ₀ ; let Pbest _i represent the optimal value searched by the ith particle, Gbest _i represent the optimal value searched by the entire cluster, calculate the fitness value of the initial population, and update the global optimal value Gbest _i , The fitness function includes the path length and security, and the objective function of the shortest path length: In the above formula, (x _ji , y _ji ) is the coordinate of point i on the path j, (x _ji-1 , y _ji-1 ) is the coordinate of a point on the path j (x _ji , y _ji ), at this time, The next point coordinates (x _jn , y _jn ) predicted by the path planning of the unmanned boat need to be connected with the target point (g ₁ , g ₂ ) to calculate the path. The entire path length function is: Y=Y _fit1 +Y _fit2 Safety function: In the above formula, x _k is the position coordinate of the obstacle; Based on the above, the fitness function: F=A×Y+B×OB(i), where the weighting factors of the A and B functions are any real numbers greater than or equal to zero. In order to completely avoid collisions, B<A, according to the speed, position The iteration formula is updated to generate a new particle swarm t ₁ , and the iteration formula of particle swarm velocity is: Vi _,j (t+1)=ωVi _,j (t)+c ₁ r ₁ (P _i,j (t)-xi _,j (t))+c ₂ r ₂ (P _g,j ( t)-xi _,j (t)) ω _k+1 =ω _min +(ω _k -ω _min )×((K _max -k)/K _max ) ⁿ In the above formula, r ₁ , r ₂ are random real numbers greater than 0 and less than 1, P _{i, j} is the optimal position searched by particle i so far, P _{g, i} is the global optimal position, ω _k is the current iteration obtained value, its initial value is ω _max , k represents the current number of iterations; K _max represents the maximum number of iterations; Calculate the fitness value of the new population t ₁ , if it is better than the previous generation, generate a new population t ₂ , otherwise convert it to Cartesian coordinates, and iteratively generate a new population until the optimal collision avoidance path is found.