Path planning method based on improved artificial potential field method

Abstract

The invention discloses a path planning method based on an improved artificial potential field method, and the method comprises the steps: carrying out the modeling of an environment through a grid method, and constructing a potential field map reflecting the distribution of obstacles; and searching an optimal path on the potential field map by adopting a minimum heap-first queue structure and combining a heuristic function considering the potential field gradient. For a local minimum trap problem, a new intermediate target point is selected near a trap point for re-searching by analyzing the potential field gradient change on a path. For special terrains such as narrow channels, a search strategy is dynamically adjusted to adapt to constraints. And the smoothness of the path is improved by normalizing and smoothing the potential field value of the node. In the path execution process, the potential field map can be dynamically updated according to real-time environment information, local re-planning is carried out on the affected path, and therefore dynamic obstacles can be effectively dealt with. According to the method, the intelligence, the safety and the adaptability of path planning are remarkably improved.

Description

Technical Field

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

[0002] When planning the path of a mobile robot in a complex environment, the traditional artificial potential field method has problems such as local minimum traps and unreachable targets. When there are concave obstacles in the environment, the robot is prone to oscillation or stop moving in the concave area; when there are many obstacles and they are densely distributed, the resultant force may tend to zero and stop the robot; when the target point is surrounded by obstacles, the robot cannot reach the target point. In addition, for special terrains such as narrow passages and sharp-angled convex areas, the adaptability of existing improved methods is insufficient. How to optimize the potential field function according to the characteristics of the environment, improve the planning efficiency while meeting the path smoothness and safety, and solve the special terrain constraints in a targeted manner is an urgent problem to be solved. At the same time, how to reduce the algorithm complexity and improve the convergence speed while taking into account real-time performance is also a direction worth exploring. Designing an adaptive, efficient and robust improved artificial potential field method is of great significance to improving the path planning performance of mobile robots in complex environments. Summary of the invention

[0003] In order to solve the above technical problems, the present invention provides a path planning method based on an improved artificial potential field method, comprising:

[0004] According to the environmental map information, the grid method is used to model the environment, and the environment is divided into several grid units. Each grid unit is assigned different quantitative indicators of passage difficulty according to the potential field value of the obstacle, and an environmental potential field map reflecting the spatial distribution of the potential field value is obtained;

[0005] In the environmental potential field map, the current position of the robot is taken as the starting point and the target position is taken as the end point. Through the minimum heap priority queue data structure and the heuristic function considering the gradient change of the potential field value, the path with the minimum potential field value is searched to obtain the initial path from the starting point to the end point;

[0006] In view of the local minimum trap problem existing on the initial path, by analyzing the gradient change of the potential field value of the grid points on the path, it is determined whether there is a local minimum. If there is a local minimum, a grid point with a potential field value lower than the current point and a decreasing gradient is selected as a new intermediate target point through the heap insertion and deletion operations near the local minimum point, and the path search is performed again using the minimum heap priority queue algorithm;

[0007] In order to solve the special terrain constraint problems in narrow passages and sharp-angle convex areas, the spatial distribution information of the neighborhood potential field values ​​of the grid points on the path is analyzed to determine whether there is special terrain. If there is special terrain, the node expansion and search strategy of the minimum heap priority queue algorithm are dynamically adjusted according to the type of special terrain.

[0008] In order to solve the problem of smoothness of the initial path, the potential field values ​​of the extended nodes are normalized and smoothed during the path search process. By adjusting the spatial distribution of the potential field values, the path is smoothly planned under the premise of satisfying the path safety constraints.

[0009] During the path execution process, environmental information is obtained in real time through sensors, and the environmental potential field map is dynamically updated. If a dynamic obstacle is detected, the potential field value is updated according to the obstacle information. The updated potential field value is used as a new constraint condition, and a local re-search is performed on the affected part of the planned path to obtain an updated path.

[0010] Preferably, the process of obtaining an environmental potential field map reflecting the spatial distribution of potential field values ​​includes:

[0011] According to the acquired environmental map information, the environment is modeled using the grid method, and the environment is divided into a number of grid units of equal size;

[0012] For each grid unit, the potential field value of the obstacle in the unit is obtained, and the quantitative index of the difficulty of passage of the grid unit is determined through the corresponding relationship between the preset potential field value and the difficulty of passage;

[0013] According to the quantitative index of traffic difficulty, an environmental potential field map reflecting the spatial distribution of potential field values ​​is generated;

[0014] If the quantified index of the difficulty of passage of a certain grid unit exceeds a preset threshold, the grid unit is judged to be an area where an obstacle is located, and is marked as an inaccessible area in the environmental potential field map;

[0015] The adjacent grid cells where obstacles are located are aggregated by clustering algorithm to obtain the aggregated obstacle area, and the quantitative index of the difficulty of passage in the corresponding area in the environmental potential field map is updated;

[0016] A path planning algorithm is used to search for the optimal path from the starting point to the target point in the environmental potential field map, and the quantitative index of the difficulty of passage is used as the weight parameter of the path cost;

[0017] According to the dynamic changes of the environmental potential field map, the optimal path is updated in real time to obtain an adaptive path planning result.

[0018] Preferably, the process of obtaining the initial path from the starting point to the end point includes:

[0019] Load the environmental potential field map, determine the current position coordinates of the robot, and obtain the target position coordinates;

[0020] According to the robot's current position coordinates and the target position coordinates, the potential field value and the gradient change of the potential field value are used as the heuristic function to construct the minimum heap priority queue;

[0021] If the priority queue is not empty, the node with the smallest potential field value is popped out from the minimum heap. The node contains the position coordinates and the accumulated potential field value of the current path.

[0022] For the popped-up node, if the position coordinates coincide with the target position coordinates, the minimum potential field value path from the starting position to the target position is obtained; if the position coordinates do not coincide with the target position coordinates, the position coordinates of the neighboring nodes of the node position are obtained;

[0023] After obtaining the coordinates of the neighboring node positions, for each neighboring node position, the corresponding potential field value is obtained by table lookup operation; the cumulative potential field value of the path from the starting position to the neighboring node is calculated, wherein the cumulative potential field value is equal to the sum of the cumulative potential field value of the pop-up node and the potential field value of the neighboring node position; the neighboring node position coordinates and their corresponding path cumulative potential field values ​​are added to the minimum heap priority queue;

[0024] Re-determine whether the priority queue is empty, and pop the node with the smallest potential field value from the minimum heap until the path with the minimum potential field value from the starting position to the target position is determined.

[0025] Preferably, in view of the local minimum trap problem existing on the initial path, the process of re-searching the path using the minimum heap priority queue algorithm includes:

[0026] Obtain all grid points on the initial path, and calculate the potential field value and gradient of each grid point;

[0027] Traverse the grid points on the initial path and determine whether there is a local minimum trap by analyzing the potential field values ​​and gradient changes of adjacent points;

[0028] If it is judged to be a local minimum trap, then with the trap point as the center, among the nearby grid points, a point with a potential field value lower than the trap point and a decreasing gradient is selected as the new intermediate target point;

[0029] Insert the new intermediate target point into the minimum heap priority queue and delete other points in the queue whose potential field values ​​are higher than this point;

[0030] Taking the new intermediate target point as the starting point, the minimum heap priority queue algorithm is used to re-search the path and obtain a new path from the starting point to the target point;

[0031] The new path is compared with the original path, and the better path is determined by calculating the sum of the potential field values ​​of the grid points on the path. If the new path is better, it is used as the final planned path; otherwise, the original path is kept unchanged and the remaining grid points are traversed, and repeated until the optimal path is found.

[0032] Preferably, taking the new intermediate target point as the starting point, adopting the minimum heap priority queue algorithm, re-searching the path, and obtaining a new path from the starting point to the target point includes:

[0033] The new intermediate target point coordinates are set as the starting point coordinates, and a minimum heap priority queue with the starting point coordinates as the root node is constructed;

[0034] Obtain node information directly connected to the starting point from the graph data structure, including node coordinates and distance weights, and insert the node information into a minimum heap priority queue;

[0035] Take out the node with the smallest distance weight from the minimum heap priority queue and mark it as visited, and use it as the new current node coordinate;

[0036] Determine whether the coordinates of the current node are the coordinates of the target point. If so, output the path from the starting point to the current node as the new path; if the current node is not the target point, obtain all unvisited adjacent node information of the current node, calculate the distance weight and insert it into the minimum heap priority queue until the target point is found or all nodes are traversed to obtain a new path from the starting point to the target point; if the target point is still not found after searching all nodes, it is determined that the current path planning has no solution, and output a search failure prompt message.

[0037] Preferably, in view of the special terrain constraint problems of narrow passages and sharp-angle convex areas, the process of dynamically adjusting the node expansion and search strategy of the minimum heap priority queue algorithm includes:

[0038] Obtain the spatial distribution information of the neighborhood potential field values ​​of the grid points on the path, and determine whether there are special terrain constraints such as narrow channels and sharp-angle convex areas based on the distribution information;

[0039] If it is determined that there is a special terrain constraint problem, the specific type of the special terrain is determined;

[0040] Dynamically adjust the node expansion strategy and search strategy of the minimum heap priority queue algorithm according to the special terrain type;

[0041] Through the dynamically adjusted minimum heap priority queue algorithm, the targeted node expansion sequence and node search path are obtained;

[0042] The node expansion sequence and the node search path are used as passing strategies under special terrain constraints to guide the motion planning of the robot under special terrains such as narrow passages and sharp-angle convex areas.

[0043] Preferably, with respect to the smoothness problem of the initial path, under the premise of satisfying the path safety constraint, the process of smoothly planning the path includes:

[0044] According to the preset path safety constraint conditions, the potential field value of the extended node in the path search process is obtained;

[0045] Normalizing the potential field value of the extended node to obtain a normalized potential field value;

[0046] According to the normalized potential field value, a smoothing algorithm is used to smooth the potential field value to obtain a smoothed potential field value;

[0047] According to the potential field value after smoothing, the spatial distribution of the potential field value of the extended node is adjusted to obtain the adjusted spatial distribution of the potential field value;

[0048] According to the adjusted spatial distribution of potential field values, a search algorithm is used to search for paths and obtain a preliminary planned path;

[0049] Determine whether the preliminary planned path meets a preset path smoothness threshold, and if so, determine the preliminary planned path as the final planned path;

[0050] If the preliminary planned path does not meet the preset path smoothness threshold, the process returns to readjust the spatial distribution of the potential field value until the searched planned path meets the preset path smoothness threshold.

[0051] Preferably, the process of performing a local re-search on the affected portion of the planned path to obtain an updated path includes:

[0052] Acquire real-time environmental information during the path execution process, and dynamically update the environmental potential field map;

[0053] If a dynamic obstacle is detected, obtain the position and speed information of the obstacle and update the potential field value of the area where the obstacle is located;

[0054] Use the updated potential field value as a new constraint to determine the affected part of the planned path;

[0055] For the affected path parts, a local search algorithm is used to replan and obtain the locally updated path segments;

[0056] The locally updated path fragment is concatenated with the unaffected part of the original path to obtain the complete updated path;

[0057] According to the updated path, the robot is controlled to continue to perform the task, and during the subsequent path execution, real-time environmental information is continuously obtained to achieve dynamic update of the path.

[0058] Compared with the prior art, the present invention has the following advantages and technical effects:

[0059] The present invention first uses the grid method to model the environment and construct a potential field map that reflects the distribution of obstacles; then, the minimum heap priority queue structure is used, combined with a heuristic function that considers the potential field gradient, to search for the optimal path on the potential field map. In response to the local minimum trap problem, the present invention selects a new intermediate target point near the trap point for re-search by analyzing the potential field gradient changes on the path. For special terrains such as narrow passages, the present invention dynamically adjusts the search strategy to adapt to the constraints. In addition, the present invention also improves the smoothness of the path by normalizing and smoothing the node potential field values. During the path execution process, the present invention can dynamically update the potential field map according to real-time environmental information, and locally re-plan the affected path, thereby effectively dealing with dynamic obstacles. This method significantly improves the intelligence, safety and adaptability of path planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] The drawings constituting a part of the present application are used to provide a further understanding of the present application. The illustrative embodiments and descriptions of the present application are used to explain the present application and do not constitute an improper limitation on the present application. In the drawings:

[0061] Figure 1 The figure is a schematic diagram of a method flow of an embodiment of the present invention. DETAILED DESCRIPTION

[0062] It should be noted that, in the absence of conflict, the embodiments and features in the embodiments of the present application can be combined with each other. The present application will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0063] It should be noted that the steps shown in the flowcharts of the accompanying drawings can be executed in a computer system such as a set of computer executable instructions, and that, although a logical order is shown in the flowcharts, in some cases, the steps shown or described can be executed in an order different from that shown here.

[0064] like Figure 1 As shown, this embodiment provides a path planning method based on an improved artificial potential field method, including:

[0065] According to the environmental map information, the grid method is used to model the environment, and the environment is divided into several grid units. Each grid unit is assigned different quantitative indicators of passage difficulty according to the potential field value of the obstacle, and an environmental potential field map reflecting the spatial distribution of the potential field value is obtained;

[0066] In the environmental potential field map, the robot's current position is taken as the starting point and the target position is taken as the end point. Through the minimum heap priority queue data structure and the heuristic function that considers the gradient change of the potential field value, the path with the minimum potential field value is searched to obtain the initial path from the starting point to the end point.

[0067] In order to solve the local minimum trap problem on the initial path, the gradient change of the potential field value of the grid points on the path is analyzed to determine whether there is a local minimum. If there is a local minimum, a grid point with a potential field value lower than the current point and a decreasing gradient is selected as a new intermediate target point through the heap insertion and deletion operations near the local minimum point, and the path search is performed again using the minimum heap priority queue algorithm.

[0068] In order to solve the special terrain constraint problems in narrow passages and sharp-angle convex areas, the spatial distribution information of the neighborhood potential field values ​​of the grid points on the path is analyzed to determine whether there is special terrain. If there is special terrain, the node expansion and search strategy of the minimum heap priority queue algorithm are dynamically adjusted according to the type of special terrain.

[0069] In order to solve the problem of smoothness of the initial path, the potential field values ​​of the extended nodes are normalized and smoothed during the path search process. By adjusting the spatial distribution of the potential field values, the path is smoothly planned while satisfying the path safety constraints.

[0070] During the path execution process, environmental information is obtained in real time through sensors, and the environmental potential field map is dynamically updated. If a dynamic obstacle is detected, the potential field value is updated according to the obstacle information. The updated potential field value is used as a new constraint condition, and a local re-search is performed on the affected part of the planned path to obtain an updated path.

[0071] When the potential field is 0, the virtual target point method is used. The collision distance and safety angle can be set in advance. When the potential field is 0, a circle is constructed with the obstacle as the center and the collision distance as the radius, and a circumscribed regular hexagon is constructed outside the circle. The angles between the six vertices and the robot are calculated respectively, and the vertex with the smallest angle within the range that meets the safety angle is selected as the virtual target point.

[0072] Furthermore, the process of obtaining an environmental potential field map reflecting the spatial distribution of potential field values ​​includes:

[0073] According to the acquired environmental map information, the environment is modeled using the grid method, and the environment is divided into a number of grid units of equal size;

[0074] For each grid unit, the potential field value of the obstacle in the unit is obtained, and the quantitative index of the difficulty of passage of the grid unit is determined through the corresponding relationship between the preset potential field value and the difficulty of passage;

[0075] Based on the quantitative index of traffic difficulty, an environmental potential field map reflecting the spatial distribution of potential field values ​​is generated;

[0076] If the quantified index of the difficulty of passage of a certain grid cell exceeds the preset threshold, the grid cell is judged as the area where the obstacle is located and marked as an inaccessible area in the environmental potential field map;

[0077] The adjacent grid cells where obstacles are located are aggregated by clustering algorithm to obtain the aggregated obstacle area, and the quantitative index of the difficulty of passage in the corresponding area in the environmental potential field map is updated;

[0078] A path planning algorithm is used to search for the optimal path from the starting point to the target point in the environmental potential field map, and the quantitative index of the difficulty of passage is used as the weight parameter of the path cost;

[0079] According to the dynamic changes of the environmental potential field map, the optimal path is updated in real time to obtain an adaptive path planning result.

[0080] Specifically, this implementation discretizes the continuous space into grid cells of equal size through the grid method. Assume that in a warehouse environment, the ground is divided into one hundred times one hundred grid cells, and each grid represents one square meter of the actual ground. In this way, the entire warehouse is transformed into a plane consisting of ten thousand grids. Through this method, the complex actual space can be transformed into a digital model that can be calculated and analyzed. Then, a potential field value is assigned to each grid cell, which reflects the obstacle situation in the area and indirectly reflects the difficulty of passage. For example, still taking the warehouse as an example, if a grid cell is filled with goods, its potential field value will be very high, which may be set to eighty; if a grid cell is an open space without any obstacles, its potential field value can be set to five, indicating that it can pass. Such a setting allows environmental information to be expressed in a digital form. This method can quantify the impact of obstacles on passage, so that subsequent path planning has a basis.

[0081] This embodiment also takes into account that different types of obstacles in the real environment have different degrees of obstruction to traffic, and therefore sets a corresponding relationship between the potential field value and the difficulty of passage. For example, a corner where debris is piled up is more difficult to pass than a smooth passage. Set a corresponding relationship, for example, the grid unit with a potential field value between zero and twenty is a smooth area, and the quantitative index of the difficulty of passage is one; the potential field value between twenty and sixty is a difficult area to pass, and the quantitative index of difficulty is three; the potential field value exceeds sixty is an obstacle area, and the quantitative index of difficulty is ten. After quantification in this way, it can be more intuitive to judge which areas are relatively easy to pass, which areas are difficult to pass, or even impassable. Generate an environmental potential field map based on the quantitative index of the difficulty of passage of each grid unit. In the example of the warehouse, the environmental potential field map looks like a heat map, and different difficulties of passage can be marked with different colors. The obstacle area is represented by dark red, indicating that it is impassable; the smooth area is represented by green, and the difficulty of passage is one; the difficult area is represented by yellow, and the difficulty of passage is three. The environmental potential field map can intuitively reflect the difficulty of the entire warehouse environment and provide basic information for path planning. When the quantitative index of the difficulty of a grid unit exceeds the preset threshold, for example, more than 60, the grid unit can be determined as an obstacle area and marked as inaccessible on the environmental potential field map. By clearly identifying these high-risk areas, the path planning algorithm can avoid setting the path points in the obstacle area, thereby improving the effectiveness and safety of path planning. For example, if a large machine is placed in a grid in the warehouse, and its potential field value is very high, exceeding the preset threshold, then this grid will be marked as an inaccessible area. In this way, when planning the route of the handling robot, this grid will be automatically avoided to prevent collisions. Then, the adjacent obstacle grid units are aggregated through the clustering algorithm. For example, in a warehouse, multiple grids for placing goods are adjacent. Through the clustering algorithm, these grids can be aggregated into an obstacle area, thereby more accurately describing the actual range occupied by the obstacle. This can simplify the environmental model, reduce the amount of calculation, and make the map clearer and easier to understand.

[0082] This embodiment uses a clustering method based on connectivity to merge adjacent inaccessible grids into an obstacle area, and updates the quantitative index of the difficulty of passage in the area in the environmental potential field map. For example, the difficulty index of the area is set to ten, which means that it is completely inaccessible. In this way, the path planning algorithm can better handle complex obstacle distribution situations. The path planning algorithm is the key to searching for the optimal path on the environmental potential field map. It should be mentioned here that the quantitative index of passage difficulty can be used as a weight parameter of the path cost. By setting the quantitative index of passage difficulty as the weight parameter of the path cost, the path planning algorithm will tend to choose a path with lower passage difficulty rather than the path with the shortest straight-line distance. For example, a path with a short distance but passing through a large number of yellow areas, that is, a difficult-to-pass area, may have a higher total cost than a path with a slightly longer distance but mainly walking in the green unobstructed area. In the scenario of warehouse handling, this means that the handling robot will choose a path that is far away but avoids a large number of cargo accumulation areas, rather than a path that is straight through but full of potential collision risks, thereby improving overall efficiency and safety. Since the environment may change, for example, the stacking location of goods in the warehouse will change frequently, the environmental potential field map also needs to be updated in real time. Based on the updated map, the path planning algorithm needs to recalculate the optimal path to obtain an adaptive path planning result. For example, a previously unobstructed area is now filled with new goods. This change is reflected in the environmental potential field map as an increase in the quantitative index of the difficulty of passage in the area, which in turn causes the planned path to change. In this way, the agent can adjust its navigation decisions in real time according to the dynamic changes in the environment, improving the flexibility and accuracy of navigation. In practical applications, such as the application of automated guided vehicles in warehouses, such dynamic path planning can effectively avoid new obstacles, smoothly reach the destination, and ensure efficient warehouse operations.

[0083] Through the comprehensive application of the above links, the intelligent agent can make efficient navigation decisions in complex environments. From the acquisition and modeling of environmental information, to the identification and processing of obstacles, to the planning and real-time adjustment of paths, by setting reasonable potential field values, accurately quantifying the difficulty of passage, and updating the environmental potential field map in real time, the intelligent agent can better adapt to the dynamically changing environment and achieve efficient and safe autonomous navigation. For example, in a warehouse environment, using this technology, the handling robot can autonomously plan the optimal route and avoid obstacles, thereby significantly improving logistics efficiency, reducing manual intervention, and reducing operating costs.

[0084] Furthermore, the process of obtaining the initial path from the starting point to the end point includes:

[0085] Load the environmental potential field map, determine the robot's current position coordinates, and obtain the target position coordinates;

[0086] According to the robot's current position coordinates and the target position coordinates, the potential field value and the gradient change of the potential field value are used as the heuristic function to construct the minimum heap priority queue;

[0087] If the priority queue is not empty, the node with the smallest potential field value is popped out from the minimum heap. The node contains the position coordinates and the accumulated potential field value of the current path.

[0088] For the popped-up node, if the position coordinates coincide with the target position coordinates, the minimum potential field value path from the starting position to the target position is obtained; if the position coordinates do not coincide with the target position coordinates, the position coordinates of the neighboring nodes of the node position are obtained;

[0089] After obtaining the coordinates of the neighboring node positions, for each neighboring node position, the corresponding potential field value is obtained by table lookup operation; the cumulative potential field value of the path from the starting position to the neighboring node is calculated, where the cumulative potential field value is equal to the sum of the cumulative potential field value of the pop-up node and the potential field value of the neighboring node position; the coordinates of the neighboring node position and the corresponding cumulative potential field value of the path are added to the minimum heap priority queue;

[0090] Re-determine whether the priority queue is empty, and pop the node with the smallest potential field value from the minimum heap until the path with the minimum potential field value from the starting position to the target position is determined.

[0091] Specifically, the construction and application of potential field maps are key links in robot navigation in complex environments. First, after the potential field map is loaded, the current position coordinates and target position coordinates of the robot need to be determined. Assume that in a 10x10 grid environment, the starting position coordinates of the robot are (1, 1) and the target position coordinates are (10, 10). When using the A* algorithm for path planning, the potential field value and its gradient change are used as heuristic functions, which can effectively guide the robot to avoid high potential field areas, that is, areas with dense obstacles. For example, the potential field value of a grid cell is 5, the potential field value of its neighboring grid is 3, and the gradient change is -2, which indicates that the neighboring grid is easier to pass. The minimum heap priority queue is constructed to efficiently select the current optimal path node. Assume that at the initial time, the potential field value of the robot position (1, 1) is 2, and it is added to the priority queue. The priority queue is sorted from small to large according to the potential field value to ensure that each pop-up node is the current optimal choice. If the priority queue is not empty, the node with the smallest potential field value is popped out from the minimum heap. Assume that the pop-up node is (2, 2) and the cumulative potential field value is 4. Check whether the node position coincides with the target position. If so, the optimal path is obtained; if not, continue to explore. For the popped-up node (2, 2), obtain its neighboring nodes, such as (2, 3), (3, 2), etc. Obtain the potential field values ​​of these neighboring nodes through table lookup operations. Assume that the potential field value of (2, 3) is 3, and the potential field value of (3, 2) is 2. Calculate the cumulative potential field value of the path from the starting position to these neighboring nodes. For example, the cumulative potential field value from (1, 1) to (2, 3) is 4+3=7, and the cumulative potential field value from (1, 1) to (3, 2) is 4+2=6. Add these neighboring nodes and their corresponding path cumulative potential field values ​​to the minimum heap priority queue. At this point, the priority queue may contain multiple nodes, such as (3, 2, 6), (2, 3, 7), etc. Re-execute the steps of popping out the node with the minimum potential field value and continue to explore. Repeat the above steps until the target position is found. Assuming the final path is (1, 1) → (3, 2) → (5, 4) → (7, 6) → (10, 10), the node with the smallest cumulative potential field value is selected at each step to ensure the optimality of the path. In this way, the robot can avoid obstacles in a complex environment and find the optimal path. The introduction of potential field values ​​and their gradient changes makes path planning more intelligent and can dynamically adapt to environmental changes. For example, if a new obstacle suddenly appears in a certain area, the potential field value will increase accordingly, and the node order in the priority queue will also be adjusted accordingly to ensure that the robot always chooses the optimal path. In addition, the application of clustering algorithms further optimizes the identification of obstacle areas. Assuming that there are multiple scattered obstacle grids in the environment, these grids are aggregated into larger obstacle areas through clustering algorithms, and the quantitative indicators of the difficulty of passage in the potential field map are updated. In this way, when planning the path, the robot can avoid these areas more effectively, improving the efficiency and safety of navigation.In short, through the dynamic update of the potential field map and the combination of the A* algorithm, the robot can achieve adaptive path planning in complex environments, ensuring the accuracy and efficiency of navigation decisions. This method not only improves the robot's obstacle avoidance ability, but also provides a strong guarantee for its stable operation in dynamic environments.

[0092] Furthermore, in order to solve the local minimum trap problem existing on the initial path, the process of re-searching the path using the minimum heap priority queue algorithm includes:

[0093] Get all grid points on the initial path and calculate the potential field value and gradient of each grid point;

[0094] Traverse the grid points on the initial path and determine whether there is a local minimum trap by analyzing the potential field values ​​and gradient changes of adjacent points;

[0095] If it is judged to be a local minimum trap, then with the trap point as the center, among the nearby grid points, a point with a potential field value lower than the trap point and a decreasing gradient is selected as the new intermediate target point;

[0096] Insert the new intermediate target point into the minimum heap priority queue and delete other points in the queue whose potential field values ​​are higher than this point;

[0097] Taking the new intermediate target point as the starting point, the minimum heap priority queue algorithm is used to re-search the path and obtain a new path from the starting point to the target point;

[0098] The new path is compared with the original path, and the better path is determined by calculating the sum of the potential field values ​​of the grid points on the path. If the new path is better, it is used as the final planned path; otherwise, the original path is kept unchanged and the remaining grid points are traversed, and repeated until the optimal path is found.

[0099] Specifically, in path planning, the use of potential field maps can effectively help robots avoid obstacles and find the optimal path. First, obtain all grid points on the initial path, which represent the initial path of the robot from the starting point to the end point. Assume that there is a 10x10 potential field map, and each grid point has a potential field value. The higher the value, the more obstacles there are in the area or the greater the danger. For each grid point, calculate its potential field value and gradient. For example, the potential field value of a grid point A is 30, and the potential field values ​​of its four adjacent grid points above, below, left and right are 25, 35, 28 and 32 respectively. Through these values, the gradient change of point A can be calculated, that is, the direction and magnitude of the change of the potential field value. If the gradient of point A points to the direction with a lower potential field value, it means that this direction is a safer path. Traverse the grid points on the initial path, and determine whether there is a local minimum trap by analyzing the potential field values ​​and gradient changes of adjacent points. A local minimum trap means that the robot cannot find a path with a lower potential field value near the point, causing the path planning to stagnate. Assume that the potential value of point A is 30, and the potential values ​​of all its neighboring points are higher than 30, then point A is a local minimum trap. If it is judged to be a local minimum trap, take the trap point as the center, and select the point with a potential value lower than the trap point and a decreasing gradient among the grid points near it as the new intermediate target point. For example, find a grid point B with a potential value of 28 around point A, and the gradient of point B points to the direction of a lower potential value, then point B can be used as the new intermediate target point. Insert the new intermediate target point into the minimum heap priority queue, and delete other points in the queue with a potential value higher than this point. The function of the minimum heap priority queue is to always ensure that the nodes in the queue are arranged from small to large according to the potential value, so as to give priority to nodes with lower potential values. After inserting point B, other nodes with high potential values ​​in the queue will be deleted to ensure that the path search is always carried out in the direction of lower potential values. Take the new intermediate target point as the starting point, use the minimum heap priority queue algorithm, re-search the path, and obtain a new path from the starting point to the target point. Assume that the original path is S→A→C→T, where S is the starting point, T is the target point, and A is the local minimum trap. After inserting a new intermediate target point B, the new path may become S→B→D→T, where D is the new path point. Compare the new path with the original path, and determine which path is better by calculating the sum of the potential field values ​​of the grid points on the path. For example, the sum of the potential field values ​​of the original path is 100, while the sum of the potential field values ​​of the new path is 90, indicating that the new path is better. If the new path is better, it will be used as the final planning path; otherwise, keep the original path unchanged, and continue to traverse the remaining grid points, repeating the above steps until the optimal path is found. Assume that in the subsequent traversal, a new local minimum trap point E is found. By processing in the same way, a path S→F→G→T with a sum of potential field values ​​of 85 is finally obtained. This path becomes the final planning path.In this way, the robot can effectively avoid the local minimum trap and find a safe path with lower potential field value.

[0100] The method of this embodiment has the advantage that it not only considers the potential field value of the current point, but also analyzes the feasibility of the path through gradient changes, thereby improving the robustness and efficiency of path planning. In practical applications, this method can be widely used in unmanned driving, robot navigation and other fields. For example, in unmanned driving, the vehicle needs to dynamically adjust the driving path according to road conditions and traffic rules. Through the potential field map and the minimum heap priority queue algorithm, the vehicle can avoid congested areas and high-risk sections in real time and choose the optimal path to travel, thereby improving driving safety and efficiency. In addition, this method can also be combined with other sensor data, such as laser radar, camera, etc., to further optimize path planning. For example, the obstacle information obtained by the laser radar can dynamically update the potential field map to make path planning more accurate and real-time. In short, through the potential field map and the minimum heap priority queue algorithm, combined with the processing of local minimum traps, the difficult problems in path planning can be effectively solved, and the performance and reliability of robots and unmanned driving systems can be improved.

[0101] Furthermore, taking the new intermediate target point as the starting point, the minimum heap priority queue algorithm is used to re-search the path, and the process of obtaining a new path from the starting point to the target point includes:

[0102] Set the new intermediate target point coordinates to the starting point coordinates, and construct a minimum heap priority queue with the starting point coordinates as the root node;

[0103] Obtain node information directly connected to the starting point from the graph data structure, including node coordinates and distance weights, and insert the node information into the minimum heap priority queue;

[0104] Take out the node with the smallest distance weight from the minimum heap priority queue and mark it as visited, and use it as the new current node coordinate;

[0105] Determine whether the coordinates of the current node are the coordinates of the target point. If so, output the path from the starting point to the current node as the new path; if the current node is not the target point, obtain all unvisited adjacent node information of the current node, calculate the distance weight and insert it into the minimum heap priority queue until the target point is found or all nodes are traversed to obtain a new path from the starting point to the target point; if the target point is still not found after searching all nodes, it is determined that the current path planning has no solution, and output a search failure prompt message.

[0106] Specifically, in path planning, the application of the minimum heap priority queue algorithm can effectively solve the local minimum trap problem and improve the efficiency and accuracy of path search. First, assume that there is a new intermediate target point coordinate (x, y), set it as the starting point coordinate, and build a minimum heap priority queue with the starting point as the root node. The construction of the minimum heap priority queue is to ensure that the node with the smallest distance weight can be processed first each time, so as to quickly find the optimal path. For example, in a 10x10 grid map, the starting point coordinate is (1, 1) and the target point coordinate is (10, 10). Assuming that the new intermediate target point coordinate is (5, 5), first take (5, 5) as the starting point to build a minimum heap priority queue. Get the node information directly connected to (5, 5) from the graph data structure, including node coordinates and distance weights. Assume that the nodes directly connected to (5, 5) are (4, 5), (6, 5), (5, 4) and (5, 6), and their distance weights are 1, 1, 1 and 1 respectively. Insert these node information into the minimum heap priority queue. Next, take out the node with the smallest distance weight from the minimum heap priority queue, assuming it is (4, 5), mark it as visited, and use its coordinates as the new current node coordinates. Determine whether the current node coordinates are the target point coordinates (10, 10). Obviously not, so proceed to the next step. Get all the unvisited adjacent node information of the current node (4, 5). Assume that these nodes are (3, 5), (4, 4) and (4, 6). Calculate their distance weights as 1, 1 and 1 respectively, and insert them into the minimum heap priority queue. Repeat the above steps, take out the node with the smallest distance weight from the minimum heap priority queue, mark it as visited, and update the current node coordinates. In this way, gradually approach the target point. Suppose that in a certain step, the current node coordinates become (10, 9), which is only one step away from the target point (10, 10). Continue to take out the node with the smallest distance weight from the minimum heap priority queue, and finally reach the target point (10, 10). At this time, the path from the starting point to the current node is output as the new path, and the algorithm ends. If the target point is not found after traversing all nodes during the search process, it is determined that the current path planning has no solution, and a search failure prompt message is output. For example, in a complex map, due to too many obstacles or other reasons, it is impossible to reach the target point from the starting point. At this time, the algorithm will terminate and prompt that the path planning failed. In this way, the minimum heap priority queue algorithm can not only effectively avoid local minimum traps, but also ensure the efficiency and accuracy of path search. Specifically, the role of the minimum heap priority queue is to always give priority to processing nodes with the smallest distance weight, so as to quickly approach the target point. The operation of marking visited nodes avoids repeated searches and improves the efficiency of the algorithm.

[0107] The method of this embodiment has broad application prospects in practical applications. For example, in unmanned vehicle navigation, through the potential field map and the minimum heap priority queue algorithm, the vehicle can avoid congested areas and high-risk sections in real time and choose the optimal path to travel. In robot navigation, the robot can effectively avoid obstacles and find the optimal path based on the potential field map and the minimum heap priority queue algorithm. In short, through the minimum heap priority queue algorithm, combined with the processing of local minimum traps, the difficult problems in path planning can be effectively solved, and the performance and reliability of robots and unmanned driving systems can be improved. This method not only takes into account the potential field value of the current point, but also analyzes the feasibility of the path through gradient changes, thereby improving the robustness and efficiency of path planning.

[0108] Furthermore, in view of the special terrain constraints of narrow passages and sharp-angle convex areas, the process of dynamically adjusting the node expansion and search strategy of the minimum heap priority queue algorithm includes:

[0109] Obtain the spatial distribution information of the neighborhood potential field values ​​of the grid points on the path, and determine whether there are special terrain constraints such as narrow channels and sharp-angle convex areas based on the distribution information;

[0110] If it is determined that there is a special terrain constraint problem, the specific type of the special terrain is determined;

[0111] Dynamically adjust the node expansion strategy and search strategy of the minimum heap priority queue algorithm according to the special terrain type;

[0112] Through the dynamically adjusted minimum heap priority queue algorithm, the targeted node expansion sequence and node search path are obtained;

[0113] The node expansion sequence and node search path are used as passing strategies under special terrain constraints to guide the robot's motion planning under special terrains such as narrow passages and sharp-angle convex areas.

[0114] Specifically, in the process of path planning, obtaining the spatial distribution information of the neighborhood potential field values ​​of the grid points on the path is the key first step. The potential field value reflects the "attraction" or "repulsion" of the point in the environment, while the neighborhood distribution information provides the potential field changes in the area around the point. For example, assuming that the potential field value of a grid point is 10, and the potential field values ​​in its neighborhood are 8, 12, 9, and 11, this distribution may indicate that the point is in a relatively flat area, but if there are significant differences in the neighborhood values, such as 5, 15, 7, and 13, there may be narrow passages or sharp-angled convex areas. By analyzing this distribution information, it can be determined whether there are special terrain constraints. The characteristic of a narrow passage is that the potential field values ​​are higher on both sides and lower in the middle, forming a narrow and long low potential field corridor; while the sharp-angled convex area is characterized by a sharp increase in the potential field value in a certain direction, forming a sharp potential field gradient change. For example, in a 10x10 grid map, the potential field values ​​of grid point A and its neighboring points B, C, D, and E on a path are A(10), B(20), C(5), D(15), and E(25), respectively. This distribution may indicate that point A is at the entrance of a narrow passage. After determining the specific type of special terrain, the node expansion strategy of the minimum heap priority queue algorithm needs to be dynamically adjusted according to the type. In a narrow passage, node expansion should give priority to grid points along the passage direction to avoid unnecessary lateral exploration. For example, if the current node is in the passage, the nodes in front and behind are preferred when expanding, rather than the nodes on the left and right sides. In the acute convex area, the expansion strategy should tend to bypass the convex corner and select nodes with lower potential field values ​​and gentle gradient changes. Assuming that the current node is at the edge of the acute convex area, nodes away from the convex corner and with lower potential field values ​​are preferred when expanding. Dynamic adjustment of the search strategy is also important. In a narrow passage, the search strategy should emphasize the linearity of the path and reduce turns and return to improve traffic efficiency. For example, a path linearity weight is set to give priority to straight paths. In the acute convex area, the search strategy should focus on the smoothness of the path, avoid sharp turns, set a path smoothness weight, and give priority to smooth paths. Through the dynamically adjusted minimum heap priority queue algorithm, a targeted node expansion sequence can be obtained. For example, in a narrow channel, the expansion sequence may be A→B→C→D, where A, B, C, and D are arranged in the direction of the channel in sequence. In the acute convex area, the expansion sequence may be A→E→F→G, where E, F, and G bypass the convex corner in sequence and the potential field value gradually decreases. Similarly, the dynamically adjusted algorithm can also obtain a targeted node search path. In a narrow channel, the search path may be a straight line or a path that is approximately straight, such as A→B→C→D→target point. In the acute convex area, the search path may be a smooth curve, such as A→E→F→G→target point, avoiding direct crossing of the convex corner. Using these node expansion sequences and search paths as passing strategies under special terrain constraints can guide the robot's motion planning in special terrains such as narrow channels and acute convex areas.For example, in a narrow passage, the robot moves in sequence according to the expansion sequence, maintains the linearity of the path, and avoids collisions; in an acute angle convex area, the robot moves according to a smooth path, reduces sharp turns, and ensures stable passage. This embodiment can significantly improve the adaptability and efficiency of path planning by dynamically adjusting the algorithm strategy. In a narrow passage, a straight path reduces exploration time and energy consumption; in an acute angle convex area, a smooth path improves passage safety and stability. Ultimately, these strategies help the robot achieve efficient and safe motion planning in a complex environment, and improve the success rate of overall task execution. This embodiment can adjust the expansion and search strategies by setting different weight parameters for different situations or needs. For example, in a narrow passage, the linearity weight is set to 0.7 and the smoothness weight is set to 0.3; in an acute angle convex area, the linearity weight is set to 0.3 and the smoothness weight is set to 0.7. These weight parameters can be adjusted according to the actual environmental characteristics and task requirements to achieve the optimal path planning effect. In this way, the path planning algorithm can not only cope with conventional terrain, but also effectively deal with special terrain constraints, ensuring the efficient passage of robots in various complex environments. The application of this dynamic adjustment strategy significantly improves the flexibility and robustness of path planning, providing strong technical support for robot motion planning.

[0115] Furthermore, in view of the smoothness problem of the initial path, under the premise of satisfying the path safety constraint, the process of smooth planning of the path includes:

[0116] According to the preset path safety constraint conditions, the potential field value of the extended node in the path search process is obtained;

[0117] Normalizing the potential field value of the extended node to obtain a normalized potential field value;

[0118] According to the normalized potential field value, a smoothing algorithm is used to smooth the potential field value to obtain a smoothed potential field value;

[0119] According to the potential field value after smoothing, the spatial distribution of the potential field value of the extended node is adjusted to obtain the adjusted spatial distribution of the potential field value;

[0120] According to the adjusted spatial distribution of potential field values, a search algorithm is used to search for paths and obtain a preliminary planned path;

[0121] Determine whether the preliminary planned path meets a preset path smoothness threshold, and if so, determine the preliminary planned path as the final planned path;

[0122] If the initial planned path does not meet the preset path smoothness threshold, the potential field value spatial distribution is readjusted until the searched planned path meets the preset path smoothness threshold.

[0123] Specifically, in the process of path planning, the preset path safety constraints are the basis for ensuring that the robot can pass safely. First, obtaining the potential field value of the extended node in the path search process is a key step. The potential field value reflects the "attraction" or "repulsion" of the node in the environment, which directly affects the rationality of path selection. For example, in a 10x10 grid map, assuming that the potential field value of the current extended node A is 15, the potential field values ​​of its neighboring nodes B, C, D, and E are 12, 18, 14, and 20, respectively. These potential field values ​​directly reflect the environmental characteristics around node A. High potential field values ​​may indicate obstacles or dangerous areas, while low potential field values ​​may indicate safe or passable areas. Next, the obtained extended node potential field values ​​are normalized. The purpose of normalization is to unify potential field values ​​of different magnitudes to the same scale for subsequent processing. For example, assuming that the potential field values ​​of all nodes in the map range from 0 to 30, the potential field value of node A is 15, and the normalized value is 0.5. The normalized potential field value can more intuitively reflect the relative safety between nodes. Then, the normalized potential field value is smoothed by a smoothing algorithm. The purpose of smoothing is to reduce the mutation of the potential field value and make the path planning smoother. For example, the normalized potential field value of node A and its neighboring nodes is processed by the moving average method. Assuming that the normalized potential field value of node A is 0.5, the normalized potential field values ​​of neighboring nodes B, C, D, and E are 0.4, 0.6, 0.45, and 0.55, respectively, after smoothing, the new potential field value of node A may be 0.48 ((0.5+0.4+0.6+0.45+0.55) / 5). The smoothed potential field value can reduce sharp turns and return in the path and improve the smoothness of the path. According to the smoothed potential field value, the spatial distribution of the potential field value of the extended node is adjusted. The purpose of the adjustment is to make the distribution of the potential field value more in line with the actual traffic needs. For example, in a narrow channel, the potential field value of the node in the center of the channel should be relatively low, while the potential field value of the node at the edge of the channel should be relatively high to guide the robot to pass along the center of the channel. Through adjustment, the potential field values ​​of node A and its neighboring nodes may become A (0.45), B (0.55), C (0.4), D (0.5), E (0.6), which is more in line with the traffic characteristics of narrow channels. The A search algorithm is used to search for the path and obtain the preliminary planned path. The A search algorithm is a classic path planning algorithm that selects expansion nodes by evaluating a function (usually the actual cost from the current node to the starting point plus the estimated cost from the current node to the end point). For example, under the adjusted potential field value spatial distribution, the A search algorithm may obtain a preliminary planned path from the starting point to the end point, such as S→A→B→C→D→E→G. Determine whether the preliminary planned path meets the preset path smoothness threshold. The path smoothness threshold is an important indicator to ensure the feasibility and safety of the path. For example, the preset path smoothness threshold may be 0.7, indicating that the potential field value change between adjacent nodes in the path should not exceed 0.7.If the smoothness of the preliminary planned path meets the threshold, it is determined as the final planned path. For example, the smoothness of the path S→A→B→C→D→E→G is 0.65, which meets the threshold and is therefore determined as the final planned path. If the preliminary planned path does not meet the preset path smoothness threshold, return to the step of adjusting the spatial distribution of the potential field value and readjust it. For example, if the smoothness of the path S→A→B→C→D→E→G is 0.8, which exceeds the threshold, it is necessary to readjust the potential field value distribution of node A and its neighboring nodes. The potential field value of node A may be adjusted to 0.4, and the potential field values ​​of the neighboring nodes may be adjusted to B (0.5), C (0.35), D (0.45), and E (0.55) accordingly. The A search algorithm is performed again until the searched planned path meets the preset path smoothness threshold. Through this method of dynamic adjustment and iterative optimization, the smoothness and safety of path planning can be ensured. The preset path safety constraints and path smoothness threshold can effectively guide the robot to avoid dangerous areas and select the optimal path. Normalization and smoothing make the potential field value distribution more reasonable and reduce mutations and unstable factors in the path. The high efficiency of the A search algorithm, combined with the dynamic adjustment strategy, significantly improves the adaptability and robustness of path planning, ensuring that the robot can achieve efficient and safe motion planning in complex environments. For example, in a narrow channel, by adjusting the potential field value distribution, the robot can pass along the center of the channel to avoid collision and deviation; in the acute angle convex area, through smoothing and dynamic adjustment, the robot can choose a smooth path, reduce sharp turns, and ensure stable passage. These strategies not only improve the rationality of path planning, but also reduce the energy consumption and travel time of the robot, and improve the success rate of the overall task execution. By setting different weight parameters, the expansion and search strategies can be further optimized. For example, in a narrow channel, the linearity weight is set to 0.7 and the smoothness weight is set to 0.3; in the acute angle convex area, the linearity weight is set to 0.3 and the smoothness weight is set to 0.7. These weight parameters can be adjusted according to the actual environmental characteristics and task requirements to achieve the optimal path planning effect. In this way, the path planning algorithm can not only cope with conventional terrain, but also effectively deal with special terrain constraints, ensuring the efficient passage of the robot in various complex environments. The application of this dynamic adjustment strategy significantly improves the flexibility and robustness of path planning, providing strong technical support for robot motion planning. Through reasonable reasoning and specific numerical analysis, this strategy can be better understood and applied to ensure the rationality and safety of path planning.

[0124] Furthermore, the process of locally re-searching the affected part of the planned path to obtain an updated path includes:

[0125] Obtain real-time environmental information during path execution and dynamically update the environmental potential field map;

[0126] If a dynamic obstacle is detected, obtain the position and speed information of the obstacle and update the potential field value of the area where the obstacle is located;

[0127] Use the updated potential field value as a new constraint to determine the affected part of the planned path;

[0128] For the affected path parts, a local search algorithm is used to replan and obtain the locally updated path segments;

[0129] The locally updated path fragment is concatenated with the unaffected part of the original path to obtain the complete updated path;

[0130] According to the updated path, the robot is controlled to continue to perform the task, and during the subsequent path execution, real-time environmental information is continuously obtained to achieve dynamic update of the path.

[0131] Specifically, in the process of robots or unmanned vehicles performing tasks, the acquisition of real-time environmental information and the dynamic update of paths are the key to ensuring the smooth progress of tasks. The following is a detailed analysis and examples of the above content. First, the real-time environmental information during the execution of the path is obtained. This step relies on sensor technology. For example, the laser radar (LiDAR) and camera equipped with the unmanned vehicle can scan the surrounding environment in real time and obtain high-precision three-dimensional map data. Assume that the unmanned vehicle is driving, and the LiDAR detects that a moving car suddenly appears 50 meters ahead. The camera further confirms that the speed of the car is 30 kilometers per hour and the direction is the same as the driving direction of the unmanned vehicle. Next, the environmental potential field map is dynamically updated. The potential field map is a method of converting environmental information into potential field values. The potential field value near the obstacle is higher, indicating a dangerous area. According to the detected dynamic obstacle information, the system will update the potential field value of the area where the obstacle is located. For example, the potential field value of the area where the front vehicle is located increases from 0.2 to 0.8, indicating that the risk of this area has increased significantly. The updated potential field value is used as a new constraint to determine the affected part of the planned path. Assume that the originally planned path passes 20 meters away from the obstacle. Due to the increase in the potential field value, the system determines that there is a risk of collision in this section of the path. At this time, the system will mark the affected path segment for local replanning. For the affected path portion, a local search algorithm is used for replanning. Common local search algorithms include Dijkstra algorithm and A* algorithm. Assume that the system uses the A* algorithm, starting from the current position of the unmanned vehicle, avoiding the high potential field value area, and re-searching for a safe path segment. The new path segment may detour to the right side of the obstacle to ensure a safe distance from the obstacle. The locally updated path segment is spliced ​​with the unaffected part of the original path to obtain a complete updated path. For example, the first half of the original path remains unchanged, and the second half is replaced by a new path segment to ensure the continuity and safety of the overall path. According to the updated path, the actuator such as the robot or unmanned vehicle is controlled to continue to perform the task. After receiving the new path instruction, the unmanned vehicle control system adjusts the steering wheel and vehicle speed and continues to drive according to the updated path. Assume that the new path instructions require the unmanned vehicle to slow down to 20 km / h at a distance of 30 meters from the obstacle and gradually turn right. The unmanned vehicle will strictly execute these instructions. In the subsequent path execution process, real-time environmental information is continuously obtained, and the above steps are repeated to achieve dynamic update and adaptation of the path. For example, after the unmanned vehicle bypasses the obstacle and continues to move forward, LiDAR again detects a pedestrian crossing the road 100 meters ahead. The system immediately updates the potential field map, and the potential field value in the area where the pedestrian is located increases from 0.1 to 0.9, indicating a high degree of danger. The system again determines the affected path segment and replans it locally to bypass the area where the pedestrian is located. The advantage of this dynamic update and adaptation mechanism is that it can respond to environmental changes in real time to ensure the safety of the path.By continuously updating the potential field map and replanning the path, the unmanned vehicle can flexibly respond to various emergencies in a complex and changeable traffic environment and improve driving safety. In addition, dynamically updating the path also helps to improve the efficiency of task execution. Assuming that the unmanned vehicle follows the optimal path when no obstacles are detected, once an obstacle is detected, the path is adjusted immediately to avoid efficiency loss caused by waiting or detouring a long distance. Through the above analysis and examples, it can be seen that the acquisition of real-time environmental information and the dynamic update of the path are important guarantees for robots or unmanned vehicles to perform tasks efficiently and safely in complex environments. Each step is closely connected to form a complete dynamic path planning system, which ensures the reliability and flexibility of task execution.

[0132] Furthermore, the triggering condition of the virtual target point method of this embodiment is: during the path search or execution process, when the potential field of the robot's position is 0, that is, it is impossible to find the direction of travel through the guidance of the conventional potential field, the virtual target point method is triggered to provide the robot with a new navigation target to avoid stagnation near obstacles.

[0133] The specific process includes:

[0134] (1) Construction of virtual target points;

[0135] Setting parameters: Set reasonable collision distance and safety angle in advance according to actual needs. The collision distance is determined based on factors such as the robot size, motion characteristics, and environmental complexity, ensuring that the robot can safely avoid obstacles within this distance; the safety angle is set based on the robot's steering performance and obstacle avoidance requirements, ensuring that the robot has sufficient safety margin when selecting the direction of a new target point.

[0136] Construct a circle and a regular hexagon: With the obstacle as the center and the collision distance as the radius, a circle is constructed, which defines the boundary of the robot's safe range of activities around the obstacle. Next, a circumscribed regular hexagon is constructed outside the circle. The six vertices of the regular hexagon have evenly distributed orientations relative to the center of the circle, providing the robot with multiple potential candidate target point directions.

[0137] Calculate the angle: Calculate the angles between the six vertices of the regular hexagon and the current position of the robot, that is, the angle between the current orientation of the robot and the direction from the robot position to each vertex. This step is to evaluate the difficulty of turning from the current position of the robot to each vertex. The smaller the angle, the smaller the angle required for the robot to turn, and the better the continuity and smoothness of the path planning.

[0138] Select virtual target point: within the range of safety angles, select the vertex with the smallest angle as the virtual target point. This ensures the convenience of the robot when turning and the safety of the direction of the new target point, so that the robot can safely avoid obstacles with relatively small steering adjustments and continue to move along the planned path.

[0139] (2) Virtual target point guidance: The selected virtual target point is used as the new intermediate target, and the path search is restarted or the current movement direction of the robot is adjusted to guide the robot to move toward the virtual target point. During the movement process, the robot continues to perceive environmental changes in real time and dynamically adjusts the path as needed until the robot successfully bypasses the obstacle, returns to the normal path planning process, and continues to move toward the final target point.

[0140] This embodiment improves the artificial potential field method, combines multiple optimization strategies and dynamic processing mechanisms, so that the path planning algorithm can intelligently respond to various complex environments and emergencies, automatically identify and solve local minimum traps, narrow channel passage, and dynamic obstacle avoidance, and improve the robot's autonomous navigation ability in unknown or dynamic environments. In the path planning process, the safe distance between the robot and the obstacle is fully considered. By setting the collision distance, safety angle, and adopting the virtual target point method, the robot is always in a safe state during obstacle avoidance and obstacle bypassing, reducing the risk of collision and ensuring the safety of the robot and the surrounding environment. The method of this embodiment has strong adaptability to environmental changes, can dynamically update the potential field map according to the real-time perceived environmental information, and timely re-plan the affected path locally, so that the robot can quickly respond to the new, moving or disappearing obstacles in the environment, as well as terrain changes, etc., to ensure that the path planning result always conforms to the actual conditions of the current environment, and improve the robot's survivability and task execution success rate in complex dynamic environments. By normalizing and smoothing the node potential field values ​​and optimizing the angle when the virtual target point method is used when the potential field is 0, the smoothness of the planned path is effectively improved, the path mutations and broken lines are reduced, and the robot can move more steadily and smoothly when executing the path, reducing energy consumption. For manned robots or robots with riding equipment, it can improve movement efficiency and ride comfort.

[0141] The path planning method of this embodiment can significantly improve the navigation performance and task execution effect of the robot in these application scenarios, and has important practical application value and broad application prospects.

[0142] The above are only preferred specific implementations of the present application, but the protection scope of the present application is not limited thereto. Any changes or substitutions that can be easily thought of by a person skilled in the art within the technical scope disclosed in the present application should be included in the protection scope of the present application. Therefore, the protection scope of the present application should be based on the protection scope of the claims.

Claims

1. A path planning method based on an improved artificial potential field method, characterized in that: include: According to the environmental map information, the grid method is used to model the environment, and the environment is divided into several grid units. Each grid unit is assigned different quantitative indicators of passage difficulty according to the potential field value of the obstacle, and an environmental potential field map reflecting the spatial distribution of the potential field value is obtained; In the environmental potential field map, the current position of the robot is taken as the starting point and the target position is taken as the end point. Through the minimum heap priority queue data structure and the heuristic function considering the gradient change of the potential field value, the path with the minimum potential field value is searched to obtain the initial path from the starting point to the end point; In view of the local minimum trap problem existing on the initial path, by analyzing the gradient change of the potential field value of the grid points on the path, it is determined whether there is a local minimum. If there is a local minimum, a grid point with a potential field value lower than the current point and a decreasing gradient is selected as a new intermediate target point through the heap insertion and deletion operations near the local minimum point, and the path search is performed again using the minimum heap priority queue algorithm; In order to solve the special terrain constraint problems in narrow passages and sharp-angle convex areas, the spatial distribution information of the neighborhood potential field values ​​of the grid points on the path is analyzed to determine whether there is special terrain. If there is special terrain, the node expansion and search strategy of the minimum heap priority queue algorithm are dynamically adjusted according to the type of special terrain. In order to solve the problem of smoothness of the initial path, the potential field values ​​of the extended nodes are normalized and smoothed during the path search process. By adjusting the spatial distribution of the potential field values, the path is smoothly planned under the premise of satisfying the path safety constraints. During the path execution process, environmental information is obtained in real time through sensors, and the environmental potential field map is dynamically updated. If a dynamic obstacle is detected, the potential field value is updated according to the obstacle information. The updated potential field value is used as a new constraint condition, and a local re-search is performed on the affected part of the planned path to obtain an updated path.

2. The method according to claim 1, characterized in that The process of obtaining an environmental potential field map that reflects the spatial distribution of potential field values ​​includes: According to the acquired environmental map information, the environment is modeled using the grid method, and the environment is divided into a number of grid units of equal size; For each grid unit, the potential field value of the obstacle in the unit is obtained, and the quantitative index of the difficulty of passage of the grid unit is determined through the corresponding relationship between the preset potential field value and the difficulty of passage; According to the quantitative index of traffic difficulty, an environmental potential field map reflecting the spatial distribution of potential field values ​​is generated; If the quantified index of the difficulty of passage of a certain grid unit exceeds a preset threshold, the grid unit is judged to be an area where an obstacle is located, and is marked as an inaccessible area in the environmental potential field map; The adjacent grid cells where obstacles are located are aggregated by clustering algorithm to obtain the aggregated obstacle area, and the quantitative index of the difficulty of passage in the corresponding area in the environmental potential field map is updated; A path planning algorithm is used to search for the optimal path from the starting point to the target point in the environmental potential field map, and the quantitative index of the difficulty of passage is used as the weight parameter of the path cost; According to the dynamic changes of the environmental potential field map, the optimal path is updated in real time to obtain an adaptive path planning result.

3. The method according to claim 1, characterized in that The process of obtaining the initial path from the starting point to the end point includes: Load the environmental potential field map, determine the current position coordinates of the robot, and obtain the target position coordinates; According to the robot's current position coordinates and the target position coordinates, the potential field value and the gradient change of the potential field value are used as the heuristic function to construct the minimum heap priority queue; If the priority queue is not empty, the node with the smallest potential field value is popped out from the minimum heap. The node contains the position coordinates and the accumulated potential field value of the current path. For the popped-up node, if the position coordinates coincide with the target position coordinates, the minimum potential field value path from the starting position to the target position is obtained; if the position coordinates do not coincide with the target position coordinates, the position coordinates of the neighboring nodes of the node position are obtained; After obtaining the coordinates of the neighboring node positions, for each neighboring node position, the corresponding potential field value is obtained by table lookup operation; the cumulative potential field value of the path from the starting position to the neighboring node is calculated, wherein the cumulative potential field value is equal to the sum of the cumulative potential field value of the pop-up node and the potential field value of the neighboring node position; the neighboring node position coordinates and their corresponding path cumulative potential field values ​​are added to the minimum heap priority queue; Re-determine whether the priority queue is empty, and pop the node with the smallest potential field value from the minimum heap until the path with the minimum potential field value from the starting position to the target position is determined.

4. The method according to claim 1, characterized in that: In view of the local minimum trap problem existing on the initial path, the process of re-searching the path using the minimum heap priority queue algorithm includes: Obtain all grid points on the initial path, and calculate the potential field value and gradient of each grid point; Traverse the grid points on the initial path and determine whether there is a local minimum trap by analyzing the potential field values ​​and gradient changes of adjacent points; If it is judged to be a local minimum trap, then with the trap point as the center, among the nearby grid points, a point with a potential field value lower than the trap point and a decreasing gradient is selected as the new intermediate target point; Insert the new intermediate target point into the minimum heap priority queue and delete other points in the queue whose potential field values ​​are higher than this point; Taking the new intermediate target point as the starting point, the minimum heap priority queue algorithm is used to re-search the path and obtain a new path from the starting point to the target point; The new path is compared with the original path, and the better path is determined by calculating the sum of the potential field values ​​of the grid points on the path. If the new path is better, it is used as the final planned path; otherwise, the original path is kept unchanged and the remaining grid points are traversed, and repeated until the optimal path is found.

5. The method according to claim 4, characterized in that Taking the new intermediate target point as the starting point, the minimum heap priority queue algorithm is used to re-search the path to obtain a new path from the starting point to the target point. The process includes: The new intermediate target point coordinates are set as the starting point coordinates, and a minimum heap priority queue with the starting point coordinates as the root node is constructed; Obtain node information directly connected to the starting point from the graph data structure, including node coordinates and distance weights, and insert the node information into a minimum heap priority queue; Take out the node with the smallest distance weight from the minimum heap priority queue and mark it as visited, and use it as the new current node coordinate; Determine whether the coordinates of the current node are the coordinates of the target point. If so, output the path from the starting point to the current node as the new path; if the current node is not the target point, obtain all unvisited adjacent node information of the current node, calculate the distance weight and insert it into the minimum heap priority queue until the target point is found or all nodes are traversed to obtain a new path from the starting point to the target point; if the target point is still not found after searching all nodes, it is determined that the current path planning has no solution, and output a search failure prompt message.

6. The method according to claim 1, characterized in that In view of the special terrain constraints of narrow channels and sharp-angle convex areas, the process of dynamically adjusting the node expansion and search strategy of the minimum heap priority queue algorithm includes: Obtain the spatial distribution information of the neighborhood potential field values ​​of the grid points on the path, and determine whether there are special terrain constraints such as narrow channels and sharp-angle convex areas based on the distribution information; If it is determined that there is a special terrain constraint problem, the specific type of the special terrain is determined; Dynamically adjust the node expansion strategy and search strategy of the minimum heap priority queue algorithm according to the special terrain type; Through the dynamically adjusted minimum heap priority queue algorithm, the targeted node expansion sequence and node search path are obtained; The node expansion sequence and the node search path are used as passing strategies under special terrain constraints to guide the motion planning of the robot under special terrains such as narrow passages and sharp-angle convex areas.

7. The method according to claim 1, characterized in that In view of the smoothness problem of the initial path, under the premise of satisfying the path safety constraint, the process of smooth planning of the path includes: According to the preset path safety constraint conditions, the potential field value of the extended node in the path search process is obtained; Normalizing the potential field value of the extended node to obtain a normalized potential field value; According to the normalized potential field value, a smoothing algorithm is used to smooth the potential field value to obtain a smoothed potential field value; According to the potential field value after smoothing, the spatial distribution of the potential field value of the extended node is adjusted to obtain the adjusted spatial distribution of the potential field value; According to the adjusted spatial distribution of potential field values, a search algorithm is used to search for paths and obtain a preliminary planned path; Determine whether the preliminary planned path meets a preset path smoothness threshold, and if so, determine the preliminary planned path as the final planned path; If the preliminary planned path does not meet the preset path smoothness threshold, the process returns to readjust the spatial distribution of the potential field value until the searched planned path meets the preset path smoothness threshold.

8. The method according to claim 1, characterized in that The process of locally re-searching the affected part of the planned path to obtain an updated path includes: Acquire real-time environmental information during the path execution process, and dynamically update the environmental potential field map; If a dynamic obstacle is detected, obtain the position and speed information of the obstacle and update the potential field value of the area where the obstacle is located; Use the updated potential field value as a new constraint to determine the affected part of the planned path; For the affected path parts, a local search algorithm is used to replan and obtain the locally updated path segments; The locally updated path fragment is concatenated with the unaffected part of the original path to obtain the complete updated path; According to the updated path, the robot is controlled to continue to perform the task, and during the subsequent path execution, real-time environmental information is continuously obtained to achieve dynamic update of the path.

Images