Robot dynamic path planning method and system based on improved artificial potential field algorithm

Abstract

The present invention proposes a robot dynamic path planning method and system based on an improved artificial potential field algorithm, and relates to the field of robot path planning technology. It includes judging whether the robot will collide with a dynamic obstacle and defining a dynamic obstacle avoidance area; in the dynamic obstacle avoidance area, defining multiple layers of repulsive potential fields, and introducing an exponential function to calculate the repulsion of each layer of repulsive potential fields; on the basis of the repulsion of each layer of repulsive potential fields, combining the relationship between the movement direction of the robot relative to the obstacle and the obstacle velocity vector, obtaining the repulsion of each layer of repulsive potential fields after strengthening; superimposing the repulsion of each layer of repulsive potential fields after strengthening to obtain the total repulsive field, and updating the nodes between the starting point and the end point of the dynamic path and located in the dynamic obstacle avoidance area, and planning the dynamic path of the robot. The present invention sets a multi-layer repulsive field in combination with an exponential function, so that the repulsion of the robot is sharply enhanced when it approaches an obstacle, effectively avoiding collisions.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot path planning, and in particular relates to a robot dynamic path planning method and system based on an improved artificial potential field algorithm. Background Art

[0002] In recent years, with the continuous advancement of hardware, robots have been widely used in many fields such as medicine, military, industry, agriculture and service industry. Robot path planning refers to finding an optimal or suboptimal collision-free path from the starting point to the end point under certain evaluation criteria. Many mature algorithms have been proposed to solve this problem, among which the RRT (rapid exploration tree based on sampling) algorithm performs well in high-dimensional and multi-constrained path planning, especially suitable for single query. Commonly used algorithms for local path planning include DWA (dynamic window method) and APF (artificial potential field method).

[0003] The artificial potential field method relies on the global potential function to guide the robot's movement, but it is easy to fall into the local minimum. At the same time, it fails to consider the motion characteristics of moving obstacles.

[0004] However, in actual applications, robots are often in dynamic environments. To avoid dynamic obstacles during robot movement, local path planning is still necessary. Currently, most robot path planning is based on static obstacles. When dynamic obstacles appear, the robot's movement is less adaptable to the original path. In addition, the local path planning of current robots facing dynamic obstacles still has at least the following technical problems:

[0005] (1) During the movement, the robot cannot accurately determine whether the moving obstacle affects the original movement trajectory;

[0006] (2) It is unable to accurately grasp the timing of local path dynamic planning, and instead chooses to dynamically avoid all obstacles, wasting computing resources;

[0007] (3) Traditional artificial potential field algorithms include attractive potential fields and repulsive potential fields, among which the repulsive potential field is mainly used to avoid collisions between robots and obstacles. However, the above repulsive potential field does not take into account the motion information of moving obstacles, and cannot achieve targeted dynamic adjustment of the repulsive potential field. In addition, there is still room for improvement in avoiding collisions. Summary of the invention

[0008] In order to overcome the shortcomings of the above-mentioned prior art, the present invention provides a robot dynamic path planning method and system based on an improved artificial potential field algorithm, which takes into account the motion information of the moving obstacles, introduces the velocity vector of the moving obstacles, and adjusts the size of the repulsive force according to the relative motion direction of the robot, thereby enhancing the effectiveness of the repulsive force. By setting a multi-layer repulsive field and combining it with an exponential function, the repulsive force of the robot is sharply enhanced when it approaches an obstacle, thereby effectively avoiding collision.

[0009] To achieve the above objectives, one or more embodiments of the present invention provide the following technical solutions:

[0010] A first aspect of the present invention provides a robot dynamic path planning method based on an improved artificial potential field algorithm.

[0011] The robot dynamic path planning method based on the improved artificial potential field algorithm includes the following steps:

[0012] Get the movement speed of the dynamic obstacles on the original movement path of the robot;

[0013] Determine whether the robot will collide with a dynamic obstacle. If so, define a dynamic obstacle avoidance area.

[0014] Based on the dynamic obstacle avoidance area and the original motion path, determine the starting point and end point of the dynamic path;

[0015] In the dynamic obstacle avoidance area, multiple layers of repulsive potential fields are defined, and an exponential function is introduced to calculate the repulsive force of each layer of repulsive potential fields;

[0016] Based on the repulsive force of each layer of the repulsive potential field, the repulsive force of each layer of the repulsive potential field after strengthening is obtained by combining the relationship between the movement direction of the robot relative to the obstacle and the obstacle velocity vector;

[0017] The repulsive force of each layer of repulsive potential field after superposition and strengthening is obtained to obtain the total repulsive field, and combined with the attractive potential field to obtain the total potential field;

[0018] Based on the total potential field, the total potential field force is obtained, and based on the total potential field force, the nodes between the starting point and the end point of the dynamic path and located in the dynamic obstacle avoidance area are updated to plan the dynamic path of the robot.

[0019] A second aspect of the present invention provides a robot dynamic path planning system based on an improved artificial potential field algorithm.

[0020] The robot dynamic path planning system based on the improved artificial potential field algorithm includes:

[0021] The obstacle speed acquisition module is configured to: acquire the movement speed of the dynamic obstacle on the original movement path of the robot;

[0022] The collision judgment module is configured to: judge whether the robot will collide with a dynamic obstacle, and if so, define a dynamic obstacle avoidance area;

[0023] The dynamic path starting point determination module is configured to: determine the starting point and the end point of the dynamic path based on the dynamic obstacle avoidance area and the original motion path;

[0024] The multi-layer repulsive potential field division module is configured to: delineate multiple layers of repulsive potential fields in the dynamic obstacle avoidance area, and introduce an exponential function to calculate the repulsive force of each layer of the repulsive potential field;

[0025] The repulsion strengthening module is configured to: based on the repulsion of each layer of the repulsion potential field, combine the relationship between the movement direction of the robot relative to the obstacle and the obstacle velocity vector to obtain the repulsion of each layer of the repulsion potential field after strengthening;

[0026] The total potential field calculation module is configured to: superimpose the repulsive force of each layer of repulsive potential field after strengthening to obtain the total repulsive field, and combine it with the attractive potential field to obtain the total potential field;

[0027] The update module is configured to obtain the total potential field force based on the total potential field, and based on the total potential field force, update the nodes between the starting point and the end point of the dynamic path and located in the dynamic obstacle avoidance area to plan the dynamic path of the robot.

[0028] One or more of the above technical solutions have the following beneficial effects:

[0029] The present invention provides a robot dynamic path planning method and system based on an improved artificial potential field algorithm, which introduces a layered artificial potential field algorithm of dynamic obstacle motion vectors. By combining a layered manner and an exponential function, the repulsive force of the robot is sharply enhanced when approaching an obstacle, thereby effectively avoiding collision. The repulsive force is dynamically adjusted according to the speed and direction of the moving obstacle to ensure that the repulsive force in the direction of obstacle movement is greater than that in other directions, thereby enhancing the robustness of the algorithm.

[0030] Through the method of the present invention, the robot can safely and effectively plan the path in a complex dynamic environment to avoid false triggering and collision. This optimization method provides a more flexible strategy for path planning of moving obstacles, making the robot more adaptable in a dynamic environment.

[0031] The present invention uses a speed obstacle algorithm to determine whether the robot will collide with a dynamic obstacle, establishes collision judgment conditions, and determines whether the robot will collide with a dynamic obstacle. Speed Whether it is within the speed barrier, so as to accurately judge whether the dynamic obstacle will affect the original movement of the robot.

[0032] The present invention defines a dynamic obstacle avoidance area, sets a collision range, uses the collision range as the dynamic obstacle avoidance range, and takes the center position of the dynamic obstacle as the center of the circle and the dynamic obstacle avoidance range as the center. The radius is used to determine the dynamic obstacle avoidance area to avoid wasting computing resources.

[0033] Advantages of additional aspects of the present invention will be given in part in the following description, and in part will become obvious from the following description, or will be learned through practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] The accompanying drawings in the specification, which constitute a part of the present invention, are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute improper limitations on the present invention.

[0035] Figure 1 This is a flow chart of the method of the first embodiment.

[0036] Figure 2 This is a diagram of the force field effect using the traditional artificial potential field algorithm.

[0037] Figure 3 This is a force field effect diagram of improving the artificial potential field algorithm using the first embodiment.

[0038] Figure 4 This is a demonstration of the obstacle avoidance effect of a robot moving towards the target point using the traditional artificial potential field algorithm.

[0039] Figure 5 This is a demonstration diagram of the obstacle avoidance effect of a robot reaching the target point using the traditional artificial potential field algorithm.

[0040] Figure 6 This is a demonstration diagram of the obstacle avoidance effect of the robot moving towards the target point using the artificial potential field algorithm improved by the first embodiment.

[0041] Figure 7 This is a demonstration diagram of the obstacle avoidance effect of the robot reaching the target point using the first embodiment to improve the artificial potential field algorithm. DETAILED DESCRIPTION

[0042] It should be noted that the following detailed descriptions are exemplary and are intended to provide further explanation of the present invention. Unless otherwise specified, all technical and scientific terms used herein have the same meanings as those commonly understood by those skilled in the art to which the present invention belongs.

[0043] It should be noted that the terms used herein are for describing specific embodiments only and are not intended to be limiting of exemplary embodiments according to the present invention.

[0044] In the absence of conflict, the embodiments of the present invention and the features of the embodiments may be combined with each other.

[0045] Embodiment 1

[0046] This embodiment discloses a robot dynamic path planning method based on an improved artificial potential field algorithm.

[0047] like Figure 1 As shown, the robot dynamic path planning method based on the improved artificial potential field algorithm includes the following steps:

[0048] Get the movement speed of the dynamic obstacles on the original movement path of the robot;

[0049] Determine whether the robot will collide with a dynamic obstacle. If so, define a dynamic obstacle avoidance area.

[0050] Based on the dynamic obstacle avoidance area and the original motion path, determine the starting point and end point of the dynamic path;

[0051] In the dynamic obstacle avoidance area, multiple layers of repulsive potential fields are defined, and an exponential function is introduced to calculate the repulsive force of each layer of repulsive potential fields;

[0052] Based on the repulsive force of each layer of the repulsive potential field, the repulsive force of each layer of the repulsive potential field after strengthening is obtained by combining the relationship between the movement direction of the robot relative to the obstacle and the obstacle velocity vector;

[0053] The repulsive force of each layer of repulsive potential field after superposition and strengthening is obtained to obtain the total repulsive field, and combined with the attractive potential field to obtain the total potential field;

[0054] Based on the total potential field, the total potential field force is obtained, and based on the total potential field force, the nodes between the starting point and the end point of the dynamic path and located in the dynamic obstacle avoidance area are updated to plan the dynamic path of the robot.

[0055] In the above technical solution, the original motion path of the robot can be understood as a pre-planned global path. The robot moves along the pre-planned global path. When there are dynamic obstacles within the global path, the robot dynamic path planning method based on the improved artificial potential field algorithm provided in this embodiment is executed to prevent the robot from colliding with the moving dynamic obstacles. Specifically, a global path can be obtained through the global planning informed-RRT* (information-enhanced fast random tree) algorithm, and then dynamic path planning can be performed on it during the robot's movement.

[0056] The above-mentioned dynamic path planning can be understood as the local path planning for dynamic obstacles performed when the robot moves along the global path and encounters dynamic obstacles.

[0057] It can be understood that before obtaining the movement speed of the dynamic obstacle on the original movement path of the robot, it also includes:

[0058] Determine whether there are dynamic obstacles on the robot's set motion path. If not, Figure 1 As shown, the robot will continue to move according to the pre-planned global path.

[0059] If there is a dynamic obstacle, in some embodiments, the motion tracking of the dynamic obstacle is implemented based on Kalman filtering to obtain the velocity vector information of the dynamic obstacle in motion.

[0060] The velocity vector information of the dynamic obstacle obtained above will be used in the subsequent speed obstacle algorithm to determine whether the robot and the dynamic obstacle will collide, and will be used in the layered artificial potential field algorithm that introduces the motion vector of the dynamic obstacle when the traditional artificial potential field algorithm is improved.

[0061] (I) Next, the process of tracking the motion of a dynamic obstacle based on Kalman filtering and obtaining the velocity vector information of the dynamic obstacle in motion will be described in detail:

[0062] 1. Set the state vector

[0063] State vector:

[0064] in:

[0065] is the position of the obstacle in the x direction;

[0066] is the position of the obstacle in the y direction;

[0067] is the speed of the obstacle in the x direction;

[0068] is the speed of the obstacle in the y direction.

[0069] 2. Set the observation vector

[0070] Observation vector:

[0071] The obstacle to be measured is Direction location;

[0072] The obstacle to be measured is The location of the direction.

[0073] 3. Set up the state transfer matrix and observation matrix

[0074] State transition matrix

[0075] Observation Matrix

[0076] Where Δt is the time interval when the continuous-time system is discretized and used to update the system state.

[0077] 4. Set initial conditions

[0078] In actual situations, the initial conditions of obstacle motion are obtained through the lidar sensor :

[0079]

[0080] and is the measured or estimated value of the initial position;

[0081] is the measured or estimated value of the initial velocity;

[0082] is the measurement or estimate of the initial direction of motion.

[0083] 5. Set the initial covariance matrix

[0084] The initial covariance matrix represents the uncertainty in the initial state estimates.

[0085] Tentative , and the deviation value will be dynamically corrected gradually through Kalman filtering.

[0086] For example:

[0087]

[0088] in:

[0089] The initial position The variance of

[0090] The initial position The variance of

[0091] is the initial speed The variance of

[0092] The initial direction The variance of .

[0093] Kalman filter algorithm steps:

[0094] Prediction Steps:

[0095] Predicted state vector:

[0096]

[0097] in, represents the predicted estimate of the obstacle state at time step k given the information at time step k−1. represents the estimate of the obstacle state at time step k−1 given the information at time step k−1.

[0098] Prediction covariance matrix:

[0099]

[0100] in, represents the covariance matrix of the predicted state estimate at time step k given the information at time step k−1. represents the covariance matrix of the state estimate at time step k−1 given the information at time step k−1. represents the process noise covariance matrix.

[0101] Update steps:

[0102] Calculate the Kalman gain:

[0103]

[0104] in, represents the Kalman gain at K time steps, represents the measurement noise covariance matrix, and H is the observation matrix, which maps the state vector to the measurement space.

[0105] Update the state estimate:

[0106]

[0107] in, Represents the updated obstacle state at k time steps.

[0108] Update the covariance matrix:

[0109]

[0110] in, represents the covariance matrix of the predicted state estimate at time k, represents the identity matrix, which has the same size as the dimension of the state vector x.

[0111] Among them, the physical meaning of the state vector is:

[0112] , represents the estimated position of the obstacle at time k, , represents the estimated speed of the obstacle at time k. , is the velocity vector of the obstacle at time k.

[0113] In the above process, if Figure 1 As shown in FIG. 1 , after obtaining the velocity vector information of the moving dynamic obstacle based on the Kalman filter, it is also necessary to obtain the center point coordinates and area of ​​the dynamic obstacle. The obtained center point coordinates of the dynamic obstacle will be applied to the subsequent process of demarcating the dynamic obstacle avoidance area.

[0114] (ii) After obtaining the movement speed of the dynamic obstacle on the robot's original motion path, use the VelocityObstacle algorithm to determine whether the dynamic obstacle has an impact on the path.

[0115] Specifically,

[0116] Use the speed obstacle function to determine whether the robot collides with an obstacle:

[0117] The logic of determining whether a collision occurs is for the robot and obstacles , if the robot The speed at the obstacle speed barrier, a collision may occur.

[0118] The formula of collision judgment logic:

[0119] Relative position and relative velocity:

[0120] : For robots and obstacles The relative position of For robots location; For obstacles location.

[0121] : For robots and obstacles The relative speed of For robots speed; For obstacles The speed barrier function is defined as:

[0122]

[0123] in, Represents the dot product of vectors; represents the magnitude of a vector; Indicates time; Represents a robot The radius of .

[0124] Collision judgment condition: judge robot Speed Is it a speed barrier? Inside:

[0125]

[0126] If the above inequality holds, the robot will If it is determined that there will be no collision, then Figure 1 As shown, the robot continues to move according to the global planning path; if it is determined that a collision will occur, it is necessary to further define the dynamic obstacle avoidance area.

[0127] 3. Delineating dynamic obstacle avoidance areas

[0128] The dynamic obstacle avoidance strategy of this embodiment mainly includes the following two steps:

[0129] Define the speed obstacle function based on the relative position and relative speed of the robot and the dynamic obstacle ;

[0130] Set up collision judgment conditions to judge the robot Speed Is there a speed barrier? Inside.

[0131] The obstacles that may collide are divided into obstacle avoidance ranges using the encirclement method. The obstacle avoidance range is the sum of the robot's radius, the obstacle's own radius, and four times the robot's speed. Dynamic obstacle avoidance is performed within this range.

[0132] Define variables:

[0133] Represents a robot The position vector of Represents a robot The radius of

[0134] Represents a robot The velocity vector of Represents a robot The speed of

[0135] Indicates obstacles The position vector of Indicates obstacles The radius of .

[0136] Collision Range:

[0137] The collision range is defined as:

[0138]

[0139] in, is the collision range.

[0140] Dynamic obstacle avoidance range:

[0141] obstacle For robots The obstacle avoidance range can be represented by an enclosing circle, with the center being the location of the obstacle and the radius being the collision range:

[0142]

[0143] in, is the dynamic obstacle avoidance range.

[0144] It can be understood that after defining the dynamic obstacle avoidance range Then, the center of the dynamic obstacle is taken as the center of the circle and the collision range is taken as the center of the circle. The radius is used to determine the dynamic obstacle avoidance area.

[0145] Determine whether it is within the dynamic obstacle avoidance range:

[0146] Judging Robot Is there an obstacle? Dynamic obstacle avoidance range:

[0147]

[0148] If the robot In the obstacle If the dynamic obstacle avoidance range is reached, the improved artificial potential field algorithm is called to perform dynamic obstacle avoidance;

[0149] If the robot Not in obstacles If the dynamic obstacle avoidance range is within the range, the movement will continue along the original path.

[0150] (iv) Determine the starting point and end point of the dynamic path based on the dynamic obstacle avoidance area and the original motion path.

[0151] Determine whether the original path node passes through the dynamic obstacle avoidance area:

[0152] Set the center of the dynamic obstacle avoidance area (the position of the obstacle) is , , are the centers of the dynamic obstacle avoidance areas The horizontal and vertical coordinates of , the point on the original motion path is ,in is the index of the point, , are points on the original motion path For each point on the original motion path , calculate it to the center of the circle Distance , using the distance formula:

[0153]

[0154] If for all points on the original motion path , both have , then the entire original motion path is within the circle. The mathematical expression is:

[0155]

[0156] If there is at least one point Make , then at least part of the original motion path is in the dynamic obstacle avoidance area, which is equivalent to the original motion path passing through the obstacle avoidance area.

[0157] When the original path passes through the dynamic obstacle area, look for dynamic path nodes, including:

[0158] Determine whether the starting point and end point of the original motion path are within the dynamic obstacle avoidance area:

[0159] If the starting point of the original motion path is within the dynamic obstacle avoidance area, then find a node on the original motion path that is closest to the dynamic obstacle avoidance area as the end point of the dynamic path, and use the starting point of the original motion path as the starting point of the dynamic path;

[0160] If the end point of the original motion path is within the dynamic obstacle avoidance area, find a node on the original motion path that is closest to the dynamic obstacle avoidance area as the starting point of the dynamic path, and use the end point of the original motion path as the end point of the dynamic path;

[0161] If the starting point and the end point of the original motion path are not in the dynamic obstacle avoidance area, that is, the original motion path passes through the dynamic obstacle avoidance area, then find the two nodes closest to the dynamic obstacle avoidance area on the original motion path and use them as the starting point and the end point of the dynamic path respectively.

[0162] In order to perform dynamic obstacle avoidance within the obstacle avoidance range, we need to determine the two nodes Node1 (node ​​1) and Node2 (node ​​2) on the original path that are closest to the obstacle avoidance area.

[0163] Find the node:

[0164]

[0165]

[0166] in, for A point on (the original path).

[0167] 5. Improving artificial potential field algorithm based on velocity vector to achieve dynamic obstacle avoidance

[0168] Before introducing the improved artificial potential field algorithm, the traditional artificial potential field algorithm is first introduced.

[0169] Basic functions of traditional artificial potential field (mainly for avoiding static obstacles, without considering the motion information of moving obstacles):

[0170] Attractive potential field: The purpose of the attractive potential field is to guide the robot to the target position. Its basic formula is:

[0171]

[0172] in:

[0173] is the current position of the robot;

[0174] is the target location;

[0175] is the attraction coefficient;

[0176] It is an attractive potential field.

[0177] Repulsive potential field: The repulsive potential field is used to avoid collision between the robot and obstacles. Its basic form is:

[0178]

[0179] in:

[0180] is the location of the obstacle;

[0181] is the repulsion coefficient;

[0182] is the repulsive potential field;

[0183] Is a threshold distance. When the distance between q and qobs is less than or equal to this threshold, the function Urep(q) will calculate a repulsion value, otherwise it is 0.

[0184] This embodiment introduces a hierarchical artificial potential field algorithm for dynamic obstacle motion vectors, which mainly improves and optimizes the repulsive potential field in the traditional artificial potential field from two aspects:

[0185] (1) In the dynamic obstacle avoidance area, multiple layers of repulsive potential fields are defined, and an exponential function is introduced to calculate the repulsive force of each layer of repulsive potential fields;

[0186] (2) Based on the repulsive force of each layer of the repulsive potential field, the repulsive force of each layer of the repulsive potential field after strengthening is obtained by combining the relationship between the robot's movement direction relative to the obstacle and the obstacle's velocity vector.

[0187] Velocity vector and repulsion control:

[0188] To enhance the effectiveness of the repulsive force, we introduce the velocity vector of the moving obstacle , and adjust the repulsive force according to the relative motion direction of the robot. Assume that the current position of the robot is , and the relative position to the obstacle is .

[0189] Repulsive potential field optimization (only the repulsive field is optimized):

[0190] 1) Multi-level repulsive potential field:

[0191] Divide the repulsive field into layer, and the repulsion of each layer is enhanced by an exponential function.

[0192] The formula is as follows:

[0193]

[0194] in:

[0195] It is The distance threshold of the layer; Indicates the distance between each layer;

[0196] and It is Repulsion parameters of the layers;

[0197] For the The repulsive force of the layer repulsive potential field.

[0198] 2) Adjust the repulsive force:

[0199] On the basis of each layer of repulsion, according to the relationship between the direction of movement and the speed of the obstacle, the repulsion in the direction of movement is further enhanced. as follows:

[0200]

[0201] in, is the repulsive force of each layer of repulsive potential field after strengthening;

[0202] is the velocity vector of the dynamic obstacle;

[0203] is the relative motion direction between the robot and the dynamic obstacle;

[0204] is a positive scalar coefficient used to control the influence of the velocity relationship on the repulsive potential field.

[0205] (6) The repulsive force of each layer of repulsive potential field after superposition and strengthening is obtained to obtain the total repulsive field, and combined with the attractive potential field to obtain the total potential field, and based on the total potential field, the total potential field force is obtained.

[0206] Total repulsive field

[0207] The total repulsive potential field is the superposition of the repulsive forces of all layers:

[0208]

[0209] Potential field function

[0210] Combining the attractive potential field and the repulsive potential field, the total potential field is defined as:

[0211]

[0212] Potential Field Force

[0213] Compute the forces acting on the robot by computing the gradient of the potential field:

[0214]

[0215] The gradient of attraction is:

[0216]

[0217] The gradient of the repulsive force is:

[0218]

[0219] The gradient of the repulsive force for each layer can be expressed as:

[0220]

[0221] The total force is a combination of attractive and repulsive forces:

[0222]

[0223] This embodiment improves the traditional artificial potential field algorithm and proposes a layered artificial potential field algorithm that introduces dynamic obstacle motion vectors to effectively avoid collisions and improve robustness from the following two aspects:

[0224] Multi-layer repulsion field: By combining the layered method with the exponential function, the repulsion of the robot increases sharply when it approaches an obstacle, effectively avoiding collision.

[0225] Application of velocity vector: Dynamically adjust the repulsive force according to the speed and direction of the moving obstacle to ensure that the repulsive force in the direction of the obstacle's movement is greater than in other directions, thereby enhancing the robustness of the algorithm.

[0226] In this way, the robot can safely and effectively plan paths in complex dynamic environments and avoid false triggering of collisions. This optimization method provides a more flexible strategy for path planning around moving obstacles, making the robot more adaptable in dynamic environments.

[0227] (VII) Based on the total potential field force, update the nodes between the starting point and the end point of the dynamic path and within the dynamic obstacle avoidance area to plan the dynamic path of the robot.

[0228] (1) Dynamic path node update mechanism:

[0229] When a dynamic obstacle or environmental change is detected, the position of the path node can be updated by the following formula:

[0230]

[0231] is the updated path node position;

[0232] is the original path node position;

[0233] It is an artificially set adjustment step to control the update amplitude;

[0234] It is the resultant force of the repulsive potential field and the gravitational potential field.

[0235] (2) Path correction algorithm

[0236] When the resultant force is zero or the robot is stagnant, a path correction strategy can be introduced, such as using a certain reverse force to readjust the path node:

[0237]

[0238] The reverse adjustment coefficient is set artificially to guide the robot out of the local minimum.

[0239] The reverse force is artificially set, usually set to a force opposite to the target direction.

[0240] 8. Experimental part

[0241] like Figure 2 and Figure 3 As shown, there are force field effect diagrams using the traditional artificial potential field algorithm and the force field effect diagrams using the improved artificial potential field algorithm of this embodiment.

[0242] exist Figure 2 and Figure 3 middle:

[0243] The orange dots represent dynamic obstacles; the blue dotted line represents the gravitational potential field emitted by the target point; each purple closed line from the inside to the outside represents the contour line of the potential energy value of the potential field at different degrees; the red closed line represents the boundary of each layer of the layered potential field.

[0244] Figure 3 The blue arrow in the figure represents the velocity vector of the moving obstacle.

[0245] exist Figure 2 In the figure, the force field around the moving obstacle is uniformly distributed, and the influence of the obstacle motion vector on the potential field is not reflected; in the subsequent potential field obstacle avoidance process, Figure 4 and Figure 5 In the process, it is very likely that obstacle avoidance will not be timely and the movement path will be far.

[0246] exist Figure 3 In the process, the force field around the moving obstacle is affected by the obstacle's motion vector and expands in the direction of motion. Figure 6 and Figure 7 In the process of movement, the robot will actively move behind the moving obstacle, with good obstacle avoidance effect, and the final movement path is shorter than that of traditional potential energy.

[0247] in, Figure 4 , Figure 5 , Figure 6 and Figure 7 The Path Length in indicates the path length. Figure 4 , Figure 5 , Figure 6 and Figure 7 The path lengths in are 0.5m, 7.4m, 0.7m and 6.9m respectively.

[0248] In addition, the following verification tests were performed in the present invention:

[0249] The movement interval (in meters) is delineated with the robot's movement starting point as the origin, the robot's movement starting point is selected as (0,0) and the end point is selected as (4.5,5), the obstacle is in the positive direction interval from (0,0) to (5,5), the position is random and the movement speed is ±0.1m / s, and after 10 global path planning, the following Tables 1 and 2 are obtained. Among them, Table 1 is the average path length using the traditional artificial potential field algorithm, and Table 2 is the average path length using the improved artificial potential field algorithm of this embodiment.

[0250] It can be seen from Table 1 and Table 2 that the average path length obtained by performing 10 global path plannings using the improved artificial potential field algorithm of this embodiment when the number of moving obstacles is 1 and 3 is lower than the average path length using the traditional artificial potential field algorithm as a whole.

[0251] Especially when the number of moving obstacles is 3, the average path length of 10 global path plannings performed using the improved artificial potential field algorithm of this embodiment is lower than the average path length of the traditional artificial potential field algorithm.

[0252] Table 1: Data of 10 global path planning of traditional APF algorithm

[0253]

[0254] Table 2: Data of 10 global path plannings after the improved APF algorithm

[0255]

[0256] Embodiment 2

[0257] This embodiment discloses a robot dynamic path planning system based on an improved artificial potential field algorithm.

[0258] The robot dynamic path planning system based on the improved artificial potential field algorithm includes:

[0259] The obstacle speed acquisition module is configured to: acquire the movement speed of the dynamic obstacle on the original movement path of the robot;

[0260] The collision judgment module is configured to: judge whether the robot will collide with a dynamic obstacle, and if so, define a dynamic obstacle avoidance area;

[0261] The dynamic path starting point determination module is configured to: determine the starting point and the end point of the dynamic path based on the dynamic obstacle avoidance area and the original motion path;

[0262] The multi-layer repulsive potential field division module is configured to: delineate multiple layers of repulsive potential fields in the dynamic obstacle avoidance area, and introduce an exponential function to calculate the repulsive force of each layer of the repulsive potential field;

[0263] The repulsion strengthening module is configured to: based on the repulsion of each layer of the repulsion potential field, combine the relationship between the movement direction of the robot relative to the obstacle and the obstacle velocity vector to obtain the repulsion of each layer of the repulsion potential field after strengthening;

[0264] The total potential field calculation module is configured to: superimpose the repulsive force of each layer of repulsive potential field after strengthening to obtain the total repulsive field, and combine it with the attractive potential field to obtain the total potential field;

[0265] The update module is configured to obtain the total potential field force based on the total potential field, and based on the total potential field force, update the nodes between the starting point and the end point of the dynamic path and located in the dynamic obstacle avoidance area to plan the dynamic path of the robot.

[0266] Those skilled in the art should understand that the modules or steps of the present invention described above can be implemented by a general-purpose computer device, or alternatively, they can be implemented by a program code executable by a computing device, so that they can be stored in a storage device and executed by the computing device, or they can be made into individual integrated circuit modules, or multiple modules or steps therein can be made into a single integrated circuit module for implementation. The present invention is not limited to any specific combination of hardware and software.

[0267] Although the above describes the specific implementation mode of the present invention in conjunction with the accompanying drawings, it is not intended to limit the scope of protection of the present invention. Those skilled in the art should understand that various modifications or variations that can be made by those skilled in the art on the basis of the technical solution of the present invention without creative work are still within the scope of protection of the present invention.

Claims

1. A robot dynamic path planning method based on an improved artificial potential field algorithm, characterized in that: The following steps are involved: Get the movement speed of the dynamic obstacles on the original movement path of the robot; Determine whether the robot will collide with a dynamic obstacle. If so, define a dynamic obstacle avoidance area. Based on the dynamic obstacle avoidance area and the original motion path, determine the starting point and end point of the dynamic path; In the dynamic obstacle avoidance area, multiple layers of repulsive potential fields are defined, and an exponential function is introduced to calculate the repulsive force of each layer of repulsive potential fields; The repulsive force of each layer of the repulsive potential field is specifically: ； in, It is The distance threshold of the layer, Indicates the distance between each layer; Indicates layer; and It is Repulsion parameters of the layers; is the current position of the robot; is the location of the obstacle; For the The repulsive force of the layer repulsive potential field; Based on the repulsive force of each layer of the repulsive potential field, the repulsive force of each layer of the repulsive potential field after strengthening is obtained by combining the relationship between the movement direction of the robot relative to the obstacle and the obstacle velocity vector; The repulsive force of each layer of the repulsive potential field after the strengthening is specifically: ； in, is the repulsive force of each layer of repulsive potential field after strengthening; is the velocity vector of the dynamic obstacle; is the relative motion direction between the robot and the dynamic obstacle; is a positive scalar coefficient; The repulsive force of each layer of repulsive potential field after superposition and strengthening is obtained to obtain the total repulsive field, and combined with the attractive potential field to obtain the total potential field; Based on the total potential field, the total potential field force is obtained, and based on the total potential field force, the nodes between the starting point and the end point of the dynamic path and located in the dynamic obstacle avoidance area are updated to plan the dynamic path of the robot.

2. The robot dynamic path planning method based on the improved artificial potential field algorithm as claimed in claim 1, characterized in that: The motion tracking of dynamic obstacles is realized based on Kalman filtering, and the velocity vector information of dynamic obstacles in motion is obtained.

3. The robot dynamic path planning method based on the improved artificial potential field algorithm as claimed in claim 1, characterized in that: Use the speed obstacle algorithm to determine whether the robot will collide with a dynamic obstacle, including: Define the speed obstacle function based on the relative position and relative speed of the robot and the dynamic obstacle : ； in, Represents the dot product of vectors; represents the magnitude of a vector; Represents a robot and obstacles The relative position of Represents a robot speed; For obstacles speed; Indicates time; Represents a robot The radius of Set up collision judgment conditions to judge the robot Speed Is it a speed barrier? Inside: ； If the collision judgment condition is met, it is judged that the robot will The obstacle will collide with it within the specified time, otherwise there will be no collision.

4. The robot dynamic path planning method based on the improved artificial potential field algorithm according to claim 1, characterized in that: Define dynamic obstacle avoidance areas, including: Set the dynamic obstacle avoidance range , specifically: ； in, For robots The radius of For obstacles The radius of For robots speed; For robots The speed of The center of the circle is the center of the dynamic obstacle and the dynamic obstacle avoidance range is The radius is used to determine the dynamic obstacle avoidance area.

5. The robot dynamic path planning method based on the improved artificial potential field algorithm according to claim 1, characterized in that: Based on the dynamic obstacle avoidance area and the original motion path, determine the starting point and end point of the dynamic path, including: First, determine whether the original motion path passes through the dynamic obstacle avoidance area; If the original motion path passes through the dynamic obstacle avoidance area, further determine whether the starting point and end point of the original motion path are within the dynamic obstacle avoidance area: If the starting point of the original motion path is within the dynamic obstacle avoidance area, then find a node on the original motion path that is closest to the dynamic obstacle avoidance area as the end point of the dynamic path, and use the starting point of the original motion path as the starting point of the dynamic path; If the end point of the original motion path is within the dynamic obstacle avoidance area, find a node on the original motion path that is closest to the dynamic obstacle avoidance area as the starting point of the dynamic path, and use the end point of the original motion path as the end point of the dynamic path; If the starting point and the end point of the original motion path are not in the dynamic obstacle avoidance area, the two nodes closest to the dynamic obstacle avoidance area are found on the original motion path and used as the starting point and the end point of the dynamic path respectively.

6. The robot dynamic path planning method based on the improved artificial potential field algorithm according to claim 1, characterized in that: The total potential field is specifically: ； ； in, is the total potential field, To attract the potential field, is the total number of layers divided for the repulsive field; is the total repulsive field; The total potential field force is specifically: ； ； ； in, is the total potential field force; for the gradient of attraction; is the gradient of the repulsive force; is the gradient of the repulsive force of each layer of repulsive potential field after strengthening; Based on the total potential field force, update the nodes between the start and end points of the dynamic path and within the dynamic obstacle avoidance area, specifically: ； in, is the updated path node position; is the original motion path node position; To adjust the step size.

7. The robot dynamic path planning method based on the improved artificial potential field algorithm as claimed in claim 1, characterized in that: Before obtaining the movement speed of the dynamic obstacle on the original movement path of the robot, it also includes determining whether there is a dynamic obstacle on the set movement path of the robot; When the resultant force is zero or the robot is stagnant, a path correction strategy is introduced to readjust the path nodes using a certain reverse force.

8. A robot dynamic path planning system based on an improved artificial potential field algorithm, characterized in that: include: The obstacle speed acquisition module is configured to: acquire the movement speed of the dynamic obstacle on the original movement path of the robot; The collision judgment module is configured to: judge whether the robot will collide with a dynamic obstacle, and if so, define a dynamic obstacle avoidance area; The dynamic path starting point determination module is configured to: determine the starting point and the end point of the dynamic path based on the dynamic obstacle avoidance area and the original motion path; The multi-layer repulsive potential field division module is configured to: delineate multiple layers of repulsive potential fields in the dynamic obstacle avoidance area, and introduce an exponential function to calculate the repulsive force of each layer of the repulsive potential field; The repulsive force of each layer of the repulsive potential field is specifically: ； in, It is The distance threshold of the layer, Indicates the distance between each layer; Indicates layer; and It is Repulsion parameters of the layers; is the current position of the robot; is the location of the obstacle; For the The repulsive force of the layer repulsive potential field; The repulsion strengthening module is configured to: based on the repulsion of each layer of the repulsion potential field, combine the relationship between the movement direction of the robot relative to the obstacle and the obstacle velocity vector to obtain the repulsion of each layer of the repulsion potential field after strengthening; The repulsive force of each layer of the repulsive potential field after the strengthening is specifically: ； in, is the repulsive force of each layer of repulsive potential field after strengthening; is the velocity vector of the dynamic obstacle; is the relative motion direction between the robot and the dynamic obstacle; is a positive scalar coefficient; The total potential field calculation module is configured to: superimpose the repulsive force of each layer of repulsive potential field after strengthening to obtain the total repulsive field, and combine it with the attractive potential field to obtain the total potential field; The update module is configured to obtain the total potential field force based on the total potential field, and based on the total potential field force, update the nodes between the starting point and the end point of the dynamic path and located in the dynamic obstacle avoidance area to plan the dynamic path of the robot.

Images