Scene-interactive unmanned vehicle path planning method
By employing a path planning method based on multi-level graph search and grid map updates, the path planning problem of unmanned robots in dynamic scenarios is solved, achieving optimality and completeness in the handling of objects by unmanned robots in complex environments.
Patent Information
- Application Number
- CN202411144395.2
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-20
- Publication Date
- 2025-11-28
- Estimated Expiration
- 2044-08-20
AI Technical Summary
Existing path planning algorithms struggle to guarantee path feasibility in dynamic scenarios, especially when unmanned robots are transporting goods, leading to scene changes, abnormal situations such as violations of dynamics, changes in robot expansion radius, and no solution.
A multi-level graph search path planning method is adopted, which combines grid maps and graph search technology. It uses sensors such as binocular cameras and LiDAR for positioning, updates the grid map to adapt to scene changes, and uses technologies such as VINS and Cartographer for path planning.
This invention achieves optimality and completeness in the handling of objects by unmanned robots in complex environments, provides feasible solutions for handling multiple objects, and solves the path planning problem for unmanned robots that actively change the scene.
Smart Images
Figure CN118938928B_ABST
Abstract
Description
TECHNICAL FIELD
[0001] The present application relates to a scene-interactive unmanned vehicle path planning method, belonging to the field of unmanned robot path planning. BACKGROUND
[0002] Path planning problem is a key problem in the field of robotics and even automation. The definition of path planning problem is very simple: "find a collision-free motion between the initial (start) and final configuration (goal) in a specific environment". The simplest case is to plan a path in a static and known environment; however, more generally, the path planning problem can be formulated as any robotic system subject to kinematic constraints in a dynamic and unknown environment. It has a wide range of applications in real life, including unmanned driving, logistics distribution, robot navigation, etc. With the development of artificial intelligence and computer technology, path planning technology is also constantly being improved and applied.
[0003] Path planning algorithms can be divided into roadmap techniques, cell decomposition algorithms, and artificial potential methods according to the method of generating geometric paths.
[0004] Roadmap techniques are based on simplifying the n-dimensional configuration space into a set of one-dimensional paths to be searched, possibly on a graph. This approach maps free space connectivity into a system of one-dimensional curves (roadmaps) in the discrete configuration space or its closure. The resulting roadmap R contains a set of paths, so path planning is to connect the initial and final configurations with R to find a feasible path between the two configurations. It is very natural to associate a graph with a roadmap and define some optimality index (e.g. Euclidean length): then the graph can be searched to obtain the optimal solution (in most cases, this is represented by the shortest path) of the path planning problem. According to different roadmap construction methods, it can be divided into visibility graph, Voronoi diagram, probabilistic roadmap, etc. According to different search graph methods, many roadmap path planning methods are derived: RRT, RRT*, Informed RRT*, etc.
[0005] The free space of the robot is subdivided into several regions, called cells, so that a path between any two configurations located in the same cell can be generated directly. Then a so-called connection graph is defined naturally, which represents the adjacency relationship between cells. That is, the nodes of the graph represent the cells extracted from the free space, and there is an edge between two nodes if and only if the corresponding cells are adjacent. The path planning problem is transformed into a graph search problem again, so that it can be solved using graph search techniques. According to different cell decomposition methods, it can be divided into: grid map, octree map, etc. According to different graph search algorithms, many cell decomposition path planning methods are derived: BFS, DFS, Dijkstra's, A*, etc.
[0006] The artificial potential field method is another method to solve the path planning problem. The basic idea is to regard the robot in the configuration space as a moving point, which is affected by the potential field generated by the target configuration and the obstacles in the C-space. That is, the target configuration generates an attractive potential, and the obstacle generates a repulsive potential. The sum of the two effects is the total potential, which can be regarded as an artificial force representing the most promising direction of motion in the free path.
[0007] The current mainstream path planning algorithm mainly considers the planning problem in the static scene. For the application in the scene with dynamic elements, additional supplements to the above path planning are needed, such as adding a local planning link, DynamicA(D), D*Lite, etc. However, the dynamic factors considered in the current dynamic planning method are mainly non-cooperative targets (active movement), and the case of scene change caused by the movement of unmanned robots (passive movement) is not considered.
[0008] The warehouse robot warehouse sorting scene is a typical passive dynamic scene, that is, only when the robot transfers the box will the scene change. In this scenario, if the above planning method is used, abnormal situations such as violation of dynamics, change of robot inflation radius, and no solution will occur. SUMMARY
[0009] The purpose of the present application is to solve the problem that the existing method uses the existing static / dynamic path planning algorithm in the interactive scene, and to propose a scene-interactive unmanned vehicle path planning method.
[0010] The specific process of a scene-interactive unmanned vehicle path planning method is as follows:
[0011] Step 1: Construct a grid map;
[0012] Step 2: Scene-interactive unmanned vehicle path planning based on the grid map;
[0013] Step 3: The unmanned vehicle executes the scene-interactive unmanned vehicle path planning of step 2.
[0014] The beneficial effects of the present application are:
[0015] The present application proposes a scene-interactable unmanned vehicle path planning algorithm, which solves the problem of path planning of unmanned robots actively changing scenes from the principle level.
[0016] The present application proposes a multi-level graph search path planning method for the problem of unmanned robots carrying objects in complex environments.
[0017] The present application considers a scene-interactable unmanned vehicle path planning algorithm, which can solve the problem of path planning of unmanned robots actively changing scenes from the principle level.
[0018] The path planning method of the present application has optimality and completeness when solving the problem of unmanned vehicles carrying single objects, and can give a feasible solution when carrying multiple objects. BRIEF DESCRIPTION OF DRAWINGS
[0019] Figure 1 The flowchart of the present application;
[0020] Figure 2 The flowchart of the initialization process of the method of the present application;
[0021] Figure 3 The flowchart of the scene-interactable unmanned vehicle path planning algorithm. DETAILED DESCRIPTION
[0022] Detailed implementation one: combined with Figure 1 This embodiment describes the specific process of a scene-interactable unmanned vehicle path planning method:
[0023] Step one: construct a grid map;
[0024] Step two: scene-interactable unmanned vehicle path planning based on the grid map;
[0025] Given a final configuration, the desired position of the unmanned vehicle transfer scene element is described;
[0026] These scene elements that need to be transferred will be set as dynamic and interactive; other elements will be set as static and non-interactive.
[0027] Step three: the unmanned vehicle executes the scene-interactable unmanned vehicle path planning of step two.
[0028] The unmanned vehicle needs to use sensors such as binocular cameras and laser radars for positioning, and can use relatively mature VINS, Cartographer, etc.; push and pull operations need to use the execution mechanism of the unmanned vehicle such as mechanical arms, etc.
[0029] When the path planning is completed by the unmanned vehicle, the grid map is updated so as to run the path planning algorithm again, as shown in Figure 3 The specific process is as follows:
[0030] The size of the grid map does not change, and only the occupancy state of the grid needs to be modified. All grids are set to an unknown state, and the state of the corresponding grid is updated according to the information of the elements in the scene. If a grid is occupied by an obstacle, the state of the grid is updated to occupied. If the grid is free, the state of the grid is updated to free.
[0031] Specific implementation two: the difference between this implementation and the specific implementation one is that the grid map is constructed in step one.
[0032] The specific process is as follows:
[0033] Step one: given the scene boundary, the grid size, the coordinates of the origin of the grid map in the world coordinate system (used for backtracking the path), the initial position of the unmanned vehicle in the world coordinate system (used for constructing the initial node), the positions and geometric shapes of other elements in the scene (including static and interactive elements);
[0034] Step two: obtain the minimum bounding rectangle according to the given scene boundary, and the length and width of the minimum bounding rectangle are Width scenario and Height scenario respectively.
[0035] Step three: obtain the size of the grid map according to the given grid size and the length and width of the minimum bounding rectangle.
[0036] Step four: apply a two-dimensional array to represent the grid map according to the size of the grid map, and initialize each grid to an unknown state.
[0037] The value of each position in the array indicates whether the unmanned vehicle is allowed to pass through.
[0038] The state of the grid can be divided into three types: unknown, free (or free space), and occupied (or obstacle).
[0039] Step five: update the state of the grid according to the positions and geometric shapes of other elements in the scene to complete the construction of the grid map. The flowchart of this step is shown in Figure 2 .
[0040] The other steps and parameters are the same as those in the specific implementation one.
[0041] Specific implementation three: the difference between this implementation and the specific implementation one or two is that the size of the grid map is obtained according to the given grid size and the length and width of the minimum bounding rectangle in step three.
[0042] Expression is:
[0043] Width map = Width scenario / GridSize
[0044] Height map = Height scenario / GridSize
[0045] Wherein, Width map is the width of the grid map;
[0046] Height map is the length of the grid map;
[0047] GridSize is the given grid size;
[0048] The size of the grid will affect the accuracy and computational complexity of the map. Smaller grids can provide more accurate environment representation, but will increase the amount of calculation. Larger grids are the opposite.
[0049] Other steps and parameters are the same as in embodiment one or two.
[0050] Embodiment four: the difference between this embodiment and one of embodiments one to three is that in step one five, the state of the grid is updated according to the position and geometry of other elements in the scene, and the construction of the grid map is completed;
[0051] The specific process is:
[0052] If a grid is occupied, the grid state is updated to the occupied state; if the grid is free, the grid state is updated to the free state.
[0053] Other steps and parameters are the same as in one of embodiments one to three.
[0054] Embodiment five: the difference between this embodiment and one of embodiments one to four is that in step two, the scene interactive unmanned vehicle path planning is carried out based on the grid map (the operation sequence of the unmanned vehicle transporting all the interactive elements from the initial position to the desired position is obtained);
[0055] Given a terminal configuration, the desired position of the unmanned vehicle transporting the scene elements is described. These scene elements that need to be transported will be set as dynamic and interactive; other elements will be set as static and non-interactive. This step is completed by three algorithms; this algorithm will perform graph search in the entire scene state space;
[0056] The specific process is:
[0057] Step two one, set the node;
[0058] The node contains: the current position of all interactive elements (cargo that can be moved by the unmanned vehicle, excluding walls), the position of all interactive elements (cargo that can be moved by the unmanned vehicle, excluding walls) at the last time, the path length G value (1 value) of the unmanned vehicle from the initial position to the current position, the distance sum H value (1 value) of all interactive elements from the current position to the target position, the total estimated cost F value (1 value), the current position and posture of the unmanned vehicle (the posture refers to the direction of the vehicle head), the operation sequence (throttle, steering degree) of the unmanned vehicle from the position and posture at the last time to the current position and posture;
[0059] The open set and the closed set respectively store the nodes to be accessed and the nodes that have been accessed, and each time an element is added or deleted from the open set, the elements contained therein are sorted in ascending order according to the F value.
[0060] Step two, according to the positions of all interactive elements and the initial position of the unmanned vehicle, an initial node is constructed, and the initial node is added to the open set;
[0061] For the initial node:
[0062] G = 0,
[0063]
[0064] F = G + H
[0065] Wherein, k is the serial number of the interactive element;
[0066] σ is the total number of interactive elements;
[0067] is the current position of the interactive element k;
[0068] is the target position of the interactive element k;
[0069] Step two, judge whether the open set is empty, if the open set is empty, the search fails, which means that the target position of the unmanned vehicle cannot be reached;
[0070] If the open set is not empty, take the node with the minimum total cost F in the open set as the current node (i.e. the first node in the open set); if the current node has all the interactive elements in the target position, end the graph search, backtrack the nodes and obtain the operation sequence and the total cost F (here, backtracking means following the predecessor nodes until the starting node is reached); if the current node has all the interactive elements not in the target position, delete the current node from the open set and add it to the closed set; expand the evolution state of the neighboring nodes of the current node and add the neighboring nodes of the current node to the neighbor node list; repeat steps two and three (two stopping conditions for repeating steps two and three: 1. If the open set is empty, the search fails, indicating that the target position of the UAV cannot be reached; 2. If the open set is not empty, take the node with the minimum total cost in the open set as the current node; if the current node has all the interactive elements in the target position, end the graph search, backtrack the nodes and obtain the operation sequence and the total cost).
[0071] Call algorithm three, i.e. obtain the operation sequence of the UAV for transporting all the interactive elements from the initial position to the desired position.
[0072] The other steps and parameters are the same as those in one of the first to fourth embodiments.
[0073] The sixth embodiment is different from one of the first to fifth embodiments in that in step two and three, the evolution state of the neighboring nodes of the current node is expanded (the UAV transports any one interactive element to the target position), and the neighboring nodes of the current node are added to the neighbor node list.
[0074] The specific process is as follows:
[0075] The UAV transports one interactive element not in the target position to the target position, which is one case;
[0076] The UAV transports N interactive elements not in the target position to the target position, which is N cases;
[0077] N is a positive integer;
[0078] Obtain the start and end poses of the UAV and the start and end positions of all the interactive elements in each case;
[0079] Construct the neighbor nodes according to the start and end poses of the UAV and the start and end positions of all the interactive elements in all cases, and add them to the neighbor node list;
[0080] The neighbor node list includes: the position of all interactive elements after transfer, the position of all interactive elements at the last time (i.e. the current node) after transfer, the operation sequence of the unmanned vehicle (including throttle, steering and how to operate the scene elements (push, pull, transfer)) and the position after operation, the path Path and cost Cost (call algorithm two) of the unmanned vehicle when performing operation, the path length G value of the unmanned vehicle from the initial position to the current position, the distance sum H value (optional) of all interactive elements from the current position to the target position, the total estimated cost F value; the current position and posture of the unmanned vehicle (the posture means the direction of the vehicle head), the operation sequence (throttle, steering degree) of the unmanned vehicle from the position and posture at the last time to the current position and posture;
[0081] The process of expanding the neighbor node is:
[0082] When the cost Cost is infinite, the neighbor node cannot be expanded;
[0083] When the cost Cost is not infinite, the neighbor node is continuously judged:
[0084] 1) If the ithneighbor node is in the closed set, it is ignored that the ithneighbor node has been searched, and the ithneighbor node is ignored;
[0085] 2) If the ithneighbor node is not in the open set:
[0086] a. Update the path length G of the ithneighbor node i ′ value to
[0087] b. Update the H of the ithneighbor node i ′;
[0088]
[0089] Wherein, k is the serial number of the interactive element;
[0090] σ is the total number of interactive elements;
[0091] is the current position of the interactive element k in the ithneighbor node;
[0092] is the target position of the interactive element k;
[0093] c. Update the total estimated cost F of the ithneighbor node i ′ = G i ′ + H i ′;
[0094] d. Update other information of the ithneighbor node:
[0095] The node contains the unmanned vehicle operation sequence and the movement path Path set as the unmanned vehicle operation sequence and the movement path Path obtained by path planning of the unmanned vehicle transporting a single interactive element;
[0096] e. Add the ith neighbor node to the open set, and sort the nodes contained in the open set in ascending order according to the total estimated cost F value;
[0097] 3) If the ith neighbor node is in the open set and Remove the ith neighbor node from the open set;
[0098] a. Update the path length G i of the ith neighbor node to Update the total estimated cost F i of the ith neighbor node to i ; i
[0099] b. Update other information of the ith neighbor node:
[0100] The node contains the unmanned vehicle operation sequence and the movement path Path set as the unmanned vehicle operation sequence and the movement path Path obtained by path planning of the unmanned vehicle transporting a single interactive element;
[0101] c. Add the ith neighbor node to the open set, and sort the nodes contained in the open set in ascending order according to the total estimated cost F value;
[0102] 4) Do nothing in other cases.
[0103] The other steps and parameters are the same as one of the first to fifth embodiments.
[0104] The seventh embodiment is different from one of the first to sixth embodiments in that the path Path and the cost Cost of the neighbor node list when the unmanned vehicle performs the operation are obtained by path planning of the unmanned vehicle transporting a single interactive element (algorithm two);
[0105] This algorithm will perform graph search in the combined state space of the unmanned vehicle and the interactive element;
[0106] The specific process is as follows:
[0107] I. Set the node;
[0108] The node comprises: the position of the current interactive element (the goods that can be moved by the unmanned vehicle, excluding walls), the position of the current interactive element (the goods that can be moved by the unmanned vehicle, excluding walls) at the last time, the path length G value (1 value) of the unmanned vehicle from the initial position to the current position, the distance sum H value (1 value) of the current interactive element from the current position to the desired position, the total estimated cost F value (1 value), the current position and posture of the unmanned vehicle (the posture refers to the direction of the vehicle head), and the path of the unmanned vehicle from the position and posture at the last time to the current position and posture;
[0109] The open set and the closed set are used to store the nodes to be accessed and the nodes that have been accessed respectively, and the nodes contained in the open set are sorted in ascending order according to the F value each time a node is added or deleted from the open set;
[0110] Two: the initial node is constructed according to the initial position of the scene element and the initial position of the unmanned vehicle, the initial node is added to the open set, and the nodes contained in the open set are sorted in ascending order according to the F value;
[0111] Three: it is judged whether the open set is empty, if the open set is empty, the search fails, and it is indicated that the target position of the unmanned vehicle cannot be reached;
[0112] If the open set is not empty, the node with the minimum total estimated cost F in the open set (i.e., the first node in the open set) is taken as the current node; if the position of the interactive element of the current node is the same as the target position of the interactive element, the graph search is ended, the nodes are backtracked, and the operation sequence and the total cost F value are obtained; if the position of the interactive element of the current node is not the same as the target position of the interactive element, the current node is deleted from the open set and added to the closed set; the joint state adjacent to the current node is expanded, and the neighbor node list is added; the step three is repeatedly executed.
[0113] The other steps and parameters are the same as one of the first to sixth embodiments.
[0114] The eighth embodiment is different from one of the first to seventh embodiments in that the joint state adjacent to the current node is expanded in the step three, and the neighbor node list is added;
[0115] The specific process is as follows:
[0116] After the unmanned vehicle performs any one operation on the current transfer element, the start and end positions and postures of the unmanned vehicle and the start and end positions of the current transfer element are obtained;
[0117] The neighbor node is constructed according to the start and end positions and postures of the unmanned vehicle and the start and end positions of the current transfer element, and is added to the neighbor node list;
[0118] The operation includes the throttle, the steering degree and how to operate the scene element (push, pull, transfer);
[0119] The three neighbor node lists include: the positions of all interactive elements after transfer, the positions of all interactive elements at the last moment (i.e. the current node) after transfer, the operation sequence of the unmanned vehicle (including throttle, steering, and how to operate the scene elements (push, pull, transfer)) and the position after operation, the path Path and cost Cost of the unmanned vehicle when moving to perform the operation (obtained by calling an algorithm), the path length G value of the unmanned vehicle from the initial position to the current position, the distance sum H value of all interactive elements from the current position to the target position (optional), and the total estimated cost F value.
[0120] The other steps and parameters are the same as one of embodiments 1 to 7.
[0121] Embodiment 9: Different from one of embodiments 1 to 8, the process of obtaining the path Path and cost Cost of the unmanned vehicle when moving to perform the operation in the three neighbor node lists is as follows:
[0122] The HybridA* algorithm is used to process the current node position and neighbor node position to obtain the path Path and cost Cost of the unmanned vehicle when moving to perform the operation (algorithm 1).
[0123] The other steps and parameters are the same as one of embodiments 1 to 8.
[0124] Embodiment 10: Different from one of embodiments 1 to 9, when the cost Cost is infinite, the neighbor node cannot be expanded;
[0125] When the cost Cost is not infinite, the neighbor node is continuously judged as follows:
[0126] 1) If the ith neighbor node is in the closed set, it means that the ith neighbor node has been searched, and the ith neighbor node is ignored;
[0127] 2) If the ith neighbor node is not in the open set:
[0128] a. Update the path length G i of the ith neighbor node to
[0129] b. Update the H i of the ith neighbor node to
[0130]
[0131] (x i ,y i ) is the current position of the ith neighbor node;
[0132] (x goal ,ygoal ) is the target position of the i-th neighbor node;
[0133] c. update the total estimated cost F of the i-th neighbor node i ′ = G i ′ + H i ′;
[0134] d. update other information of the i-th neighbor node:
[0135] The path of the unmanned vehicle moving when performing the operation contained by the node is set as the path Path of the unmanned vehicle moving when performing the operation obtained by processing the current node position and the neighbor node position by using the Hybrid A* algorithm, and the operation performed by the unmanned vehicle is set as the operation performed by the unmanned vehicle when expanding the current node.
[0136] e. add the i-th neighbor node to the open set, and sort the nodes contained in the open set in ascending order according to the total estimated cost F value;
[0137] 3) if the i-th neighbor node is in the open set and remove the i-th neighbor node from the open set.
[0138] a. update the path length G i ′ value of the i-th neighbor node as update the total estimated cost F of the i-th neighbor node i ′ = G i ′ + H i ′;
[0139] b. update other information of the i-th neighbor node:
[0140] The path of the unmanned vehicle moving when performing the operation contained by the node is set as the path Path of the unmanned vehicle moving when performing the operation obtained by processing the current node position and the neighbor node position by using the Hybrid A* algorithm, and the operation performed by the unmanned vehicle is set as the operation performed by the unmanned vehicle when expanding the current node.
[0141] c. add the i-th neighbor node to the open set, and sort the nodes contained in the open set in ascending order according to the total estimated cost F value;
[0142] H′ is not updated, H′ has been calculated in the branch that is not in the open set, and H′ will not be changed due to the search process;
[0143] The H′ value in the algorithm is obtained by a heuristic function, the heuristic function estimates the cost from the current state to the target state, and if the estimated cost is an upper bound of the true cost, the optimality of the algorithm can be guaranteed.
[0144] 4) do nothing in other cases.
[0145] Algorithm two solves the path planning problem of unmanned vehicle transporting single interactive element to target position, and gives the operation sequence with optimal operation times of unmanned vehicle.
[0146] Whether a node is reachable or not depends on the path cost Cost obtained by algorithm two, if the path cost Cost is infinite, the node is unreachable, otherwise, the node is reachable.
[0147] H value is obtained by heuristic function, the heuristic function estimates the cost from current state to target state, if the estimated cost is upper bounded by the real cost, the optimality of the algorithm is guaranteed.
[0148] Other steps and parameters are the same as one of the first to ninth embodiments. The pseudo code of the unmanned vehicle path search process is as follows:
[0149]
[0150] The pseudo code of the interactive element and unmanned vehicle combined state search process is as follows:
[0151]
[0152] The pseudo code of the scene state search process is as follows
[0153]
[0154] The present application can also have other various embodiments, those skilled in the art can make various corresponding changes and modifications according to the present application without departing from the spirit and essence of the present application, but these corresponding changes and modifications should all belong to the protection scope of the claims attached to the present application.
Claims
1. A method for path planning of autonomous vehicles with interactive scenarios, characterized in that: The specific process of the method is as follows: Step 1: Construct a raster map; Step 2: Plan interactive autonomous vehicle paths based on grid maps; Step 3: The autonomous vehicle performs the interactive autonomous vehicle path planning in Step 2. The process of constructing a raster map in step one is as follows: Step 11: Given the scene boundary, grid size, coordinates of the grid map origin in the world coordinate system, the initial position of the autonomous vehicle in the world coordinate system, and the positions and geometry of other elements in the scene; Steps 1 and 2: Obtain the minimum bounding rectangle based on the given scene boundaries. The length and width of the minimum bounding rectangle are respectively Width. scenario and Height scenario ; Step 13: Obtain the dimensions of the raster map based on the given raster size and the length and width of the minimum bounding rectangle; Step 14: Allocate a two-dimensional array to represent the raster map based on its size, and initialize each raster to an unknown state; Step 15: Update the grid state based on the position and geometry of other elements in the scene to complete the construction of the grid map; In steps one and three, the size of the raster map is obtained based on the given raster size and the length and width of the minimum bounding rectangle; the expression is: Width map =Width scenario / GridSize Height map =Height scenario / GridSize Among them, Width map Height is the width of the raster map. map GridSize is the length of the raster map; GridSize is the given raster size. In step one five, the state of the raster is updated based on the position and geometry of other elements in the scene to complete the construction of the raster map; the specific process is as follows: If a grid cell is occupied, update the grid state to occupied; if a grid cell is free, update the grid state to free. Step two involves planning interactive autonomous vehicle paths based on a grid map; the specific process is as follows: Step Two:
1. Set up nodes; The node contains: the current position of all interactive elements, the previous position of all interactive elements, the path length G of the autonomous vehicle from the initial position to the current position, the total distance H of all interactive elements from the current position to the target position, the total estimated cost F, the current position and pose of the autonomous vehicle, and the operation sequence of the autonomous vehicle from the previous position and pose to the current position and pose. Step 22: Construct an initial node based on the positions of all interactive elements and the initial position of the autonomous vehicle, and add the initial node to the open set; For the initial node: G＝0 ， F = G + H Where k is the index of the interactive element, and σ is the total number of interactive elements; The current position of the interactive element k. The target position of the interactive element k; Steps 2 and 3: Determine if the open set is empty. If the open set is empty, the search fails, indicating that the drone's target location cannot be reached. If the open set is not empty, select the node with the smallest total estimated cost F in the open set as the current node; if the current position of all interactive elements of the current node is the same as the target position, end the graph search, backtrack the node and obtain the operation sequence and total cost F; if the current position of all interactive elements of the current node is not the same as the target position, remove the current node from the open set and add it to the closed set; expand the evolution state of the current node's neighboring nodes and add the current node's neighboring nodes to the neighbor node list; repeat steps two and three.
2. The method for path planning of an interactive unmanned vehicle according to claim 1, characterized in that: In steps two and three, the evolution states of the current node's neighboring nodes are expanded, and the current node's neighboring nodes are added to the neighbor node list. The specific process is as follows: One scenario is an autonomous vehicle transporting an interactive element that is not currently at its target location to that location. The driverless car will transport N interactive elements that are not in the target location to the target location. There are N possible scenarios. Obtain the starting and ending poses of the autonomous vehicle in each case, as well as the starting and ending positions of all interactive elements; Based on the starting and ending poses of the autonomous vehicle in all situations and the starting and ending positions of all interactive elements, construct neighbor nodes and add them to the neighbor node list. The neighbor node list includes: the positions of all interactive elements after the transfer, the positions of all interactive elements at the previous moment after the transfer, the operation sequence of the autonomous vehicle and its position after the operation, the path and cost of the autonomous vehicle when performing the operation, the path length G of the autonomous vehicle from the initial position to the current position, the total distance H of all interactive elements from the current position to the target position, the total estimated cost F; the current position and attitude of the autonomous vehicle, and the operation sequence of the autonomous vehicle from the position and attitude at the previous moment to the current position and attitude. The process of expanding neighbor nodes is as follows: When the cost is infinite, the neighboring nodes cannot be expanded. When the cost is not infinite, the neighboring nodes are further evaluated: 1) If the i-th neighbor node is in the closed set, it means that the i-th neighbor node has already been searched, so ignore the i-th neighbor node; 2) If the i-th neighbor node is not in the open set: a. Update the path length G′ of the i-th neighbor node. i Value b. Update the H′ of the i-th neighbor node. i ; Where k is the index of the interactive element, and σ is the total number of interactive elements; Let k be the current position of the interactive element k in the i-th neighbor node. The target position of the interactive element k; c. Update the total estimated cost F of the i-th neighbor node. i ′=G′ i +H′ i ; d. Update other information about the i-th neighbor node: The node contains the unmanned vehicle operation sequence and movement path Path, which are set to the unmanned vehicle operation sequence and movement path Path obtained by the path planning of the unmanned vehicle to transport a single interactive element. e. Add the i-th neighbor node to the open set, and sort the nodes in the open set in ascending order according to the total estimated cost F value; 3) If the i-th neighbor node is in an open set and Remove the i-th neighbor node from the open set; a. Update the path length G′ of the i-th neighbor node. i Value The estimated total cost F for updating the i-th neighbor node i ′=G′ i +H′ i ; b. Update other information about the i-th neighbor node: The node contains the unmanned vehicle operation sequence and movement path Path, which are set to the unmanned vehicle operation sequence and movement path Path obtained by the path planning of the unmanned vehicle to transport a single interactive element. c. Add the i-th neighbor node to the open set, and sort the nodes in the open set in ascending order according to the total estimated cost F value; 4) No action will be taken for other situations.
3. The method for path planning of an interactive unmanned vehicle according to claim 2, characterized in that: The path and cost of the autonomous vehicle when performing operations in the neighbor node list are obtained by planning the path of the autonomous vehicle to transport a single interactive element; the specific process is as follows:
1. Set up nodes; The node contains: the position of the currently interactive element, the position of the currently interactive element at the previous moment, the path length G of the autonomous vehicle from the initial position to the current position, the total distance H of the currently interactive element from the current position to the desired position, the total estimated cost F, the current position and pose of the autonomous vehicle, and the path of the autonomous vehicle from the previous position and pose to the current position and pose.
2. Construct initial nodes based on the initial positions of scene elements and the initial positions of autonomous vehicles, add the initial nodes to the open set, and sort the nodes contained in the open set in ascending order according to the F value.
3. Determine if the open set is empty. If the open set is empty, the search fails, indicating that the drone's target location cannot be reached. If the open set is not empty, take the node with the smallest total estimated cost F in the open set as the current node; If the position of the interactive element in the current node is the same as the target position of the interactive element, then end the graph search, backtrack the node, and obtain the operation sequence and the total cost F value. If the position of the interactive element of the current node is not the same as the target position of the interactive element, remove the current node from the open set and add it to the closed set; Expand the joint state of the current node's neighbors and add it to the list of neighbor nodes; Repeat step three.
4. The interactive unmanned vehicle path planning method according to claim 3, characterized in that: The third step involves expanding the joint state of the current node's neighbors and adding it to the neighbor node list; the specific process is as follows: After the unmanned vehicle performs any operation on the current transfer element, it obtains the starting and ending poses of the unmanned vehicle and the starting and ending positions of the current transfer element. Based on the starting and ending poses of the autonomous vehicle and the starting and ending positions of the current transfer element, construct neighbor nodes and add them to the neighbor node list; The three neighbor node lists include: the positions of all interactive elements after the transfer, the positions of all interactive elements at the previous moment after the transfer, the operation sequence of the unmanned vehicle and its position after the operation, the path and cost of the unmanned vehicle when performing the operation, the path length G of the unmanned vehicle from the initial position to the current position, the total distance H of all interactive elements from the current position to the target position, and the total estimated cost F.
5. The method for path planning of an interactive unmanned vehicle according to claim 4, characterized in that: The process of obtaining the path and cost of the autonomous vehicle when performing operations in the three neighbor node lists is as follows: The Hybrid A* algorithm is used to process the current node position and the neighbor node positions to obtain the path and cost of the autonomous vehicle when performing operations.
6. The method for path planning of an interactive unmanned vehicle according to claim 5, characterized in that: When the cost is infinite, the neighboring nodes cannot be expanded. When the cost is not infinite, the neighboring nodes are further evaluated: 1) If the i-th neighbor node is in the closed set, it means that the i-th neighbor node has already been searched, so ignore the i-th neighbor node; 2) If the i-th neighbor node is not in the open set: a. Update the path length G′ of the i-th neighbor node. i Value b. Update the H′ of the i-th neighbor node. i ; (x i ,y i Let be the current position of the i-th neighbor node, and (x) be the position of the i-th neighbor node. goal ,y goal Let be the target location of the i-th neighboring node; c. Update the total estimated cost F′ of the i-th neighbor node. i =G′ i +H′ i ; d. Update other information about the i-th neighbor node: The path that the autonomous vehicle moves when it performs an operation is set to the path obtained by processing the current node position and the neighbor node positions using the HybridA* algorithm. The operation performed by the autonomous vehicle in the node is set to the operation performed by the autonomous vehicle when expanding the current node. e. Add the i-th neighbor node to the open set, and sort the nodes in the open set in ascending order according to the total estimated cost F value; 3) If the i-th neighbor node is in an open set and Remove the i-th neighbor node from the open set; a. Update the path length G′ of the i-th neighbor node. i Value The estimated total cost F for updating the i-th neighbor node i ′=G′ i +H′ i ; b. Update other information about the i-th neighbor node: The path that the autonomous vehicle moves when it performs an operation is set to the path obtained by processing the current node position and the neighbor node positions using the HybridA* algorithm. The operation performed by the autonomous vehicle in the node is set to the operation performed by the autonomous vehicle when expanding the current node. c. Add the i-th neighbor node to the open set, and sort the nodes in the open set in ascending order according to the total estimated cost F value; 4) No action will be taken for other situations.