Scene-interactive unmanned vehicle path planning method

By constructing grid maps and multi-level map search algorithms in unmanned vehicle path planning, the path planning problem of unmanned robots in interactive scenarios is solved, and the optimal and complete path planning in complex environments is achieved.

CN118938928AActive Publication Date: 2024-11-12HARBIN INST OF TECH +1
View PDF 7 Cites 0 Cited by

Patent Information

Application Number
CN202411144395.2
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-20
Publication Date
2024-11-12
Estimated Expiration
2044-08-20

AI Technical Summary

Technical Problem

Existing path planning algorithms are difficult to ensure path feasibility in interactive scenarios, especially when the scene changes after the unmanned robot moves, it is easy to cause abnormal situations such as violating dynamics, changing the robot's expansion radius, and no solution.

Method used

A method of unmanned vehicle path planning that can interact in scenes is proposed. By constructing a grid map and using a multi-level map search algorithm, the optimality and completeness of path planning are achieved by considering the situation where the unmanned robot actively changes the scene.

Benefits of technology

The path planning of unmanned robots when moving objects in complex environments is optimal and complete, and can provide feasible solutions when moving multiple objects, solving the path planning problem of unmanned robots actively changing scenes.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118938928A_ABST
    Figure CN118938928A_ABST
Patent Text Reader

Abstract

The invention relates to a scene-interactive unmanned vehicle path planning method, and belongs to the field of unmanned robot path planning. The invention aims to solve the problem that the feasibility of a path is difficult to ensure in an interactive scene by adopting an existing static / dynamic path planning algorithm in the existing method, and provides an unmanned vehicle path planning method with an interactive scene. The unmanned vehicle path planning method capable of realizing scene interaction comprises the following specific processes: step 1, constructing a grid map; 2, performing scene interactive unmanned vehicle path planning based on the grid map; and step 3, the unmanned vehicle executes the scene interactive unmanned vehicle path planning in the step 2.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention relates to a scene-interactive unmanned vehicle path planning method, belonging to the field of unmanned robot path planning. Background Art

[0002] The path planning problem is a key issue in the field of robotics and even automation. The definition of the path planning problem is very simple: "find a collision-free motion between an initial (starting point) and a final configuration (target) 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 and other fields. 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 the following categories according to the method of generating geometric paths: roadmap techniques (Roadmap Techniques), cell segmentation (Cell Decomposition Algorithms), and artificial potential fields (Artificial Potential Methods).

[0004] Roadmap techniques are based on reducing the n-dimensional configuration space to a set of one-dimensional paths to be searched, possibly on a graph. This approach maps free space connectivity to a one-dimensional system of curves (roadmap) in a discrete configuration space or its closure. The resulting roadmap R contains a set of paths, so path planning is about connecting the initial configuration and the final configuration with R to find a feasible path between the two configurations. It is very natural to relate the graph to the roadmap and define some optimality index (such as the Euclidean length): the graph can then be searched to obtain the optimal solution to the path planning problem (in most cases, this is represented by the shortest path). 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] In the region segmentation method, the robot's free space is subdivided into several regions, called cells, so that the path between any two configurations in the same cell can be directly generated. Then it is natural to define a so-called connection graph, which represents the adjacency relationship between cells. That is, the nodes of the graph represent 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 once again transformed into a graph search problem, so it can be solved using graph search technology. According to different unit decomposition methods, it can be divided into: grid map, octree map, etc.; according to different graph search algorithms, many unit 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 obstacles generate a repulsive potential. The sum of these two effects is the total potential, which can be regarded as an artificial force applied, representing the most promising direction of movement in the free path.

[0007] The current mainstream path planning algorithms mainly consider planning problems in static scenes. For applications in scenes with dynamic elements, additional supplements to the above path planning are required, such as adding local planning links, DynamicA(D), D*Lite, etc. However, the dynamic factors considered in the current dynamic planning methods are mostly non-cooperative targets (active movement), and the situation where the scene changes (passive movement) caused by the movement of the unmanned robot is not considered.

[0008] The warehouse robot warehouse sorting scenario is a typical passive dynamic scenario, that is, the scenario changes only when the robot transfers the cargo box. If the above planning method is used in this scenario, abnormal situations such as violation of dynamics, change of robot expansion radius, and no solution will occur. Summary of the invention

[0009] The purpose of the present invention is to solve the problem that the existing methods using existing static / dynamic path planning algorithms are difficult to ensure the feasibility of the path in interactive scenes, 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: Build a raster map;

[0012] Step 2: Perform scene-interactive unmanned vehicle path planning based on the grid map;

[0013] Step 3: The unmanned vehicle executes the interactive unmanned vehicle path planning in the scenario of step 2.

[0014] The beneficial effects of the present invention are:

[0015] The present invention proposes a scene-interactive unmanned vehicle path planning algorithm, which solves the path planning problem of an unmanned robot actively changing scenes from a principle level.

[0016] Aiming at the problem of unmanned robots carrying objects in complex environments, the present invention proposes a multi-level graph search path planning method.

[0017] The present invention considers a scene-interactive unmanned vehicle path planning algorithm, which can solve the path planning problem of an unmanned robot actively changing scenes from a principle level.

[0018] The path planning method of the present invention is optimal and complete when solving the problem of unmanned vehicle transporting a single object, and can provide a feasible solution when transporting multiple objects. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 It is a flow chart of the present invention;

[0020] Figure 2 It is a flowchart of the initialization process of the method of the present invention;

[0021] Figure 3 Flowchart of the scene-interactive unmanned vehicle path planning algorithm. DETAILED DESCRIPTION

[0022] Specific implementation method 1: Combination Figure 1 This embodiment describes a scene-interactive unmanned vehicle path planning method. The specific process is as follows:

[0023] Step 1: Build a raster map;

[0024] Step 2: Perform scene-interactive unmanned vehicle path planning based on the grid map;

[0025] Given a final configuration, it describes the desired position of the elements of the unmanned vehicle transport scene;

[0026] Those scene elements that need to be transported will be set to be dynamic and interactive; other elements will be set to be static and non-interactive.

[0027] Step 3: The unmanned vehicle executes the interactive unmanned vehicle path planning in the scenario of step 2.

[0028] Unmanned vehicles need to use binocular cameras, lidar and other sensors for positioning, and can use more mature VINS, Cartographer, etc.; operations such as pushing and pulling require the use of unmanned vehicle actuators such as robotic arms.

[0029] When the unmanned vehicle completes the path planning, the grid map is updated so that a new path planning algorithm can be run again. Figure 3 As shown; the specific process is:

[0030] The size of the grid map does not change. You only need to modify the grid's occupancy status. Set all grids to an unknown state and update the corresponding grid's state based on the information of the elements in the scene. If a grid is occupied by an obstacle, update the grid state to occupied. If the grid is idle, update the grid state to idle.

[0031] Specific implementation method 2: This implementation method is different from specific implementation method 1 in that a grid map is constructed in step 1;

[0032] The specific process is:

[0033] Step 1: Given the scene boundary, grid size, coordinates of the grid map origin in the world coordinate system (used for backtracking path), the initial position of the unmanned vehicle in the world coordinate system (used for constructing the initial node), and the positions and geometric shapes of other elements in the scene (including static and interactive ones);

[0034] Step 1 and 2: Get the minimum bounding rectangle based on the given scene boundary. The length and width of the minimum bounding rectangle are Width scenario and Height scenario ;

[0035] Step 13: Obtain the size of the grid map according to the given grid size and the length and width of the minimum enclosing rectangle;

[0036] Step 14: Apply for 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 driverless car is allowed to pass;

[0038] The states of a grid can be divided into three categories: unknown, idle (or free space), and occupied (or obstacle).

[0039] Step 15: Update the grid status according to the position and geometry of other elements in the scene to complete the construction of the grid map. The flowchart of this step is as follows: Figure 2 shown.

[0040] The other steps and parameters are the same as those in the first embodiment.

[0041] Specific implementation method three: This implementation method is different from specific implementation methods one or two in that in step one or three, the size of the grid map is obtained according to the given grid size and the length and width of the minimum circumscribed rectangle;

[0042] The expression is:

[0043] Width map =Width scenario / GridSize

[0044] Height map =Height scenario / GridSize

[0045] Among them, 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 affects the accuracy and computational complexity of the map. Smaller grids provide a more accurate representation of the environment, but increase the amount of computation. Larger grids have the opposite effect.

[0049] The other steps and parameters are the same as those in the first or second embodiment.

[0050] Specific implementation method 4: This implementation method is different from one of specific implementation methods 1 to 3 in that in step 15, the state of the grid is updated according to the position and geometric shape of other elements in the scene to complete the construction of the grid map;

[0051] The specific process is:

[0052] If a grid is occupied, the grid state is updated to the occupied state; if the grid is idle, the grid state is updated to the idle state.

[0053] The other steps and parameters are the same as those in Specific Embodiments 1 to 3.

[0054] Specific implementation method five: This implementation method is different from any one of specific implementation methods one to four in that in step two, the scene-interactive unmanned vehicle path planning is performed based on the grid map (obtaining the operation sequence of the unmanned vehicle transporting all interactive elements from the initial position to the desired position);

[0055] Given a final configuration, it describes the expected position of the scene elements to be transported by the unmanned vehicle. The scene elements that need to be transported will be set to dynamic and interactive; other elements will be set to static and non-interactive. This step is completed by three algorithms; this algorithm will perform a graph search in the entire scene state space;

[0056] The specific process is:

[0057] Step 21: Set up nodes;

[0058] The nodes include: the current position of all interactive elements (goods that the unmanned vehicle can move, excluding walls), the position of all interactive elements (goods that the unmanned vehicle can move, excluding walls) at the previous moment, the path length G value (1 value) of the unmanned vehicle from the initial position to the current position, the sum of the distances H value of all interactive elements from the current position to the target position (1 value), the total estimated cost F value (1 value), the current position and posture of the unmanned vehicle (posture refers to the direction of the vehicle head), and the operation sequence (throttle, steering degree) of the unmanned vehicle from the position and posture of the previous moment to the current position and posture;

[0059] The open set and closed set store the nodes to be visited and the nodes that have been visited respectively. Each time an element is added to or deleted from the open set, the elements it contains are sorted in ascending order according to the F value.

[0060] Step 22: Construct an initial node based on the positions of all interactive elements and the initial position of the unmanned vehicle, and add the initial node to the open set;

[0061] For the initial node:

[0062] G = 0,

[0063]

[0064] F=G+H

[0065] Among them, 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 23: Determine whether the open set is empty. If the open set is empty, the search fails, indicating that the drone cannot reach the target position;

[0070] If the open set is not empty, take the node with the smallest total estimated cost F in the open set (i.e., the first node in the open set) as the current node; if the current position of all the 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 (backtracking here means visiting along the previous node until the starting node is reached); if the current position of all the interactive elements of the current node is different from the target position, delete the current node from the open set and add it to the closed set; expand the evolution state of the adjacent nodes of the current node, and add the adjacent nodes of the current node to the neighbor node list; repeat steps two and three (repeat steps two and three with two stop conditions: 1. If the open set is empty, the search fails, indicating that the drone target position cannot be reached; 2. If the open set is not empty, take the node with the smallest total cost in the open set as the current node; if the position 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).

[0071] By calling Algorithm 3, we can obtain the operation sequence of the unmanned vehicle to transfer all interactive elements from the initial position to the desired position.

[0072] The other steps and parameters are the same as those in Specific Embodiments 1 to 4.

[0073] Specific implementation method 6: This implementation method is different from specific implementation methods 1 to 5 in that the evolution state of the neighboring nodes of the current node is expanded in step 23 (the unmanned vehicle transports any interactive element to the target location), and the neighboring nodes of the current node are added to the neighbor node list;

[0074] The specific process is:

[0075] One situation is when an unmanned vehicle transports an interactive element that is not at the target location to the target location;

[0076] The unmanned vehicle transports N interactive elements that are not at the target location to the target location respectively. This is N situations;

[0077] N is a positive integer;

[0078] Obtain the starting and ending poses of the autonomous vehicle and the starting and ending positions of all interactive elements in each case;

[0079] Construct neighbor nodes based on the starting and ending positions of the unmanned vehicle in all cases and the starting and ending positions of all interactive elements, and add them to the neighbor node list;

[0080] The neighbor node list includes: the position of all interactive elements after the transfer, the position of all interactive elements at the previous moment after the transfer (i.e., the current node), the operation sequence of the unmanned vehicle (including the throttle, steering, and how to operate the scene elements (push, pull, transfer)) and the position after the operation, the path Path and the cost Cost (obtained by calling Algorithm 2) of the unmanned vehicle when performing the operation, the path length G value from the initial position to the current position of the unmanned vehicle, the total distance H value of all interactive elements from the current position to the target position (optional), and the total estimated cost F value; the current position and posture of the unmanned vehicle (the posture refers to the direction of the vehicle head), and the operation sequence (throttle, steering degree) of the unmanned vehicle from the position and posture of the previous moment to the current position and posture;

[0081] The process of expanding neighbor nodes is:

[0082] When the cost Cost is infinite, the neighbor node cannot be expanded;

[0083] When the cost Cost is non-infinite, the neighboring nodes are further judged:

[0084] 1) If the i-th neighbor node is in the closed set, it means that the i-th neighbor node has been searched and the i-th neighbor node is ignored;

[0085] 2) If the i-th neighbor node is not in the open set:

[0086] a. Update the path length G of the i-th neighbor node i The value of ′ is

[0087] b. Update H of the i-th neighbor node i ′;

[0088]

[0089] Among them, 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 i-th neighbor node;

[0092] is the target position of the interactive element k;

[0093] c. Update the total estimated cost F of the i-th neighbor node i ′=G i ′+H i ′;

[0094] d. Update other information of the i-th neighbor node:

[0095] The unmanned vehicle operation sequence and mobile path Path contained in the node are set as the unmanned vehicle operation sequence and mobile path Path obtained by the path planning of the unmanned vehicle transporting a single interactive element;

[0096] e. Add the i-th neighbor node to the open set, and sort the nodes included in the open set in ascending order according to the total estimated cost F value;

[0097] 3) If the i-th neighbor node is in the open set and Remove the i-th neighbor node from the open set;

[0098] a. Update the path length G of the i-th neighbor node i The value of ′ is Update the total estimated cost F of the i-th neighbor node i ′=G i ′+H i ′;

[0099] b. Update other information of the i-th neighbor node:

[0100] The unmanned vehicle operation sequence and mobile path Path contained in the node are set as the unmanned vehicle operation sequence and mobile path Path obtained by the path planning of the unmanned vehicle transporting a single interactive element;

[0101] c. Add the i-th neighbor node to the open set, and sort the nodes included in the open set in ascending order according to the total estimated cost F value;

[0102] 4) No processing will be done for other situations.

[0103] The other steps and parameters are the same as those in Specific Implementation Methods 1 to 5.

[0104] Specific implementation method 7: This implementation method is different from any one of specific implementation methods 1 to 6 in that the path Path and the cost Cost of the unmanned vehicle in the neighbor node list when performing the operation are obtained by path planning (algorithm 2) for the unmanned vehicle to transport a single interactive element;

[0105] This algorithm will perform a graph search in the combined state space of the autonomous vehicle and the interactive elements;

[0106] The specific process is:

[0107] 1. Set up nodes;

[0108] The nodes include: the position of the current interactive element (goods that the unmanned vehicle can move, excluding walls), the position of the current interactive element (goods that the unmanned vehicle can move, excluding walls) at the previous moment, the path length G value (1 value) of the unmanned vehicle from the initial position to the current position, the total distance H value of the current interactive element from the current position to the expected position (1 value), the total estimated cost F value (1 value), the current position and posture of the unmanned vehicle (posture refers to the direction of the vehicle head), and the path of the unmanned vehicle from the position and posture of the previous moment to the current position and posture;

[0109] Use open sets and closed sets to store nodes to be visited and nodes that have been visited respectively. Every time a node is added or deleted from an open set, the nodes in the open set are sorted in ascending order according to the F value.

[0110] Second: construct the initial node according to the initial position of the scene elements and the initial position of the unmanned vehicle, add the initial node to the open set, and sort the nodes contained in the open set in ascending order according to the F value;

[0111] 3: Determine whether the open set is empty. If the open set is empty, the search fails, indicating that the drone cannot reach the target position;

[0112] If the open set is not empty, take the node with the smallest total estimated cost F in the open set (i.e., the first node in the open set) 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, end the graph search, backtrack the nodes 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, delete the current node from the open set and add it to the closed set; expand the adjacent joint state of the current node and add it to the neighbor node list; repeat step three.

[0113] The other steps and parameters are the same as those in Specific Embodiments 1 to 6.

[0114] Specific implementation eight: This implementation differs from one of specific implementations one to seven in that in the third implementation, the adjacent joint state of the current node is expanded to add a neighbor node list;

[0115] The specific process is:

[0116] After the unmanned vehicle performs any operation on the current transfer element, the starting and ending positions of the unmanned vehicle and the starting and ending positions of the current transfer element are obtained;

[0117] Construct neighbor nodes according to the starting and ending positions of the unmanned vehicle and the starting and ending positions of the current transfer element, and add them to the neighbor node list;

[0118] The operations include throttle, steering, and how to operate scene elements (push, pull, transfer);

[0119] The three neighbor node lists include: the position of all interactive elements after the transfer, the position of all interactive elements at the previous moment after the transfer (that is, the current node), the operation sequence of the unmanned vehicle (including the throttle, steering and how to operate the scene elements (push, pull, transfer)) and the position after the operation, the path Path and cost Cost of the unmanned vehicle when performing the operation (obtained by calling algorithm 1), the path length G value that the unmanned vehicle travels from the initial position to the current position, the total distance 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 those in Specific Embodiments 1 to 7.

[0121] Specific implementation method 9: This implementation method is different from any one of specific implementation methods 1 to 8 in that the process of obtaining the path Path and the cost Cost of the unmanned vehicle when performing the operation in the three neighbor node lists is as follows:

[0122] The HybridA* algorithm is used to process the current node position and the neighboring node position to obtain the path and cost of the unmanned vehicle when performing the operation (Algorithm 1).

[0123] The other steps and parameters are the same as those in Specific Embodiments 1 to 8.

[0124] Specific implementation method 10: This implementation method is different from any one of specific implementation methods 1 to 9 in that when the cost Cost is infinite, the neighbor node cannot be expanded;

[0125] When the cost Cost is non-infinite, the neighboring nodes are further judged:

[0126] 1) If the i-th neighbor node is in the closed set, it means that the i-th neighbor node has been searched and the i-th neighbor node is ignored;

[0127] 2) If the i-th neighbor node is not in the open set:

[0128] a. Update the path length G of the i-th neighbor node i The value of ′ is

[0129] b. Update H of the i-th neighbor node i ′;

[0130]

[0131] (x i ,y i ) is the current position of the i-th 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 that the unmanned vehicle contained in the node moves when performing an operation is set to the path Path obtained by processing the current node position and the neighboring node position using the HybridA* algorithm, and the operation performed by the unmanned vehicle contained in the node is set to 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 included 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 of the i-th neighbor node i The value of ′ is 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 that the unmanned vehicle contained in the node moves when performing an operation is set to the path Path obtained by processing the current node position and the neighboring node position using the HybridA* algorithm, and the operation performed by the unmanned vehicle contained in the node is set to 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 included in the open set in ascending order according to the total estimated cost F value;

[0142] H′ is not updated. H′ has already been calculated in the branch where it is not in the open set, and it will not change during the search process.

[0143] The H′ value in this algorithm is obtained by a heuristic function, which estimates the cost from the current state to the target state. If the estimated cost is bounded by the true cost, the optimality of the algorithm can be guaranteed.

[0144] 4) No processing will be done for other situations.

[0145] Algorithm 2 solves the path planning problem of an unmanned vehicle transporting a single interactive element to the target location, and gives the optimal operation sequence for the unmanned vehicle.

[0146] Whether a node is reachable depends on whether the path cost Cost obtained by Algorithm 2 is infinite. If the path cost Cost is infinite, it is unreachable, otherwise it is reachable.

[0147] The H value is obtained by the heuristic function, which estimates the cost from the current state to the target state. If the estimated cost is bounded by the true cost, the optimality of the algorithm can be guaranteed.

[0148] The other steps and parameters are the same as those in the specific implementations 1 to 9. Pseudo code of the unmanned vehicle path search process:

[0149]

[0150] Pseudo code of the joint state search process of interactive elements and unmanned vehicles:

[0151]

[0152] Pseudo code of the scene state search process

[0153]

[0154] The present invention may also have many other embodiments. Without departing from the spirit and essence of the present invention, those skilled in the art may make various corresponding changes and modifications based on the present invention, but these corresponding changes and modifications should all fall within the scope of protection of the claims attached to the present invention.

Claims

1. A scene-interactive unmanned vehicle path planning method, characterized by: The specific process of the method is: Step 1: Build a raster map; Step 2: Perform scene-interactive unmanned vehicle path planning based on the grid map; Step 3: The unmanned vehicle executes the interactive unmanned vehicle path planning in the scenario of step 2.

2. The scene-interactive unmanned vehicle path planning method according to claim 1, characterized in that: In the step 1, a grid map is constructed; the specific process is as follows: Step 1: Given the scene boundary, grid size, coordinates of the grid map origin in the world coordinate system, the initial position of the unmanned vehicle in the world coordinate system, and the positions and geometric shapes of other elements in the scene; Step 1 and 2: Get the minimum bounding rectangle based on the given scene boundary. The length and width of the minimum bounding rectangle are Width scenario and Height scenario ; Step 13: Obtain the size of the grid map according to the given grid size and the length and width of the minimum enclosing rectangle; Step 14: Apply for 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; Step 15: Update the state of the grid according to the position and geometry of other elements in the scene to complete the construction of the grid map.

3. The scene-interactive unmanned vehicle path planning method according to claim 2, characterized in that: In the step 1-3, the size of the grid map is obtained according to the given grid size and the length and width of the minimum circumscribed rectangle; the expression is: Width map =Width scenario / GridSize Height map =Height scenario / GridSize Among them, Width map is the width of the grid map, Height map is the length of the grid map; GridSize is the given grid size.

4. The scene-interactive unmanned vehicle path planning method according to claim 3, characterized in that: In the step 15, the state of the grid is updated according to the position and geometric shape of other elements in the scene to complete the construction of the grid map; the specific process is: If a grid is occupied, the grid state is updated to the occupied state; if the grid is idle, the grid state is updated to the idle state.

5. The scene-interactive unmanned vehicle path planning method according to claim 4, characterized in that: In the step 2, scene-interactive unmanned vehicle path planning is performed based on the grid map; the specific process is as follows: Step 21: Set up nodes; The nodes include: the current position of all interactive elements, the position of all interactive elements at the previous moment, the path length G value of the unmanned vehicle from the initial position to the current position, the sum of the distances H value 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, and the operation sequence of the unmanned vehicle transferring from the position and posture of the previous moment to the current position and posture; Step 22: Construct an initial node based on the positions of all interactive elements and the initial position of the unmanned vehicle, and add the initial node to the open set; For the initial node: G=0, F=G+H Among them, k is the serial number of the interactive element, and σ is the total number of interactive elements; is the current position of the interactive element k, is the target position of the interactive element k; Step 23: Determine whether the open set is empty. If the open set is empty, the search fails, indicating that the drone cannot reach the target position; 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 current position of all the interactive elements of the current node is the same as the target position, end the graph search, backtrack the nodes and obtain the operation sequence and the total cost F; if the current position of all the interactive elements of the current node is not the same as the target position, delete the current node from the open set and add it to the closed set; expand the evolution state of the adjacent nodes of the current node, and add the adjacent nodes of the current node to the neighbor node list; repeat steps two and three.

6. The scene-interactive unmanned vehicle path planning method according to claim 5, characterized in that: In the steps 2 and 3, the evolution status of the neighboring nodes of the current node is expanded, and the neighboring nodes of the current node are added to the neighboring node list; The specific process is: One situation is when an unmanned vehicle transports an interactive element that is not at the target location to the target location; The unmanned vehicle transports N interactive elements that are not at the target location to the target location respectively. This is N situations; Obtain the starting and ending poses of the autonomous vehicle and the starting and ending positions of all interactive elements in each case; Construct neighbor nodes based on the starting and ending positions of the unmanned vehicle in all cases and the starting and ending positions of all interactive elements, 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 unmanned vehicle and the position after the operation, the path Path and the cost Cost of the unmanned vehicle when performing the operation, the path length G value of the unmanned vehicle from the initial position to the current position, the sum of the distances H of all interactive elements from the current position to the target position, and the total estimated cost F value; the current position and posture of the unmanned vehicle, and the operation sequence of the unmanned vehicle transferring from the position and posture of the previous moment to the current position and posture; The process of expanding neighbor nodes is: When the cost Cost is infinite, the neighbor node cannot be expanded; When the cost Cost is non-infinite, the neighboring nodes are further judged: 1) If the i-th neighbor node is in the closed set, it means that the i-th neighbor node has been searched and the i-th neighbor node is ignored; 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 The value of ′ is b. Update H of the i-th neighbor node i ′; Among them, k is the serial number of the interactive element, and σ is the total number of interactive elements; is the current position of the interactive element k in the i-th neighbor node, is 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 of the i-th neighbor node: The unmanned vehicle operation sequence and mobile path Path contained in the node are set as the unmanned vehicle operation sequence and mobile path Path obtained by the path planning of the unmanned vehicle transporting a single interactive element; e. Add the i-th neighbor node to the open set, and sort the nodes included in the open set in ascending order according to the total estimated cost F value; 3) If the i-th neighbor node is in the 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 The value of ′ is Update the total estimated cost F of the i-th neighbor node i ′=G i ′+H i ′; b. Update other information of the i-th neighbor node: The unmanned vehicle operation sequence and mobile path Path contained in the node are set as the unmanned vehicle operation sequence and mobile path Path obtained by the path planning of the unmanned vehicle transporting a single interactive element; c. Add the i-th neighbor node to the open set, and sort the nodes included in the open set in ascending order according to the total estimated cost F value; 4) No processing will be done for other situations.

7. The scene-interactive unmanned vehicle path planning method according to claim 6, characterized in that: The path Path and the cost Cost of the unmanned vehicle when performing the operation in the neighbor node list are obtained by planning the path of the unmanned vehicle transporting a single interactive element; the specific process is:

1. Set up nodes; The nodes include: the current position of the interactive element, the position of the current interactive element at the previous moment, the path length G value of the unmanned vehicle from the initial position to the current position, the total distance H value of the current interactive element from the current position to the expected position, the total estimated cost F value, the current position and posture of the unmanned vehicle, and the path of the unmanned vehicle from the position and posture of the previous moment to the current position and posture; Second: construct the initial node according to the initial position of the scene elements and the initial position of the unmanned vehicle, add the initial node to the open set, and sort the nodes contained in the open set in ascending order according to the F value; 3: Determine whether the open set is empty. If the open set is empty, the search fails, indicating that the drone cannot reach the target position; 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 of the current node is the same as the target position of the interactive element, then end the graph search, backtrack the nodes and obtain the operation sequence and the total cost F value; If the position of the interactive element of the current node is different from the target position of the interactive element, delete the current node from the open set and add it to the closed set; Expand the adjacent joint state of the current node and add it to the neighbor node list; Repeat three times.

8. The scene-interactive unmanned vehicle path planning method according to claim 7, characterized in that: The third step expands the adjacent joint state of the current node and adds it to the neighbor node list; the specific process is: After the unmanned vehicle performs any operation on the current transfer element, the starting and ending positions of the unmanned vehicle and the starting and ending positions of the current transfer element are obtained; Construct neighbor nodes according to the starting and ending positions of the unmanned vehicle and the starting and ending positions of the current transfer element, and add them to the neighbor node list; The three neighbor node lists include: the positions of all interactive elements after transfer, the positions of all interactive elements at the previous moment after transfer, the operation sequence of the unmanned vehicle and the position after the operation, the path Path and cost Cost of the unmanned vehicle when performing the operation, the path length G value of the unmanned vehicle from the initial position to the current position, the total distance H value of all interactive elements from the current position to the target position, and the total estimated cost F value.

9. The scene-interactive unmanned vehicle path planning method according to claim 8, characterized in that: The process of obtaining the path Path and cost Cost when the unmanned vehicle performs an operation in the three neighbor node lists is as follows: The HybridA* algorithm is used to process the current node position and the neighboring node position to obtain the path Path and cost Cost of the unmanned vehicle when performing the operation.

10. The scene-interactive unmanned vehicle path planning method according to claim 9, characterized in that: When the cost Cost is infinite, the neighbor node cannot be expanded; When the cost Cost is non-infinite, the neighboring nodes are further judged: 1) If the i-th neighbor node is in the closed set, it means that the i-th neighbor node has been searched and the i-th neighbor node is ignored; 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 The value of ′ is b. Update H of the i-th neighbor node i ′; (x i ,y i ) is the current position of the i-th neighbor node, (x goal ,y goal ) is the target position of the i-th neighbor node; c. Update the total estimated cost F of the i-th neighbor node i ′=G i ′+H i ′; d. Update other information of the i-th neighbor node: The path that the unmanned vehicle contained in the node moves when performing an operation is set to the path Path obtained by processing the current node position and the neighboring node position using the HybridA* algorithm, and the operation performed by the unmanned vehicle contained in the node is set to the operation performed by the unmanned vehicle when expanding the current node. e. Add the i-th neighbor node to the open set, and sort the nodes included in the open set in ascending order according to the total estimated cost F value; 3) If the i-th neighbor node is in the 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 The value of ′ is Update the total estimated cost F of the i-th neighbor node i ′=G i ′+H i ′; b. Update other information of the i-th neighbor node: The path that the unmanned vehicle contained in the node moves when performing an operation is set to the path Path obtained by processing the current node position and the neighboring node position using the HybridA* algorithm, and the operation performed by the unmanned vehicle contained in the node is set to the operation performed by the unmanned vehicle when expanding the current node. c. Add the i-th neighbor node to the open set, and sort the nodes included in the open set in ascending order according to the total estimated cost F value; 4) No processing will be done for other situations.

Citation Information

Patent Citations

  • Dynamic path planning method based on Hybrid-astar

    CN112923946A

  • Hierarchical decoupling unmanned anthropomorphic trajectory planning method

    CN116147631A

  • Target point distribution and cooperative path planning method for multiple unmanned vehicles

    CN117420838A

  • Unmanned forklift outdoor loading and unloading path planning method and device

    CN117553819A

  • Unstructured scene-oriented automatic driving tractor path planning method

    CN118192601A