AGV adaptive path planning and reciprocal obstacle avoidance method in complex environment

Abstract

The application relates to an AGV adaptive path planning and reciprocal obstacle avoidance method in a complex environment, which comprises a global path planning layer, a local obstacle avoidance planning layer and a path adaptive correction layer; the global path planning layer generates a global planning path position vector by combining an improved A* with a JPS jump point algorithm; the local obstacle avoidance planning layer solves an AGV local planning path position vector by minimizing a cost function by using an improved RVO model based on a dynamic threat function; the path adaptive correction layer ensures the coordination and consistency of the global and local strategies, triggers the global path planning layer to regenerate a task global path based on the deviation of the local planning path position vector and the global planning path position vector and the obstacle density, and realizes adaptive adjustment to improve the planning efficiency and obstacle avoidance performance. Compared with the prior art, the application has the advantages of small model calculation amount, good AGV obstacle avoidance real-time performance, and the ability to effectively cope with high-speed dynamic obstacles and multi-AGV mutual avoidance.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of mobile robot path planning, in particular to an AGV adaptive path planning and reciprocal obstacle avoidance method suitable for complex dynamic industrial scenes with multiple obstacles. BACKGROUND

[0002] The AGV has become a key component of intelligent production and intelligent logistics systems. When handling complex industrial environment tasks, AGVs need to have efficient path planning capabilities and stable obstacle avoidance capabilities. However, the existing path planning and obstacle avoidance algorithms have the following shortcomings:

[0003] In the global planning part, the traditional A* algorithm has low search efficiency and many redundant nodes, and the path calculation time is too long in a high-density obstacle environment, which affects the production rhythm. In the local obstacle avoidance part, the existing multi-AGV obstacle avoidance method based on the velocity obstacle (VO) model is prone to unstable behavior when dealing with dynamic obstacles, and the motion conflict resolution between multiple agents is not timely. In the fusion planning part, a single global or local algorithm is difficult to balance global optimality and local real-time performance in complex industrial scenes, and there are problems of local optimality and insufficient dynamic adaptability.

[0004] The Chinese invention with publication number CN119085651A discloses a local path planning method, system, device and medium that fuses improved velocity obstacle and TEB algorithm, introduces curvature constraint and velocity obstacle constraint, and uses DBScan and monotone chain convex hull algorithm for obstacle information clustering and boundary, reduces the amount of calculation, and improves the obstacle avoidance ability. The above-mentioned invention can output a smooth and safe path using local path planning, reducing the calculation amount of TEB algorithm in processing local cost map; on the basis of considering the influence of the real environment, the velocity obstacle and TEB algorithm are improved, and the obstacle avoidance ability for obstacles, especially dynamic obstacles, is improved. However, there are still problems of slow path planning generation speed, poor adaptability to dynamic obstacles, and inconsistency between global path planning and local obstacle avoidance planning.

[0005] In summary, there is currently a lack of an AGV adaptive path planning and reciprocal obstacle avoidance method in complex environments to solve or partially solve the aforementioned problems. SUMMARY

[0006] The purpose of the present application is to overcome the defects of the prior art and provide an AGV adaptive path planning and reciprocal obstacle avoidance method in complex environments to solve or partially solve the problems of slow path planning generation speed, poor adaptability to dynamic obstacles, and inconsistency between global path planning and local obstacle avoidance planning.

[0007] The purpose of the present application can be achieved by the following technical solutions:

[0008] The application provides an AGV adaptive path planning and reciprocal obstacle avoidance method in a complex environment, which adopts a hierarchical planning structure and specifically comprises the following steps:

[0009] A global path planning layer: in a static or semi-static environment, an improved A* algorithm and a JPS jump search algorithm are combined to generate a global planning path position vector as a task global path;

[0010] A local obstacle avoidance planning layer: in a dynamic environment, an improved reciprocal velocity obstacle model based on a dynamic threat function is used to minimize a cost function to solve a local planning path position vector of an automatic guided vehicle;

[0011] A path adaptive correction layer: based on the deviation of the local planning path position vector and the global planning path position vector and the obstacle density, the global path planning layer is triggered to regenerate a task global path, thereby realizing AGV adaptive path planning and reciprocal obstacle avoidance in a complex environment.

[0012] As a preferred technical solution, the specific steps of the global path planning layer are as follows:

[0013] Input a starting node and a target node, initialize an open list and a closed list, and add the starting node to the open list;

[0014] Obtain the number of obstacles in the current grid cell, and switch the improved A* algorithm or the JPS jump search algorithm according to the comparison result with a preset threshold value;

[0015] In the improved A* algorithm, for each node in the open list n Calculate an A* cost function based on a dynamic weight factor, and select the node with the minimum cost as an expansion node;

[0016] In the JPS jump search algorithm, the target node or a jump point with a forced neighbor is added to the open list based on the JPS jump search algorithm, and the node with the highest jump point priority is selected as the expansion node;

[0017] The neighborhood nodes of the current expansion node are subjected to feasibility screening and added to the open list, and the current expansion node is added to the closed list;

[0018] Move to the next grid cell where the expansion node is located, judge whether to enter a new grid cell, if not, continue to move, if yes, update the cell information and switch the improved A* algorithm or the JPS jump search algorithm again, and after the target node is added to the closed list, backtrack the predecessor relationship of the nodes in the closed list to generate a global planning path;

[0019] The automatic guided vehicle is controlled to move along the globally planned path, and during the movement of the automatic guided vehicle, the obstacle density and path complexity information of the current cell are transmitted to the subsequent local path planning link through dynamic cost propagation.

[0020] As a preferred technical solution, the cost function definition of the improved A* algorithm in the global path planning layer is:

[0021]

[0022]

[0023]

[0024] In the formula, is the actual cost from the starting point to the node , is the estimated cost from the node to the target point, is a dynamic weight factor, is the coordinates of the node , is the coordinates of the target point, is the predecessor node of the node , is the coordinates of the node .

[0025] As a preferred technical solution, the dynamic weight factor is adaptively adjusted according to the local obstacle density , and the formula is:

[0026]

[0027] In the formula, is a constant for controlling the degree of influence of the obstacle density on the weight, each node has a passable identification and a local obstacle density attribute, the is calculated by counting the number of obstacle grids within a preset range around the node , is a set of all effective nodes in the two-dimensional grid map.

[0028] As a preferred technical solution, the node jump condition of the JPS jump search in the global path planning is defined by a piecewise function, and the formula is:

[0029]

[0030] In the formula, is the search direction, is the node, For the next node of the recursive search, the forced neighbor represents a node whose certain neighborhood node is an obstacle, and the direction forms a turning requirement with the search direction .

[0031] As a preferred technical solution, the specific steps of the local obstacle avoidance planning layer are:

[0032] Obtain the dynamic obstacle and the state of the current AGV, calculate the relative position vector and the relative speed vector of the current AGV and the dynamic obstacle;

[0033] Calculate the speed obstacle area of the RVO, i.e. the collision risk speed set, based on the relative position vector and the relative speed vector;

[0034] Calculate the obstacle priority according to the remaining distance between the AGV and the dynamic obstacle, and then adjust the obstacle avoidance responsibility proportion of the AGV and the obstacle based on the obstacle priority, to determine the respective avoidance responsibility weight of both parties;

[0035] Initialize the feasible speed domain of the AGV under kinematic constraints, after excluding the collision risk speed set, construct a cost function based on the dynamic threat function and the speed obstacle area, aiming at approaching the expected speed and avoiding threats, and obtain the optimal speed by solving the minimum cost function, and in the solving process, adjust the search direction of the feasible speed domain based on the avoidance responsibility weight.

[0036] As a preferred technical solution, in the local obstacle avoidance planning layer, the definition of the dynamic threat function is:

[0037]

[0038] wherein, is the current AGV, is the obstacle or other AGV, the relative position vector is , are the position vectors of and respectively, is the speed vector of , is the position influence coefficient, is the relative position module length of A and B , is the speed module length of , is the maximum speed of the AGV.

[0039] As a preferred technical solution, the solving process of the local planning path position vector specifically includes:

[0040] The optimal velocity vector is obtained by minimizing the cost function:

[0041]

[0042] In the formula, For the optimal velocity vector, For the set of feasible speeds for automated guided vehicles, The cost function for the obstacle avoidance velocity vector. This represents the parameter value at which the function achieves its minimum value in its domain.

[0043] The optimal velocity vector is discretized to obtain the instantaneous velocity at each time step. The sequence of instantaneous velocities at each time step is numerically integrated to obtain discrete positions. The discrete positions are then smoothed to generate a smooth path, i.e., a locally planned path position vector.

[0044] As a preferred technical solution, the cost function of the obstacle avoidance velocity vector is:

[0045]

[0046]

[0047] In the formula, For balance coefficient, For the speed of the AGV, The desired speed of the AGV, For indicator functions, hour ,otherwise , For traditional reciprocal speed barrier areas, To predict the time window, For dynamic threat functions, For the safety radius of AGVs and obstacles, For The central disk area It is a relative position vector. For time.

[0048] As a preferred technical solution, in the path adaptive correction layer, the deviation between the local planned path position vector and the global planned path position vector... Defined as:

[0049]

[0050] In the formula, for The local planning path position vector of the AGV at time step. for The global planning path position vector of the AGV is updated in real time. When an impassable area is sensed, the global path planning layer is triggered to regenerate the global path of the task. The deviation threshold is set.

[0051] Compared with the prior art, the present application has at least one of the following beneficial effects:

[0052] 1) The global planning efficiency is significantly improved: the present application introduces the JPS jump point search algorithm to optimize the extended node in the global planning layer, combines the improved A* algorithm based on dynamic weight factors, generates the global path planning, solves the problem of low search efficiency and many redundant nodes in the traditional path planning algorithm, effectively reduces the redundant path search, reduces the calculation amount, and significantly improves the generation speed of the global path.

[0053] 2) Strong dynamic environment adaptability: the present application uses the technical means based on the improved RVO model and introduces the dynamic threat function, solves the problem that the existing obstacle avoidance method is prone to unstable behavior when dealing with dynamic obstacles, realizes the avoidance of AGV sudden turn and sudden stop, etc. Unstable operation, strengthens the real-time of AGV obstacle avoidance, effectively deals with high-speed dynamic obstacles and multi-AGV mutual avoidance technical effect.

[0054] 3) Double-layer coordination mechanism of global and local: the present application triggers the global path planning layer to regenerate the global path of the task through the constraint of the deviation threshold of the local planning path position vector and the global planning path position vector and the judgment of the obstacle density size, solves the problem that the single global or local algorithm is difficult to balance the global optimality and the local real-time in the complex industrial scene, realizes the coordination of the global path planning and the local obstacle avoidance planning technical effect, avoids the local planning into local optimum, and also avoids the problem of slow reaction of global planning to dynamic obstacles.

[0055] 4) Hierarchical planning structure fusion: the present application combines the three planning layers of the global path planning layer, the local obstacle avoidance planning layer and the path adaptive correction layer, so that the AGV can adaptively adjust to improve the planning efficiency and obstacle avoidance performance, solves the problem that the AGV system in the prior art is difficult to have global optimization ability and local obstacle avoidance real-time response ability, realizes the fast planning of the optimal global path in the static environment, the real-time calculation of collision avoidance in the dynamic environment, and the adaptive correction of the path to ensure the coordination of the global path and the local path. BRIEF DESCRIPTION OF DRAWINGS

[0056] Figure 1 It is the AGV adaptive path planning and mutual obstacle avoidance strategy system framework schematic diagram of the present application.

[0057] Figure 2A flowchart of the improved A* combined with JPS algorithm in the global planning module of the present application is shown in the figure.

[0058] Figure 3 A calculation diagram of the improved RVO obstacle avoidance strategy in the local planning module of the present application is shown in the figure. DETAILED DESCRIPTION

[0059] The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative work should fall within the protection scope of the present application.

[0060] In view of the problems in the prior art, the embodiment provides an AGV adaptive path planning and reciprocal obstacle avoidance method in a complex environment. The method is applied to a differential AGV in a complex dynamic industrial scene (such as an automobile parts assembly workshop or an e-commerce intelligent warehouse sorting area), and is equipped with a laser radar, an inertial measurement unit and a wheel speed encoder. Finally, the AGV adaptive path planning and reciprocal obstacle avoidance in the scene are realized.

[0061] The AGV adaptive path planning and reciprocal obstacle avoidance method in a complex environment provided by the embodiment adopts a hierarchical planning structure, as shown in the figure. Figure 1 Based on the input of a starting node and a target node in a grid map, in the global planning layer, a path position vector of a global path is generated by combining an improved A* algorithm and a JPS jump point search algorithm. Dynamic cost propagation and obstacle density feedback are performed according to the generated global planning path position vector. The global path planning is re-performed based on the obstacle density feedback. In the local obstacle avoidance planning layer, an RVO obstacle avoidance model is adopted and a dynamic threat function is introduced. A velocity feasible region (i.e., a set of feasible velocities of the AGV ) and a threat field gradient of the dynamic threat function (i.e., the spatial variation characteristics of the dynamic threat function) are calculated. Finally, trajectory correction is performed in the path adaptive coordination layer according to the generated dynamic cost propagation, the velocity feasible region and the threat field gradient, and AGV control instructions are output.

[0062] The hierarchical planning structure specifically includes:

[0063] 1. Global path planning layer: in a static or semi-static environment, a global planning path position vector is generated by combining an improved A* algorithm and a JPS jump point search algorithm, as a task global path.

[0064] All planning processes in this invention are based on the construction of a two-dimensional grid map. The specific map parameters are set as follows: the grid size is a square grid, the map range is set to 100m×100m according to a typical industrial scenario, corresponding to 100×100 grids; effective node set. Each node is composed of the center coordinates of the accessible grid cells on the map. All are marked with accessibility (1 indicates accessibility, 0 indicates obstacles); local obstacle density The 3×3 neighborhood statistical method is used for calculation, that is, the statistical nodes are calculated. Number of obstacle grids within a 3x3 grid radius We can obtain:

[0065]

[0066] For example, nodes When there are 2 obstacle grids in the neighborhood, The core constants are set as dynamic weighting factor coefficients. (The degree to which barrier density affects the heuristic function), RVO prediction time window (Predicting collision risk within the next 1.5 seconds), AGV safety radius Path deviation threshold (Critical deviation value triggering global replanning), location influence coefficient of dynamic threat function Cost function balance coefficient (Prioritizing speeds close to the desired speed) and (Pay attention to collision risks).

[0067] The core of the global path planning layer is "improved A algorithm + JPS jump point search", which aims to quickly generate the globally optimal path in static or semi-static environments, such as Figure 2 As shown, after starting path planning, parameters are initialized. First, it is determined whether the obstacle is greater than or equal to the threshold. If so, the improved A* algorithm is used to calculate the path. If the path exists, the user moves along the path and iteratively checks whether the destination has been reached. If the path does not exist, the path planning fails. If not, the path is calculated using the JPS jump point search algorithm and it is determined whether the path exists. If it exists, the user moves along the path and checks whether the destination has been reached. If the destination has been reached, the path planning ends. If not, the user enters a new unit and updates the current position during the movement. Otherwise, the user iteratively checks whether the destination has been reached until the path planning ends successfully. If the path does not exist, the user attempts to calculate the path using the A* algorithm. If the path exists, the user moves along the path and iteratively checks whether the destination has been reached. If the path does not exist, the path planning fails.

[0068] The specific implementation process of the global path planning layer is as follows:

[0069] S101. Perform initialization and parameter input, input the starting node. (e.g., coordinates) ) and target node (e.g., coordinates) ), initialize the open list (stores nodes to be evaluated) and the closed list (stores nodes that have been evaluated), and set the starting node The actual cost of joining the open list .

[0070] After initialization, the AGV first obtains the number of the current cell and the number of obstacle grids in that cell. It then compares this number with a preset obstacle threshold to determine whether to enable the JPS algorithm. If the number of obstacles in the current cell is greater than or equal to the threshold, the JPS jump point search mode is triggered, i.e., step S103 is executed; otherwise, the improved A* algorithm traversal search mode is used, i.e., step S102 is executed. The AGV moves according to the initially generated global path framework, and at the same time, it iteratively checks whether the path exists based on the obstacle density and path branch complexity, dynamically correcting and updating the path.

[0071] S102. Calculate the improved A* cost function for each node in the open list. According to the formula:

[0072]

[0073] In the formula, From the starting point to the node The actual cost, For nodes The estimated cost to reach the target point This is a dynamic weighting factor.

[0074] Calculate the total cost, where the actual cost is... Calculate the starting node using Euclidean distance To the node The path cost, if the node The predecessor node is ,but:

[0075]

[0076] In the formula, For nodes coordinates For nodes The predecessor node, For nodes The coordinates.

[0077] For example , hour, Heuristic Costs Euclidean distance is used to estimate the cost of nodes to the target node The formula is as follows:

[0078]

[0079] In the formula, is the coordinate of the node , and is the coordinate of the target point.

[0080] When , the dynamic weight factor is adaptively adjusted according to the local obstacle density, and the formula is as follows:

[0081]

[0082] In the formula, is a constant for controlling the degree to which the weight is affected by the obstacle density, and each node has a passable identification and a local obstacle density attribute, which is calculated by counting the number of obstacle grids within a preset range around the node , and is a set of all effective nodes in a two-dimensional grid map.

[0083] For example, when the node has three obstacles in a 3*3 neighborhood, the weight is increased to guide the path away from the high obstacle density area.

[0084] If the path calculated by the improved A* algorithm exists, the robot continues to move along the path, and if the path does not exist, it is determined that the path planning fails.

[0085] S103. Perform JPS jump point search acceleration. To reduce redundant node expansion, a “jump point” is selected by the JPS algorithm, which needs to meet the “target node” or “existence of forced neighbor” condition. The specific execution logic is based on the formula:

[0086]

[0087] In the formula, is the search direction (supporting up, down, left, right and 45° diagonal for a total of 8 directions), is the node, is the next node of the recursive search, and the forced neighbor judgment standard is that if a neighborhood node (such as the “up” direction node) of the node is an obstacle, and the direction forms a “turning requirement” with the search direction , then ​​To force neighbors, such as nodes When searching in the "upper right" direction, if the node above is an obstacle and the node below is passable, you need to turn downwards. To enforce neighbor; starting with the node To the right ( Taking a search as an example, the first node Non-target and no forced neighbors, recursive search Right side , If it is not the target but there is a compelling neighbor (obstacles below), then Set a jump point and add it to the open list to skip. Non-jump points.

[0088] If the path calculated by the JPS algorithm exists, continue moving along the path; if the path does not exist, try to use the improved A* algorithm for path planning, i.e., execute step S102.

[0089] S104. Generate and optimize the global path, repeating the "cost calculation - jump point selection - node expansion" steps until the target node is reached. The node is added to the closed list, and a global path is generated by tracing the preorder relationships of nodes in the closed list. The open list stores nodes added through the JPS jump point algorithm, including target nodes and nodes with mandatory neighbors. The node expansion and closed list addition mechanism is to select the node with the smallest total cost f(n) from the open list as the current expansion node (in the improved A* algorithm, it is sorted by dynamic weighted cost; in the JPS algorithm, it is sorted by jump point priority, and its jump point priority is a combination of the JPS jump point selection conditions and the cost evaluation of the improved A* algorithm, with priority inversely proportional to cost). After performing a feasibility evaluation on the neighboring nodes of the current expansion node (improved A* algorithm) or the next-hop node obtained by recursive search (JPS algorithm) (whether it is an obstacle or already in the closed list), the current expansion node is removed from the open list and added to the closed list to avoid subsequent repeated searches and redundant node expansion.

[0090] When the AGV moves along the planned path, it monitors whether it enters a new grid cell using real-time positioning information. If not, it continues to move; if so, it immediately updates the current cell ID upon entering the new cell and obtains the obstacle density data of the new cell to switch back to the improved A* algorithm or the JPS jump point search algorithm, i.e., it returns to step S101. At the same time, a dynamic cost propagation mechanism is activated, which transmits cost information to subsequent cells along the global path based on the obstacle density and path branch complexity of each cell. This provides real-time data support for the dynamic adjustment of the heuristic function weight coefficients, ensuring the continuity of algorithm mode switching and the optimality of path planning.

[0091] After completing the new cell identification and algorithm switching, a dynamic cost propagation mechanism is introduced: starting from the current cell, along the preset direction of the global path planning (from the starting node to the target node), the core environmental parameters of the current cell are passed to the subsequent unplanned grid cells, and the obstacle density of the current cell is also passed. During propagation, a corresponding cost coefficient is assigned to each parameter, among which the obstacle density cost coefficient is... Based on the experimental design.

[0092] Subsequent units, after receiving the propagation cost, use it as a dynamic weighting factor to adjust the heuristic function. This is an important basis. A higher transmission cost indicates a more complex environment in the preceding unit, affecting the subsequent unit nodes. This will increase accordingly, thereby guiding path planning to shift towards lower-cost areas in advance, avoiding frequent path adjustments after entering complex environments, and ensuring the consistency and optimality of the global path.

[0093] With a 30% obstacle density, the planning time for traditional A* is 125ms, while the improved (A+JPS) plan reduces the planning time to 48ms, lowering the time complexity from that of traditional A*. ( , ) reduced to approximately ( , ).

[0094] 2. Local Obstacle Avoidance Planning Layer: In a dynamic environment, an improved reciprocal speed obstacle model based on a dynamic threat function is used to solve the local planning path position vector of the automated guided vehicle by minimizing the cost function.

[0095] The local obstacle avoidance planning layer, based on an improved RVO model, calculates the safe velocity vector of the AGV in real time in dynamic environments (such as the movement of other AGVs or the appearance of temporary obstacles). Figure 3 As shown, when moving along the current path, it checks whether dynamic obstacles are found. If no obstacles are found, it maintains the original path until the target point is reached and the task ends. If dynamic obstacles are detected, the priority of the obstacle is first calculated, and then the velocity obstacle cone (VO) is calculated based on this to predict the potential collision area. Then the responsibility ratio is adjusted and a feasible velocity domain is generated. Finally, an optimal velocity vector is selected from the velocity domain and the real-time path of the AGV is updated accordingly, and it continues to move towards the target point.

[0096] The specific implementation process of the local obstacle avoidance planning layer is as follows:

[0097] S201. Obtain dynamic targets and states, and define the current AGV as... Dynamic obstacles (such as another AGV) are , the real-time state of both is obtained by laser radar and IMU, for example the position vector of the velocity vector of , the position vector of the velocity vector of .

[0098] After obtaining the state information of the current AGV and dynamic obstacles (or other AGVs), first, the obstacle priority is calculated, and the passing priority is determined by quantifying the remaining distance of the AGV to the target point. The closer the AGV is to the target point, the higher the priority, and the lower the corresponding obstacle avoidance responsibility. Then, based on the priority calculation result, the obstacle avoidance responsibility ratio is adjusted. If the remaining distance of the current AGV (A) to the target point is , and the remaining distance of the obstacle (B) to its target point is , then the obstacle avoidance responsibility weight of AGV (A) is , and the responsibility weight of obstacle B is , through this responsibility allocation mechanism, mutual avoidance between multiple AGVs is realized, and path redundancy or efficiency decline caused by single subject excessive adjustment is avoided.

[0099] According to the obstacle avoidance responsibility weight calculated above, the path adjustment subject is determined according to the principle of "the higher the responsibility weight, the more the avoidance obligation needs to be borne". The specific rules are as follows:

[0100] 1. If > (current AGV responsibility weight is lower): the passing priority of the current AGV (A) is higher, which can maintain the original expected speed or only make a small speed adjustment;

[0101] 2. If < (current AGV responsibility weight is higher): the current AGV (A) needs to bear the main avoidance responsibility and needs to adjust the path actively. By expanding the search range of the speed feasible region (increasing the search angle by ±30°), the speed vector away from the opposite AGV (B) is preferentially selected to realize path offset avoidance;

[0102] 3. If = (responsibility weight is equal, such as the remaining distance of both is the same): both sides bear 50% of the avoidance responsibility and need to adjust the path synchronously, avoiding head-on or crossing collision.

[0103] S202. Calculate the relative state and traditional RVO region, relative position vector:

[0104]

[0105] Substitute the data to get ;

[0106] The relative speed vector is:

[0107]

[0108] Substitute the data to get ;

[0109] The speed barrier area of the traditional RVO defined by the formula:

[0110]

[0111] wherein, is the traditional reciprocal speed barrier area, is the prediction time window, is the safety radius of the AGV and the obstacle, is the circular disc area with as the center and (πr2) as the radius, is the relative position vector, is the time, for example, the speed , , , , the distance is , so (non-obstacle speed).

[0112] S203. Dynamic threat function and optimal speed solving of improved RVO are performed , the formula is:

[0113]

[0114] wherein, is the current AGV, is the obstacle or other AGV, and the relative position vector is , are the position vectors of and respectively, is the speed vector of , is the position influence coefficient, is the relative position module length of A and B , is the speed module length of , is the maximum speed of the AGV.

[0115] Dynamic Threat Function Spatial variation characteristics through its dynamic threat gradient field Quantization is used to characterize the rate of change of threat intensity in space. Gradient calculation is based on relative position vectors. and obstacle speed Through the Taking the partial derivative, we get:

[0116]

[0117] Among them, the location-related gradient components The decay trend of the dominant threat field in the near-field obstacle region, velocity-related components This gradient field reflects the modulating effect of the dynamic obstacle's movement direction on threat distribution. It is used for threat field gradient descent optimization in local path planning, guiding the AGV to avoid obstacles along the threat-reducing direction.

[0118] Substitution , , , We can obtain:

[0119]

[0120] S204. Cost Function Combining the objectives of "approaching the desired speed" and "avoiding threats," the formula is:

[0121]

[0122] in, For balance coefficient, For the speed of the AGV, (AGV) (expected speed) For indicator functions ( hour ,otherwise ), For traditional reciprocal speed barrier areas, To predict the time window, For dynamic threat functions, speed hour, ,like ( ),but ;speed hour, .

[0123] like ( ),but .

[0124] S205. Optimal velocity By minimizing the cost function of the obstacle avoidance velocity vector (velocity module ≤ 1.0 m / s) in the feasible velocity set of AGV Solving, the formula is:

[0125]

[0126] In the formula, is the optimal velocity vector, is the feasible velocity set of AGV, is the cost function of the obstacle avoidance velocity vector, represents the parameter value of the function that takes the minimum value in its domain.

[0127] The optimal velocity vector calculated above is discretized on the time axis, thereby decomposed into a series of instantaneous velocities at a series of continuous time steps; by numerically integrating these discrete velocity sequences, the discrete positions of AGV at each time can be calculated; in order to ensure the smoothness of the operation, the discrete positions need to be processed by trajectory smoothing, and finally a smooth path that can be continuously executed, i.e. the local planning path position vector, is generated.

[0128] Where the feasible velocity domain The specific steps are as follows:

[0129] First, determine the basic kinematic constraint boundary, set the lower limit and upper limit of the velocity and the motion state constraint based on the physical performance of AGV, form the initial velocity pool, and exclude the velocities beyond the physical ability. Specifically, the velocity size constraint is: 2 m / s ≤ ≤ 6 m / s; the heading angle change constraint is: -135° ≤ Δθ ≤ 135°; the acceleration / angle acceleration constraint is: linear acceleration ≤ 1 m / s 2 , angular acceleration ≤ π / 6 rad / s 2 .

[0130] Secondly, RVO velocity obstacle cone clipping. Based on the traditional RVO region (collision risk velocity set) calculated in step S202, the "possible collision" velocities are removed from the initial velocity pool. Each velocity in the initial velocity pool is traversed , if , the velocity is deleted from the set; if there are multiple dynamic obstacles, all , regions need to be removed at the same time, and only the vectors located in the "non-obstacle velocity space" are retained.

[0131] Then, dynamic threat function optimization. Combine the dynamic threat function The further optimized speed set is further optimized, and the range of the feasible speed domain is adjusted according to the threat degree of the obstacle.

[0132] Finally, the obstacle avoidance responsibility weight adaptation. Combined with the obstacle avoidance responsibility weight calculated in step S201, the search direction of the feasible speed domain is adjusted to ensure that the responsibility matches the action, and the AGV with high responsibility actively avoids (searches in the direction away from the obstacle), and the AGV with low responsibility searches in the direction of the priority expected speed.

[0133] In this example , the minimum , realize the obstacle avoidance effect of "small adjustment direction, avoid and close to the expected speed".

[0134] 3. Path adaptive correction layer: based on the deviation of the local planning path position vector and the global planning path position vector and the obstacle density, the global path planning layer is triggered to regenerate the task global path, realizing the adaptive path planning and reciprocal obstacle avoidance of AGV in complex environment.

[0135] The path adaptive correction layer triggers the global path update through "deviation monitoring + environment perception", ensuring the coordination between global and local planning. The specific implementation process is as follows: first, calculate the path deviation, and calculate the deviation between the local planning path position of the AGV at the moment and the global planning path position in real time , the formula is:

[0136]

[0137] When or the impassable area is perceived, the global path planning layer is triggered to regenerate the task global path, is the deviation threshold.

[0138] For example , , , then (equal to , do not trigger update); , , , then (trigger global path re-planning). Then, the impassable area perception and triggering are performed. The AGV detects the passability of the nodes on the global path in real time through the laser radar. If the obstacle density of the node (such as coordinate ) in the original global path From 0 to 0.8 (7 new obstacle grids in the neighborhood, judged as impassable), directly trigger the global path planning layer to re-execute the "Improved A*+JPS" process to generate a new global path that bypasses the impassable area. Finally, the global path is updated and connected. After triggering the re-planning, the new global path takes the current AGV position as the starting node and the original target node as the end point. After generation, B-spline curve fitting is used for path smoothing to reduce the inflection points and ensure smooth transition of the local obstacle avoidance planning layer to the new global path. In this embodiment, the re-planning response time is ≤100ms, and there is no path interruption or AGV stalling phenomenon.

[0139] The above is only a specific embodiment of the present application, but the protection scope of the present application is not limited thereto. Any skilled person in the art can easily think of various equivalent modifications or replacements within the technical scope disclosed by the present application, and these modifications or replacements should be covered within the protection scope of the present application. Therefore, the protection scope of the present application should be subject to the protection scope of the claims.

Claims

1. An AGV adaptive path planning and reciprocal obstacle avoidance method in a complex environment, characterized in that, The method adopts a hierarchical planning structure, and specifically comprises the following steps: A global path planning layer: in a static or semi-static environment, a global planning path position vector is generated by combining an improved A* algorithm and a JPS jump search algorithm, serving as a task global path; A local obstacle avoidance planning layer: in a dynamic environment, an improved reciprocal velocity obstacle model based on a dynamic threat function is used to minimize a cost function to solve a local planning path position vector of the autonomous guided vehicle; A path adaptive correction layer: the global path planning layer is triggered to regenerate a task global path based on the deviation of the local planning path position vector and the global planning path position vector and the obstacle density, so as to realize adaptive path planning and reciprocal obstacle avoidance of the autonomous guided vehicle in a complex environment; The specific steps of the local obstacle avoidance planning layer are as follows: The relative position vector and the relative velocity vector of the current autonomous guided vehicle and the dynamic obstacle are calculated based on the state of the dynamic obstacle and the current autonomous guided vehicle; The velocity obstacle region of the RVO, i.e., the collision risk velocity set, is calculated based on the relative position vector and the relative velocity vector; The obstacle priority is calculated according to the remaining distance between the autonomous guided vehicle and the dynamic obstacle, and the obstacle avoidance responsibility proportion of the autonomous guided vehicle and the obstacle is adjusted based on the obstacle priority, so as to determine the avoidance responsibility weight of each party; The feasible velocity domain of the autonomous guided vehicle under the kinematic constraint is initialized, the collision risk velocity set is removed, and a cost function based on the dynamic threat function and the velocity obstacle region is constructed to obtain the optimal velocity by solving the minimized cost function, wherein the search direction of the feasible velocity domain is adjusted based on the avoidance responsibility weight during the solving process; In the local obstacle avoidance planning layer, the dynamic threat function is defined as: wherein is the current automated guided vehicle, is an obstacle or other automated guided vehicle, the relative position vector is , are the position vectors of and , are the velocity vectors of , is the position influence coefficient, are the relative position magnitudes of A and B , are the velocity magnitudes of , is the maximum velocity of the automated guided vehicle. 2.The AGV adaptive path planning and reciprocal obstacle avoidance method in a complex environment according to claim 1, wherein, The specific steps of the global path planning layer are as follows: The starting node and the target node are input, the open list and the closed list are initialized, and the starting node is added to the open list; The number of obstacles in the current grid unit is obtained, and the improved A* algorithm or the JPS jump search algorithm is switched according to the comparison result with the preset threshold value; In the improved A* algorithm, for each node in the open list n An A* cost function based on dynamic weight factors is calculated, and the node with the minimum cost is selected as the expansion node; In the JPS jump search algorithm, the target node or the jump point with a forced neighbor is added to the open list based on the JPS jump search algorithm, and the node with the highest jump point priority is selected as an expansion node; The neighbor nodes of the current expansion node are screened for feasibility and added to the open list, and the current expansion node is added to the closed list; The next grid unit where the expansion node is located is moved to, it is judged whether to enter a new grid unit, if not, the moving continues, and if yes, the unit information is updated and the improved A* algorithm or the JPS jump search algorithm is switched again, after the target node is added to the closed list, the global planning path is generated by backtracking the predecessor relationship of the nodes in the closed list; The autonomous guided vehicle is controlled to move along the global planning path, and during the movement of the autonomous guided vehicle, the obstacle density and the path complexity information of the current unit are transmitted to the subsequent local path planning link through dynamic cost propagation.

3. The AGV adaptive path planning and reciprocal obstacle avoidance method in a complex environment according to claim 1, characterized in that, In the global path planning layer, the cost function of the improved A* algorithm is defined as: wherein is the actual cost from the start point to the node , is the estimated cost from the node to the goal point, is the dynamic weight factor, is the coordinates of the node , is the coordinates of the goal point, is the predecessor node of the node , is the coordinates of the node .

4. The AGV adaptive path planning and reciprocal obstacle avoidance method in a complex environment according to claim 3, characterized in that, The dynamic weight factor By local obstacle density Adaptive adjustment, formula is: In the formula, is a constant controlling the degree of influence of the obstacle density on the weight, each node has a passability identifier and a local obstacle density attribute, the is calculated by counting the number of obstacle grids within a preset range around the node , and is a set of all valid nodes in the two-dimensional grid map.

5. The AGV adaptive path planning and reciprocal obstacle avoidance method in a complex environment according to claim 1, characterized in that, The node jump condition of the JPS jump search in the global path planning is defined by a piecewise function, and the formula is: wherein is the search direction, is the node, is the next node of the recursive search, the forced neighbor represents the node whose certain neighborhood node is an obstacle, and the direction forms a turning requirement with the search direction .

6. The AGV adaptive path planning and reciprocal obstacle avoidance method in a complex environment according to claim 1, characterized in that, The solving process of the local planning path position vector specifically comprises: An optimal velocity vector is obtained by minimizing a cost function: wherein is the optimal velocity vector, is the feasible velocity set of the automated guided vehicle, is the cost function of the obstacle avoidance velocity vector, denotes the parameter value at which the function takes its minimum value in its domain of definition; The optimal velocity vector is discretized to obtain an instantaneous velocity at each time step, a sequence of the instantaneous velocity at each time step is numerically integrated to obtain a discrete position, and the discrete position is smoothed to generate a smooth path, that is, the local planning path position vector.

7. The AGV adaptive path planning and reciprocal obstacle avoidance method in a complex environment according to claim 6, characterized in that, The cost function of the obstacle avoidance velocity vector is: wherein is a balancing coefficient, is the speed of the automated guided vehicle, is the desired speed of the automated guided vehicle, is an indicator function, when otherwise , is a traditional mutual speed barrier region, is a prediction time window, is a dynamic threat function, is the safety radius of the AGV and the obstacle, is a disc region centered at is a disc region centered at is a relative position vector, is time.

8. The AGV adaptive path planning and reciprocal obstacle avoidance method in a complex environment according to claim 1, characterized in that, The path adaptive correction layer, the deviation of the local planning path position vector and the global planning path position vector is defined as: In the formula, is the local planning path position vector of the automatic guiding vehicle at the moment, is the global planning path position vector of the automatic guiding vehicle at the moment; when or a non-passable area is perceived, the global path planning layer is triggered to regenerate a task global path, is a deviation threshold.

Images