Mobile robot path planning method based on improved artificial potential field method

Abstract

The invention relates to a mobile robot path planning method based on an improved artificial potential field method. According to the method, a new repulsive force function is established by introducing a sine distance factor, so that the repulsive force borne by the robot is gradually reduced to zero along with approaching of the robot to a target point, and the problem that the robot cannot reach the target point can be avoided; when a local minimum value problem is encountered, an obstacle boundary point group tangent vector algorithm is adopted to calculate a temporary target point, some virtual obstacle visible point groups are established at the boundary of an obstacle, the size and shape of the obstacle are fully considered, and the local minimum value is jumped out by reaching the temporary target point first by means of the gravitation of the temporary target point; the distance that the robot needs to move in the next step is calculated through the self-adaptive step length method, the number of algorithm iterations and the planning time are reduced, meanwhile, the obstacle can be better avoided, and the problem that the robot is too close to the obstacle and then rebounds can be avoided. According to the method, the efficiency of path planning of a traditional APF algorithm is greatly improved, and the method has important theoretical significance and practical value.

Description

Technical Field

[0001] The invention belongs to the field of robot navigation and artificial intelligence, and specifically is a mobile robot path planning method based on an improved artificial potential field method. Background Art

[0002] Path planning is one of the important means to enable robots to move autonomously and efficiently in the environment. There are two main types of robot path planning: global path planning and local path planning. Global path planning is to plan a shortest path from the starting point to the target point based on the static environment map, while local path planning is to use sensors to obtain dynamic environmental information in real time, avoid dynamic obstacles, and allow the robot to move along the waypoints obtained by global path planning. In local path planning, APF is a commonly used method. This method has the advantages of simple mathematical model, easy implementation, and good adaptability to unknown or dynamic environments. It is widely used in active real-time collision avoidance and path planning of robots. This method regards the robot's task environment as a potential field. During the path planning process, the target point generates an attractive potential field to guide the robot to move in the target direction, while the obstacles in the environment generate a repulsive potential field to keep the robot away from the obstacles. However, the traditional artificial potential field method has the disadvantages of local minima and unreachable targets, resulting in the generated path being less than ideal. However, the most well-known and concerned problem in most current studies is falling into a local minimum. However, these methods all ignore the impact of the size and shape of obstacles on the path planning problem. In order to simplify the problem, obstacles and robots are considered as a particle whose size and shape are ignored, which is inconsistent with the actual situation. Summary of the invention

[0003] The purpose of the present invention is to provide a mobile robot path planning method based on an improved artificial potential field method. This method improves the repulsive field function by introducing a sinusoidal distance factor to solve the problem that the APF algorithm target is unreachable, and then proposes an obstacle boundary point group tangent vector algorithm to take the size and shape of the obstacle into consideration, thereby solving the local minimum problem of the APF algorithm and improving the path planning efficiency of the APF algorithm and its adaptability to complex environments. Finally, an adaptive step size method is proposed, which uses a larger step size in a safe area to reduce the number of algorithm iterations and path planning time, while reducing the step size near obstacles. This method of dynamically adjusting the step size can not only effectively and safely avoid obstacles, but also avoid the problem of the robot being bounced back due to excessive step size, thereby improving the path planning efficiency overall.

[0004] The technical solution adopted by the present invention to achieve the above-mentioned purpose is:

[0005] A mobile robot path planning method based on an improved artificial potential field method comprises the following steps:

[0006] 1) Determine the robot's starting position and original target point position;

[0007] 2) Initialize the robot's current target point as the original target point;

[0008] 3) Determine whether the robot has reached the original target point. If it has, save the robot path. Otherwise, execute step 4);

[0009] 4) Determine whether the robot has reached the current target point. If it has, set the current target point as the original target point and return to step 3). Otherwise, execute step 5);

[0010] 5) Obtain the obstacle information of the robot at the current position facing the current target point, and use the improved repulsion function to calculate the repulsion exerted on the robot;

[0011] 6) Calculate the gravitational force on the robot and calculate the resultant force based on the repulsive force;

[0012] 7) Based on the combined force, determine whether the robot is trapped in a local minimum. If so, execute step 8); otherwise, execute step 9);

[0013] 8) Use the obstacle boundary point group tangent vector algorithm to calculate the temporary target point, set the temporary target point as the current target point, and return to step 5);

[0014] 9) Use the adaptive step size method to calculate the current moving step size;

[0015] 10) Use the moving step length to calculate the next position of the robot, move to the next position, and return to step 4).

[0016] The improved repulsion function F rep Specifically:

[0017]

[0018] Among them, ζ represents the proportional gain factor of the repulsive potential field, γ represents the safe distance that the robot needs to maintain with the obstacle, and ρ 0 represents the maximum influence distance of the obstacle repulsion field, ρ i Represents the robot and the i-th obstacle O i Distance:

[0019] ρ i =ρ(R,O i )=‖RO i ‖ 2

[0020] Among them, R represents the current position of the robot, O iis the position of the i-th obstacle detected by the robot at the current position facing the current target point and within the range of its repulsive force, i = 1, ..., N, N represents the total number of obstacles detected by the robot at the current position facing the current target point and within the range of its repulsive force;

[0021] D j It indicates the distance factor from the robot to the target point when the robot is at position j, specifically:

[0022]

[0023] Among them, ρ j,g Indicates the current position of the robot R j To the target point T g Euclidean distance, n is the empirical value of the improved repulsion function, ρ 0,g R represents the starting point of the robot 0 To the target point T g The Euclidean distance is:

[0024] ρ j,g =||R j -T g || 2

[0025] ρ 0,g =||R 0 -T g || 2 .

[0026] The method of calculating the temporary target point using the obstacle boundary point group tangent vector algorithm comprises the following steps:

[0027] (1) The position information of all obstacle boundary points detected by the laser radar within the robot's repulsive force range is added to the list Q in sequence. Q is an ordered set, that is, any obstacle boundary point O in Q i ∈Q, is a monotone sequence, where Represents the boundary point O i The x-coordinate of Represents the boundary point O i The y coordinate of

[0028] (2) Calculate the boundary point group of each obstacle;

[0029] (3) Processing the set storing each obstacle boundary point group to obtain possible temporary target points;

[0030] (4) From the possible temporary target points, according to the movement cost function, select the temporary target point with the minimum movement cost as the final temporary target point.

[0031] The step (2) comprises the following steps:

[0032] (2.1) Let Q = (O 1 ,O 2 ,O 3 ,…,O i ,…), O 1 ,O 2 ,O 3 ,…,Q i ,…is the location of each boundary point in the set Q of all obstacle boundary point groups, where O i The location is Mark all points as unvisited;

[0033] (2.2) Traverse all boundary points in Q. If the current boundary point O i If not visited, create a new set S j Store the current boundary point, j = 1, 2, 3, ..., and mark the current boundary point as visited;

[0034] (2.3) Find the obstacle boundary endpoint O by judging the distance between adjacent points and the threshold e p ;

[0035] (2.4) i+1 ,O i+2 ,…,O p Put the current boundary point O into storage i The collection S j At the same time, the above boundary points are marked as visited;

[0036] (2.5) Repeat steps (2.2) to (2.4) until all points in set Q have been visited. At this time, set Q is divided into S 1 , S 2 , S 3 ,...,S K There are K sets in total, where K represents the number of obstacles currently around the robot, and S j ,j=1,2,3,…,K respectively stores the position of the boundary point group of each obstacle in the current perspective of the robot.

[0037] The step (2.3) is specifically:

[0038] If there is a point O among the obstacle boundary points p , so that d i,i+1 , d i+1,i+2 , d i+2,i+3 , d p-1,p The values ​​of are all less than the threshold e, and d p,p+1 The value of is greater than or equal to the threshold value e, then point Op is the endpoint of the obstacle boundary, where d i,i+1 Represents the adjacent points O in the set Q i and O i+1 The Euclidean distance between them is:

[0039] d i,i+1 =‖O i -O i+1 ‖ 2 .

[0040] The step (3) comprises the following steps:

[0041] (3.1) If K = 1, that is, there is only one obstacle around the robot, the corresponding boundary point group is S 1 , note S 11 , S 1N The set S 1 The first and last boundary points of the obstacle boundary point group correspond to the two endpoints, and the current position R of the robot and the endpoint S are obtained respectively. 11 , S 1N The left intersection point T of the tangent line of the circle with center r and radius r 11 and right intersection point T 1N As a possible temporary destination point;

[0042] (3.2) If K ≥ 2, there is more than one obstacle around the robot. When K = 2, there are two obstacles around the robot. The corresponding boundary point groups are S i , S j , note S i1 , S iN The set S i The first and last boundary points of the obstacle i correspond to the two endpoints of the boundary point group, S j1 , S jN The set S j The first and last boundary points of the obstacle j correspond to the two endpoints of the boundary point group, and S is calculated respectively. i , S j The corresponding possible temporary target points T i1 , T iN , T j1 , T jN , and when the distance d between the two obstacles i,j , satisfying the conditions: d i,j ≥2r+μ, the midpoint M of the two obstacles i,j It can also be used as a possible temporary target point, where r and μ represent the robot's circumcircle radius and a given robot turning safety threshold, respectively.

[0043] The T 11 and T1N The calculation methods are:

[0044] T 11 The calculation method is to solve the following quadratic equation:

[0045]

[0046]

[0047] For the two solutions of the system of equations, take 1N Distance d 11,1N The larger of:

[0048] d 11,1N =||T 11 -S 1N || 2

[0049] Among them, d 11,1N Represents endpoint S 1N Intersection point T 11 The Euclidean distance of

[0050] T 1N The calculation method is to solve the following quadratic equation:

[0051]

[0052]

[0053] For the two solutions of the system of equations, take 11 Distance d 1N,11 The larger of:

[0054] d 1N,11 =||T 1N -S 11 || 2

[0055] Among them, d 1N,11 Represents endpoint S 11 Intersection point T 1N The Euclidean distance.

[0056] The M i,j The calculation method is:

[0057] According to S i , S j The order in the set Q, using the index to get S i , S j The adjacent endpoints are T iN and T j1 , then their midpoint M i,j The position is calculated as:

[0058] M i,j x =(T iN x +T j1 x ) / 2

[0059] M i,j y =(T iN y +T j1 y ) / 2

[0060] Among them, M i,j x , M i,j y , T iN x ,T iN y ,T j1 x ,T j1 y Represent possible temporary target points M i,j , T iN and T j1 The x- and y-coordinates of .

[0061] The step (4) comprises the following steps:

[0062] (4.1) Bring each possible temporary target point position into the evaluation function and calculate the moving cost of each possible temporary target point;

[0063] The evaluation function is:

[0064] h i =f i +g i

[0065] f i =||T i -R|| 2

[0066] g i =||T i -T g || 2

[0067] Among them, f i represents the actual cost of the i-th possible temporary target point position from the robot's current position, g i represents the estimated cost from the i-th possible temporary target point to the target point, T irepresents the position of the i-th possible temporary target point, R represents the coordinates of the robot's current position, T g Indicates the location of the target point;

[0068] (4.2) Select the total cost h i The smallest possible temporary target point is taken as the final temporary target point;

[0069] (4.3) When the robot reaches the temporary target point, replace the temporary target point with the original target point and let the robot continue to move towards the original target.

[0070] The adaptive step length method is used to calculate the current moving step length, specifically:

[0071] λ=w 1 d 1 +(w 2 +w 3 )d 2

[0072] Among them, λ is the step size, w i is a Boolean value, i=1,2,3, w 1 Indicates whether the robot is outside the range of the obstacle repulsion force and l meters away from the target point, w 2 Indicates whether the robot is within the range of the obstacle repulsion force, w 3 Indicates whether the robot is within l meters of the target point, l is the set threshold, d 1 and d 2 are steps of different lengths, and d 1 >d 2 .

[0073] The present invention has the following beneficial effects and advantages:

[0074] 1. The present invention adopts a new repulsive field function, so that the robot can still reach the target point safely when there are obstacles near the target point, and will not collide with other obstacles during the journey;

[0075] 2. The present invention enables the robot to avoid local minimum problems in various situations by establishing a virtual obstacle point group at the obstacle boundary, including single-point obstacles without considering the obstacle volume, and obstacles considering the size and shape of the obstacle, such as planar obstacles and U-shaped obstacles. Moreover, even in complex environments, the robot can safely and effectively avoid obstacles and reach the target point;

[0076] 3. The present invention designs an adaptive step-size method, that is, the algorithm dynamically adjusts the iterative step-size of the algorithm according to the safety conditions of the robot's surrounding environment, which can not only reduce the path planning time, but also avoid the problem of the robot being bounced back due to suddenly getting too close to an obstacle, thereby improving the path planning efficiency overall. BRIEF DESCRIPTION OF THE DRAWINGS

[0077] Figure 1 The overall flow chart of the present invention;

[0078] Figure 2 Force diagram for situations where the target is unreachable;

[0079] Among them, a means the target point is between the obstacle and the robot, b means the obstacle is between the target point and the robot, and c means multiple obstacles exist between the robot and the target point;

[0080] Figure 3 Flow chart of the obstacle boundary point group tangent vector algorithm of the present invention;

[0081] Figure 4 Schematic diagram of possible temporary target points of the tangent vector algorithm for a single obstacle boundary point group;

[0082] Figure 5 Schematic diagram of possible temporary target points of the tangent vector algorithm for multiple obstacle boundary points;

[0083] Figure 6 Comparison of the repulsive field distribution of the present invention and the traditional APF method under a single obstacle;

[0084] Wherein, a is the APF path and the repulsive field distribution at the corresponding target point, and b is the repulsive field distribution at the present invention and the corresponding target point;

[0085] Figure 7 Comparison of the repulsive field distribution of the present invention and the traditional APF method under multiple obstacles;

[0086] Wherein, a is the APF path and the repulsive field distribution at the corresponding target point, and b is the repulsive field distribution at the present invention and the corresponding target point;

[0087] Figure 8 Comparison of path planning results between the present invention and the traditional APF algorithm in the case of a single-point obstacle;

[0088] Wherein, a is APF, and b is the present invention;

[0089] Fig. 9 Comparison of path planning results of the present invention and traditional APF algorithm considering the shape of obstacles;

[0090] Wherein, a is APF (case 1), b is the present invention (case 1), c is APF (case 2), and d is the present invention (case 2);

[0091] Fig.10 Comparison of path planning results between the present invention and the traditional APF algorithm under obstacles of various shapes;

[0092] Wherein, a is APF (case 1), b is the present invention (case 1), c is APF (case 2), and d is the present invention (case 2);

[0093] Fig.11 Comparison diagram of path planning results between the present invention and other algorithms;

[0094] Among them, a is a single obstacle and b is multiple obstacles. DETAILED DESCRIPTION

[0095] The present invention is further described in detail below in conjunction with the accompanying drawings and embodiments.

[0096] The present invention discloses a mobile robot path planning method based on an improved artificial potential field method (improved APF), which has three main improvements. First, a new repulsive field function is designed by introducing a sinusoidal distance factor, so that the repulsive force on the robot gradually decreases to zero as it approaches the target point, solving the problem that the robot cannot reach the target point; second, in order to solve the local minimum problem of the APF algorithm, a virtual obstacle point group is designed in the robot's visible perspective at the obstacle boundary. The algorithm takes into account the size and dimensions of the obstacle, changing the previous situation of simply treating the obstacle as a point with mass without volume, which is more in line with the actual environment; finally, by dynamically adjusting the step size, that is, using a larger step size in the safe area to reduce the number of algorithm iterations and path planning time, and using a smaller step size when near the obstacle, so as to better avoid the obstacle, while avoiding the problem of the robot being bounced back when it is too close to the obstacle.

[0097] The following steps are involved:

[0098] 1) Determine the robot's starting position and original target point position;

[0099] 2) Initialize the robot's current target point as the original target point;

[0100] 3) Determine whether the robot has reached the original target point. If it has, save the robot path. Otherwise, execute step 4);

[0101] 4) Determine whether the robot has reached the current target point. If it has, set the current target point as the original target point and return to step 3). Otherwise, execute step 5);

[0102] 5) Obtain the obstacle information of the robot at the current position facing the current target point, and use the improved repulsion function to calculate the repulsion exerted on the robot;

[0103] 6) Calculate the gravitational force on the robot and calculate the resultant force based on the repulsive force;

[0104] 7) Determine whether the combined force falls into a local minimum value based on the combined force. If yes, execute step 8); otherwise, execute step 9);

[0105] 8) Use the obstacle boundary point group tangent vector algorithm to calculate the temporary target point, set the temporary target point as the current target point, and return to step 5);

[0106] 9) Use the adaptive step size method to calculate the current moving step size;

[0107] 10) Use the moving step length to calculate the next position of the robot, move to the next position, and return to step 4).

[0108] The specific improvements of the present invention are as follows:

[0109] 1. As Figure 1 As shown, the present invention mostly performs path planning according to the conventional APF algorithm process, with three differences. First, unlike the conventional APF algorithm, the present invention adopts an improved repulsion function when calculating the repulsion of the robot. Secondly, when trapped in a local minimum, the obstacle boundary point group tangent vector algorithm is used to calculate the temporary target point, allowing the robot to jump out of the local minimum by first reaching the temporary target point and then moving to the original target point. Finally, when calculating the next position point to be moved by the robot, an adaptive step length is used to calculate the moving step length.

[0110] 2. To solve the problem of unreachable target (such as Figure 2 As shown in FIG. 1 , when calculating the repulsive force exerted on the robot, an improved repulsive force function is used, where the improved repulsive force function F rep The expression is as follows:

[0111]

[0112] Where ζ represents the proportional gain factor of the repulsive potential field, γ represents the safe distance that the robot needs to maintain from the obstacle, and ρ 0 represents the maximum influence distance of the obstacle repulsion field, ρ i Represents the robot and the i-th obstacle O i Distance:

[0113] ρ i =ρ(R,O i )=‖RO i ‖ 2

[0114] Where R represents the current position of the robot, O i It is the position of the i-th obstacle detected by the robot at the current position facing the current target point and within the range of its repulsive force, i = 1, ..., N, N represents the total number of obstacles detected by the robot at the current position facing the current target point and within the range of its repulsive force. D j It indicates the distance factor from the robot to the target point when the robot is at position j. The expression is:

[0115]

[0116] where ρ j,g Indicates the current position of the robot R j To the target point T g Euclidean distance, n is the empirical value of the improved repulsion function, ρ 0,g R represents the starting point of the robot 0 To the target point T g The Euclidean distance is:

[0117] ρ j,g =||R j -T g || 2

[0118] ρ 0,g =||W 0 -T g || 2

[0119] By (ρ j,g / ρ 0,g ) n The distance factor D j As the robot approaches the target point, it gradually decreases, and then the parameter is π(ρ j,g / ρ 0,g ) n / 2 sine function to normalize it, and make its repulsive force change curve amplitude similar to the sine function curve, so as to reduce the distance factor D j The repulsive force on obstacles at non-target points has a greater impact, thereby reducing the risk of collision between the robot and obstacles.

[0120] 3. When falling into a local minimum, the obstacle boundary point group tangent vector algorithm is used to calculate the temporary target point, such as Figure 3 As shown, the specific calculation process is:

[0121] 3.1 If the robot falls into a local minimum, that is, when the current position of the robot is far away from the original target position, but the resultant force on the robot is 0, the position information of the boundary points of all obstacles detected by the laser radar within the robot's repulsive force range is added to the list μ in sequence. μ is an ordered set, that is, any obstacle boundary point O in c i ∈μ, there is

[0122] is a monotonic sequence

[0123] in Represents the boundary point O i The x-coordinate of Represents the boundary point O i The y coordinate of

[0124] 3.2 Calculate the boundary point group of each obstacle, record

[0125] Q=(O 1 ,O 2 ,O 3 ,…,O i ,…)

[0126] O 1 ,O 2 ,O 3 ,…,O i ,…is the location of each boundary point in the set Q of all obstacle boundary point groups, where O i The location is Mark all points as unvisited, traverse all boundary points in Q, if the current boundary point O i If not visited, create a new set S j , (j=1,2,3,…) stores the current boundary point and marks it as visited. Then, the obstacle boundary endpoint O is found by judging whether the distance between adjacent points is less than the threshold e. p , that is, d i,i+1 , d i+1,i+2 , d i+2,i+3 , d p-1,p The values ​​of are all less than the threshold e, and d p,p+1 The value of is greater than or equal to the threshold e, where d i,i+1 Represents the adjacent point O in the set Q i and O i+1 The Euclidean distance between them. That is:

[0127] d i,i+1 =‖O i -O i+1 ‖ 2

[0128] Then O i+1 ,Oi+2 ,…,O p Put the current boundary point O into storage i The collection S j At the same time, mark these boundary points as visited, repeat the above process until all points in set Q have been visited, then set Q will be divided into S 1 , S 2 , S 3 ,...,S K There are K sets in total, where K represents the number of obstacles currently surrounding the robot. j , (j=1,2,3,…,K) respectively stores the position of the boundary point group of each obstacle in the current perspective of the robot.

[0129] 3.3 Calculate possible temporary target points, and store the set S of each obstacle boundary point group calculated in 3.2 j , (j=1,2,3,…,K) is processed as follows:

[0130] 3.3.1 Figure 4 As shown, if K = 1, that is, there is only one obstacle around the robot, the corresponding boundary point group is S 1 , note S 11 , S 1N The set S 1 The first and last boundary points of the obstacle point group correspond to the two endpoints of the obstacle boundary point group. Get the robot's current position R and the endpoint S 11 , S 1N The left intersection point T of the tangent line of the circle with center r and radius r 11 and right intersection point T 1N As a possible temporary target point, T 11 The calculation method is to solve the following quadratic equation:

[0131]

[0132]

[0133] For the two solutions of the system of equations, take 1N Distance d 11,1N The larger of:

[0134] d 11,1N =||T 11 -S 1N || 2

[0135] where d 11,1N Represents endpoint S 1N Intersection point T 11 The Euclidean distance.

[0136] T 1N The calculation method is to solve the following quadratic equation:

[0137]

[0138]

[0139] For the two solutions of the system of equations, take 11 Distance d 1N,11 The larger of:

[0140] d 1N,11 =||T 1N -S 11 || 2

[0141] where d 1N,11 Represents endpoint S 11 Intersection point T 1N The Euclidean distance.

[0142] 3.3.2 As Figure 5 As shown in the figure, if K ≥ 2, that is, there is more than one obstacle around the robot, we can take K = 2, that is, assume that there are two obstacles around the robot, and the corresponding boundary point groups are S i , S j . Remember S i1 , S iN The set S i The first and last boundary points of S correspond to the two endpoints of this obstacle boundary point group. j1 , S jN The set S j The first and last boundary points of correspond to the two endpoints of this obstacle boundary point group. In addition to calculating S according to the method in 3.3.1 i , S j The corresponding possible temporary target points T i1 , T iN , T j1 , T jN In addition, the distance d between the two obstacles should also be considered. i,j , if the conditions are met:

[0143] d i,j ≥2r+μ

[0144] Then their midpoint M i,j It is also a possible temporary target point. Where r and μ represent the radius of the robot's circumscribed circle and the given robot turning safety threshold, respectively. This paper takes the radius r of the circumscribed circle. For the midpoint M i,j The position is calculated as follows:

[0145] According to S i , S j The order in set Q can be obtained from the index S i , S j Adjacent endpoints, let’s assume that the adjacent endpoints are T iN and T j1 , then their midpoint M i,j The position is calculated as:

[0146] M i,j x =(T iN x +T j1 x ) / 2

[0147] M i,j y =(T iN y +T j1 y ) / 2

[0148] Among them, M i,j x , M i,j y , T iN x ,T iN y ,T j1 x ,T j1 y Respectively represent M i,j and T iN and T j1 The x- and y-coordinates of the three possible temporary target points.

[0149] 3.4 From the possible temporary target points, according to the corresponding movement cost function, select the temporary target point with the minimum movement cost as the final temporary target point. First, bring the position of each possible temporary target point into the evaluation function and calculate the movement cost of each possible temporary target point.

[0150] The evaluation function is:

[0151] h i =f i +g i

[0152] Among them, f i represents the actual cost of the i-th possible temporary target point position from the robot's current position, g i It represents the estimated cost from the i-th possible temporary target point to the target point, and the calculation formula is:

[0153] f i =||T i -R|| 2

[0154] g i =||T i -T g || 2

[0155] Among them, T i represents the position of the i-th possible temporary target point, R represents the coordinates of the robot's current position, T g Indicates the location of the target point.

[0156] Then choose the total cost h i The smallest possible temporary target point is used as the final temporary target point. Finally, when the robot reaches the temporary target point, the temporary target point is replaced with the original target point, allowing the robot to continue moving towards the original target.

[0157] 4. When calculating the next position point to be moved by the robot, an adaptive step length is used to calculate the moving step length. When the robot is not within the repulsive force range of the obstacle, a larger step length d can be used. 1 , in order to reduce the number of algorithm iterations, thereby reducing the path planning time. When the robot is within the range of the obstacle repulsion, a smaller step size d is used. 2 , which can prevent the robot from oscillating back and forth when it falls into a local minimum. At the same time, in order to accurately reach the target point, if the robot is not within the range of the repulsive force of the obstacle, but the robot is already close to the target point, a smaller step size d should also be used. 2 , to prevent the robot from iterating back and forth around the target point under the limitation of accuracy, and ultimately failing to reach the target point. The calculation formula for dynamically adjusting the step length is:

[0158] λ=w 1 d 1 +(w 2 +w 3 )d 2

[0159] where w i (i=1,2,3) are all Boolean values, w 1 represents whether the robot is outside the range of the obstacle repulsion force and l meter away from the target point, where l = 1.0 meter in the present invention, and w 2 Indicates whether the robot is within the range of the obstacle repulsion force, w 3 Indicates whether the robot is within the distance to the target point l Within meters.

[0160] like Figure 6 As shown in the figure, there is an obstacle very close to the target point, which is just within the repulsive potential field of the obstacle and is located between the robot's starting point and the final target point. Using the improved APF algorithm, path planning is performed based on the given starting point and target point. It can be found that due to the improved repulsive field function, the repulsive force on the robot will decrease as the distance from the target point decreases, so that the robot can successfully reach the final target without oscillating around the target point, while the traditional APF algorithm cannot reach the target point.

[0161] like Figure 7 As shown in the figure, the final target point is located between two or more obstacle potential fields. The traditional APF algorithm is used for path planning. Due to the large total repulsive force at the target point, the robot cannot reach the target point. However, the improved APF algorithm weakens the repulsive force at the target point, so the robot can successfully reach the target point.

[0162] like Figure 8 As shown in Figure 1, the obstacle is set as a single point obstacle without volume, placing the robot and the target point on the same straight line, and the obstacle point is exactly in the middle of them. The improved APF algorithm is used for path planning. Figure 8 It can be seen that the traditional APF algorithm is stuck in a local minimum near an obstacle, while the improved APF algorithm selects a temporary target point as a waypoint through the tangent vector of the obstacle boundary point group, allowing the robot to jump out of the local minimum and finally successfully reach the target point.

[0163] like Fig. 9 As shown, the obstacle is set as a single obstacle of a certain size and shape, namely a plane obstacle (case 1) and a U-shaped obstacle (case 2), both of which are the most likely to cause the traditional APF algorithm to fall into a local minimum. Since the point group involved in the obstacle boundary point group tangent vector algorithm proposed in the present invention is considered from the actual environment, that is, these point groups are all visible point groups from the perspective of the robot, after considering the volume and shape of the obstacle, the point group behind the obstacle will obviously only be detected when the robot moves behind the obstacle. Using the improved APF algorithm for path planning experiments, it can be seen that the use of the improved APF algorithm for path planning can safely and effectively reach the target point, while the use of the traditional APF algorithm will fail in path planning.

[0164] like Fig.10As shown, multiple obstacles of various shapes are set, and different starting points and end points are set respectively (corresponding to situation 1 and situation 2 respectively). The improved APF algorithm and the traditional APF algorithm are used for path planning respectively. It can be seen that the improved APF algorithm can reach the target point safely and effectively, while the traditional APF algorithm fails in path planning. The effectiveness of the present invention is verified.

[0165] like Fig.11 As shown, an environment with a single obstacle and an environment with multiple obstacles are set respectively, and the traditional APF algorithm and the improved APF algorithm as well as the BUG algorithm and the RRT algorithm are used for path planning respectively. The results are shown in Table 1. It can be seen that the algorithm effect in the present invention is significantly better than these algorithms.

[0166] Table 1 Comparison of path length and calculation time of the path planning of the present invention and other algorithms in different environments.

[0167]

Claims

1. A mobile robot path planning method based on an improved artificial potential field method, characterized in that: The following steps are involved: 1) Determine the robot's starting position and original target point position; 2) Initialize the robot's current target point as the original target point; 3) Determine whether the robot has reached the original target point. If it has, save the robot path. Otherwise, execute step 4); 4) Determine whether the robot has reached the current target point. If it has, set the current target point as the original target point and return to step 3). Otherwise, execute step 5); 5) Obtain the obstacle information of the robot at the current position facing the current target point, and use the improved repulsion function to calculate the repulsion exerted on the robot; 6) Calculate the gravitational force on the robot and calculate the resultant force based on the repulsive force; 7) Based on the combined force, determine whether the robot is trapped in a local minimum. If so, execute step 8); otherwise, execute step 9); 8) Use the obstacle boundary point group tangent vector algorithm to calculate the temporary target point, set the temporary target point as the current target point, and return to step 5); 9) Use the adaptive step size method to calculate the current moving step size; 10) Use the moving step length to calculate the next position of the robot, move to the next position, and return to step 4).

2. The mobile robot path planning method based on the improved artificial potential field method according to claim 1 is characterized in that: The improved repulsion function f rep Specifically: Among them, ζ represents the proportional gain factor of the repulsive potential field, γ represents the safe distance that the robot needs to maintain with the obstacle, ρ0 represents the maximum influence distance of the obstacle repulsive field, and ρ i Indicates that the robot and i Obstacle O i Distance: r i =ρ(r,O i )=‖RO i ‖2 Among them, R represents the current position of the robot, O i is the position of the ith obstacle detected by the robot at the current position facing the current target point and within the range of its repulsive force, i = 1, ..., N, N represents the total number of obstacles detected by the robot at the current position facing the current target point and within the range of its repulsive force; D j It indicates the distance factor from the robot to the target point when the robot is at position j, specifically: Among them, ρ j,g Indicates the current position of the robot R j To the target point T g Euclidean distance, n is the empirical value of the improved repulsion function, ρ 0,g Indicates the robot's starting point E0 to the target point T g The Euclidean distance is: r j,g =||E j -T g ||2 r 0,g =||E0-T g ||2。 3. The mobile robot path planning method based on the improved artificial potential field method according to claim 1 is characterized in that: The method of calculating the temporary target point using the obstacle boundary point group tangent vector algorithm comprises the following steps: (1) The position information of all obstacle boundary points detected by the laser radar within the robot's repulsive force range is added to the list Q in sequence. Q is an ordered set, that is, any obstacle boundary point O in Q i ∈Q, is a monotone sequence, where Represents the boundary point O i The x-coordinate of Represents the boundary point O i The y coordinate of (2) Calculate the boundary point group of each obstacle; (3) Processing the set storing each obstacle boundary point group to obtain possible temporary target points; (4) From the possible temporary target points, according to the movement cost function, select the temporary target point with the minimum movement cost as the final temporary target point.

4. The mobile robot path planning method based on the improved artificial potential field method according to claim 3 is characterized in that: The step (2) comprises the following steps: (2.1) Let Q = (O1, O2, O3, ..., O i ,…), O j ,O2,O3,…,O i ,…is the location of each boundary point in the set Q of all obstacle boundary point groups, where O i The location is Mark all points as unvisited; (2.2) Traverse all boundary points in Q. If the current boundary point O i If not visited, create a new set S j Store the current boundary point, j = 1, 2, 3, ..., and mark the current boundary point as visited; (2.3) Find the obstacle boundary endpoint O by judging the distance between adjacent points and the threshold e P ; (2.4) i+1 ,O i+2 ,…,O p Put the current boundary point O into storage i The collection S j At the same time, mark the above boundary points as visited; (2.5) Repeat steps (2.2) to (2.4) until all points in set Q have been visited. At this time, set Q is divided into S1, S2, S3, ..., S K There are K sets in total, where K represents the number of obstacles currently around the robot, and S j ,j=1,2,3,…,K respectively stores the position of the boundary point group of each obstacle in the current perspective of the robot.

5. The mobile robot path planning method based on the improved artificial potential field method according to claim 4 is characterized in that: The step (2.3) is specifically: If there is a point O among the obstacle boundary points p , so that d i,i+1 , d i+1,i+2 , d i+2,i+3 , d p-1,p The values ​​of are all less than the threshold e, and d p,p+1 The value of is greater than or equal to the threshold e, then point O p is the endpoint of the obstacle boundary, where d i,i+1 Represents the adjacent points O in the set Q i and O i+1 The Euclidean distance between them is: d i,i+1 =‖O i -O i+1 ‖2。 6. The mobile robot path planning method based on the improved artificial potential field method according to claim 3 is characterized in that: The step (3) comprises the following steps: (3.1) If K = 1, that is, there is only one obstacle around the robot, the corresponding boundary point group is S1, denoted by S 11 , S 1N The first and last boundary points of the set S1 correspond to the two endpoints of the obstacle boundary point group, and the current position R of the robot and the endpoint S are obtained respectively. 11 , S 1N The left intersection point T of the tangent line of the circle with center r as the radius 11 and right intersection point T 1N As a possible temporary destination point; (3.2) If K ≥ 2, there is more than one obstacle around the robot. When K = 2, there are two obstacles around the robot. The corresponding boundary point groups are S i , S j , note S i1 , S iN The set S i The first and last boundary points of the obstacle i correspond to the two endpoints of the boundary point group, S j1 , S jN The set S j The first and last boundary points of the obstacle j correspond to the two endpoints of the boundary point group, and S is calculated respectively. i , S j The corresponding possible temporary target points T i1 , T iN , T j1 , T jN , and when the distance d between the two obstacles i,j , satisfying the conditions: d i,j ≥2r+μ, the midpoint M of the two obstacles i,j It can also be used as a possible temporary target point, where r and μ represent the robot's circumcircle radius and a given robot turning safety threshold, respectively.

7. The mobile robot path planning method based on the improved artificial potential field method according to claim 6 is characterized in that: The T 11 and T 1N The calculation methods are: T 11 The calculation method is to solve the following quadratic equation: For the two solutions of the system of equations, take 1N Distance d 11,1N The larger of: d 11,1N =||T 11 -S 1N ||2 Among them, d 11,1N Represents endpoint S 1N Intersection point T 11 The Euclidean distance of T 1N The calculation method is to solve the following quadratic equation: For the two solutions of the system of equations, take 11 Distance d 1N,11 The larger of: d 1N,11 =||T 1N -S 11 ||2 Among them, d 1N,11 Represents endpoint S 11 Intersection point T 1N The Euclidean distance.

8. The mobile robot path planning method based on the improved artificial potential field method according to claim 6 is characterized in that: The M i,j The calculation method is: According to S i , S j The order in the set Q, using the index to get S i , S j The adjacent endpoints are T iB and T j1 , then their midpoint M i,j The position is calculated as: M i,j x =(T iN x +T j1 x ) / 2 M i,j y =(T iN y +T j1 y ) / 2 Among them, M i,j x , M i,j y , T iN x ,T iN y ,T j1 x ,T j1 y Represent possible temporary target points M i,j 、T iN and T j1 The x- and y-coordinates of .

9. The mobile robot path planning method based on the improved artificial potential field method according to claim 3 is characterized in that: The step (4) comprises the following steps: (4.1) Bring each possible temporary target point position into the evaluation function and calculate the moving cost of each possible temporary target point; The evaluation function is: h i =f i +g i f i =||T i -R||2 g i =||T i -T g ||2 Among them, f i represents the actual cost of the i-th possible temporary target point position from the robot's current position, g i represents the estimated cost from the i-th possible temporary target point to the target point, T i represents the position of the i-th possible temporary target point, R represents the coordinates of the robot's current position, T g Indicates the location of the target point; (4.2) Select the total cost h i The smallest possible temporary target point is taken as the final temporary target point; (4.3) When the robot reaches the temporary target point, replace the temporary target point with the original target point and let the robot continue to move towards the original target.

10. The mobile robot path planning method based on the improved artificial potential field method according to claim 1, characterized in that: The adaptive step length method is used to calculate the current moving step length, specifically: λ=w1d1+(w2+w3)d2 Among them, λ is the step size, w i is a Boolean value, i=1,2,3, w1 represents whether the robot is outside the range of the obstacle repulsion force and l meter away from the target point, w2 represents whether the robot is within the range of the obstacle repulsion force, w3 represents whether the robot is within l meter from the target point, l is the set threshold, d1 and d2 are step lengths of different lengths, and d1>d2.

Images