CN115083139B - Multi-vehicle scheduling method - Google Patents

Abstract

A multi-vehicle dispatching method comprises the steps of constructing a layout map of vehicles and a closed space according to parameter information of current vehicles and layout information of parking environments, designing priority rules, calculating priorities of all vehicles according to the rules, and selecting vehicles needing to go out of a warehouse. Adopting a punishment type mixed A algorithm to plan a collision-free path of each vehicle, which accords with kinematic constraint; and according to the vehicle priority, sequentially adopting an s-t diagram strategy to obtain tracks with no conflict on time and space of multiple vehicles, and completing the dispatching of the delivery of the multiple vehicles. According to the invention, the speed is configured by planning the path of the vehicle and utilizing the S-T diagram strategy, so that other vehicles are avoided, the overall dispatching of a plurality of vehicles is completed, and the efficient delivery without collision is realized.

Description

Multi-vehicle scheduling method

Technical field

The invention relates to a technology in the field of multi-vehicle control, specifically a multi-vehicle scheduling method based on a penalty hybrid A ^* algorithm and an ST graph strategy.

Background technique

Existing multi-vehicle scheduling problems can be divided into two aspects: single-vehicle path planning and multi-vehicle scheduling. The path planning problem can be described as: on the premise that the environment is known or partially known, given a starting point and a target end point, finding a collision-free shortest path from the starting point to the end point. Multi-vehicle scheduling is designed to solve the problem of simultaneous movement of multiple vehicles interfering with each other when leaving the warehouse at the same time, ensuring that all vehicles leave the warehouse in an orderly and rapid manner, reducing the total time of leaving the warehouse, and improving efficiency. Path planning in current scheduling methods often does not meet the kinematic characteristics of the vehicle, or the path is not smooth and reasonable enough, and the scheduling of multiple vehicles is not fast enough.

Contents of the invention

In view of the above-mentioned shortcomings of the existing technology, the present invention proposes a multi-vehicle scheduling method. By planning the path of the vehicles, and then using the S-T diagram strategy to configure the speed, avoid other vehicles, complete the overall scheduling of multiple vehicles, and achieve seamless Efficient delivery at the collision location.

The present invention is achieved through the following technical solutions:

The invention relates to a multi-vehicle scheduling method based on a penalty-type hybrid A ^* algorithm and ST graph strategy. According to the parameter information of the current vehicle and the layout information of the parking environment, a layout map of vehicles and confined spaces is constructed, and priority rules are designed and based on The rules calculate the priority of each vehicle and select the vehicles that need to be shipped out of the warehouse. The penalty-type hybrid A ^* algorithm is used to plan a collision-free path for each vehicle that conforms to the kinematic constraints; according to the vehicle priority, the st graph strategy is used in turn to obtain the trajectories of multiple vehicles that are conflict-free in space and time, and complete the control of multiple vehicles. Library scheduling.

The invention relates to a system that implements the above method, including: a priority calculation unit, a path planning unit and a speed planning unit, wherein the priority calculation unit is connected to the path planning unit and transmits vehicle priority information and information about vehicles that need to leave the warehouse. , the path planning unit is connected to the speed planning unit and transmits the vehicle's path information.

Technical effect

The invention overall solves the defects of the existing technology that the planned vehicle driving path is not smooth enough and the speed is switched too frequently during driving. Compared with the existing technology, the present invention uses the improved A ^* algorithm and ST chart to plan the speed of the vehicle, ensuring that the vehicle can reduce the switching of driving states without affecting the overall time, and is more in line with the actual situation and will not cause The physically unrealizable speed planning improves the overall speed of multiple vehicles leaving the warehouse.

Description of drawings

Figure 1 is a schematic flow diagram of the present invention;

Figure 2 is a layout map of multiple vehicles according to the embodiment;

Figure 3 is a schematic diagram of the S-T diagram of the embodiment;

Figure 4 is a schematic diagram of a single vehicle path planning according to the embodiment;

Figure 5 is a schematic diagram of speed configuration using an S-T diagram according to an embodiment.

Detailed ways

As shown in Figure 1, this embodiment involves a multi-vehicle scheduling method based on the penalty hybrid A ^* algorithm and ST graph strategy, which specifically includes:

Step S1: Collect the parameter information of the current vehicle and the layout information of the parking environment, and construct a layout map of the vehicle and the confined space.

As shown in Figure 2, it is a map of this embodiment. Its length and width are 54m and 24m respectively. There are 6 vehicles parked in the map space. The vehicles are represented by rectangles, and other obstacles are represented by the smallest polygon that can contain the obstacles. To represent: Use a rectangle with a length of 10m and a width of 3.5m to represent the vehicle. The vehicles are numbered A, B, C, D, E, and F from top to bottom and from left to right. The confined space has only a single exit. .

Step S2: Design corresponding priority rules based on the task requirements, vehicle parking location, and vehicle performance. Calculate the priority of each vehicle based on the rules, and select the vehicles that need to be shipped out of the warehouse.

In this embodiment, the task requirement refers to: four vehicles leaving the warehouse.

The parking position of the vehicles refers to: arranging the vehicles in parallel with the front of the vehicle outwards. As shown in Figure 2, the distances of the vehicles relative to a single exit vary.

The performance of the vehicle includes: vehicle maintenance status, equipment assembly status, and driving mileage.

The described design principle is to drive out of the garage as quickly as possible to meet the task requirements with a number of vehicles in good condition and performance, that is, the overall vehicle exit time is minimized. The obtained priority rule is: E _i = b ₁ P _i + b ₂ C _i , where: E _i is the final comprehensive index of each vehicle, C _i is the performance index of the vehicle, which is obtained based on the comprehensive evaluation of vehicle performance. The position index of the vehicle is based on P _i = (|x _i (t _fi )-x _i (0)|+|y _i (t _fi )-y _i (0)|)/2 is obtained, where: |x _i (t _fi )-x _i (0)|, |y _i (t _fi )- y _i (0)| is the distance between the midpoint of the rear axle of each vehicle relative to the midpoint of the exit, and b ₁ and b ₂ are weight coefficients, indicating the trade-off between vehicle performance and vehicle position.

In this embodiment, the starting and ending positions and performance indexes of the six vehicles are as shown in Table 1:

Table 1 Vehicle parameter table

The weight coefficients are set to b ₁ =0.4, b ₂ =0.6. The comprehensive index of each vehicle is obtained, and the four vehicles A, E, C, and F with the highest comprehensive index are selected to leave the warehouse and specify their priority. level, as shown in Table 2:

Table 2 Vehicle comprehensive index and priority

Step S3: Use the penalty hybrid A ^* algorithm to plan a collision-free path for each vehicle that conforms to kinematic constraints, that is, meets the requirements of the end position and attitude angle.

The kinematic constraints include: |a _i (t)|≤a _maxi , |ν _i (t)|≤ν _maxi , |ω _i (t)|≤ω _maxi , where: (x _i (t), y _i (t)) is t The coordinates of the vehicle's rear axle midpoint at the moment, θ _i (t) is the attitude angle of the vehicle in the XOY coordinate system, ν _i (t) and a _i (t) are the vehicle's speed and acceleration along the longitudinal axis of the body, respectively, / > is the deflection angle of the front wheel of the vehicle, with counterclockwise being the positive direction. Correspondingly, ω _i (t) is the deflection angular velocity of the front wheel, and L is the wheelbase of the front and rear wheels.

The requirement of satisfying the position and attitude angle of the end point means: satisfying (x _i (t _fi ), y _i (t _fi ), θ _i (t _fi ))=(x ^* , y ^* , θ ^* ).

The penalized hybrid A ^* algorithm specifically includes:

Step S3-1: Discretize the map constructed in step S1, add the initial pose of the vehicle to be planned at this time as the initial node to the open list, and calculate the heuristic value of the current node.

The discretization process refers to dividing the map x and y directions and the vehicle longitudinal axis direction angle θ at small intervals Δx, Δy, and Δθ respectively, and then the map is converted into multiple discrete nodes.

The open list refers to: a list used to store the nodes that have been explored on the map.

Vehicles with lower priority are regarded as static obstacles, and the vehicle's path planning cannot reach the node occupied by the obstacle.

The calculation of the heuristic value specifically includes: N _child .f=N _child .g+N _child .h, N _child ·g=N _current ·g+length+penalty, N _child .h=|N _child .SS _end |, where: the heuristic value f consists of the g function value and the h function value. The h function value is only the distance between the s value of the node and the end point s value; the g function value is the total path length from the starting point to the current node, N _current ·g is the g function value of the parent node, length is the length from the parent node to the child node, and the penalty value penalty is the penalty for reversing during the ST graph path search process.

Step S3-2: Select the node with the smallest heuristic value from the open list as the current node and remove it from the open list and add it to the close list. When the current node is the target node, go to step S3-5, otherwise continue to step S3- 3;

Step S3-3: Use the Reeds-Shepp curve to generate a shortest path from the current node to the target node that conforms to kinematic constraints. When the Reeds-Shepp curve can avoid all obstacles, go to step S3-5; otherwise, continue According to the preset node expansion method, the current node is expanded to 6 child nodes.

The Reeds-Shepp curve refers to the 48 arrangements and combinations of all arcs and straight line segments. According to this, any starting point and end point position on the plane can be connected and ensured in the sense of vehicle kinematics. The shortest, but does not guarantee obstacle avoidance requirements.

The preset node expansion method refers to: corresponding to the two movement directions of forward and backward and the three steering directions of left turn, right turn and straight travel during the movement of the vehicle. There are a total of 6 driving states corresponding to 6 expansion nodes.

Step S3-4: When the expanded child node collides with an obstacle, that is, overlaps with the node occupied by the obstacle, or the child node is already in the close table, ignore the child node; when the child node is not in the open table, add open table, calculate and record the heuristic value of the child node and the parent node; when the expanded child node is already in the open table, compare the heuristic value of the node in the original open table. If the current heuristic value is smaller, update the node. Then go to step S3-2.

Step S3-5: The path search is successful and a collision-free path is output.

As shown in Figure 4, after searching for the target pose node, by taking the parent nodes one by one, backtracking to obtain a path plan from the starting point to the end point that satisfies the vehicle kinematic constraints.

Step S4: After completing the path planning, according to the vehicle priority, use the s-t graph strategy in sequence to obtain the trajectories of multiple vehicles without conflicts in space and time, and complete the dispatch of multiple vehicles out of the warehouse, including:

Step S4-1: For the vehicle whose speed is currently to be planned, according to the collision-free path obtained in step S3, the vehicle with a higher priority is regarded as a moving obstacle, and combined with other moving obstacles, the path and time are discretized to obtain ST diagram, where S is the vehicle's planned route in step S3. This path is discretized with a small mileage interval Δs. Similarly, T is the driving time of the vehicle. This time is discretized with a small time interval Δt, and we get ST grid diagram with T as the horizontal axis and S as the vertical axis. The trajectory of the vehicle with higher priority starts from time t = 0 to the final time t = t _end . At every small time interval Δt, it is judged whether the position of the vehicle with higher priority at this time is consistent with the currently planned position of the vehicle. Overlap occurs. When an overlap occurs, the time when the overlap occurs and the corresponding driving mileage s at this time are combined into coordinates (s, t), and marked in the ST diagram, indicating that other vehicles occupy the path at this time.

Step S4-2: First create the open list and the close list, and add the origin (0, 0) in the S-T diagram to the open list.

Step S4-3: Extract the node with the smallest heuristic value from the open list of the improved A ^* algorithm, select the node with the smallest heuristic value from the open list as the current node, remove it from the open list, and add it to the close list. When the current node is If S=S _end of the node, go to step S4-6, otherwise continue to step S4-4;

Step S4-4: Expand the current node according to the improved node expansion method, expand 3 sub-nodes and calculate the corresponding heuristic values: Compared with the eight-neighbor expansion method of the ordinary A ^* algorithm, due to the characteristics of time that cannot be retreated and the driving speed is not It can go to infinity, and the three neighborhoods of upper right, right, and lower right are selected to expand the child nodes, which correspond to forward, parking, and reversing in actual driving situations.

Step S4-5: For each child node, if it already exists in the close table or is an obstacle node in the S-T diagram, ignore the child node; when the child node is not in the open table, add it to the open table and record the child node's The heuristic value and the parent node; when the extended child node is already in the open table, compare the heuristic value of the node in the original open table. When the current heuristic value is smaller, the node is updated. Continue iteration and go to step S4-3.

Step S4-6: The path search in the S-T diagram is successful, and the speed is configured for the vehicle based on the path searched in the S-T diagram.

As shown in Figure 5, the S-T diagram is used to plan the speed configuration for the vehicle. According to the result of the speed configuration, the vehicle can travel to the end point without collision by traveling along the path planned in step S3.

After specific actual experiments, according to the vehicle parking situation shown in Figure 2, running the above method with the parameters in Table 1, while ensuring the overall time, the vehicle path is smoother, the number of vehicle driving state switching times can be reduced by half, and There is no reversing behavior.

Compared with the existing technology, this method considers the kinematic constraints of the vehicle, and can also complete path planning for vehicles with non-holonomic constraints, and the path is smooth, and reversing and steering behaviors will not occur frequently. Taking into account the position and posture requirements of the vehicle, it is more reasonable to plan the path and vehicle speed configuration that meets the vehicle's exit position and posture requirements, which can reduce braking and reversing behaviors while ensuring the minimum overall time.

The above-mentioned specific implementations can be partially adjusted in different ways by those skilled in the art without departing from the principles and purposes of the present invention. The scope of protection of the present invention is subject to the claims and is not limited by the above-mentioned specific implementations. Each implementation within the scope is subject to this invention.

Claims (2)

1. A multi-vehicle scheduling method based on the penalty hybrid A* algorithm and S-T graph strategy, which is characterized by constructing a layout map of vehicles and confined spaces and designing priorities based on the parameter information of the current vehicle and the layout information of the parking environment. rules and calculate the priority of each vehicle based on the rules, and select the vehicles that need to be shipped out of the warehouse. The penalty hybrid A* algorithm is used to plan a collision-free path for each vehicle that conforms to the kinematic constraints; according to the vehicle priority, the The s-t graph strategy obtains conflict-free trajectories of multiple vehicles in space and time, and completes the dispatch of multiple vehicles out of the warehouse, including: Step S1: Collect the parameter information of the current vehicle and the layout information of the parking environment, and construct a layout map of the vehicle and the confined space; Step S2: Design corresponding priority rules based on the task requirements, vehicle parking location, and vehicle performance, calculate the priority of each vehicle based on the rules, and select the vehicles that need to be shipped out of the warehouse; Step S3: Use the penalty hybrid A* algorithm to plan a collision-free path for each vehicle that conforms to kinematic constraints, that is, meets the requirements of the end position and attitude angle, including: Step S3-1: Discretize the map constructed in step S1, add the initial pose of the vehicle to be planned at this time as the initial node to the open list and calculate the heuristic value of the current node; Step S3-2: Select the node with the smallest heuristic value from the open list as the current node and remove it from the open list and add it to the close list. When the current node is the target node, go to step S3-5, otherwise continue to step S3- 3; Step S3-3: Use the Reeds-Shepp curve to generate a shortest path from the current node to the target node that conforms to kinematic constraints. When the Reeds-Shepp curve can avoid all obstacles, go to step S3-5; otherwise, continue According to the preset node expansion method, the current node is expanded to 6 child nodes; Step S3-4: When the expanded child node collides with an obstacle, that is, overlaps with the node occupied by the obstacle, or the child node is already in the close table, ignore the child node; when the child node is not in the open table, add open table, calculate and record the heuristic value of the child node and the parent node; when the expanded child node is already in the open table, compare the heuristic value of the node in the original open table. If the current heuristic value is smaller, update the node. Then go to step S3-2; Step S3-5: The path search is successful and a collision-free path is output; Step S4: After completing the path planning, according to the vehicle priority, use the s-t graph strategy in sequence to obtain the trajectories of multiple vehicles without conflicts in space and time, and complete the dispatch of multiple vehicles out of the warehouse, including: Step S4-1: For the vehicle whose speed is currently to be planned, according to the collision-free path obtained in step S3, the vehicle with a higher priority is regarded as a moving obstacle, and combined with other moving obstacles, the path and time are discretized to obtain ST diagram, where S is the vehicle's planned route in step S3. This path is discretized with a small mileage interval Δs. Similarly, T is the driving time of the vehicle. This time is discretized with a small time interval Δt, and we get ST grid diagram with T as the horizontal axis and S as the vertical axis; the trajectory of the vehicle with higher priority starts from time t=0 to the final time t=t _end , and at every small time interval Δt, determine this Whether the position of the vehicle with high priority overlaps with the position of the currently planned vehicle. When overlap occurs, the overlap moment and the corresponding driving mileage s at this time are combined into coordinates (s, t), and marked in ST In the picture, it means that other vehicles occupy this section of the path at this moment; Step S4-2: First create the open list and the close list, and add the origin (0,0) in the S-T diagram to the open list; Step S4-3: Extract the node with the smallest heuristic value from the open list of the improved A* algorithm, select the node with the smallest heuristic value from the open list as the current node, remove it from the open list, and add it to the close list. When the current node is If S=S _end of the node, go to step S4-6, otherwise continue to step S4-4; Step S4-4: Expand the current node according to the improved node expansion method, expand 3 sub-nodes and calculate the corresponding heuristic values: Compared with the eight-neighbor expansion method of the ordinary A* algorithm, due to the characteristics of time that cannot be retreated and the driving speed is not It can go to infinity, and the three neighborhoods of upper right, right, and lower right are selected to expand the child nodes, which correspond to forwarding, parking, and reversing in actual driving situations; Step S4-5: For each child node, if it already exists in the close table or is an obstacle node in the S-T diagram, ignore the child node; when the child node is not in the open table, add it to the open table and record the child node's The heuristic value and the parent node; when the extended child node is already in the open table, compare the heuristic value of the node in the original open table. When the current heuristic value is smaller, update the node; continue the iteration and go to step S4-3 ; Step S4-6: The path search in the S-T diagram is successful, and the speed is configured for the vehicle according to the path searched in the S-T diagram; The parking position of the vehicles refers to: arranged in parallel with the front of the vehicle outwards, and the distances of the vehicles relative to a single exit are different; the performance of the vehicle includes: the maintenance status of the vehicle, equipment assembly status, and driving mileage; The described design principle is to drive out of the garage as quickly as possible to meet the task requirements with a number of vehicles in good condition and performance, that is, the overall vehicle exit time is minimized. The obtained priority rule is: E _i = b ₁ P _i + b ₂ C _i , where: E _i is the final comprehensive index of each vehicle, C _i is the performance index of the vehicle, which is obtained based on the comprehensive evaluation of vehicle performance. The position index of the vehicle is based on P _i = (|x _i (t _fi )-x _i (0)|+|y _i (t _fi )-y _i (0)|)/2 is obtained, where: |x _i (t _fi )-x _i (0)|, |y _i (t _fi )- y _i (0)| is the distance between the midpoint of the rear axle of each vehicle relative to the midpoint of the exit, b ₁ and b ₂ are weight coefficients, indicating the trade-off between vehicle performance and vehicle position; The kinematic constraints include: |a _i (t)|≤a _maxi ,|ν _i (t)|≤ν _maxi ,|ω _i (t)|≤ω _maxi , where: (x _i (t),y _i (t)) is t The coordinates of the vehicle's rear axle midpoint at the moment, θ _i (t) is the attitude angle of the vehicle in the XOY coordinate system, ν _i (t) and a _i (t) are the vehicle's speed and acceleration along the longitudinal axis of the body, respectively, / > is the deflection angle of the front wheel of the vehicle, with counterclockwise being the positive direction. Correspondingly, ω _i (t) is the deflection angular velocity of the front wheel, and L is the wheelbase of the front and rear wheels; The requirement of meeting the position and attitude angle of the end point means: satisfying (x _i (t _fi ), y _i (t _fi ), θ _i (t _fi )) = (x*, y*, θ*); The discretization process refers to: dividing the map x, y directions and the vehicle longitudinal axis direction angle θ at small intervals Δx, Δy, Δθ respectively, then the map is converted into multiple discrete nodes; the priority is lower The vehicle is regarded as a static obstacle, and the vehicle's path planning cannot reach the node occupied by the obstacle; The calculation of the heuristic value specifically includes: N _child .f=N _child .g+N _child .h, N _child .g=N _current .g+length+penalty, N _child .h=|N _child .SS _end |, where: the heuristic value f consists of the g function value and the h function value. The h function value is only the distance between the s value of the node and the end point s value; the g function value is the total path length from the starting point to the current node, N _current .g is the g function value of the parent node, length is the length from the parent node to the child node, and the penalty value penalty is the penalty for reversing during the ST graph path search process. 2. A system for implementing the method of claim 1, characterized in that it includes: a priority calculation unit, a path planning unit and a speed planning unit, wherein: the priority calculation unit is connected to the path planning unit and transmits vehicle priority information and the information of vehicles that need to be shipped out of the warehouse. The path planning unit is connected to the speed planning unit and transmits the vehicle's path information.