CN114077255B - A Pathfinding Method for Intelligent Vehicles Based on Ellipse Model Artificial Potential Field Method - Google Patents

Abstract

The invention discloses a method of finding a way for a smart car based on an ellipse model artificial potential field method, which includes constructing a corresponding map, establishing a coordinate system, determining the position coordinates of obstacles and the position of a target point, and then judging whether the smart car has reached the end point. When the end point is reached, the path planning is completed and the final path is generated; if the end point is not reached, the artificial potential field method is used for path planning, and the gravity, repulsion, and resultant force and the direction of the current position of the smart car are calculated to determine whether it is trapped in a local area. minima problem. The advantage is that in a complex environment, the ellipse model is used to add a virtual target point. The gravitational force generated by the virtual target point will change the state of the robot at the local minimum. By continuously correcting the position of the virtual target point on the ellipse model, the smart car will jump out Locally balanced regions, while greatly reducing the time cost of the algorithm.

Description

A Pathfinding Method for Intelligent Vehicles Based on Ellipse Model Artificial Potential Field Method

technical field

The invention relates to the technical field of automatic pathfinding, in particular to an intelligent vehicle pathfinding method based on an ellipse model artificial potential field method.

Background technique

A smart car is a smart car that senses the road environment through on-board sensors, automatically plans a driving route, and controls the vehicle to reach a predetermined target. Integrating many technologies such as automatic control, architecture, artificial intelligence, and visual computing, it is a product of the highly developed computer science, pattern recognition, and intelligent control technologies. It is also an important symbol to measure a country's scientific research strength and industrial level. The field of national economy has broad application prospects. In recent years, mobile robots and various unmanned aerial vehicles have been widely developed and applied in various industries, and have become another research hotspot. Path planning technology is one of the most basic and important technologies in robot-related research. Path planning means that the robot searches for an obstacle-avoiding path from the starting point to the end point in its workspace according to some specific indicators (such as the least time spent, the shortest walking path, etc.). The research on path planning began in the 1970s. Since then, the research on it by major robot research and development countries has never stopped, and it has achieved remarkable results so far. At present, the commonly used path planning methods are divided into traditional algorithms and intelligent bionic algorithms. Among them, the traditional artificial potential field method is widely used because of its simple and convenient calculation.

The traditional artificial potential field method assumes that the target point generates gravitational force on the smart car, and obstacles generate repulsive force on the smart car, so that the smart car walks in the direction of declining force, just like water flowing down. The advantage of this algorithm is that its structure is simple, which is conducive to the real-time performance of the underlying control, which can greatly reduce the amount of calculation and calculation time, and generate a relatively smooth path, which is conducive to maintaining the stability of the automatic driving vehicle. The problems of this algorithm are also obvious. One is the problem that the target is not reachable. If the distance between the smart car and the target point is very far, the gravitational force will become extremely large, while the repulsive force will be smaller or even negligible. The smart car is likely to collide with obstacles on the way forward, causing the smart car to fail to find its way. When there is a large obstacle near the target point, the smart car will be greatly repulsed by the obstacle when it approaches the target. resulting in failure to reach the end point. The second is the local minimum problem. The zero potential energy domain problem is the bottleneck problem of the artificial potential field method. The reason for the zero potential energy point is that there are many obstacle points in the working area of the smart car, and the location distribution is relatively special, which may be in the working area. At a certain point, the attractive force and the repulsive force are just equal in size and opposite in direction, and the force on the smart car is zero, and the smart car will swing back and forth near this point and cannot reach the destination, which seriously affects the operating efficiency of the smart car. Lee, J et al. used the arbitrary force algorithm to establish a new artificial potential field function to control the obstacle avoidance planning of mobile robots, and at the same time solved the problem of symmetrical ordering of robots, obstacles, and target points. Li.Chunshu et al. proposed a method of "walking along the edge" with a memory function. This method jumps out of the local minimum point of the artificial potential field method by walking along the edge of the obstacle, and records and analyzes the local minimum point that the robot walks. Determine whether the target point is surrounded by obstacles, so as to prevent the robot from oscillating back and forth or turning the target point in circles. Although these methods solve the shortcomings of the artificial potential field method to a certain extent, the smart car often cannot achieve a good obstacle avoidance effect in a complex environment, cannot obtain the optimal path, or the path curve is tortuous.

Based on the traditional artificial potential field, the present invention solves the problems that the potential field method cannot reach the target point and falls into a local minimum in the process of path planning by improving the repulsion function and establishing an ellipse model to set a virtual target point.

Contents of the invention

The purpose of the present invention is to solve the problems in the prior art, and propose a smart car path-finding method based on the ellipse model artificial potential field method.

In order to achieve the above object, the present invention adopts following technical scheme: a kind of intelligent vehicle pathfinding method based on ellipse model artificial potential field method, comprises the following steps:

Step 1. Construct the corresponding map according to the area where the smart car is located, grid the map and establish a coordinate system, and determine the coordinates of the obstacle position and the position of the target point in the map;

Step 2. Determine whether the smart car has reached the end point;

Step 3. If the end point is reached, path planning is completed to generate the final path; if the end point is not reached, path planning is performed using the artificial potential field method;

Step 4. Calculate the magnitude and direction of the gravitational force, repulsive force, and resultant force on the current position of the smart car;

Step 5, judging whether to fall into the local minimum problem;

Step 6. If you are not stuck in the local minimum, return to step 2 to continue the judgment and path planning. If you fall into the local minimum problem, build an ellipse model based on the local minimum problem point, add a virtual target point, and increase the direction of the end position. The attractiveness of smart cars, thus jumping out of the local minimum problem;

Step 7, use the ellipse model artificial potential field method to calculate again until the end point is reached;

Step 8. Perform secondary optimization on the planned path, check the path nodes backwards from the starting point, and judge whether it can be reached in a straight line. If it can be reached in a straight line, delete the path from the starting point to the node; if it cannot be reached in a straight line, then Take the node position as the starting point and continue to judge whether it can be reached in a straight line, thereby simplifying the path.

In the above-mentioned smart car path-finding method based on the artificial potential field method of the ellipse model, the artificial potential field method is used to avoid the phenomenon that the smart car is inaccessible to the target during the movement process. Plan a safe path from the starting point to the target point, and judge whether the smart car is stuck in a local minimum problem, including:

Establish the XOY coordinate system with the map, assuming that the coordinate of the robot falling into local equilibrium is P _min (x _min , y _min ), and the coordinate of the end point is P _goal (x _goal , y _goal ), then the coordinate position of the midpoint of these two points is P _b for:

Take the connection between P _min and P _goal as the new axis X', take P _b as the origin, and establish the axis Y' perpendicular to X' through the origin. The new coordinate system is obtained by rotating and translating the original coordinate system, as follows Mode:

in is the rotation matrix, /> is the translation matrix;

The ellipse equation is established in the new coordinate system as follows:

Focal length 2c=|P _min -P _goal |, a=ψc, ψ is the major axis factor, a ² =b ² +c ² .

Take the orthogonal point of the ellipse and the X' axis as the initial virtual target point to increase the attractiveness of the end position to the smart car, thus breaking the local minimum state. The smart car walks according to this virtual target point. If it detects that it falls into local equilibrium again When , a new ellipse model is constructed to find a new fictitious target point.

In the above-mentioned smart car pathfinding method based on the ellipse model artificial potential field method, the gravitational function of the artificial potential field method is as follows:

In the formula, X _g represents the position of the target point (x _g , y _g ); λ ₁ represents the gravitational constant; ρ(X, X _g ) is the geometric distance between the current position of the smart car and the terminal position;

The gravitational force is obtained by taking the first derivative of the distance in the gravitational force function:

Similarly, the repulsion potential function is as follows:

In the formula, X ₀ = (x ₀ , y ₀ ) represents the position of the obstacle; λ ₂ represents the repulsion field constant; ρ ₀ represents the radius of influence of the obstacle; ρ(X, X ₀ ) is the distance between the smart car and the obstacle;

Find the first derivative of the repulsion function with respect to the distance, that is, repulsion:

Therefore, the resultant force F _total (X) received by the smart car in space is:

F _total (X)＝F _att (X)+F _rep (X)

The problem that the target point cannot be reached is because the repulsion generated by the obstacles around the target point is greater than the gravitational force of the target point;

Therefore, based on the traditional repulsion function, the distance factor ρ(X,X _g ) between the current point and the target point is introduced to reduce the repulsion value according to different situations; the improved repulsion function is as follows:

When the distance between the smart car and the end position ρ(X,X _g )<1, and the obstacle is close to the robot, and ρ(X,X _g ) ² are both values less than 1, so the repulsion value can be reduced to prevent the phenomenon that the target point cannot be reached due to excessive repulsion around the target point.

In the above-mentioned smart car path-finding method based on the artificial potential field method of the ellipse model, in step 3, the artificial potential field method algorithm improves its repulsion function, and according to the distance factor ρ(X, X _g ), set three different situations, when the position of the smart car appears within half of the radius value of the obstacle, the smart car is already very close to the obstacle, considering the factors of safe obstacle avoidance, the repulsion force cannot be reduced Many, the square root coefficient of the distance factor is taken here. On the other hand, when the position of the robot is between half of the obstacle’s influence radius and the maximum value, the smart car just starts to approach the obstacle. In order to prevent the attractive force from being smaller than the repulsive force, it cannot When approaching the target point, when the distance factor is greater than the obstacle influence radius, the smart car will not be affected.

In the above-mentioned smart car pathfinding method based on the artificial potential field method of the ellipse model, in step 6, the ellipse model is constructed with the local minimum point position and the target point position, and X' is established with the direction pointing to the target point as the positive direction axis, at the midpoint of the two points, make the Y' axis perpendicular to the X' axis, so as to establish a new coordinate system X'-O-Y', and set the virtual target point as the intersection point of the positive direction of the X' axis and the ellipse model , so as to increase the force of the target point on the smart car in the gravitational direction, thereby breaking the local minimum state and jumping out of the local equilibrium.

Compared with the prior art, the present invention has the advantages of:

1. In a complex environment, use the ellipse model to add a virtual target point. The gravitational force generated by the virtual target point will change the state of the robot at the local minimum. By continuously correcting the position of the virtual target point on the ellipse model, the smart car will jump out of the local area. Balance the area while greatly reducing the time cost of the algorithm;

2. According to the distance between the current position of the smart car and the end position, and according to the setting distance influencing factors, three different situations are set to better ensure the safety of the planned route of the smart car.

Description of drawings

Fig. 1 is the basic principle diagram of the artificial potential field method in a kind of intelligent vehicle pathfinding method based on the ellipse model artificial potential field method that the present invention proposes;

Fig. 2 is the method figure that ellipse model establishes virtual target point in a kind of intelligent vehicle pathfinding method based on ellipse model artificial potential field method that the present invention proposes;

Fig. 3 is the concrete flow chart of ellipse model artificial potential field method in a kind of intelligent car pathfinding method based on ellipse model artificial potential field method that the present invention proposes;

Fig. 4 is an effect diagram of planning a path by the ellipse model artificial potential field method in a smart vehicle pathfinding method based on the ellipse model artificial potential field method proposed by the present invention.

Detailed ways

The following examples are for illustrative purposes only and are not intended to limit the scope of the invention.

Example

Referring to Fig. 1-4, a smart car pathfinding method based on the ellipse model artificial potential field method includes the following steps:

Step 1. Construct the corresponding map according to the area where the smart car is located, grid the map and establish a coordinate system, and determine the coordinates of the obstacle position and the position of the target point in the map;

Step 2. Determine whether the smart car has reached the end point;

Step 3. If the end point is reached, path planning is completed to generate the final path; if the end point is not reached, path planning is performed using the artificial potential field method;

Step 4. Calculate the magnitude and direction of the gravitational force, repulsive force, and resultant force on the current position of the smart car;

Step 5, judging whether to fall into the local minimum problem;

Step 6. If you are not stuck in the local minimum, return to step 2 to continue the judgment and path planning. If you fall into the local minimum problem, build an ellipse model based on the local minimum problem point, add a virtual target point, and increase the direction of the end position. The attractiveness of smart cars, thus jumping out of the local minimum problem;

Step 7, use the ellipse model artificial potential field method to calculate again until the end point is reached;

Step 8. Perform secondary optimization on the planned path, check the path nodes backwards from the starting point, and judge whether it can be reached in a straight line. If it can be reached in a straight line, delete the path from the starting point to the node; if it cannot be reached in a straight line, then Take the node position as the starting point and continue to judge whether it can be reached in a straight line, thereby simplifying the path.

The artificial potential field method is used to avoid the phenomenon that the target is unreachable during the movement of the smart car, and a safe path from the starting point to the target point is planned for the smart car in a working environment containing obstacles, and it is judged whether the smart car is Stuck in local minima, including:

Establish the XOY coordinate system with the map, assuming that the coordinate of the robot falling into local equilibrium is P _min (x _min , y _min ), and the coordinate of the end point is P _goal (x _goal , y _goal ), then the coordinate position of the midpoint of these two points is P _b for:

Take the connection between P _min and P _goal as the new axis X', take P _b as the origin, and establish the axis Y' perpendicular to X' through the origin. The new coordinate system is obtained by rotating and translating the original coordinate system, as follows Mode:

in is the rotation matrix, /> is the translation matrix;

The ellipse equation is established in the new coordinate system as follows:

Focal length 2c=|P _min -P _goal |, a=ψc, ψ is the major axis factor, a ² =b ² +c ² ;

Take the orthogonal point of the ellipse and the X' axis as the initial virtual target point to increase the attractiveness of the end position to the smart car, thus breaking the local minimum state. The smart car walks according to this virtual target point. If it detects that it falls into local equilibrium again When , a new ellipse model is constructed to find a new fictitious target point.

The gravitational function of the artificial potential field method is as follows:

In the formula, X _g represents the position of the target point (x _g , y _g ); λ ₁ represents the gravitational constant; ρ(X, X _g ) is the geometric distance between the current position of the smart car and the terminal position;

The gravitational force is obtained by taking the first derivative of the distance in the gravitational force function:

Similarly, the repulsion potential function is as follows:

In the formula, X ₀ = (x ₀ , y ₀ ) represents the position of the obstacle; λ ₂ represents the repulsion field constant; ρ ₀ represents the radius of influence of the obstacle; ρ(X, X ₀ ) is the distance between the smart car and the obstacle;

Find the first derivative of the repulsion function with respect to the distance, that is, repulsion:

Therefore, the resultant force F _total (X) received by the smart car in space is:

F _total (X)＝F _att (X)+F _rep (X)

The problem that the target point cannot be reached is because the repulsion generated by the obstacles around the target point is greater than the gravitational force of the target point;

Therefore, based on the traditional repulsion function, the distance factor ρ(X,X _g ) between the current point and the target point is introduced to reduce the repulsion value according to different situations; the improved repulsion function is as follows:

When the distance between the smart car and the end position ρ(X,X _g )<1, and the obstacle is close to the robot, and ρ(X,X _g ) ² are both values less than 1, so the repulsion value can be reduced to prevent the phenomenon that the target point cannot be reached due to excessive repulsion around the target point.

On the one hand, when the position of the robot appears within half of the radius value of the obstacle, the smart car is already very close to the obstacle. Considering the factor of safe obstacle avoidance, the repulsive force cannot be reduced too much. Here, the distance factor is used square factor. On the other hand, when the position of the robot is between the half position and the maximum value of the influence radius value of the obstacle, the smart car just starts to approach the obstacle. In order to prevent the attractive force from being smaller than the repulsive force and thus unable to approach the target point, the repulsive force should be reduced as much as possible. size, so the square coefficient of the distance factor is taken here.

In a more complex environment, it is possible that when the smart car does not reach the target point, the gravity and repulsion force on the smart car are the same in size and opposite in direction, and the resultant force is zero, which will cause the smart car to go back and forth at the current position. oscillation.

The present invention proposes a method of establishing an ellipse model to set a virtual target point. The gravitational force generated by the virtual target point will change the state of the robot at the local minimum. By continuously correcting the position of the virtual target point on the ellipse model, the smart car will jump out of the local area. balance area.

In step 3, the artificial potential field method algorithm improves its repulsion function. According to the distance factor ρ(X,X _g ) between the current position of the smart car and the target point, three different situations are set. The position of the smart car appears in the obstacle When the object influence radius is within half of the value, the smart car is already very close to the obstacle. Considering the factor of safe obstacle avoidance, the repulsive force cannot be reduced too much. Here, the square root coefficient of the distance factor is taken. On the other hand, the robot’s When the position is between the half position and the maximum value of the influence radius value of the obstacle, the smart car starts to approach the obstacle. In order to prevent the attractive force from being smaller than the repulsive force and thus unable to approach the target point, when the distance factor is greater than the obstacle influence radius, the smart car will not be affected. Influence.

In step 6, the ellipse model is constructed with the position of the local minimum point and the target point, the X' axis is established with the direction pointing to the target point as the positive direction, and the Y perpendicular to the X' axis is drawn at the midpoint of the two points 'axis, so as to establish a new coordinate system X'-O-Y', set the virtual target point as the intersection point of the positive direction of the X' axis and the ellipse model, so as to increase the force of the target point on the smart car in the gravitational direction, thus breaking the local The state of minimum value jumps out of local equilibrium.

Claims (5)

1. An intelligent vehicle road finding method based on an elliptic model artificial potential field method is characterized by comprising the following steps of:

step 1, constructing a corresponding map according to an area where an intelligent vehicle is located, gridding the map, establishing a coordinate system, and determining coordinates of the position of an obstacle and the position of a target point in the map;

step 2, judging whether the intelligent vehicle reaches the end position currently;

step 3, if the final position is reached, path planning is completed, and a final path is generated; if the terminal position is not reached, path planning is carried out by using an artificial potential field method;

step 4, calculating the magnitude and direction of attractive force, repulsive force and resultant force of the current position of the intelligent vehicle;

step 5, judging whether the problem of local minimum value is solved;

step 6, if the local minimum is not trapped, returning to the step 2 to continue judging and path planning, if the local minimum is trapped, constructing an elliptical model according to the problem point of the local minimum, adding a virtual target point, and increasing the attractive force of the end position direction to the intelligent vehicle so as to jump out the problem of the local minimum;

step 7, calculating again by using an elliptic model artificial potential field method until the vehicle runs to the end position;

step 8, carrying out secondary optimization on the planned path, sequentially detecting path nodes backwards from the starting point, judging whether the path nodes can reach straight lines, and deleting the path from the starting point to the nodes if the path nodes can reach straight lines; if the straight line can not be reached, the node position is used as the starting point position to continuously judge whether the straight line can be reached, so that the path is simplified.

2. The intelligent vehicle road-finding method based on the elliptical model artificial potential field method of claim 1, wherein the artificial potential field method is adopted to avoid the phenomenon that the intelligent vehicle cannot reach the target in the moving process, a safe path from a starting point to a target point is planned for the intelligent vehicle in a working environment containing obstacles, and whether the intelligent vehicle falls into a local minimum point is judged, and the method comprises the following steps:

an X-O-Y coordinate system is established by a map, and the coordinate of the robot sinking into local balance is assumed to be P _min (x _min ,y _min ) The end position coordinate is P _goal (x _goal ,y _goal ) The midpoint coordinate position P of the two points _b The method comprises the following steps:

with P _min And P _goal The connection line of the two points is taken as a new axis X', and P _b As an origin, an axis Y 'which passes through the origin and is perpendicular to X' is established, and a new coordinate system is obtained by rotating and translating an original coordinate system, wherein the formula is as follows:

wherein the method comprises the steps ofIs a rotation matrix +.>Is a translation matrix;

the ellipse equation is established under the new coordinate system as follows:

focal length 2c= |p _min -P _goal I, a=ψc, ψ is the long axis factor, a ² ＝b ² +c ² ；

And taking the orthogonal point of the ellipse and the X' axis as an initial virtual target point, increasing the attraction of the end point position to the intelligent vehicle, breaking the local minimum state, enabling the intelligent vehicle to walk according to the virtual target point, and if the intelligent vehicle is detected to sink into local balance again, constructing a new ellipse model, and searching for a new virtual target point.

3. The intelligent vehicle road-finding method based on the elliptic model artificial potential field method according to claim 2, wherein the gravitation function of the artificial potential field method is as follows:

wherein X is _g Representing the position (x _g ,y _g )；λ ₁ Representing the gravitational constant; ρ (X, X) _g ) The geometric distance between the current position and the final position of the intelligent vehicle;

the first order derivative of the distance in the gravitational potential function is the gravitational force:

similarly, the repulsive potential function is as follows:

wherein X is ₀ ＝(x ₀ ,y ₀ ) Representing the obstacle position; lambda (lambda) ₂ Representing a repulsive force field constant; ρ ₀ Representing the obstacle influencing radius; ρ (X, X) ₀ ) Is the distance between the intelligent vehicle and the obstacle;

the repulsive force function is used for solving the first derivative of the distance, namely repulsive force:

so the resultant force F applied by the intelligent vehicle in space _total (X) is:

F _total (X)＝F _att (X)+F _rep (X)

the problem that the position of the target point cannot be reached is that the repulsive force generated by the obstacle around the target point of the intelligent vehicle is larger than the attractive force of the target point;

therefore, based on the traditional repulsive force function, a distance factor rho (X, X _g ) The repulsive force value is reduced according to different situations; the improved repulsive force function is shown below:

when the distance ρ (X, X) between the intelligent vehicle and the end position _g ) When the obstacle is less than 1 and the obstacle is close to the robot,and ρ (X, X) _g ) ² The values are all smaller than 1, so that the repulsive force value can be reduced, and the phenomenon that the target point cannot be reached due to overlarge repulsive force around the target point is prevented.

4. The intelligent vehicle road-finding method based on the elliptic model artificial potential field method according to claim 1, wherein in step 3, the artificial potential field method algorithm improves the repulsive force function thereof according to the distance factor ρ (X, X _g ) Setting up three kinds of different circumstances, when the position of intelligent car appears in the obstacle influences the half of radius value, the intelligent car has very near the obstacle, consider the factor of safe obstacle avoidance, can not cut down the repulsion too much, get the square coefficient of opening of distance factor here, on the other hand, the position of robot is in the obstacle when influencing the half position of radius value to the maximum value, the intelligent car just begins to be close to the obstacle, thereby in order to prevent that gravitation is less than repulsion and can't be close to the target point, when distance factor is greater than the obstacle and influences the radius, the intelligent car is not influenced.

5. The intelligent vehicle road-finding method based on the elliptic model artificial potential field method according to claim 1, wherein in step 6, an elliptic model is constructed by taking the position of a local minimum point and the position of a target point, an X 'axis is established by taking the direction pointing to the target point as a positive direction, a Y' axis perpendicular to the X 'axis is established at the midpoint position of the two points, thereby establishing a new coordinate system X' -O-Y ', setting a virtual target point as the intersection point of the positive direction of the X' axis and the elliptic model, thereby increasing the force of the target point on the intelligent vehicle in the attractive force direction, thereby breaking the local minimum state and jumping out of local balance.