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

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

Info

Publication number
CN113805597A
CN113805597A CN202111146883.3A CN202111146883A CN113805597A CN 113805597 A CN113805597 A CN 113805597A CN 202111146883 A CN202111146883 A CN 202111146883A CN 113805597 A CN113805597 A CN 113805597A
Authority
CN
China
Prior art keywords
obstacle
intelligent vehicle
value
angle
self
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202111146883.3A
Other languages
Chinese (zh)
Other versions
CN113805597B (en
Inventor
张卫波
温珍林
封士宇
黄晓军
黄赐坤
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Fuzhou University
Original Assignee
Fuzhou University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Fuzhou University filed Critical Fuzhou University
Priority to CN202111146883.3A priority Critical patent/CN113805597B/en
Publication of CN113805597A publication Critical patent/CN113805597A/en
Application granted granted Critical
Publication of CN113805597B publication Critical patent/CN113805597B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0217—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with energy consumption, time reduction or distance reduction criteria
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Control Of Driving Devices And Active Controlling Of Vehicle (AREA)

Abstract

The invention provides a particle swarm algorithm-based obstacle self-protection artificial potential field method local path planning method, which introduces a 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 limits the optimization range by adopting the particle swarm algorithm, finds a course angle according with the turning characteristic of the intelligent vehicle, obtains an optimal course angle through continuous iteration of particles, establishes a particle swarm obstacle self-protection artificial potential field method to avoid obstacles and finds an optimal path according with the turning constraint of the intelligent vehicle.

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 xmax,xmin,ymax,yminThen the coordinates of the four vertices of the rectangle are A (x)min,ymin)，B(xmax,ymin)，C(xmax,ymax)，D(xmin,ymax)，LgTo scale the length of the rectangle, WdThe width of the calibrated rectangle is obtained; the rectangle is divided into a plurality of squares, and the number N of the squares0：
Figure BDA0003284930710000031
Wherein ceil is an rounding-up function;
side length of square Lo＝WdIthoThe center coordinates of each square are:
Figure BDA0003284930710000032
the self-protection radius of the barrier is as follows:
Figure BDA0003284930710000033
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
Figure BDA0003284930710000034
The coordinates of the target point are (x)G.yG) The position coordinate of the intelligent vehicle is (x)C,yC) And the position of the next path point is as follows according to the resultant force angle alpha and the step length l:
Figure BDA0003284930710000041
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
Figure BDA0003284930710000042
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:
Figure BDA0003284930710000043
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 NlAnd each particle, in each iteration process, the speed and position expression of the particle is as follows:
Vj(t+1)＝wVj(t)+c1r1(α′pbestj(t)-α′j(t))+c2r2(α′gbestj(t)-α′j(t))
α′j(t+1)＝α′j(t)+Vj(t+1),j＝1,2,…Nl
in the formula, Vj(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. C1And c2Acceleration 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 particle1And r2Is a random number between 0 and 1;
step S5 specifically includes:
let L be the front-rear wheel base, (x)c,ycAnd alpha) represents the pose of the intelligent vehicle, (x)c,yc) 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 turningminThe maximum internal angle of the front wheel is alpha2Maximum external rotation angle alpha of front wheel1In 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 setxIs the longitudinal speed during movement, ayThe expression of the minimum turning radius and the expression of the maximum turning angle, namely the maximum internal corner, are as follows:
Figure BDA0003284930710000051
Figure BDA0003284930710000052
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:
α-α2≤α′≤α+α2
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 momentn,yn) To the ith obstacle center
Figure BDA0003284930710000053
D (i) represents the distance between the next position and the target point, and the mathematical expressions are respectively:
Figure BDA0003284930710000054
Figure BDA0003284930710000061
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:
Figure BDA0003284930710000062
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:
Figure BDA0003284930710000063
Figure BDA0003284930710000064
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 populationl) Randomly generating NlAngularity value alpha'jForming an initial population, j ═ 1,2, … Nl(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 interval0Compared with the length L of the path taken by the intelligent vehicle from the position A to the position BIf L is0>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:
Figure BDA0003284930710000071
where ρ (X, X)goal)nFor the introduced distance factor, ρ (X, X)goal) Euclidean distance k representing position of intelligent vehicle and target pointrepIs a repulsive force field gain coefficient, N represents the number of obstacles,
Figure BDA0003284930710000072
is the distance between the position of the intelligent vehicle and the ith obstacle, rhooThe 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:
Figure BDA0003284930710000073
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 xmax,xmin,ymax,yminThen the coordinates of the four vertices of the rectangle are A (x)min,ymin)，B(xmax,ymin)，C(xmax,ymax)，D(xmin,ymax). 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 figuregTo scale the length of the rectangle, WdIs the width of the rectangle after calibration. The rectangle is divided into a plurality of squares, and the number N of the squares0：
Figure BDA0003284930710000091
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 Lo＝WdIthoThe center coordinates of each square are:
Figure BDA0003284930710000092
radius of self-protection of obstacle (slightly larger than
Figure BDA0003284930710000093
Ensuring that the self-protected zones completely intersect each other surrounding the barrier):
Figure BDA0003284930710000094
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
Figure BDA0003284930710000101
The coordinates of the target point are (x)G.yG) The position coordinate of the intelligent vehicle is (x)C,yC) And the position of the next path point is as follows according to the resultant force angle alpha and the step length l:
Figure BDA0003284930710000102
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
Figure BDA0003284930710000103
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:
Figure BDA0003284930710000104
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 NlAnd each particle, in each iteration process, the speed and position expression of the particle is as follows:
Vj(t+1)＝wVj(t)+c1r1(α′pbestj(t)-α′j(t))+c2r2(α′gbestj(t)-α′j(t))
α′j(t+1)＝α′j(t)+Vj(t+1),j＝1,2,…Nl
in the formula, Vj(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. C1And c2Acceleration 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 particle1And r2Is 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,ycAnd alpha) represents the pose of the intelligent vehicle, (x)c,yc) 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 turningminThe maximum internal angle of the front wheel is alpha2Maximum external rotation angle alpha of front wheel1In 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 setxIs the longitudinal speed during movement, ayThe expression of the minimum turning radius and the expression of the maximum turning angle, namely the maximum internal corner, are as follows:
Figure BDA0003284930710000111
Figure BDA0003284930710000121
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:
α-α2≤α′≤α+α2
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,yn) To the ith obstacle center
Figure BDA0003284930710000122
D (i) represents the distance between the next position and the target point, and the mathematical expressions are respectively:
Figure BDA0003284930710000123
Figure BDA0003284930710000124
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:
Figure BDA0003284930710000125
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:
Figure BDA0003284930710000126
Figure BDA0003284930710000131
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 populationl) Randomly generating NlAngularity value alpha'jForming an initial population, j ═ 1,2, … Nl. 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 set0Comparing the length L of the path traveled by the intelligent vehicle from the position A to the position B, if L is0>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:
Figure BDA0003284930710000141
where ρ (X, X)goal)nFor 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,
Figure BDA0003284930710000142
is the distance between the position of the intelligent vehicle and the ith obstacle, rhooThe 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:
Figure BDA0003284930710000143
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
Figure BDA0003284930710000151
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)

1. 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.
2. The particle swarm algorithm-based obstacle self-protection artificial potential field method local path planning method according to claim 1, characterized in that:
the intelligent vehicle judges whether the intelligent vehicle falls into a local minimum 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.
3. The particle swarm algorithm-based obstacle self-protection artificial potential field method local path planning method according to claim 1, characterized in that: by adding the distance factor, the situation that the target point is inaccessible is avoided.
4. The particle swarm algorithm-based obstacle self-protection artificial potential field method local path planning method according to claim 1, characterized in that:
in step S1, the irregular obstacle is calibrated as a rectangular obstacle, and the abscissa and ordinate of the obstacle are set to be the maximumThe maximum and minimum values are xmax,xmin,ymax,yminThen the coordinates of the four vertices of the rectangle are A (x)min,ymin)，B(xmax,ymin)，C(xmax,ymax)，D(xmin,ymax)，LgTo scale the length of the rectangle, WdThe width of the calibrated rectangle is obtained; the rectangle is divided into a plurality of squares, and the number N of the squares0：
Figure FDA0003284930700000021
Wherein ceil is an rounding-up function;
side length of square Lo＝WdIthoThe center coordinates of each square are:
Figure FDA0003284930700000022
the self-protection radius of the barrier is as follows:
Figure FDA0003284930700000023
5. the particle swarm algorithm-based obstacle self-protection artificial potential field method local path planning method according to claim 4, characterized in that: the specific execution process of step S3 is:
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
Figure FDA0003284930700000025
The coordinates of the target point are (x)G.yG) The position coordinate of the intelligent vehicle is (x)C,yC) And the position of the next path point is as follows according to the resultant force angle alpha and the step length l:
Figure FDA0003284930700000024
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
Figure FDA0003284930700000026
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:
Figure FDA0003284930700000031
6. the particle swarm algorithm-based obstacle self-protection artificial potential field method local path planning method according to claim 1, characterized in that: 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.
7. The particle swarm algorithm-based obstacle self-protection artificial potential field method local path planning method according to claim 5, characterized in that:
step S4 specifically includes:
firstly, initializing particles, wherein each particle has two attributes of position and speed; in which the position represents the new resultant angle alpha', speedometerIndicating the change size of alpha', calculating a corresponding fitness value of each particle according to the updating of the speed and the position, and judging 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(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 NlAnd each particle, in each iteration process, the speed and position expression of the particle is as follows:
Vj(t+1)＝wVj(t)+c1r1(α′pbestj(t)-α′j(t))+c2r2(α′gbestj(t)-α′j(t))
α′j(t+1)＝α′j(t)+Vj(t+1),j＝1,2,…Nl
in the formula, Vj(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. C1And c2Acceleration 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 particle1And r2Is a random number between 0 and 1;
step S5 specifically includes:
let L be the front-rear wheel base, (x)c,ycAnd alpha) represents the pose of the intelligent vehicle, (x)c,yc) Is the coordinate of the intelligent vehicle in the two-dimensional space, alpha is the course angle of the intelligent vehicle at the current moment,simultaneously, the method is also a resultant force angle in an artificial potential field method; minimum turning radius R of intelligent vehicle during turningminThe maximum internal angle of the front wheel is alpha2Maximum external rotation angle alpha of front wheel1In 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 setxIs the longitudinal speed during movement, ayThe expression of the minimum turning radius and the expression of the maximum turning angle, namely the maximum internal corner, are as follows:
Figure FDA0003284930700000041
Figure FDA0003284930700000042
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:
α-α2≤α′≤α+α2
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 momentn,yn) To the ith obstacle center
Figure FDA0003284930700000043
D (i) represents the distance between the next position and the target point, and the mathematical expressions are respectively:
Figure FDA0003284930700000044
Figure FDA0003284930700000045
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:
Figure FDA0003284930700000051
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:
Figure FDA0003284930700000052
Figure FDA0003284930700000053
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 populationl) Randomly generating NlAngularity value alpha'jForming an initial population, j ═ 1,2, … Nl(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 valueGradually approaching the particles to an optimal value so as to find an 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.
8. The particle swarm algorithm-based obstacle self-protection artificial potential field method local path planning method according to claim 2, characterized in that: by setting the path length L in a fixed time interval0Comparing the length L of the path traveled by the intelligent vehicle from the position A to the position B, if L is0>And L, judging that the intelligent vehicle falls into the local minimum value in the motion process.
9. The particle swarm algorithm-based obstacle self-protection artificial potential field method local path planning method according to claim 3, characterized in that:
the repulsive force potential field function after introducing the distance factor is shown as follows:
Figure FDA0003284930700000061
where ρ (X, X)goal)nFor the introduced distance factor, ρ (X, X)goal) Euclidean distance k representing position of intelligent vehicle and target pointrepIs a repulsive force field gain coefficient, N represents the number of obstacles,
Figure FDA0003284930700000062
is the distance between the position of the intelligent vehicle and the ith obstacle, rhooIs a disorder ofThe influence range of the object repulsion force means that a 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:
Figure FDA0003284930700000063
10. the particle swarm algorithm-based obstacle self-protection artificial potential field method local path planning method according to claim 1, characterized in that: and (4) adopting the optimized path connected with the MATLAB to visualize the motion track of the unmanned vehicle.
CN202111146883.3A 2021-09-28 2021-09-28 Obstacle self-protection artificial potential field method local path planning method based on particle swarm optimization Active CN113805597B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111146883.3A CN113805597B (en) 2021-09-28 2021-09-28 Obstacle self-protection artificial potential field method local path planning method based on particle swarm optimization

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111146883.3A CN113805597B (en) 2021-09-28 2021-09-28 Obstacle self-protection artificial potential field method local path planning method based on particle swarm optimization

Publications (2)

Publication Number Publication Date
CN113805597A true CN113805597A (en) 2021-12-17
CN113805597B CN113805597B (en) 2023-04-11

Family

ID=78897037

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111146883.3A Active CN113805597B (en) 2021-09-28 2021-09-28 Obstacle self-protection artificial potential field method local path planning method based on particle swarm optimization

Country Status (1)

Country Link
CN (1) CN113805597B (en)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114913708A (en) * 2022-07-18 2022-08-16 深圳市华睿智兴信息科技有限公司 Parking path guiding system and method for intelligent parking lot
WO2022228358A1 (en) * 2021-04-25 2022-11-03 广州汽车集团股份有限公司 Autonomous driving obstacle avoidance method and system, and storage medium
CN115576333A (en) * 2022-12-08 2023-01-06 青岛科技大学 Optimal direction obstacle avoidance strategy
CN118225105A (en) * 2024-05-24 2024-06-21 哈尔滨工业大学（威海） Textile workshop logistics AGV navigation method based on multi-source information perception

Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2001154706A (en) * 1999-11-29 2001-06-08 Japan Atom Energy Res Inst Method for generating route of mobile object
US20110166737A1 (en) * 2008-09-03 2011-07-07 Murata Machinery, Ltd. Route planning method, route planning device and autonomous mobile device
CN106843235A (en) * 2017-03-31 2017-06-13 深圳市靖洲科技有限公司 It is a kind of towards the Artificial Potential Field path planning without person bicycle
CN109685286A (en) * 2019-01-14 2019-04-26 哈尔滨工程大学 USV is based on the collision-avoidance planning method for improving ant group optimization under unknown static-obstacle environment
CN110208819A (en) * 2019-05-14 2019-09-06 江苏大学 A kind of processing method of multiple barrier three-dimensional laser radar data
FR3084630A1 (en) * 2018-07-31 2020-02-07 Psa Automobiles Sa METHOD FOR PLANNING THE OPTIMAL TRAJECTORY OF AN AUTONOMOUS VEHICLE AND AUTONOMOUS VEHICLE EQUIPPED WITH AN ON-BOARD COMPUTER FOR IMPLEMENTING SAID METHOD
US20200150666A1 (en) * 2018-11-13 2020-05-14 Zebra Technologies Corporation Method, system and apparatus for obstacle handling in navigational path generation
CN111736611A (en) * 2020-07-06 2020-10-02 中国计量大学 Mobile robot path planning method based on A-star algorithm and artificial potential field algorithm
CN112161627A (en) * 2020-09-23 2021-01-01 同济大学 Intelligent path planning method for fire-fighting robot
CN112179367A (en) * 2020-09-25 2021-01-05 广东海洋大学 Intelligent autonomous navigation method based on deep reinforcement learning
CN112596525A (en) * 2020-12-16 2021-04-02 中国地质大学（武汉） Robot path planning method based on genetic algorithm and improved artificial potential field method
CN113189984A (en) * 2021-04-16 2021-07-30 哈尔滨理工大学 Unmanned ship path planning method based on improved artificial potential field method
CN113342047A (en) * 2021-06-23 2021-09-03 大连大学 Unmanned aerial vehicle path planning method for improving artificial potential field method based on obstacle position prediction in unknown environment

Patent Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2001154706A (en) * 1999-11-29 2001-06-08 Japan Atom Energy Res Inst Method for generating route of mobile object
US20110166737A1 (en) * 2008-09-03 2011-07-07 Murata Machinery, Ltd. Route planning method, route planning device and autonomous mobile device
CN106843235A (en) * 2017-03-31 2017-06-13 深圳市靖洲科技有限公司 It is a kind of towards the Artificial Potential Field path planning without person bicycle
FR3084630A1 (en) * 2018-07-31 2020-02-07 Psa Automobiles Sa METHOD FOR PLANNING THE OPTIMAL TRAJECTORY OF AN AUTONOMOUS VEHICLE AND AUTONOMOUS VEHICLE EQUIPPED WITH AN ON-BOARD COMPUTER FOR IMPLEMENTING SAID METHOD
US20200150666A1 (en) * 2018-11-13 2020-05-14 Zebra Technologies Corporation Method, system and apparatus for obstacle handling in navigational path generation
CN109685286A (en) * 2019-01-14 2019-04-26 哈尔滨工程大学 USV is based on the collision-avoidance planning method for improving ant group optimization under unknown static-obstacle environment
CN110208819A (en) * 2019-05-14 2019-09-06 江苏大学 A kind of processing method of multiple barrier three-dimensional laser radar data
CN111736611A (en) * 2020-07-06 2020-10-02 中国计量大学 Mobile robot path planning method based on A-star algorithm and artificial potential field algorithm
CN112161627A (en) * 2020-09-23 2021-01-01 同济大学 Intelligent path planning method for fire-fighting robot
CN112179367A (en) * 2020-09-25 2021-01-05 广东海洋大学 Intelligent autonomous navigation method based on deep reinforcement learning
CN112596525A (en) * 2020-12-16 2021-04-02 中国地质大学（武汉） Robot path planning method based on genetic algorithm and improved artificial potential field method
CN113189984A (en) * 2021-04-16 2021-07-30 哈尔滨理工大学 Unmanned ship path planning method based on improved artificial potential field method
CN113342047A (en) * 2021-06-23 2021-09-03 大连大学 Unmanned aerial vehicle path planning method for improving artificial potential field method based on obstacle position prediction in unknown environment

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
J. BATISTA等: "Trajectory Planning Using Artificial Potential Fields with Metaheuristics", 《IEEE LATIN AMERICA TRANSACTIONS》 *
XIAO-JIE GU等: "Optimization of Trajectories Based on APF-PSO with Radar Threats", 《2011 IEEE INTERNATIONAL CONFERENCE ON SIGNAL PROCESSING, COMMUNICATIONS AND COMPUTING (ICSPCC)》 *
YIFAN CAI等: "A Potential Field-based PSO Approach for Cooperative Target Searching of Multi-robots", 《PROCEEDING OF THE 11TH WORLD CONGRESS ON INTELLIGENT CONTROL AND AUTOMATION》 *
张卫波等: "改进RRT算法在复杂环境下智能车路径规划中的应用", 《中国公路学报》 *
金敬: "Dijkstra改进算法在机器人避障问题的应用", 《价值工程》 *
陈彦杰: "局部环境增量采样的服务机器人路径规划", 《仪器仪表学报》 *

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2022228358A1 (en) * 2021-04-25 2022-11-03 广州汽车集团股份有限公司 Autonomous driving obstacle avoidance method and system, and storage medium
CN114913708A (en) * 2022-07-18 2022-08-16 深圳市华睿智兴信息科技有限公司 Parking path guiding system and method for intelligent parking lot
CN114913708B (en) * 2022-07-18 2022-10-28 深圳市华睿智兴信息科技有限公司 Parking path guiding system and method for intelligent parking lot
CN115576333A (en) * 2022-12-08 2023-01-06 青岛科技大学 Optimal direction obstacle avoidance strategy
CN118225105A (en) * 2024-05-24 2024-06-21 哈尔滨工业大学（威海） Textile workshop logistics AGV navigation method based on multi-source information perception

Also Published As

Publication number Publication date
CN113805597B (en) 2023-04-11

Similar Documents

Publication Publication Date Title
CN113805597B (en) Obstacle self-protection artificial potential field method local path planning method based on particle swarm optimization
CN110320933B (en) Unmanned aerial vehicle obstacle avoidance movement planning method under cruise task
CN110018689B (en) Dynamic window-based multi-virtual target point global dynamic path planning algorithm
Chu et al. Local path planning for off-road autonomous driving with avoidance of static obstacles
US11498574B2 (en) Learning device, learning method, and storage medium
Zhao et al. Dynamic motion planning for autonomous vehicle in unknown environments
CN104635233B (en) Objects in front state estimation and sorting technique based on vehicle-mounted millimeter wave radar
CN112577506B (en) Automatic driving local path planning method and system
WO2020136978A1 (en) Path determination method
Barua et al. A self-driving car implementation using computer vision for detection and navigation
CN111508282B (en) Low-altitude unmanned farmland operation flight obstacle conflict detection method
Zhuge et al. A novel dynamic obstacle avoidance algorithm based on collision time histogram
CN115857504A (en) DWA-based robot local path planning method, equipment and storage medium in narrow environment
CN113848914A (en) Collision coefficient artificial potential field method local path planning method in dynamic environment
CN115562290A (en) Robot path planning method based on A-star penalty control optimization algorithm
CN113467476A (en) Non-collision detection rapid stochastic tree global path planning method considering corner constraint
Huang et al. Path tracking based on improved pure pursuit model and pid
CN113701777B (en) High-precision map lane associated trajectory line automatic generation method based on space vector
Park et al. Path generation algorithm based on crash point prediction for lane changing of autonomous vehicles
Park et al. Optimal Path Planning for Autonomous Vehicles Using Artificial Potential Field Algorithm
CN112665592A (en) Space-time path planning method based on multiple intelligent agents
Björnberg Shared control for vehicle teleoperation with a virtual environment interface
TW202334613A (en) Method for searching a path by using a three-dimensional reconstructed map
Tang et al. Hierarchical Path Planning based on PPO for UVs on 3D Off-Road Terrain
Jafari et al. Reactive path planning for emergency steering maneuvers on highway roads

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant