CN113805597A - Obstacle self-protection artificial potential field method local path planning method based on particle swarm optimization - Google Patents

Abstract

Translated fromChinese

本发明提出一种基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法，引入粒子群算法，并结合智能车在转弯过程中存在最小转弯半径的转弯约束，即在优化过程中加入最大转向角的约束，对初步规划的路线进行曲线优化，并建立相应适应度函数，进一步采用粒子群算法限制寻优范围并找到符合智能车转向特性的航向角，通过粒子不断迭代得到最优的航向角度，从而建立粒子群障碍物自我保护人工势场法避开障碍物，找到符合智能车转向约束的最优路径。

The invention proposes a local path planning method based on the particle swarm algorithm for obstacle self-protection artificial potential field method, introduces the particle swarm algorithm, and combines the turning constraint of the minimum turning radius of the intelligent vehicle during the turning process, that is, adding the minimum turning radius in the optimization process. Constrained by the maximum steering angle, curve optimization is performed on the preliminary planned route, and the corresponding fitness function is established. The particle swarm algorithm is further used to limit the optimization range and find the heading angle that conforms to the steering characteristics of the smart vehicle. The heading angle can be used to establish a particle swarm obstacle self-protection artificial potential field method to avoid obstacles and find the optimal path that meets the steering constraints of the smart car.

Description

Obstacle self-protection artificial potential field method local path planning method based on particle swarm optimization

Technical Field

The invention belongs to the technical field of intelligent driving path planning and autonomous navigation, and local path planning of obstacle avoidance of unmanned vehicles and mobile robots, and particularly relates to a particle swarm algorithm-based local path planning method of an obstacle self-protection artificial potential field method.

Background

In recent years, with the development of computer technology, automobiles gradually develop towards an intelligent direction combining with electronic technology and network communication, and intelligent automobiles are beneficial to improving traffic safety, relieving road congestion, improving social efficiency, advocating low-carbon life and the like. The key technologies of the intelligent vehicle comprise environment perception, navigation positioning, path planning, decision control and the like, and the path planning is a key part of the intelligent vehicle and has great significance for the research of the intelligent vehicle.

The intelligent vehicle is a research hotspot and difficulty in the intelligent field, and the path planning is an important component of the intelligent vehicle, and aims to avoid dynamic and static obstacles through an algorithm under the environment state of a known starting point and a known target point and search for an optimal (shortest distance or shortest time) path. Path planning is further divided into global planning (fixed environment) and local planning (dynamic environment). The global path planning is based on collecting all environment information in the intelligent vehicle driving area, then drawing a map according to the information, and searching a safe and economic path by using an algorithm after inputting a starting point and an end point. And local path planning is to carry out path planning in real time by using environmental information collected by the sensor, and has higher requirements on the vehicle-mounted sensor. In the actual process, the intelligent vehicle simultaneously relates to global planning and local planning, and the quality of the algorithm directly influences the effect of planning the path.

Disclosure of Invention

The invention aims to provide a local path planning method of an obstacle self-protection artificial potential field method based on a particle swarm algorithm, which respectively provides a method for adding a virtual obstacle to solve the local minimum problem and a method for adding a distance factor to solve the problem of unreachable targets on the basis of the traditional artificial potential field method, and provides an obstacle self-protection artificial potential field method based on the particle swarm algorithm to solve the path planning problem of multiple obstacles in a static environment, in order to obtain the optimal rotation angle of an intelligent vehicle, the invention introduces the particle swarm algorithm, combines the turning constraint of the intelligent vehicle with the minimum turning radius in the turning process, namely adds the constraint of the maximum turning angle in the optimization process, performs curve optimization on a preliminarily planned route, establishes a corresponding fitness function, further adopts the particle swarm algorithm to limit the optimizing range and finds a course angle according with the turning characteristics of the intelligent vehicle, and obtaining an optimal course angle through continuous iteration of the particles, thereby establishing a particle swarm barrier self-protection artificial potential field method to avoid the barrier and finding an optimal path which accords with the steering constraint of the intelligent vehicle. And finally, simulating through an MATLAB platform, and verifying the effectiveness of the algorithm.

As a path planning algorithm widely used, the artificial potential field method is often applied to a path planning problem. On the basis of the prior improved artificial potential field method, the invention provides a safer obstacle avoidance strategy in a static environment.

The work performed by the present invention mainly includes: firstly, summarizing feasibility and defects of a predecessor improved artificial potential field method, and aiming at the problem that a distance factor artificial potential field method cannot avoid an obstacle in a complex environment, an obstacle self-protection artificial potential field method (OSPAPF) is provided. Secondly, the method comprises the following steps: in order to find out the optimal course angle, a particle swarm optimization-based obstacle self-protection artificial potential field method (PSO-OSPAPF) is provided, the obstacle is avoided by rotating the course angle, a corresponding fitness function is established, and the course angle which accords with the steering characteristic of the intelligent vehicle is found out in a limited optimizing range by further adopting the Particle Swarm Optimization (PSO).

The invention specifically adopts the following technical scheme:

a local path planning method of an obstacle self-protection artificial potential field method based on a particle swarm optimization is characterized by comprising the following steps:

step S1: initializing parameters of an artificial potential field method, reading the current coordinates, the coordinates of the obstacle and the coordinates of a target point of the intelligent vehicle, and calibrating the irregular obstacle: calibrating the irregular barrier into a rectangular barrier, dividing the rectangle into a plurality of squares, and setting the self-protection radius of the barrier for each square;

step S2: calculating the angle between the current intelligent vehicle and each obstacle, and calculating repulsion, attraction and resultant force: calculating the angle between the obstacle and the current position of the intelligent vehicle, the repulsion of each obstacle to the vehicle and the attraction of the target point to the obstacle, and finally calculating the resultant force;

step S3: judging whether the position of the next step enters the position of the barrier self-protection area: setting a barrier self-protection area with each square barrier as a circle center and a radius of R so as to ensure that the intelligent vehicle does not collide with the barrier; if entering the obstacle self-protection zone, executing the steps S4-S8;

step S4: optimizing by calling a particle swarm algorithm, randomly initializing particles, and calculating the fitness value and the optimal value of the initial particles;

step S5: adding vehicle steering characteristic constraint optimization, updating the speed and position of each particle after iteration, and calculating a function adaptive value of each particle;

step S6: updating the historical optimal value of each particle after iteration, and calculating the global optimal value of the population;

step S7: stopping iteration and outputting an optimal course angle alpha' when the maximum iteration times are reached or the global optimal value is unchanged;

step S8: and returning to the step S3 to judge whether the next position enters the self-protection area of the obstacle after the optimal course angle is obtained.

Further, the intelligent vehicle judges whether the intelligent vehicle falls into a local minimum value point in the motion process: when the intelligent vehicle moves to a certain position, the attractive force and the repulsive force are the same in size and opposite in direction, the resultant force borne by the intelligent vehicle at the path point is zero, and the position of the intelligent vehicle is the lowest position of the potential field in the whole environment;

if the intelligent vehicle is trapped in the local minimum point, the intelligent vehicle jumps out of the local minimum point by arranging the virtual barrier at the intersection of the perpendicular bisector of the connecting line of the barrier and the intelligent vehicle.

Further, by adding a distance factor, the situation that the target point is inaccessible is avoided.

Further, in step S1, the irregular obstacle is calibrated as a rectangular obstacle, and the maximum and minimum values of the abscissa and the ordinate of the obstacle are respectively set as x_max,x_min,y_max,y_minThen the coordinates of the four vertices of the rectangle are A (x)_min,y_min)，B(x_max,y_min)，C(x_max,y_max)，D(x_min,y_max)，L_gTo scale the length of the rectangle, W_dThe width of the calibrated rectangle is obtained; the rectangle is divided into a plurality of squares, and the number N of the squares₀：

Wherein ceil is an rounding-up function;

side length of square L_o＝W_dIth_oThe center coordinates of each square are:

the self-protection radius of the barrier is as follows:

further, the specific implementation procedure of step S3 is as follows:

when the intelligent vehicle moves to a position A, according to the resultant force angle value alpha and the step length L of the current path point, the intelligent vehicle arrives at a position B in the next step, if the position B is judged to be in the self-protection area of the obstacle, the resultant force is rotated by theta, the intelligent vehicle can arrive at a position D under the action of the new resultant force angle alpha' and the step length, and the intelligent vehicle is adjusted to be far away from the obstacle to prevent collision; the new resultant force angle determination mode is as follows: the coordinate position of the center of the known obstacle i is

The coordinates of the target point are (x)_G.y_G) The position coordinate of the intelligent vehicle is (x)_C,y_C) And the position of the next path point is as follows according to the resultant force angle alpha and the step length l:

when the next position is judged to enter the barrier self-protection area, the difference value between the position of the intelligent vehicle and the horizontal coordinate of the barrier is calculated

Determining a resultant rotation angle theta according to the positive and negative of the delta x, when the delta x is larger than or equal to 0, indicating that the obstacle is positioned at the right side of the next position, and the next position should rotate towards the left, adding the theta on the basis of the resultant angle alpha, wherein the included angle alpha 'between the resultant force and the horizontal axis is alpha + theta, and when the delta x is smaller than 0, indicating that the obstacle is positioned at the left side of the next position, alpha' is alpha-theta, and the next path point is:

further, in step S3, the size of the obstacle self-protection zone can be set to different values according to the safety distance between the smart car and different types of obstacles.

Further, step S4 is specifically:

firstly, initializing particles, wherein each particle has two attributes of position and speed; the position represents a new resultant angle alpha ', the speed represents the change size of alpha', each particle calculates a corresponding fitness value according to the updating of the speed and the position, and the quality of the particle is judged according to the fitness value; in the searching process, the optimal value is divided into individual optimal value and global optimal value, and the individual optimal value is recorded as an individual extreme value alpha'_pbestThe particle groups share the individual extremum information, and the value with the optimal fitness value in the individual extremums in the particle swarm is selected as a global optimal solution and recorded as a global extremum alpha'_qbest(ii) a In an iterative process the particles are according to α'_pbestAnd alpha'_qbestAdjusting the speed and the position attribute to enable the particles to gradually approach the optimal value, thereby finding the optimal solution; suppose there is N_lAnd each particle, in each iteration process, the speed and position expression of the particle is as follows:

V_j(t+1)＝wV_j(t)+c₁r₁(α′_pbestj(t)-α′_j(t))+c₂r₂(α′_gbestj(t)-α′_j(t))

α′_j(t+1)＝α′_j(t)+V_j(t+1),j＝1,2,…N_l

in the formula, V_j(t +1) and α'_j(t +1) are the speed and resultant force angle values of the particle j in the t +1 th iteration respectively, the range of the new resultant force angle is the addition and subtraction of any value on the basis of the original resultant force angle, and the speed value represents the speed of the example; alpha's'_pbestj(t) is the individual optimum value, α ', for particle j at the t-th iteration'_gbestj(t) is the t-th iteration global optimum; w is an inertia weight and reflects the influence degree of the individual extreme value of the particle on the current value; c. C₁And c₂Acceleration factors of individuals and groups respectively represent empirical values generated in the optimization process of the particles and accumulated empirical values of other particles, and directly influence the next-step operation track r of each particle₁And r₂Is a random number between 0 and 1;

step S5 specifically includes:

let L be the front-rear wheel base, (x)_c,y_cAnd alpha) represents the pose of the intelligent vehicle, (x)_c,y_c) The coordinate of the intelligent vehicle in the two-dimensional space is shown, alpha is the course angle of the intelligent vehicle at the current moment, and is also the resultant force angle in the artificial potential field method; minimum turning radius R of intelligent vehicle during turning_minThe maximum internal angle of the front wheel is alpha₂Maximum external rotation angle alpha of front wheel₁In the process of turning the vehicle, the inner and outer turning angles are related to the type, the self speed and the acceleration of the vehicle, and v is set_xIs the longitudinal speed during movement, a_yThe expression of the minimum turning radius and the expression of the maximum turning angle, namely the maximum internal corner, are as follows:

the inner wheel rotating angle of the intelligent vehicle is larger than the outer wheel rotating angle in the steering process, the rotating angle does not exceed the maximum value of the inner wheel rotating angle and the outer wheel rotating angle in the steering process, namely the relation between the newly searched resultant force angle value alpha' and the inner rotating angle of the front wheel is as follows:

α-α₂≤α′≤α+α₂

the intelligent vehicle has maximum rotation angle limitation in the turning process, and the obstacle self-protection artificial potential field method optimizing angle value based on particle swarm optimization is limited within the maximum rotation angle range of the intelligent vehicle, so that the path obtained by planning accords with the kinematics model of the intelligent vehicle;

in the calculation of the fitness function, b (i) represents the position (x) at the next moment_n,y_n) To the ith obstacle center

D (i) represents the distance between the next position and the target point, and the mathematical expressions are respectively:

the next position in the path planning process should be far away from the center of the obstacle, i.e. the larger (b) (i) is, the better the position is, and the distance to the target point is as close as possible, i.e. the smaller (d) (i) is, the better the position is; combining the two distances to establish a fitness function f (i), wherein the larger the fitness value is, the safer the fitness function is, and the expression is as follows:

however, in a coordinate system, an intelligent vehicle is close to an obstacle and far away from a target point, a fitness function is represented as b (i) which is small, d (i) which is large, so that the difference between b (i) and c (i) is hundreds of times, the calculation result of the fitness value is biased to a certain value, the fitness value of each particle is not representative, the two are normalized into a dimensionless value, the distance between the two is mapped to a range between [0 and 1], and a mapping expression and a fitness function expression are respectively as follows:

f(i)＝b(i)′+c(i)′；

step S6 specifically includes:

giving the maximum iteration times, the particle speed range and the position optimizing range of the particle swarm algorithm, and giving the number (N) of the particles of the population_l) Randomly generating N_lAngularity value alpha'_jForming an initial population, j ═ 1,2, … N_l(ii) a Calculating the fitness value f (i) of each individual in the population to the ith obstacle according to the fitness function given in the step S5, and adjusting the speed and the position according to the individual extreme value and the global extreme value to enable the particles to gradually approach the optimal value so as to find the optimal solution; recording the individual extremum α 'of each particle'_pbestjAnd global extremum α'_gbestj；

Step S7 specifically includes:

comparing the global extreme value with the historical optimal value, updating the speed and the position of the particles, stopping iteration when the maximum iteration times is reached or the global optimal value is unchanged, and outputting an optimal course angle alpha';

in step S8, the output optimal heading angle α' and step length are substituted into the path formula of step S3, and the process returns to step S3 to determine whether the next position enters the obstacle self-protected area position.

Further, by setting the path length L within a fixed time interval₀Compared with the length L of the path taken by the intelligent vehicle from the position A to the position BIf L is₀>And L, judging that the intelligent vehicle falls into the local minimum value in the motion process.

Further, the repulsive force field function after introducing the distance factor is shown as follows:

where ρ (X, X)_goal)ⁿFor the introduced distance factor, ρ (X, X)_goal) Euclidean distance k representing position of intelligent vehicle and target point_repIs a repulsive force field gain coefficient, N represents the number of obstacles,

is the distance between the position of the intelligent vehicle and the ith obstacle, rho_oThe influence range of the repulsion force of the obstacle means that the repulsion force potential field is generated only when the intelligent vehicle enters the influence range of the repulsion force. The expression of obtaining the repulsive force by solving the negative gradient of the potential field is as follows:

further, the optimized path is connected on the MATLAB, and the motion track of the unmanned vehicle is visualized.

Compared with the prior art, the invention and the optimized scheme thereof respectively provide the virtual barrier for solving the local minimum problem and the distance factor for solving the problem of unreachable targets on the basis of the traditional artificial potential field method, and provide the barrier self-protection artificial potential field method based on the particle swarm algorithm for solving the path planning problem of multiple barriers in the static environment, in order to obtain the optimal rotation angle of the intelligent vehicle, the invention introduces the particle swarm algorithm, combines the turning constraint of the intelligent vehicle with the minimum turning radius in the turning process, namely adds the constraint of the maximum steering angle in the optimization process, performs curve optimization on the preliminarily planned route, establishes the corresponding fitness function, further adopts the particle swarm algorithm to limit the optimization range and find the heading angle according with the steering characteristic of the intelligent vehicle, and obtains the optimal heading angle through continuous particle iteration, therefore, a particle swarm barrier self-protection artificial potential field method is established to avoid the barriers, and an optimal path which accords with steering constraint of the intelligent vehicle is found. And finally, simulating through an MATLAB platform, and verifying the effectiveness of the algorithm.

Drawings

The invention is described in further detail below with reference to the following figures and detailed description:

FIG. 1 is a schematic diagram of irregular obstacle targeting according to an embodiment of the present invention.

Fig. 2 is a schematic view of force analysis under a multi-obstacle environment according to an embodiment of the present invention.

Fig. 3 is a schematic diagram of a barrier self-protected area according to an embodiment of the invention.

Fig. 4 is a schematic view of the rotation angle calculation according to the embodiment of the present invention.

FIG. 5 is a schematic diagram of a simple turning model of the intelligent vehicle according to the embodiment of the invention.

Fig. 6 is a schematic diagram of a calculation principle of a fitness function according to an embodiment of the present invention.

Fig. 7 is a schematic diagram of a virtual obstacle according to an embodiment of the present invention.

FIG. 8 is a schematic diagram of an exemplary application of the present invention ((a) a restricted angle path, (b) a local amplification path, (c) a [0,2 ] corner, and (d) a [ alpha-pi/4, alpha + pi/4 ] corner).

Fig. 9 is a schematic diagram of a path planning process of the method according to the embodiment of the present invention.

Detailed Description

In order to make the features and advantages of the present invention comprehensible, embodiments accompanied with figures are described in detail as follows:

it should be noted that the following detailed description is exemplary and is intended to provide further explanation of the disclosure. Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs. It is noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of example embodiments according to the present application. As used herein, the singular forms "a", "an" and "the" are intended to include the plural forms as well, and it should be understood that when the terms "comprises" and/or "comprising" are used in this specification, they specify the presence of stated features, steps, operations, devices, components, and/or combinations thereof, unless the context clearly indicates otherwise.

As shown in fig. 1 to 9, the method for planning local path by obstacle self-protection artificial potential field method based on particle swarm optimization provided in this embodiment includes the following implementation processes:

the method comprises the following steps: initializing parameters of an artificial potential field method, and reading current coordinates, obstacle coordinates and target point coordinates of the unmanned vehicle;

coordinate information of a vehicle in a map is obtained by reading vehicle-mounted sensors such as a camera, a laser radar, a GPS (global positioning system), an Inertial Measurement Unit (IMU) and the like, but obstacles collected by the sensors in the motion of the intelligent vehicle are not regular, the obstacles are simplified into a mass point, stress analysis is carried out on the mass point, a path point obtained by the mass point generates a condition of large corner at the concave-convex part of an irregular obstacle, and the requirement of the motion of the intelligent vehicle cannot be met, so that the invention provides a calibration method of the irregular obstacle, as shown in figure 1, the irregular obstacle is calibrated into a rectangular obstacle, and the maximum value and the minimum value of the horizontal coordinate and the vertical coordinate of the obstacle are respectively x_max,x_min,y_max,y_minThen the coordinates of the four vertices of the rectangle are A (x)_min,y_min)，B(x_max,y_min)，C(x_max,y_max)，D(x_min,y_max). The value of the self-protection radius R of only one obstacle is set for the elongated obstacle, and the surrounding area without the obstacle is also divided into the self-protection area of the obstacle, so that the optimal path cannot be obtained during path planning of the intelligent vehicle, and the obstacle is divided into a plurality of minimum units more reasonably. L in the figure_gTo scale the length of the rectangle, W_dIs the width of the rectangle after calibration. The rectangle is divided into a plurality of squares, and the number N of the squares₀：

Where ceil is an upward rounding function (ensuring that fewer than one square is treated as one obstacle), and the side length of the square is L_o＝W_dIth_oThe center coordinates of each square are:

radius of self-protection of obstacle (slightly larger than

Ensuring that the self-protected zones completely intersect each other surrounding the barrier):

on an unstructured road, the shape of an obstacle existing in the road is uncertain, a safety distance can be set between an intelligent vehicle and the minimum unit of the obstacle through calibrating the obstacle, and an optimal path can be found through an algorithm aiming at the rugged obstacle.

Step two: calculating the angle between the current intelligent vehicle and each obstacle, and calculating repulsion, attraction and resultant force;

after reading the obstacle information of the sensor and establishing the obstacle self-protection area, as shown in fig. 2, the angle between the obstacle and the current position of the unmanned vehicle, the repulsive force of each obstacle to the vehicle, and the attractive force of the target point to the obstacle are calculated, and finally the resultant force is obtained.

Step three: calculating whether the next position enters a position of a barrier self-protection area;

an algorithm principle for judging whether the trolley enters the barrier self-protection area in the next step is shown in fig. 3, each barrier is used as a circle center, the radius is R to set the barrier self-protection area, different values can be set according to different barrier types through the radius, and the intelligent trolley is guaranteed not to collide with the barriers. When the intelligent vehicle moves to the position A, the angle value alpha and the step length of the resultant force of the current path point are determinedAnd L, next, the intelligent vehicle arrives at the position B, the algorithm judges that the position B is in the self-protection area of the obstacle, the resultant force is rotated by theta, the position D can be reached under the action of the new resultant force angle alpha' and the step length, and the intelligent vehicle is adjusted to be away from the obstacle to prevent collision. The new resultant angle determination is shown in fig. 4: the coordinate position of the center of the known obstacle i is

The coordinates of the target point are (x)_G.y_G) The position coordinate of the intelligent vehicle is (x)_C,y_C) And the position of the next path point is as follows according to the resultant force angle alpha and the step length l:

when the algorithm judges that the next position enters the barrier self-protection area, calculating the difference value between the position of the intelligent vehicle and the horizontal coordinate of the barrier

Determining a resultant rotation angle theta according to the positive and negative of the delta x, when the delta x is larger than or equal to 0, indicating that the obstacle is positioned at the right side of the next position, and the next position should rotate towards the left, therefore, adding the theta (the anticlockwise direction is positive) on the basis of the resultant angle alpha, wherein the included angle alpha 'between the resultant force and the horizontal axis is alpha + theta, and when the delta x is smaller than 0, indicating that the obstacle is positioned at the left side of the next position, the included angle alpha' is alpha-theta, and when the next path point is:

the direction of the rotation angle can be determined according to the position relation of the intelligent vehicle and the obstacle, the method can avoid the collinear condition of the intelligent vehicle, the obstacle and the target point, after the obstacle self-protection is added, the direction of the resultant force is changed, namely the direction of the course angle of the intelligent vehicle is changed, in addition, the size of the obstacle self-protection area can be set to different values according to the safety distance between the intelligent vehicle and the obstacles of different types, and the driving safety of the intelligent vehicle is ensured.

Step four: optimizing by calling a particle swarm algorithm, randomly initializing particles, and calculating the fitness value and the optimal value of the initial particles;

the particle swarm algorithm first initializes particles, each having both position and velocity attributes. In this embodiment, the position represents a new resultant force angle α ', the speed represents the variation of α', and each particle calculates a corresponding fitness value according to the update of the speed and the position, and determines the quality of the particle according to the fitness value. In the searching process, the optimal value is divided into individual optimal value and global optimal value, and the individual optimal value is recorded as an individual extreme value alpha'_pbestThe particle groups share the individual extremum information, and the value with the optimal fitness value in the individual extremums in the particle swarm is selected as a global optimal solution and recorded as a global extremum alpha'_qbest. In an iterative process the particles are according to α'_pbestAnd alpha'_qbestAnd adjusting the speed and the position attribute to enable the particles to gradually approach the optimal value, thereby finding the optimal solution. Suppose there is N_lAnd each particle, in each iteration process, the speed and position expression of the particle is as follows:

V_j(t+1)＝wV_j(t)+c₁r₁(α′_pbestj(t)-α′_j(t))+c₂r₂(α′_gbestj(t)-α′_j(t))

α′_j(t+1)＝α′_j(t)+V_j(t+1),j＝1,2,…N_l

in the formula, V_j(t +1) and α'_j(t +1) are the speed and resultant force angle values of the particle j in the t +1 th iteration respectively, the range of the new resultant force angle is to add or subtract any value on the basis of the original resultant force angle, the speed value represents the movement speed of the example, the too fast speed can cause the particle to directly cross the optimal value, and the too small speed value can cause the optimization time of the algorithm to be increased; alpha's'_pbestj(t) is the individual optimum value, α ', for particle j at the t-th iteration'_gbestj(t) is the t-th iteration global optimum; w is an inertia weight value which is generally between 0.1 and 0.4 and reflects the influence degree of the individual extreme value of the particle on the current value; c. C₁And c₂Acceleration factor, table, for individual and group respectivelyThe empirical value generated in the optimization process of the particles and the accumulated empirical values of other particles directly influence the next-step operation track r of each particle₁And r₂Is a random number between 0 and 1.

Step five: adding vehicle steering characteristic constraint optimization, updating the speed and position of each particle after iteration, and calculating a function adaptive value of each particle;

in the existing path planning algorithm, the intelligent vehicle is generally regarded as a movable mass point, and in actual driving, the planned path should meet the motion characteristics of the intelligent vehicle in order to avoid unsafe actions such as sideslip and the like under the motion conditions such as turning, braking and the like of the intelligent vehicle. Depending on the characteristics of the vehicle steering, the maximum steering angle during steering is limited, as shown in fig. 5. In the figure, L is the front and rear wheel base (x)_c,y_cAnd alpha) represents the pose of the intelligent vehicle, (x)_c,y_c) The coordinate of the intelligent vehicle in the two-dimensional space is shown, alpha is the course angle of the intelligent vehicle at the current moment, and is also the resultant force angle in the artificial potential field method. Minimum turning radius R of intelligent vehicle during turning_minThe maximum internal angle of the front wheel is alpha₂Maximum external rotation angle alpha of front wheel₁In the process of turning the vehicle, the inner and outer turning angles are related to the type, the self speed and the acceleration of the vehicle, and v is set_xIs the longitudinal speed during movement, a_yThe expression of the minimum turning radius and the expression of the maximum turning angle, namely the maximum internal corner, are as follows:

the inner wheel rotating angle of the intelligent vehicle is larger than the outer wheel rotating angle in the steering process, the rotating angle does not exceed the maximum value of the inner wheel rotating angle and the outer wheel rotating angle in the steering process, namely the relation between the newly searched resultant force angle value alpha' and the inner rotating angle of the front wheel is as follows:

α-α₂≤α′≤α+α₂

the intelligent vehicle is limited by the maximum rotation angle in the turning process, and the optimization angle value of the obstacle self-protection artificial potential field method based on particle swarm optimization is limited within the maximum rotation angle range of the intelligent vehicle, so that the planned path conforms to the kinematics model of the intelligent vehicle, and a foundation is laid for next-step track tracking.

The fitness function calculation principle is shown in FIG. 6, where b (i) represents the next moment position (x)_n,y_n) To the ith obstacle center

D (i) represents the distance between the next position and the target point, and the mathematical expressions are respectively:

the next position in the path planning process should be far from the center of the obstacle, i.e. the larger (b) (i) the better, and the closer to the target point as possible, i.e. the smaller (d) (i) the better. Combining the two distances to establish a fitness function f (i), wherein the larger the fitness value is, the safer the fitness function is, and the expression is as follows:

however, in a coordinate system, an intelligent vehicle is close to an obstacle and far away from a target point, a fitness function is represented as b (i) which is small, d (i) which is large, so that the difference between b (i) and c (i) is hundreds of times, the calculation result of the fitness value is biased to a certain value, the fitness value of each particle is not representative, the two are normalized into a dimensionless value, the distance between the two is mapped to a range between [0 and 1], and a mapping expression and a fitness function expression are respectively as follows:

f(i)＝b(i)′+c(i)′。

step six: updating the historical optimal value of each particle after iteration, and calculating the global optimal value of the population;

giving the maximum iteration times, the particle speed range and the position optimizing range of the particle swarm algorithm, and giving the number (N) of the particles of the population_l) Randomly generating N_lAngularity value alpha'_jForming an initial population, j ═ 1,2, … N_l. And calculating the fitness value f (i) of each individual in the population to the ith obstacle according to the fitness function given in the fifth step, and adjusting the speed and the position according to the individual extreme value and the global extreme value to enable the particles to gradually approach the optimal value, thereby finding the optimal solution. Recording the individual extremum α 'of each particle'_pbestjAnd global extremum α'_gbestj。

Step seven: stopping iteration and outputting an optimal course angle alpha' when the maximum iteration times are reached or the global optimal value is unchanged;

and comparing the global extreme value with the historical optimal value, updating the speed and the position of the particles, stopping iteration when the maximum iteration times is reached or the global optimal value is unchanged, and outputting the optimal course angle alpha'.

Step eight: returning to the step three to judge whether the position of the next step enters the barrier self-protection area or not after the optimal course angle is obtained;

substituting the output optimal course angle alpha' and step length into a three-path formula, judging whether the next position is in the barrier self-protection area, and returning to the step three for repeated operation if the next position falls into the barrier self-protection area; and if the obstacle is positioned outside the self-protection area of the obstacle, performing the nine-step operation.

Step nine: judging whether the local minimum value point is trapped or not;

in the moving process of the intelligent vehicle, due to the influence of surrounding obstacles, the attractive force and the repulsive force applied when the intelligent vehicle moves to a certain position are the same in size and opposite in direction, the resultant force applied to the intelligent vehicle at the path point is zero, the algorithm judges that the position of the intelligent vehicle is the lowest position of the potential field in the whole environment, if no external force is applied, the intelligent vehicle does not move to the terminal point or wanders around the path point, and the path point is called as a local minimum point.

For the problem of the local minimum, the principle of the invention is as shown in fig. 7, the algorithm firstly judges whether the intelligent vehicle falls into the local minimum, and the path length L in a fixed time interval is set₀Comparing the length L of the path traveled by the intelligent vehicle from the position A to the position B, if L is₀>And L, the intelligent vehicle is trapped into the partial minimum value in the movement process, the method of the invention sets a virtual obstacle at the intersection of the obstacle and the perpendicular bisector of the connection line of the intelligent vehicle, and the repulsion force borne by the intelligent vehicle is changed after the virtual obstacle is added, so that the original balance is broken, the partial minimum value point is jumped out, and the intelligent vehicle continues to advance under the guidance of a target point.

Step ten: judging whether the unmanned vehicle can reach a target point, and connecting an optimized path on the MATLAB when the unmanned vehicle reaches the target point to visualize the motion track of the unmanned vehicle;

the reason for the problem of unreachable target is that obstacles exist around the target point, the distance between the intelligent vehicle and the target point is reduced when the intelligent vehicle approaches the target point, the attraction potential field tends to 0 according to the attraction function of the traditional artificial potential field, but the obstacles exist around the target point, namely the repulsion potential field exists, and the potential field of the target point position is not the lowest in the whole environment, so that the intelligent vehicle vibrates near the target point. If the repulsive force potential field is gradually reduced while the intelligent vehicle gradually approaches the target point, the target point becomes the lowest point of the potential field in the whole environment, and the condition that the target point is unreachable can be avoided. Aiming at the problem that the target cannot be reached, the invention adds a distance factor on the basis of a traditional artificial potential field method model, and the repulsive potential field function after introducing the distance factor is shown as a formula:

where ρ (X, X)_goal)ⁿFor the introduced distance factor, ρ (X, X)_goal) The Euclidean distance between the intelligent vehicle and the target point position is shown, N shows the number of obstacles,

is the distance between the position of the intelligent vehicle and the ith obstacle, rho_oThe influence range of the repulsion force of the obstacle means that the repulsion force potential field is generated only when the intelligent vehicle enters the influence range of the repulsion force. The expression of obtaining the repulsive force by solving the negative gradient of the potential field is as follows:

in order to verify the effectiveness of the method, 14 calibrated obstacles are randomly selected from a map shown in fig. 8, and combined with the algorithm path planning of the invention, fig. 8(a) is a path comparison diagram before and after the optimization angle limitation is adopted under the same obstacle environment, and according to the maximum steering angle range of 35-45 degrees of the front wheels corresponding to the passenger car, the maximum steering angle value is set to be pi/4, and the optimization range of the optimization algorithm is not set to be the whole space, namely [0,2 pi ]. For the convenience of observation, a part of the path is enlarged, and as shown in fig. 8(b), the x-axis and y-axis (600,850) interval paths are selected for comparison, and as can be seen from the results in the figure, the path with the optimal angle range [ α -pi/4, α + pi/4 ] is smoother, the turning radian is smooth, and the oscillation is reduced. As shown in fig. 8(c), the calculated angle difference between two adjacent waypoints in 477 waypoints without setting the optimization angle range is the corner of the intelligent vehicle at each waypoint (taking the counterclockwise direction as the positive direction), the corner of the waypoint without setting the optimization range is more, the corner value of the front waypoint and the rear waypoint is larger, and fig. 8(d) shows that the change of the corner size of the optimization angle value is obviously smaller than that before setting, and the route is smoother.

The optimization ranges of [ alpha-pi/4, alpha + pi/4 ] and [0,2 pi ] are respectively obtained after the random operation for 10 times, the path length, the maximum and the minimum of the rotation angle are obtained, and the comparison result of the average values is shown in the following table. The following table shows that after the optimization range is set, the path length is reduced by 7.8%, the maximum rotation angle (taking the counterclockwise direction as the positive direction) is limited to 3.6634 degrees from 51.1182 degrees, the maximum rotation angle in the negative direction is also limited to 3.6169 degrees from 49.8706 degrees, and the path obtained after the optimization range is limited is smoother and accords with the steering characteristic of the intelligent vehicle.

Comparison of simulation results before and after optimization

The present invention is not limited to the above-mentioned preferred embodiments, and any other various forms of the local path planning method based on the particle swarm optimization for the obstacle self-protection artificial potential field method can be obtained according to the teaching of the present invention.

Claims (10)

Translated fromChinese

1.一种基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法，其特征在于，包括以下步骤：1. a kind of obstacle self-protection artificial potential field method local path planning method based on particle swarm algorithm, is characterized in that, comprises the following steps:步骤S1：初始化人工势场法参数，读取智能车当前坐标、障碍物坐标和目标点坐标，并对不规则障碍物进行标定：将不规则障碍物其标定为矩形障碍物，把矩形分为若干个正方形，并对每个正方形设置障碍物自我保护半径；Step S1: Initialize the parameters of the artificial potential field method, read the current coordinates of the smart car, the coordinates of the obstacle and the coordinates of the target point, and calibrate the irregular obstacles: calibrate the irregular obstacles as rectangular obstacles, and divide the rectangle into Several squares, and set the obstacle self-protection radius for each square;步骤S2：计算智能车当前与每个障碍物的角度，计算斥力、引力、合力：包括计算出障碍物与智能车当前位置的角度以及每个障碍物对车辆的斥力，目标点对障碍物的引力，最终求出合力；Step S2: Calculate the current angle of the smart car and each obstacle, and calculate the repulsive force, gravitational force, and resultant force: including calculating the angle between the obstacle and the current position of the smart car, and the repulsion force of each obstacle to the vehicle, and the target point to the obstacle. Gravity, and finally find the resultant force;步骤S3：判断下一步位置是否进入障碍物自我保护区位置：以每个正方形障碍物为圆心，半径为R设置障碍物自我保护区，以保证智能车与障碍物不发生碰撞；如进入障碍物自我保护区，则执行步骤S4-步骤S8；Step S3: Determine whether the next position enters the obstacle self-protection area. Position: take each square obstacle as the center of the circle and set the obstacle self-protection area as the radius R to ensure that the smart car does not collide with the obstacle; if entering the obstacle Self-protection zone, then execute step S4-step S8;步骤S4：调用粒子群算法寻优，随机初始化粒子，计算初始粒子的适应度值和最优值；Step S4: call the particle swarm algorithm to optimize, initialize the particles randomly, and calculate the fitness value and optimal value of the initial particle;步骤S5：加入车辆转向特性约束寻优，更新迭代后每个粒子的速度和位置，计算每个粒子的函数适应值；Step S5: adding vehicle steering characteristic constraints for optimization, updating the velocity and position of each particle after iteration, and calculating the function fitness value of each particle;步骤S6：更新迭代后每个粒子历史最优值，计算群体的全局最优值；Step S6: update the historical optimal value of each particle after the iteration, and calculate the global optimal value of the group;步骤S7：当达到最大迭代次数或全局最优值不变时停止迭代并输出最优的航向角α′；Step S7: when the maximum number of iterations is reached or the global optimal value remains unchanged, the iteration is stopped and the optimal heading angle α′ is output;步骤S8：得到最优航向角后回到步骤S3判断下一步位置是否进入障碍物自我保护区。Step S8: After obtaining the optimal heading angle, go back to Step S3 to determine whether the next position enters the obstacle self-protection zone.2.根据权利要求1所述的基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法，其特征在于：2. the obstacle self-protection artificial potential field method local path planning method based on particle swarm algorithm according to claim 1, is characterized in that:智能车在运动过程中，判断是否陷入局部最小值点：运动至某一位置时所受的引力和斥力大小相同，方向相反，在此路径点智能车所受的合力为零，智能车该处为整个环境中势场最低位置；During the movement of the smart car, it is judged whether it falls into a local minimum point: when it moves to a certain position, the gravitational and repulsive forces are the same in magnitude and opposite in direction. is the lowest position of the potential field in the whole environment;如果陷入局部最小值点，则通过在障碍物和智能车连线的中垂线交点处设置虚拟障碍物,使智能车跳出局部最小值点。If it falls into the local minimum point, set a virtual obstacle at the intersection of the mid-perpendicular line connecting the obstacle and the smart car to make the smart car jump out of the local minimum point.3.根据权利要求1所述的基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法，其特征在于：通过加入距离因子，以避免目标点不可达的情形。3. The obstacle self-protection artificial potential field method local path planning method based on the particle swarm algorithm according to claim 1, characterized in that: by adding a distance factor, the situation that the target point is unreachable is avoided.4.根据权利要求1所述的基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法，其特征在于：4. the obstacle self-protection artificial potential field method local path planning method based on particle swarm algorithm according to claim 1, is characterized in that:在步骤S1中，将不规则障碍物其标定为矩形障碍物，设障碍物的横坐标、纵坐标最大、最小值分别为x_max,x_min,y_max,y_min，则矩形四个顶点坐标为A(x_min,y_min)，B(x_max,y_min)，C(x_max,y_max)，D(x_min,y_max)，L_g为标定后矩形的长度，W_d为标定后矩形的宽度；把矩形分为若干个正方形，正方形的个数N₀：In step S1, the irregular obstacle is calibrated as a rectangular obstacle, and the maximum and minimum values of the abscissa and ordinate of the obstacle are set as x_max , x_min , y_max , y_min respectively, then the coordinates of the four vertices of the rectangle are is A(x_min , y_min ), B(x_max , y_min ), C(x_max , y_max ), D(x_min , y_max ), L_g is the length of the calibrated rectangle, W_d is the calibration The width of the rear rectangle; the rectangle is divided into several squares, and the number of squares is N₀ :

其中ceil为向上取整函数；where ceil is the round-up function;正方形的边长为L_o＝W_d，第i_o个正方形的中心坐标为：The side length of the square is L_o =W_d , and the center coordinates of the ith square are_:

障碍物自我保护半径为：The obstacle self-protection radius is:

5.根据权利要求4所述的基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法，其特征在于：步骤S3的具体执行过程为：5. the obstacle self-protection artificial potential field method local path planning method based on particle swarm algorithm according to claim 4, is characterized in that: the concrete execution process of step S3 is:设当智能车运动到位置A时，根据当前路径点合力角度值α与步长L，下一步将到达位置B，如果判断位置B在障碍物自我保护区内，则将合力旋转θ，在新合力角α′与步长的作用下可到达位置D，将智能车调整至远离障碍物方向，防止相撞；新合力角度确定方式为：已知障碍物i中心坐标位置为

目标点坐标为(x_G.y_G)，智能车位置坐标为(x_C,y_C)，由合力角度α与步长l，下一路径点位置为:Assume that when the smart car moves to position A, according to the current path point resultant force angle value α and step length L, the next step will reach position B. If it is judged that position B is within the obstacle self-protection area, the resultant force will be rotated by θ. Under the action of the resultant force angle α′ and the step length, the position D can be reached, and the smart car is adjusted to the direction away from the obstacle to prevent collision; the new resultant force angle is determined as follows: the center coordinate position of the known obstacle i is:

The coordinates of the target point are (x_G .y_G ), and the position coordinates of the smart car are (x_C , y_C ). From the resultant force angle α and the step length l, the position of the next path point is:

当判断下一位置进入障碍物自我保护区时，计算智能车位置与障碍物横坐标的差值

根据Δx正负决定合力旋转角θ，当Δx≥0时，表示障碍物位于下一位置右侧，下一位置应该往左旋转，因此在合力角α基础上加θ，此时合力与横轴夹角α′＝α+θ，反之当Δx＜0时表示障碍物位于下一位置左侧，则α′＝α-θ，此时下一路径点为：When judging that the next position enters the obstacle self-protection zone, calculate the difference between the position of the smart car and the abscissa of the obstacle

The resultant rotation angle θ is determined according to the positive and negative Δx. When Δx≥0, it means that the obstacle is located on the right side of the next position, and the next position should be rotated to the left. Therefore, add θ to the resultant angle α. At this time, the resultant force and the horizontal axis The included angle α′=α+θ, on the contrary, when Δx<0, it means that the obstacle is on the left side of the next position, then α′=α-θ, and the next path point is:

6.根据权利要求1所述的基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法，其特征在于：在步骤S3中，障碍物自我保护区的大小能根据智能车和不同类型的障碍物安全距离设置不同的值。6. The obstacle self-protection artificial potential field method local path planning method based on particle swarm algorithm according to claim 1, it is characterized in that: in step S3, the size of obstacle self-protection zone can be based on the intelligent vehicle and different types The obstacle safety distance is set to different values.7.根据权利要求5所述的基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法，其特征在于：7. The obstacle self-protection artificial potential field method local path planning method based on particle swarm algorithm according to claim 5, is characterized in that:步骤S4具体为：Step S4 is specifically:首先初始化粒子，每个粒子具有位置和速度两个属性；其中位置代表的是新合力角α′，速度表示α′变化大小，每个粒子根据速度和位置的更新计算对应适应度值，根据适应度值判断粒子优劣；在搜索过程中最优值分为个体最优与全局最优，个体最优值记为个体极值α′_pbest，粒子群体之间通过分享个体极值信息，选出粒子群中个体极值中适应度值最优的值作为全局最优解，记为全局极值α′_qbest；在迭代过程中粒子根据α′_pbest和α′_qbest调整速度和位置属性，使粒子逐渐靠近最优值，从而找到最优解；假设存在N_l个粒子，每次迭代过程中，粒子的速度和位置表达式为：First initialize the particles, each particle has two attributes of position and speed; where the position represents the new resultant angle α', and the speed represents the change of α', each particle calculates the corresponding fitness value according to the update of the speed and position, according to the The degree value judges the quality of the particle; in the search process, the optimal value is divided into individual optimal and global optimal, and the individual optimal value is recorded as the individual extreme value α′_pbest . The optimal fitness value of the individual extreme values in the particle swarm is regarded as the global optimal solution, denoted as the global extreme value α′_qbest ; in the iterative process, the particles adjust the speed and position attributes according to α′_pbest and α′_qbest , so that the particle Gradually approach the optimal value to find the optimal solution; assuming there are N_l particles, in each iteration process, the particle velocity and position expressions are:V_j(t+1)＝wV_j(t)+c₁r₁(α′_pbestj(t)-α′_j(t))+c₂r₂(α′_gbestj(t)-α′_j(t))V_j (t+1)=wV_j (t)+c₁ r₁ (α′_pbestj (t)-α′_j (t))+c₂ r₂ (α′_gbestj (t)-α′_j ( t))α′_j(t+1)＝α′_j(t)+V_j(t+1),j＝1,2,…N_lα′_j (t+1)=α′_j (t)+V_j (t+1),j=1,2,…N_l式中，V_j(t+1)和α′_j(t+1)分别是粒子j在第t+1次迭代时的速度和合力角度值，新合力角度的范围为在原合力角的基础上加减任意值，速度值表示例子的运动快慢；α′_pbestj(t)是粒子j在第t次迭代个体最优值，α′_gbestj(t)是第t次迭代全局最优值；w为惯性权值，反应粒子的个体极值对现在值的影响程度；c₁和c₂分别是个体和群体的加速度因子，表示粒子自身的寻优过程中产生的经验值与其他粒子积累经验值，直接影响各粒子的下一步运行轨迹，r₁和r₂是0到1之间的随机数；In the formula, V_j (t+1) and α′_j (t+1) are the velocity and resultant angle of particle j at the t+1th iteration, respectively, and the range of the new resultant angle is based on the original resultant angle. Add or subtract any value, and the speed value represents the speed of the movement of the example; α′_pbestj (t) is the individual optimal value of particle j in the t-th iteration, and α′_gbestj (t) is the global optimal value of the t-th iteration; w is The inertia weight reflects the influence of the individual extreme value of the particle on the present value; c₁ and c₂ are the acceleration factors of the individual and the group, respectively, indicating the experience value generated in the optimization process of the particle itself and the accumulated experience value of other particles, Directly affects the next trajectory of each particle, r₁ and r₂ are random numbers between 0 and 1;步骤S5具体为：Step S5 is specifically:设L为前后轮轴距，(x_c,y_c,α)表示智能车位姿，(x_c,y_c)为智能车在二维空间中的坐标，α为当前时刻智能车的航向角，同时也是人工势场法中的合力角；智能车在转弯时存在最小转弯半径R_min，前轮最大内转角为α₂，前轮最大外转角α₁，车辆在转弯的过程中，内外转角与车辆的类型、自身速度和加速度有关，设v_x为运动过程中的纵向速度，a_y为横向加速度，其中最小转弯半径表达式和最大转角即最大内转角表达式为：Let L be the wheelbase of the front and rear wheels, (x_c , y_c , α) represent the posture of the smart car, (x_c , y_c ) are the coordinates of the smart car in two-dimensional space, α is the heading angle of the smart car at the current moment, and at the same time It is also the resultant angle in the artificial potential field method; the smart car has a minimum turning radius R_min when turning, the maximum inner turning angle of the front wheel is α₂ , and the maximum outer turning angle of the front wheel is α₁ . The type of , its own speed and acceleration are related, let v_x be the longitudinal speed during the movement, a_y is the lateral acceleration, and the expression of the minimum turning radius and the maximum turning angle, that is, the maximum inner turning angle, are:

智能车在转向过程中内轮转角比外轮转角大，在转弯的过程中转角不超过内外轮转角的最大值，即重新寻找的合力角度值α′与前轮内转角之间的关系为式为：During the turning process of the smart car, the rotation angle of the inner wheel is larger than that of the outer wheel, and the rotation angle does not exceed the maximum value of the rotation angle of the inner and outer wheels during the turning process. :α-α₂≤α′≤α+α₂α-α₂ ≤α′≤α+α₂智能车在转弯过程有最大转角限制，基于粒子群寻优的障碍物自我保护人工势场法寻优角度值应被限制在智能车最大转角范围内，使规划获得的路径符合智能车运动学模型；The smart car has a maximum turning angle limit during the turning process. The self-protection artificial potential field method for obstacle optimization based on particle swarm optimization should be limited to the maximum turning angle of the smart car, so that the planned path conforms to the smart car kinematics model. ;适应度函数计算中，b(i)表示下一刻位置(x_n,y_n)与第i个障碍物中心

的欧式距离，d(i)表示下一位置与目标点的距离，数学表达式分别为:In the fitness function calculation, b(i) represents the next moment position (x_n , y_n ) and the center of the ith obstacle

The Euclidean distance of , d(i) represents the distance between the next position and the target point, and the mathematical expressions are:

在路径规划过程中下一位置应离障碍物中心远，即b(i)越大越好，且距离目标点尽量近，即d(i)越小越好；将两个距离合并建立适应度函数f(i)，适应度值越大越安全，表达式为:In the process of path planning, the next position should be far away from the center of the obstacle, that is, the larger b(i), the better, and the distance to the target point should be as close as possible, that is, the smaller d(i), the better; the fitness function is established by combining the two distances. f(i), the larger the fitness value, the safer, the expression is:

但坐标系中会出现智能车离障碍物很近，离目标点距离很远，适应度函数表现为b(i)很小，d(i)很大，导致b(i)与c(i)相差几百倍，适应度值计算结果偏向某一数值，每个粒子的适应度值不具代表性，因此将两者进行归一化处理为无量纲的值，把两者的距离都映射到[0,1]之间，映射表达式及适应度函数表达式分别为:However, in the coordinate system, the smart car will be very close to the obstacle and far away from the target point. The fitness function shows that b(i) is very small and d(i) is very large, resulting in b(i) and c(i) The difference is hundreds of times, the calculation result of the fitness value is biased to a certain value, and the fitness value of each particle is not representative, so the two are normalized to a dimensionless value, and the distance of both is mapped to [ 0, 1], the mapping expression and the fitness function expression are:

f(i)＝b(i)′+c(i)′；f(i)=b(i)′+c(i)′;步骤S6具体为：Step S6 is specifically:给定粒子群算法最大迭代次数，粒子速度范围，位置寻优范围，并给定种群粒子数量(N_l),随机产生N_l个角度值α′_j构成初始种群，j＝1,2,…N_l；根据步骤S5给出的适应度函数计算种群中每个个体对第i个障碍物适应度值f(i)，根据个体极值和全局极值调整速度和位置，使粒子逐渐接近最优值，从而找到最优解；记录各个粒子的个体极值α′_pbestj及全局极值α′_gbestj；Given the maximum number of iterations of the particle swarm algorithm, the range of particle velocity, the range of position optimization, and the number of particles in the population (N_l ), N_l angle values α′_j are randomly generated to form the initial population, j=1,2,… N_l ; calculate the fitness value f(i) of each individual in the population to the ith obstacle according to the fitness function given in step S5, adjust the speed and position according to the individual extreme value and the global extreme value, so that the particle gradually approaches the most figure of merit, so as to find the optimal solution; record the individual extremum α′_pbestj and the global extremum α′_gbestj of each particle;步骤S7具体为：Step S7 is specifically:将全局极值与历史最优值对比，更新粒子的速度与位置，当达到最大迭代次数或全局最优值不变时停止迭代并输出最优的航向角α′；Compare the global extremum with the historical optimal value, update the speed and position of the particle, stop the iteration and output the optimal heading angle α' when the maximum number of iterations is reached or the global optimal value remains unchanged;在步骤S8中，将输出最优的航向角α′和步长带入步骤S3路径公式，并返回步骤S3，判断下一步位置是否进入障碍物自我保护区位置。In step S8, the output optimal heading angle α' and step length are brought into the path formula of step S3, and return to step S3 to determine whether the next step position enters the position of the obstacle self-protection zone.8.根据权利要求2所述的基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法，其特征在于：通过设置固定的时间间隔内的路径长度L₀与智能车从位置A运动到位置B走过路径长度L进行比较，如果L₀>L，则判断智能车在运动过程中陷入局部最小值。8. The obstacle self-protection artificial potential field method local path planning method based on particle swarm algorithm according to claim₂ , it is characterized in that: by setting the path length L in the fixed time interval and the intelligent vehicle moving from position A Go to position B and walk through the path length L for comparison. If L₀ >L, it is judged that the smart car falls into the local minimum value during the movement.9.根据权利要求3所述的基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法，其特征在于：9. The obstacle self-protection artificial potential field method local path planning method based on particle swarm algorithm according to claim 3, is characterized in that:引入距离因子后斥力势场函数如下式所示：After introducing the distance factor, the repulsive potential field function is as follows:

其中ρ(X,X_goal)ⁿ为引入的距离因子，ρ(X,X_goal)表示智能车与目标点位置的欧式距离，k_rep为斥力场增益系数，N表示障碍物个数，

为智能车位置与第i个障碍物的距离，ρ_o为障碍物斥力的影响范围，表示只有智能车进入斥力影响范围内才产生斥力势场；对势场求负梯度得到斥力的表达式为：where ρ(X,X_goal )ⁿ is the introduced distance factor, ρ(X, X_goal ) represents the Euclidean distance between the smart car and the target point, k_rep is the repulsion field gain coefficient, N represents the number of obstacles,

is the distance between the position of the smart car and the i-th obstacle, and ρ_o is the influence range of the obstacle repulsion, which means that the repulsion potential field is generated only when the smart car enters the influence range of the repulsion force; the expression of the repulsion force obtained by calculating the negative gradient of the potential field is: :

10.根据权利要求1所述的基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法，其特征在于：采用MATLAB上连线优化后的路径，将无人车运动轨迹可视化。10 . The particle swarm algorithm-based method for local path planning using artificial potential field method for self-protection of obstacles according to claim 1 , wherein the path optimized by the connection on MATLAB is used to visualize the trajectory of the unmanned vehicle.

Images