A mobile robot path planning method based on improved artificial potential field method

Abstract

The application relates to a kind of mobile robot path planning methods based on improved artificial potential field method.The method establishes a new repulsion function by introducing sine distance factor, so that the repulsion force received by the robot gradually decreases to zero as it approaches the target point, which can avoid the problem that the robot cannot reach the target point;When encountering local minimum value problem, the temporary target point is calculated by using obstacle boundary point group tangent vector algorithm, some virtual obstacle visible point group is established on the obstacle boundary, the size and shape of the obstacle are fully considered, and the temporary target point is attracted by the gravity, and the local minimum value is jumped out by reaching the temporary target point first;Adaptive step method is used to calculate the distance of the next step of the robot, which reduces the number of iterations and planning time of the algorithm, and can better avoid obstacles, and can avoid the problem that the robot is bounced back due to being too close to the obstacle.The application greatly improves the efficiency of the traditional APF algorithm path planning, and has important theoretical significance and practical value.

Description

TECHNICAL FIELD

[0001] The application belongs to the field of robot navigation and artificial intelligence, and particularly relates to a mobile robot path planning method based on an improved artificial potential field method. BACKGROUND

[0002] Path planning is one of important means for enabling a robot to move autonomously in an environment efficiently. The path planning of the robot mainly includes global path planning and local path planning. The global path planning is to plan a shortest path from a starting point to a target point according to a static environment map, and the local path planning is to use a sensor to acquire dynamic information of an environment in real time, avoid dynamic obstacles, and enable the robot to move along a way point obtained by the global path planning. In the local path planning, the APF is a commonly used method. The method has advantages of simple mathematical model, easy implementation, good adaptability to unknown or dynamic environment, and the like, and is widely applied to active real-time collision avoidance and path planning of the robot. In the method, the task environment of the robot is regarded as a potential field, in a path planning process, an attractive potential field generated by a target point guides the robot to move towards a target direction, and a repulsive potential field generated by an obstacle in the environment enables the robot to move away from the obstacle. However, the traditional artificial potential field method has defects of local minimum value and unattainable target, resulting in an unsatisfactory path. However, the most familiar and concerned problem in most current researches is the situation of falling into a local minimum value, but these methods ignore the influence of the size and shape of the obstacle on the path planning problem, and in order to simplify the problem, the obstacle and the robot are regarded as a particle ignoring the size and shape, which is inconsistent with the actual situation. SUMMARY

[0003] The application aims to provide a mobile robot path planning method based on an improved artificial potential field method. The method improves a repulsive potential field function by introducing a sine distance factor, solves the problem of unattainable target of the APF algorithm, then proposes an obstacle boundary point group tangent vector algorithm, considers the size and shape of the obstacle, solves the problem of local minimum value of the APF algorithm, and improves the path planning efficiency of the APF algorithm and the adaptability to a complex environment. Finally, an adaptive step length method is proposed, a larger step length is used in a safe area to reduce the iteration number of the algorithm and the path planning time, and the step length is reduced near the obstacle. The method of dynamically adjusting the step length can effectively and safely avoid the obstacle, and can avoid the problem of the robot being bounced back because the step length is too large and the robot suddenly approaches the obstacle, and improves the path planning efficiency in general.

[0004] The technical scheme adopted by the application to achieve the above-mentioned purpose is as follows:

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

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

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

[0008] 3) Determine whether the robot has reached the original target point, if yes, save the robot path, otherwise, execute step 4);

[0009] 4) Determine whether the robot has reached the current target point, if yes, 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 facing the current target point position at the current position, and calculate the repulsive force received by the robot using the improved repulsive force function;

[0011] 6) Calculate the attractive force received by the robot, and calculate the resultant force based on the repulsive force;

[0012] 7) Determine whether the robot is trapped in a local minimum based on the resultant force, if yes, execute step 8), otherwise, execute step 9);

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

[0014] 9) Calculate the current moving step using the adaptive step method;

[0015] 10) Calculate the next position point of the robot using the moving step, and move to the next position point, and return to step 4).

[0016] The improved repulsive force function F rep Specifically:

[0017]

[0018] Where ζ represents the positive proportional gain factor of the repulsive force potential field, γ represents the safety distance that the robot needs to maintain from the obstacle, ρ0 represents the maximum influence distance of the obstacle repulsive force field, and ρ i represents the distance between the robot and the i-th obstacle O i :

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

[0020] Where R represents the current position of the robot, and 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 repulsive force range of the robot, 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 repulsive force range of the robot;

[0021] D j represents the distance factor of the robot to the target point when the robot is at the j position, specifically:

[0022]

[0023] wherein ρ j,g represents the Euclidean distance from the current position R j of the robot to the target point T g , n is an empirical value of the improved repulsive force function, ρ 0,g represents the Euclidean distance from the start position R0 of the robot to the target point T g :

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

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

[0026] The temporary target point is calculated by using the obstacle boundary point group tangent vector algorithm, comprising the following steps:

[0027] (1) The position information of all boundary points of obstacles detected by the laser radar within the repulsive force range of the robot is sequentially added to the list Q, Q is an ordered set, that is, any obstacle boundary point O i ∈Q, is a monotonic sequence, wherein, represents the x coordinate of the boundary point O i , represents the y coordinate of the boundary point O i ;

[0028] (2) The boundary point group of each obstacle is calculated;

[0029] (3) The set storing the boundary point group of each obstacle is processed to obtain a possible temporary target point;

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

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

[0032] (2.1) Let Q = (O1, O2, O3, ..., O i ,…), O1, O2, O3,…, 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) O i+1 ,O i+2 ,…,O p Put the current boundary point O i The set S j At the same time, mark the above boundary points 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 S1, S2, S3, ..., S K There are K sets in total, where K represents the number of obstacles currently surrounding 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 as follows:

[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 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:

[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, i.e. there is only one obstacle around the robot currently, the corresponding boundary point group is S1, and let S 11 , S 1N be the first and last boundary points of the set S1 respectively, the two end points corresponding to the obstacle boundary point group, the left intersection point T 11 , S 1N of the tangent line of the circle with the center of S 11 , S 1N and the radius of r is taken as the possible temporary target point;

[0042] (3.2) If K ≥ 2, i.e. there are more than one obstacles around the robot currently, when K = 2, i.e. there are two obstacles around the robot, the corresponding boundary point groups are S i , S j , and let S i1 , S iN be the first and last boundary points of the set S i respectively, the two end points corresponding to the obstacle boundary point group i, S j1 , S jN be the first and last boundary points of the set S j respectively, the two end points corresponding to the obstacle boundary point group j, S i , S j corresponding to the possible temporary target points T i1 , T iN , T j1 , T jN are calculated respectively, and when the distance d i,j , between the two obstacles satisfies the condition: d i,j ≥ 2r + μ, the midpoint M i,j of the two obstacles is also taken as the possible temporary target point, wherein r and μ represent the robot circumscribed circle radius and the given robot turning safety threshold respectively.

[0043] The calculation method of T 11 and T 1N is as follows:

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

[0045]

[0046]

[0047] For the two solutions of the equation group, the one closer to S1N distance d 11,1N the larger one:

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

[0049] wherein d 11,1N denotes the Euclidean distance between the end point S 1N and the left intersection point T 11 ;

[0050] T 1N is calculated by solving the following binary quadratic equation:

[0051]

[0052]

[0053] For the two solutions of the equation system, take the larger one with S 11 distance d 1N,11 :

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

[0055] wherein d 1N,11 denotes the Euclidean distance between the end point S 11 and the right intersection point T 1N .

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

[0057] According to the order of S i , S j in the set Q, the index is obtained S i , S j adjacent end points are T iN and T j1 , then the calculation formula of the midpoint M i,j position is:

[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] wherein M i,j x , M i,j y , T iN x , T iN y , T j1 x , T j1 y respectively represent the x coordinate and y coordinate of the possible temporary target point M i,j , T iN and T j1 .

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

[0062] (4.1) bringing each possible temporary target point position into the evaluation function, and calculating 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] wherein f i represents the actual cost of the i-th possible temporary target point position from the current position of the robot, g i represents the estimated cost of the i-th possible temporary target point position to the target point, T i represents the position of the i-th possible temporary target point, R represents the coordinates of the current position of the robot, and T g represents the position of the target point;

[0068] (4.2) selecting the possible temporary target point with the minimum total cost h i as the final temporary target point;

[0069] (4.3) when the robot reaches the temporary target point position, replacing the temporary target point with the original target point, and letting the robot continue to move towards the original target.

[0070] The current moving step is calculated using the adaptive step method, specifically:

[0071] λ = w1d1 + (w2 + w3)d2

[0072] Wherein, λ is step length, w i is a Boolean value, i = 1, 2, 3, w1 represents whether the robot is outside the range of obstacle repulsion force and outside the distance of l meters from the target point, w2 represents whether the robot is within the range of obstacle repulsion force, w3 represents whether the robot is within the distance of l meters from the target point, l is a set threshold, d1 and d2 are step lengths of different lengths, and d1 > d2.

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

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

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

[0076] 3. The present application designs a self-adaptive step length method, that is, the algorithm dynamically adjusts the iteration step length of the algorithm according to the safety situation of the robot's surrounding environment, which can reduce the time of path planning, and can avoid the problem of being bounced back by the robot suddenly approaching the obstacle too close, and overall improves the efficiency of path planning. BRIEF DESCRIPTION OF DRAWINGS

[0077] Figure 1 The overall flowchart of the present application;

[0078] Figure 2 Force diagram of target unreachable existence condition;

[0079] Wherein, a is the target point between the obstacle and the robot, b is the obstacle between the target point and the robot, and c is the plurality of obstacles existing between the robot and the target point;

[0080] Figure 3 Flowchart of the tangent vector algorithm of the obstacle boundary point group of the present application;

[0081] Figure 4 Possible temporary target point diagram of single obstacle boundary point group tangent vector algorithm;

[0082] Figure 5Multiple obstacle boundary point group tangent vector algorithm possible temporary target point schematic diagram;

[0083] Figure 6 Single obstacle under the application and the traditional APF method repulsion field distribution comparison chart;

[0084] Wherein, a is the APF path and the corresponding target point at the repulsion field distribution, b is the application and the corresponding target point at the repulsion field distribution;

[0085] Figure 7 Multiple obstacles under the application and the traditional APF method repulsion field distribution comparison chart;

[0086] Wherein, a is the APF path and the corresponding target point at the repulsion field distribution, b is the application and the corresponding target point at the repulsion field distribution;

[0087] Figure 8 Single point obstacle under the application and the traditional APF algorithm path planning result comparison chart;

[0088] Wherein, a is the APF, b is the application;

[0089] Figure 9 Consider the shape of the obstacle under the application and the traditional APF algorithm path planning result comparison chart;

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

[0091] Figure 10 Multiple different shape obstacles under the application and the traditional APF algorithm path planning result comparison chart;

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

[0093] Figure 11 The application and other algorithm path planning result comparison chart;

[0094] Wherein, a is a single obstacle, b is a multiple obstacle. DETAILED DESCRIPTION

[0095] The application will be further described in detail below in conjunction with the drawings and examples.

[0096] The application discloses a mobile robot path planning method based on an improved artificial potential field method (improved APF), which mainly has three improvements. Firstly, a new repulsive force field function is designed by introducing a sine distance factor, so that the repulsive force received by the robot gradually decreases to zero as the robot approaches the target point, solving the problem that the robot cannot reach the target point; secondly, in order to solve the local minimum value problem of the APF algorithm, a virtual obstacle point group in the visible angle of the robot on the obstacle boundary is designed, the algorithm considers the size and dimension of the obstacle, changes the previous situation of simply regarding the obstacle as a mass point without volume, and is more in line with the actual environment; finally, the step length is dynamically adjusted, that is, a larger step length is used in the safe area to reduce the iteration number and path planning time of the algorithm, and a smaller step length is used near the obstacle to better avoid the obstacle and avoid the problem that the robot is bounced back due to being too close to the obstacle.

[0097] comprising the following steps:

[0098] 1) determining the starting position of the robot and the original target point position;

[0099] 2) initializing the current target point of the robot as the original target point;

[0100] 3) judging whether the robot reaches the original target point, if yes, saving the robot path, otherwise, executing step 4);

[0101] 4) judging whether the robot reaches the current target point, if yes, setting the current target point as the original target point and returning to step 3), otherwise, executing step 5);

[0102] 5) obtaining the obstacle information of the robot facing the current target point position at the current position, and calculating the repulsive force received by the robot by using the improved repulsive force function;

[0103] 6) calculating the attractive force received by the robot, and calculating the resultant force based on the repulsive force;

[0104] 7) judging whether the robot falls into a local minimum value based on the resultant force, if yes, executing step 8), otherwise, executing step 9);

[0105] 8) calculating a temporary target point by using the obstacle boundary point group tangent vector algorithm, setting the temporary target point as the current target point, and returning to step 5);

[0106] 9) calculating the current moving step length by using the adaptive step length method;

[0107] 10) calculating the next position point of the robot by using the moving step length, and moving to the next position point, and returning to step 4).

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

[0109] 1. Such as Figure 1 As shown, the present invention largely follows the conventional APF algorithm process for path planning, with three differences. First, unlike the conventional APF algorithm, the present invention uses an improved repulsion function when calculating the robot's repulsion. Second, when trapped in a local minimum, the obstacle boundary point group tangent vector algorithm is used to calculate a temporary target point, allowing the robot to escape the local minimum by first reaching the temporary target point and then moving towards the original target point. Finally, when calculating the robot's next position to move, an adaptive step size is used to calculate the movement step size.

[0110] 2. In order to solve the problem of unreachable target (such as Figure 2 As shown), when calculating the repulsive force 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 between itself and the obstacle, ρ0 represents the maximum influence distance of the obstacle repulsive field, and ρ 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 D 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, where 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. j It represents 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 Indicates the robot's starting point R0 to the target point T g The Euclidean distance of :

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

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

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

[0120] 3. When trapped in a local minimum, the temporary target point is calculated by using the obstacle boundary point group tangent vector algorithm, as shown in the specific calculation process: Figure 3

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

[0122] is a monotonic sequence

[0123] wherein represents the x coordinate of the boundary point O i , and represents the y coordinate of the boundary point O i ;

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

[0125] Q=(O1,O2,O3,…,O i ,…)

[0126] O1,O2,O3,…,O i ,… is the position of each boundary point in the set Q containing all the boundary point groups of the obstacles, wherein the position of O i is Mark all the points as unvisited, and traverse all the boundary points in Q. If the current boundary point O i ​If not, create a new set S j , j = 1, 2, 3, …) and mark the current boundary point as visited, then find the boundary end point O of the obstacle by judging whether the distance between adjacent points is less than the threshold value e p , i.e. d i,i+1 < e i+1,i+2 , d i+2,i+3 < e p-1,p , d p,p+1 > e i,i+1 , where d i represents the Euclidean distance between adjacent points O i+1 and O i,i+1 in set Q. That is:

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

[0128] Then put O i+2 , O p , …, O i into the set S j that stores the current boundary point O K , and mark these boundary points as visited, repeat the above process until all points in set Q are visited, at this time set Q will be divided into S1, S2, S3, …, SK, where K represents the number of obstacles around the robot at the current time. And S j , j = 1, 2, 3, …, K) respectively stores the position of the boundary point group of each obstacle in the current view of the robot.

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

[0130] 3.3.1 Figure 4 As shown in FIG. 3, if K = 1, i.e. there is only one obstacle around the robot at the current time, the corresponding boundary point group is S1, and S 11 and S 1N are the first and last boundary points of set S1, respectively, corresponding to the two end points of the boundary point group of this obstacle. Get the left intersection point T 11 and the right intersection point T 1N of the tangent line of the circle with center S 11 , S 1N and radius r as possible temporary target points, and the calculation method of T 11 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 If Figure 5 As shown, 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 the obstacle boundary point group. j1 , S jN The set S j The first and last boundary points of the obstacle point group correspond to the two endpoints of the obstacle boundary point group. i , S jThe corresponding possible temporary target points T i1 , T iN , T j1 , T jN , 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. Among them, r and μ represent the robot's circumscribed circle radius and the given robot turning safety threshold, respectively. In this paper, the value is taken as its circumscribed circle radius r. For the midpoint M i,j The position is calculated as follows:

[0145] According to S i , S j The order in the 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 Represents 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, the temporary target point with the minimum movement cost is selected as the final temporary target point according to the corresponding movement cost function. First, the position of each possible temporary target point is brought into the evaluation function to calculate the movement cost of each possible temporary target point.

[0150] wherein the evaluation function is:

[0151] h i = f i + g i

[0152] wherein f i represents the actual cost of the i-th possible temporary target point position from the current position of the robot, g i represents the estimated cost of the i-th possible temporary target point position 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] wherein T i represents the position of the i-th possible temporary target point, R represents the coordinates of the current position of the robot, and T g represents the position of the target point.

[0156] Then, the possible temporary target point with the minimum total cost h i is selected as the final temporary target point. Finally, when the robot reaches the position of the temporary target point, the temporary target point is replaced with the original target point, and the robot continues to move towards the original target.

[0157] 4. When calculating the next position point to which the robot is to move, an adaptive step length is used to calculate the movement step length. When the robot is not within the repulsion range of the obstacle, a larger step length d1 can be used to reduce the number of iterations of the algorithm, thereby reducing the path planning time. When the robot is within the repulsion range of the obstacle, a smaller step length d2 can be used to avoid the situation of back and forth oscillation when the robot is trapped in a local minimum. At the same time, in order to accurately reach the target point, if the robot is not within the repulsion range of the obstacle, but the robot is already relatively close to the target point, a smaller step length d2 should also be used to prevent the robot from oscillating around the target point under the limitation of precision and ultimately failing to reach the target point. The calculation formula of the dynamic adjustment of the step length is:

[0158] λ = w1d1 + (w2 + w3)d2

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

[0160] like Figure 6 As shown in the figure, a very close obstacle is placed behind the target point, just within the range of the obstacle's repulsive potential field and located between the robot's starting point and the final target point. Using the improved APF algorithm to plan a path based on the given starting and target points, it is found that due to the improved repulsive field function, the repulsive force on the robot decreases as the distance from the target point decreases, allowing the robot to successfully reach the final target without oscillating back and forth around the target point. 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, allowing the robot to successfully reach the target point.

[0162] like Figure 8 As shown in Figure 2, 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 falls into a local minimum near the obstacle and stagnates, 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 Figure 9As shown, the obstacles are set as single obstacles considering size and shape, which are respectively planar obstacle (case 1) and U-shaped obstacle (case 2), both of which are the most likely to cause the traditional APF algorithm to fall into local minimum. Since the point group tangential vector algorithm for obstacle boundary points involved in the present application considers the actual environment, that is, these point groups are visible point groups from the perspective of the robot, and considering the volume and shape of the obstacle, the point group behind the obstacle is obviously detected only when the robot moves to the rear of the obstacle. Using the improved APF algorithm for path planning experiments, it can be seen that using the improved APF algorithm for path planning can safely and effectively reach the target point, while using the traditional APF algorithm fails to plan the path.

[0164] As shown in Figure 10 , a plurality of obstacles of various shapes are set, and different starting points and ending points are set (corresponding to case 1 and case 2, respectively). Using the improved APF algorithm and the traditional APF algorithm for path planning, respectively, it can be seen that the improved APF algorithm can safely and effectively reach the target point, while the traditional APF algorithm fails to plan the path. The effectiveness of the present application is verified.

[0165] As shown in Figure 11 , 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, and the results are shown in Table 1. It can be seen that the algorithm in the present application is significantly better than these algorithms.

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

[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 its current position facing the current target point, and use the improved repulsion function to calculate the repulsive force on the robot; 6) Calculate the gravitational force on the robot and, based on the repulsive force, calculate the net force; 7) Determine whether the robot is trapped in a local minimum based on the combined force. If so, execute step 8); otherwise, execute step 9); 8) Calculate the temporary target point using the obstacle boundary point group tangent vector algorithm, set the temporary target point as the current target point, and return to step 5); 9) Calculate the current moving step length using the adaptive step length method; 10) Calculate the next position of the robot using the moving step length, move to the next position, and return to step 4); 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 Represents the robot and the i-th 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 i-th obstacle detected by the robot at its current position facing the current target point and within the range of its repulsive force, i = 1, …, N, where N is the total number of obstacles detected by the robot at its current position facing the current target point and within the range of its repulsive force; D j It represents 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 R0 to the target point T g The Euclidean distance of : r j,g =||R j -T g ||2 r 0,g =||R0-T g ||2。 2. The mobile robot path planning method based on the improved artificial potential field method according to claim 1, characterized in that: The method of calculating the temporary target point using the obstacle boundary point group tangent vector algorithm includes the following steps: (1) The position information of all obstacle boundary points detected by the laser radar within the robot's repulsive range is sequentially added to the list Q, where Q is an ordered set, that is, any obstacle boundary point O in Q i ∈Q, is a monotonic 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 of boundary point groups of each obstacle 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.

3. The mobile robot path planning method based on the improved artificial potential field method according to claim 2, characterized in that: The step (2) comprises the following steps: (2.1) Let Q = (O1, O2, O3, ..., O i ,…), O1, 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) O i+1 ,O i+2 ,…,O p Put the current boundary point O i The set 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 surrounding 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.

4. The mobile robot path planning method based on the improved artificial potential field method according to claim 3, 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。 5. The mobile robot path planning method based on the improved artificial potential field method according to claim 2, 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, and S 11 , S 1N They are the first and last boundary points of the set S1, corresponding 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 and radius r 11 and right intersection point T 1N As a possible temporary destination point; (3.2) If K ≥ 2, that is, there is more than one obstacle around the robot. When K = 2, that is, 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 correspond to the two endpoints of the obstacle i 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 , meeting 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 the given robot turning safety threshold, respectively.

6. The mobile robot path planning method based on the improved artificial potential field method according to claim 5, 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.

7. The mobile robot path planning method based on the improved artificial potential field method according to claim 5, characterized in that: The M i,j The calculation method is: According to S i , S j The order in set Q, using the index to get S i , S j Adjacent endpoints are T iN 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 .

8. The mobile robot path planning method based on the improved artificial potential field method according to claim 2, 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 movement 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 Indicates 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 used 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.

9. 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 It is a Boolean value, i = 1, 2, 3, w1 represents whether the robot is outside the range of the obstacle repulsion force and 1 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 1 meter away from the target point, l is the set threshold, d1 and d2 are step lengths of different lengths, and d1>d2.

Images