CN107037812A - A kind of vehicle path planning method based on storage unmanned vehicle - Google Patents

Abstract

The invention discloses a vehicle path planning method based on a storage unmanned vehicle. This method first determines the operating nodes of the unmanned vehicle in the storage environment, and makes a topological map based on the environment; then, by improving the traditional A* algorithm, the shortest path from the starting point to the target point of the unmanned vehicle is calculated offline; During the path tracking process of the unmanned vehicle, at the same time, the sensor carried by itself is used to detect whether there is an obstacle on the path. If there is an obstacle and the obstacle does not completely block the passage, it will switch to the artificial potential field method for online real-time obstacle avoidance; If the obstacle completely obstructs the passage, switch to the A* algorithm to re-plan the path until the unmanned vehicle reaches the target point. This method can not only make full use of the known information to generate the global optimal path, but also effectively avoid random obstacles on the path.

Description

A vehicle path planning method based on warehouse unmanned vehicles

technical field

The invention belongs to path planning technology, in particular to a vehicle path planning method based on storage unmanned vehicles.

Background technique

With the rapid development of the Internet of Things, intelligent storage has become particularly important. Large warehouses have many warehouses, a lot of goods, and complex road conditions. How to quickly and effectively find goods in this complex environment is particularly prominent. To solve this problem, warehouse unmanned vehicles came into being. Warehousing unmanned vehicle refers to a car equipped with an automatic guiding device, capable of traveling along a designated path, capable of intelligently controlling the running action, and equipped with a safety protection device with a handling function.

In the research of unmanned vehicle-related technologies, path planning technology is an important topic. According to the degree of environmental information perception of unmanned vehicles, path planning is divided into two types: global path planning with fully known environmental information and local path planning with completely unknown or partially unknown environmental information. Global path planning is generally carried out offline. Usually, the cost value of each node can be comprehensively evaluated by setting a suitable heuristic function. By comparing the cost value of each expanded node, the most promising point is selected for expansion until the target node is found ( 1. Li Weiguang, Su Xia. AGV Path Planning Based on Improved A* Algorithm [J]. Modern Manufacturing Engineering, 2015(10): 33-36.2. Li Ji, Sun Xiuxia. UAV Track Based on Improved A-Star Algorithm Research on Planning Algorithm [J]. Journal of Ordnance Engineering, 2008,29(7):788-792.). Local path planning can adopt the method of simulating the movement of objects under gravitational repulsion. The gravitational force is between the target point and the moving body, and the repulsive force is between the moving body and obstacles. Path optimization is carried out by establishing gravitational field and repulsive field functions (3. Tan Baocheng, Cui Jiachao .Application of improved artificial potential field method in unmanned vehicle obstacle avoidance[J].Journal of Xi'an Technological University,2014(12):1007-1011.4.Sciavicco L,Siciliano B.Asolution algorithm to the inverse kinematic problem for redundantmanipulators[J] ]. Robotics & Automation IEEE Journal of, 2010, 4(4): 403-410.). However, such methods usually have a large amount of calculation and high positioning difficulty, and the single use of global path planning or local path planning methods has poor flexibility and cannot solve the problems of optimization and obstacle avoidance at the same time.

Contents of the invention

The purpose of the present invention is to provide a path planning method that combines the global path planning and local path planning of the unmanned vehicle, so as to effectively avoid obstacles while finding the optimal path.

The technical solution to realize the purpose of the present invention is: a vehicle path planning method based on warehouse unmanned vehicles, the steps are as follows:

The first step is to make a topological map of the storage environment, that is, to collect the relative position of the feasible path of the unmanned vehicle in the storage environment and the shelf, according to the collected information, set the nodes that the unmanned vehicle can reach, and then create an adjacency based on the topological map based on the node information matrix;

The second step is to plan the global path offline, that is, use the improved A* algorithm to calculate the optimal path from the starting point to the target point in an offline manner;

The third step is path tracking, which is to track the optimal path planned offline with the tracking module carried by the unmanned vehicle itself;

The fourth step is to detect and collect random obstacle information, judge the path environment, and switch between the two algorithms of global path planning and local path planning according to the obstacle information;

In the fifth step, the vehicle drives according to the path planned in the fourth step, and avoids random obstacles according to the local path planning algorithm during the driving process, so as to achieve the purpose of local obstacle avoidance.

Compared with the prior art, the present invention has the following significant advantages: 1) in the global path planning project, the topological nodes are optimized, so that the calculation amount is reduced and the efficiency is higher; 2) in the path tracking process, it is only necessary to find the planned Topological nodes are enough, so path tracking can be carried out in the way of tracing, making positioning more accurate; 3) Combining global path planning and local path planning, local obstacle avoidance can be performed while finding the optimal path, so that no one The operation of the vehicle is more flexible, and the running distance is shorter when encountering obstacles, which improves the working efficiency of the storage unmanned vehicle.

The present invention will be described in further detail below in conjunction with the accompanying drawings.

Description of drawings

Fig. 1 is a flow chart of the vehicle path planning method based on warehouse unmanned vehicles in the present invention.

FIG. 2 is a schematic diagram of a storage environment.

Figure 3 is the created topology map.

Fig. 4 is a flowchart of global path planning.

Figure 5 is a graph of the results of global path planning.

Fig. 6 is the result diagram when the unmanned vehicle avoids obstacles.

Figure 7 is the result of re-planning the global path when the unmanned vehicle encounters an obstacle that cannot be bypassed.

detailed description

In conjunction with the accompanying drawings, a vehicle path planning method based on warehouse unmanned vehicles of the present invention, the steps are as follows:

Step 1. Make a topological map of the storage environment. Specifically, collect the relative positions of the feasible path of the unmanned vehicle in the storage environment and the shelf. According to the collected information, set the nodes that the unmanned vehicle can reach, and then create an adjacency based on the topology map based on the node information. matrix; the steps for making a topological map of the storage environment are as follows:

Step 1-1. Collect environmental map information. Take each intersection turning point in the environmental map and the point where the unmanned vehicle needs to stop as a topological node, and the road connected to the topological node as a topological edge. The environmental map is abstracted by topological nodes and A topological map composed of topological edges, the topological map is symbolically expressed as G=(V, E), where V is a set of nodes, and E is a set of edges connecting nodes;

Step 1-2. Use the adjacency matrix to represent the relationship between nodes. If there are n topological nodes v ₁ , v ₂ ,...,v _n in the topological map, the adjacency matrix is an n×n matrix. Different attributes are given to each edge to distinguish the passable path from the impassable path. The weight attribute of the passable path is set to 1, and the weight attribute of the impassable path is set to 0; the (i, j) elements a _ij are expressed as

Step 2, perform global path planning offline, that is, use the improved A* algorithm to calculate the optimal path from the starting point to the target point in an offline manner; the offline global path planning steps are as follows:

Step 2-1. Use the A* algorithm to design an evaluation function for each road node, as shown in the following formula:

f(s)=g(s)+h(s)

In the formula, f(s) represents the estimated length from the starting node to the target node through node s, g(s) represents the path length from the starting node to the current node, and h(s) is a heuristic function, which is the path length from the current node to the current node. Estimated value of the target node;

The design method of the evaluation function is:

Step 2-1-1, record the path length g(s) from the starting node to the current node;

Step 2-1-2. Determine the heuristic function h(s), where the prerequisites for the A* algorithm to be able to search for the optimal path are:

h(s)≤cost*(s,s _goal )

In the formula, cost*(s, s _goal ) is the optimal distance from the current node to the target node, and the larger the value of h(s) satisfying the above formula, the fewer expanded nodes;

The heuristic function h(s) is a Euclidean distance function, and for given two position coordinates ( _xi , y _i ) and (x _j , y _j ), their Euclidean distance d _e is as follows The formula shows:

Step 2-1-3, determine the valuation function, as shown in the following formula:

f(s)=g(s)+h(s).

Step 2-2, create two sets of OPEN and CLOSED to manage road nodes, wherein the OPEN set is used to store the child nodes of the expanded road nodes, and these nodes belong to the nodes to be expanded; the CLOSED set is used to store the expanded nodes;

Step 2-3, carry out global path planning, each time select the node s with the smallest f(s) value from the OPEN set for expansion, and the child nodes to which the node s is expanded are stored in the OPEN set; after the expansion of the node s is completed, Move from the OPEN set to the CLOSED set; then loop the above process until the target node is expanded or the OPEN set is empty, then the algorithm is terminated and the planned path is recorded.

Step 3. Track the path, that is, use the tracking module carried by the unmanned vehicle to track the optimal path planned offline;

Step 4. Detect and collect random obstacle information, judge the path environment, and switch between the two algorithms of global path planning and local path planning according to the obstacle information; the specific steps are as follows:

Step 4-1. The unmanned vehicle detects random obstacles on the path in real time through the ultrasonic module carried by itself, and generates obstacle boundaries and relative position information;

Step 4-2, judge the road environment through the obstacle information collected by the sensor, and switch between the global road strength planning and the local path planning algorithm according to the judgment result; specifically:

Set the safe distance between the unmanned vehicle and the obstacle. If the distance between the boundary of the obstacle and the wall is smaller than the safe distance, it is considered that the unmanned vehicle cannot bypass it. The topological node where the current unmanned vehicle is located is taken as the starting point, and set The topological edge between this topological node and the next node is impassable, update the adjacency matrix, and return to step 2 to replan the global path; if the distance between the obstacle boundary and the wall is greater than the safety distance, switch to step 5 to perform local path planning algorithm for obstacle avoidance.

Step 5. The vehicle drives according to the path planned in step 4, and avoids random obstacles according to the local path planning algorithm during the driving process, so as to achieve the purpose of local obstacle avoidance. Specific steps are as follows:

Step 5-1, establish the starting point and end point of the local path planning, specifically take the current node of the unmanned vehicle as the starting point, and use the end point of the next node of the node in the global path planning;

Step 5-2, according to the size of the unmanned vehicle, determine the range of obstacles;

Step 5-3, establish the gravitational potential field function and the repulsion potential field function of the artificial potential field method, and perform local obstacle avoidance; the specific steps are as follows:

Step 5-3-1. Establish the gravitational potential field function U _att , the formula is as follows:

In the formula: K _a is the gravitational potential field constant, Q _c is the position vector of the earth coordinate system of the unmanned vehicle, and Q _g is the position vector of the target point in the earth coordinate system;

Step 5-3-2, establish the repulsion potential field function U _rep , the formula is as follows:

In the formula: K _r is the repulsion potential field constant, ρ(Q _c , Q _obs ) is the relative distance between the unmanned vehicle and the obstacle, and D is the safety distance.

The present invention combines global path planning with local path planning, and can perform local obstacle avoidance while searching for an optimal path, so that the operation of the unmanned vehicle is more flexible, the running distance is shorter when encountering obstacles, and the storage space is improved. Work efficiency of people and vehicles.

The present invention will be further described in detail below in conjunction with the examples.

Example

In conjunction with Fig. 1, the present invention is based on the vehicle path planning method of warehouse unmanned vehicles, and the specific implementation steps are as follows:

Step 1. Make a topological map of the storage environment, as shown in Figure 2. The schematic map of the storage environment is shown in Figure 2. The rectangle in the figure represents the shelf. Collect the feasible path of the unmanned vehicle in the storage environment and the relative position of the shelf. According to the collected information, set the unmanned The nodes that the car can reach, and then create an adjacency matrix based on the topological map according to the node information;

Step 1-1, as shown in Figure 3, collect the environmental map information, take each intersection turning point and the point where the unmanned vehicle needs to stop in the environmental map as a topological node, and the road connected to the topological node as a topological edge, so The environment map can be abstracted as a topological map composed of topological nodes and topological edges. The topological map can be expressed as G=(V, E), where V is a set of nodes, and E is a set of edges connecting nodes. In this In the storage environment, a total of 15 topological nodes from A to O as shown in Figure 3 can be set, that is, the matrix

V=[0.5,0.5; 2.5,0.5; 4.5,0.5; 0.5,2.5; 2.5,2.5; 4.5,2.5; 5.5,2.5; 0.5,4.5; ;2.5,6.5;4.5,6.5;5.5,0.5];

Step 1-2. In order to facilitate the storage and search of the topological graph, an adjacency matrix is used to represent the relationship between nodes. If there are n topological nodes v ₁ , v ₂ ,...,v _n in the topological map, then the adjacency matrix is an n×n matrix, and the passable path and the impassable path are distinguished by assigning attributes to each edge, Here, the weight attribute of the passable path is set to 1, and the weight attribute of the impassable path is set to 0. Furthermore, the (i, j)th element in the adjacency matrix can be expressed as

In this topological map n=15, then the adjacency matrix is a 15×15 matrix, expressed as

Step 2. Perform global path planning offline, that is, use the improved A* algorithm to calculate the optimal path from the starting point to the target point in an offline manner, as shown in Figure 4, which is a flow chart of global path planning;

Step 2-1. Use the A* algorithm to design an evaluation function for each road node, as shown in the following formula:

f(s)=g(s)+h(s)

In the formula, f(s) represents the estimated length from the starting node to the target node through node s, g(s) represents the path length from the starting node to the current node, and h(s) is a heuristic function, which is the path length from the current node to the current node. Estimated value of the target node;

Step 2-1-1, record the path length from the starting node to the current node as g(s);

Step 2-1-2, h(s) is a heuristic function, which is the estimated value from the current node to the target node. A* algorithm must be able to search the prerequisites for the optimal path:

h(s)≤cost*(s,s _goal )

In the formula, cost*(s,s _goal ) is the optimal distance from the current node to the target node. The larger the value of h(s) satisfying the above formula, the fewer the expanded nodes. In order to ensure the optimality of the search path, this method uses Euclidean distance as a heuristic function. For given two position coordinates ( _xi , y) and (x _j , _{y j )} , their Euclidean distance d _e is shown as follows:

Step 2-2. Create two collections, OPEN and CLOSED, to manage road nodes. OPEN stores the child nodes of the expanded road node. They belong to the nodes to be expanded. CLOSED stores expanded nodes;

Step 2-3: Start global path planning, and select the node s with the smallest f(s) value from OPEN for expansion each time. The child nodes to which node s is expanded are stored in OPEN. After node s is expanded, it is moved from OPEN to CLOSED. Repeat the above process until the target node is expanded or OPEN is empty, then the algorithm is terminated and the planned path is recorded. As shown in Figure 5, it is the result map of the global path planning.

Step 3, path tracking, that is, the unmanned vehicle uses the tracking module carried by itself and the markers preset in the storage environment to locate the unmanned vehicle relative to the storage environment, so as to realize the tracking of the planned global path. As shown by the thick solid line in Figure 6, tracking is directly performed on road sections without obstacles to reduce positioning costs.

Step 4. Detect and collect random obstacle information, judge the path environment, and switch between the two algorithms of global path planning and local path planning;

Step 4-1. The unmanned vehicle detects random obstacles on the path in real time through the ultrasonic module carried by itself, and generates information such as obstacle boundaries and relative positions;

Step 4-2, judge the road environment through the obstacle information collected by the sensor, and switch between the global road strength planning and the local path planning algorithm according to the judgment result;

Step 4-3. Design the safe distance between the unmanned vehicle and the obstacle. The storage model sets the safe distance to 0.3 units. If the distance between the boundary of the obstacle and the wall is less than the safe distance, it is considered that the unmanned vehicle cannot pass. As shown in Figure 7, take the topological node where the current unmanned vehicle is located as the starting point, and set the topological edge between this topological node and the next node as inaccessible, update the adjacency matrix, and update the adjacency matrix to:

Return to step 2 to re-plan the global path, and the re-planned path is shown in the thick solid line in Figure 7;

Step 4-4, as shown in Figure 6, if the distance between the obstacle boundary and the wall is greater than the safety distance designed in step 4-3, switch to the local path planning algorithm for obstacle avoidance.

Step 5. Avoid random obstacles, that is, use the artificial potential field method to perform local path planning online to achieve the purpose of local obstacle avoidance. The effect is shown in Figure 6. Switch to the artificial potential field method for local obstacle avoidance on the road section with obstacles .

Step 5-1. Establish the starting point and end point of the local path planning. This method takes the current node of the unmanned vehicle as the starting point, and takes the end point of the next node of the node in the global path planning. In the storage model, L Point and M point are the starting point and end point of local path planning;

Step 5-2, according to the size of the unmanned vehicle, determine the range of obstacles;

Step 5-3, establish the gravitational potential field function and the repulsion potential field function of the artificial potential field method, and switch to the A* algorithm for local obstacle avoidance;

Step 5-3-1, establish the gravitational potential field function, the formula is as follows:

In the formula: K _a is the gravitational potential field constant, Q _c is the position vector of the earth coordinate system of the unmanned vehicle, Q _g is the position vector of the target point in the earth coordinate system, in this method, the gravitational potential field constant is set to 1, ( Q _c -Q _g ) ² is the Euclidean distance between the unmanned vehicle and the target point, which is updated at any time as the unmanned vehicle moves, and the final gravitational potential field function is

Step 5-3-2, establish the repulsion potential field function, the formula is as follows:

In the formula: K _r is the repulsion potential field constant, ρ(Q _c , Q _obs ) is the relative distance between the unmanned vehicle and the obstacle, D is the safety distance, in this method, the repulsion potential field constant is set to 1, ρ( Q _c , Q _obs ) are updated in real time with the movement of the unmanned vehicle, and D is set to 0.3, then the repulsion potential field function is

The present invention combines global path planning with local path planning, and can perform local obstacle avoidance while searching for an optimal path, so that the operation of the unmanned vehicle is more flexible, the running distance is shorter when encountering obstacles, and the storage space is improved. Work efficiency of people and vehicles.

Claims (7)

1. a kind of vehicle path planning method based on storage unmanned vehicle, it is characterised in that step is as follows：

Step 1, the relative position for making storage environment topological map, specifically collection storage environment unmanned vehicle feasible path and shelf Put, according to the information collected, then the node for setting unmanned vehicle to reach creates according to nodal information and be based on topological map Adjacency matrix；

Step 2, global path planning is carried out offline, i.e., using A* algorithms after improving, calculate off-line manner from starting point To the optimal path of target point；

Step 3, path is tracked, i.e., the optimal path that segregation reasons go out entered with unmanned vehicle self-contained tracking module Line trace；

Step 4, detect and gather random moving obstacle information, judge path circumstances, global path rule are carried out according to obstacle information Draw the switching between two kinds of algorithms of local paths planning；

The route that step 5, vehicle are planned according to step 4, in the process of moving according to local paths planning algorithm hide with Machine barrier, reaches the purpose of local avoidance.

2. the vehicle path planning method according to claim 1 based on storage unmanned vehicle, it is characterised in that in step 1 Make storage environment topological map step as follows：

Step 1-1, collection Environmental Map Information, using in environmental map each crossing flex point and unmanned vehicle need the point of stop as Topological node, the road communicated with the topological node is abstract for by topological node and topological side by environmental map as topological side The topological map of composition, the topological map symbolically is G=(V, E), and wherein V is node set, and E is the side of connecting node Set；

Step 1-2, relation between each node is represented with adjacency matrix, if having n topological node v in topological map₁, v₂..., v_n, then the adjacency matrix is n × n matrix, by assigning each bar side different attributes, to distinguish the road that can pass through Footpath and impassabitity path, by can the weights attribute of pass be set to 1, the weights attribute in impassabitity path is set to 0；The neighbour Meet (i, j) individual element a in matrix_ijIt is expressed as

3. the vehicle path planning method according to claim 1 based on storage unmanned vehicle, it is characterised in that in step 2 Offline progress global path planning step is as follows：

Step 2-1, using A* algorithms, to each one evaluation function of road design of node, be shown below：

F (s)=g (s)+h (s)

In formula, f (s) is represented to reach the estimated length of destination node by node s from start node, and g (s) is represented from start node To the path length of present node, h (s) is heuristic function, is estimate of the present node to destination node；

Step 2-2, establishment OPEN and two set of CLOSED carry out management road node, and wherein OPEN gathers to be propagated through for storage Road circuit node child node, these nodes, which belong to, treats expanding node；CLOSED gathers for depositing the node propagated through；

Step 2-3, progress global path planning, the minimum node s of selection f (s) value is extended from OPEN set every time, The child node that node s is extended to is deposited in OPEN set；After the completion of node s extensions, CLOSED is moved on to from OPEN set In set；Posterior circle said process, until expanding to destination node or OPEN collection is combined into space-time, termination algorithm, record rule The path marked.

4. the vehicle path planning method according to claim 3 based on storage unmanned vehicle, it is characterised in that step 2-1 The design method of middle evaluation function is：

The path length g (s) of step 2-1-1, record from start node to present node；

Step 2-1-2, heuristic function h (s) is determined, the precondition that wherein A* algorithms one surely search optimal path is：

h(s)≤cos t^*(s,s_goal)

In formula, cos t^*(s,s_goal) for the optimal distance of present node to destination node, h (s) values for meeting above formula are bigger, then Expanding node is fewer；

Step 2-1-3, evaluation function is determined, be shown below：

F (s)=g (s)+h (s).

5. the vehicle path planning method according to claim 4 based on storage unmanned vehicle, it is characterised in that step 2-1- Heuristic function h (s) described in 2 is Euclidean distance function, for two given position coordinates (x_i, y_i) and (x_j, y_j), Their Euclidean distance d_eIt is shown below：

6. the vehicle path planning method according to claim 1 based on storage unmanned vehicle, it is characterised in that in step 4 Detect and gather random moving obstacle information, judge path circumstances, global path planning and local road are carried out according to obstacle information The switching between two kinds of algorithms is planned in footpath, is comprised the following steps that：

Step 4-1, unmanned vehicle are by self-contained ultrasonic wave module, the random moving obstacle on real-time detection path, generation barrier Hinder border and relative position information；

Step 4-2, the obstacle information collected by sensor, judge road environment, and entered according to judged result Switching between the strength planning of row overall situation road and local path planning algorithm；Specially：

Safe distance between unmanned vehicle and barrier is set, if obstacles borders and distance between the walls be less than the safety away from From, it is believed that unmanned vehicle can not be bypassed, then using topological node where current unmanned vehicle as starting point, and set the topological node with Topological side between next node is impassabitity, updates adjacency matrix, and return to step 2 plans global path again；If obstacle Thing border is more than safe distance with distance between the walls, then switches to step 5 and perform local path planning algorithm progress avoidance.

7. the vehicle path planning method according to claim 1 based on storage unmanned vehicle, it is characterised in that in step 5 Hide random moving obstacle according to local paths planning algorithm in the process of moving, step is as follows：

Step 5-1, the starting point and terminal for setting up local paths planning, are specifically presently in node as starting using unmanned vehicle Point, with the next node terminal of the node in global path planning；

Step 5-2, according to unmanned vehicle size, determine obstacle effect domain；

Step 5-3, the gravitational potential field function for setting up Artificial Potential Field Method and repulsion potential field function, carry out local avoidance；Specific steps It is as follows：

Step 5-3-1, set up gravitational potential field function U_att, formula is as follows：

In formula：K_aFor gravitation potential field constant, Q_cFor unmanned vehicle terrestrial coordinate system position vector, Q_gFor target point in terrestrial coordinate system Position vector；

Step 5-3-2, set up repulsion potential field function U_rep, formula is as follows：

In formula：K_rFor repulsion potential field constant, ρ (Q_c,Q_obs) for the relative distance of unmanned vehicle and barrier, D is safe distance.