Global optimal multi-vehicle cooperative trajectory planning method based on parameter enumeration simulation
Through the method based on parameter enumeration simulation, the alternative solutions for multi-vehicle collaborative trajectory planning are generated and screened, and the problem that multi-vehicle collaborative trajectory planning algorithm in the existing technology cannot achieve global optimality, and efficient global optimal trajectory is achieved planning.
Patent Information
- Application Number
- CN202411331583.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-09-24
- Publication Date
- 2024-12-06
AI Technical Summary
The existing multi-vehicle collaborative trajectory planning algorithm cannot guarantee the global optimality of collaborative trajectory planning within reasonable calculation time, especially because the search-based algorithm will fall into a dimensional disaster when searching space with high precision, resulting in a significant decrease in search efficiency. .
Using the method based on parameter enumeration simulation, first generates a driving path without considering obstacle avoidance factors, and generates a large number of alternative solutions for multi-vehicle collaborative trajectory planning through module A and module B enumeration, and selects the global optimal from them through the screening process. The results of the plan ensure the global optimality of the planning results.
By quickly generating alternative solutions for multi-vehicle collaborative trajectory, the planning efficiency is improved, the possibility of global optimal solutions is ensured, the problem of local optimal solutions is avoided, and high-quality collaborative trajectory planning results are output in a short time.
Smart Images

Figure CN119085687A_ABST
Abstract
Description
Technical Field
[0001] This scheme belongs to the field of intelligent transportation, and specifically involves a global optimal multi-vehicle collaborative trajectory planning method based on parameter enumeration simulation. Background Art
[0002] The trajectory planning of an intelligent vehicle aims to generate a space-time trajectory that reflects the local path and speed of the vehicle, conforming to the vehicle's internal kinematic constraints and external environment obstacle avoidance constraints, and optimizing the established performance indicators as much as possible while strictly ensuring that the above constraints are met.
[0003] Multi-vehicle collaborative trajectory planning is aimed at multiple intelligent driving vehicles, and is committed to generating safe, comfortable, efficient, and energy-saving driving trajectories for each vehicle in the same time and space that fully reflect the collaborative potential. As shown in the following video, collaborative trajectory planning is committed to generating trajectories for multiple vehicles at the same time: https: / / www.bilibili.com / video / BV1rr421W7dy / .
[0004] At present, the mainstream multi-vehicle collaborative trajectory planning algorithms on the market include sampling-based collaborative planning algorithms, search-based collaborative planning algorithms, and numerical optimization-based collaborative planning algorithms. The sampling-based collaborative planning algorithms are represented by the RRT series and lattice series planning methods. Their advantages lie in the sampling efficiency and good kinematic feasibility of the trajectory segments, but their disadvantages are that they cannot ensure that the collaborative trajectory solution is globally optimal. Similarly, the collaborative trajectory planning method based on numerical optimization only guarantees local optimality, and the numerical optimization algorithm has an excessively high dependence on the quality of the upstream initial solution. The search-based collaborative planning algorithm can guarantee global optimality based on resolution, but this method often falls into the dimensionality disaster when the discretization accuracy of the search space is high, resulting in a cliff-like drop in search efficiency and an inability to complete high-quality collaborative trajectory planning tasks within a limited time.
[0005] Currently, the existing multi-vehicle collaborative trajectory planning algorithms on the market cannot guarantee the global optimality of the collaborative trajectory planning solution within a reasonable computing time. Summary of the invention
[0006] The purpose of this scheme is to provide a globally optimal multi-vehicle collaborative trajectory planning method based on parameter enumeration simulation, so as to solve the problem that the existing multi-vehicle collaborative trajectory planning algorithm cannot guarantee the global optimality of the collaborative trajectory planning solution within a reasonable computing time.
[0007] In order to achieve the above objectives, this scheme provides a global optimal multi-vehicle collaborative trajectory planning method based on parameter enumeration simulation, including the following steps:
[0008] S100: Generate a driving path without considering obstacle avoidance factors based on the starting position and end position information of each vehicle and the environment layout information, and then use the preset modules A and B to generate a large number of multi-vehicle collaborative trajectory planning alternative solutions through driving path enumeration;
[0009] S200: Screening the numerous multi-vehicle collaborative trajectory planning alternative solutions enumerated, considering the most reasonable set of results as the globally optimal collaborative trajectory planning result, and outputting it as the final output of the entire algorithm.
[0010] The principle and technical effect of this scheme are as follows: first, this scheme generates a driving path without considering obstacle avoidance factors, and can quickly build a preliminary path plan for each vehicle, so that a large number of alternative solutions can be obtained in a short time, providing a rich basis for subsequent fine planning, greatly improving the efficiency of overall planning; at the same time, it can also quickly generate a large number of driving paths, avoiding the slow process of considering various complex situations one by one from scratch, and saving the planning time of multi-vehicle collaborative trajectory. Secondly, this scheme can widely explore the solution space of multi-vehicle collaborative trajectory planning by generating and enumerating driving paths that do not consider obstacle avoidance factors. In step S100, various possible driving route combinations can be covered by using the starting position, end position information and environmental layout information of the vehicle, so that potential better solutions will not be missed, providing greater possibility for finding the global optimal collaborative trajectory planning result. In this scheme, the numerous alternative solutions generated by enumeration provide sufficient room for choice for the algorithm, making the planning process more comprehensive and systematic. Finally, this scheme selects the most reasonable set of results from many alternative solutions as the global optimal collaborative trajectory planning result. This screening process can comprehensively consider multiple factors such as the relationship between multiple vehicles, driving efficiency, safety, etc. Through the synergy of the preset modules A and B, the alternative solutions can be deeply analyzed and evaluated to ensure that the final output result is the most optimized solution overall.
[0011] In summary, this solution solves the problem that the existing multi-vehicle collaborative trajectory planning algorithm cannot guarantee the global optimality of the collaborative trajectory planning solution within a reasonable computing time.
[0012] Further, the step S100 specifically includes the following steps:
[0013] S110: Import the multi-vehicle collaborative trajectory planning problem, including the starting position and end position information of each vehicle and the environment layout information, initialize three empty sets S, SS, SSS, and use a sampling method that satisfies the kinematic law to generate a driving path for each vehicle without considering obstacle avoidance factors;
[0014] S120: using a preset module A to process the driving path into various parameter vectors P, and storing them in a set SS;
[0015] S130: Take a parameter vector P from the set SS and record it as P i , then in the parameter vector P i Under the setting, the preset module B is called to generate the multi-vehicle collaborative trajectory result and recorded as solution_i, and solution_i is stored in the set SSS;
[0016] S140: Repeat the above S130 until all parameter vectors P in the set SS are taken, and output SSS.
[0017] By using a sampling method that satisfies the laws of kinematics for each vehicle to generate a driving path that does not consider obstacle avoidance factors, it can widely cover various possible driving routes, providing a rich foundation for subsequent multi-vehicle collaborative trajectory planning and increasing the possibility of finding a better solution. Step S100 can evaluate and verify each result in detail by gradually generating and analyzing the results of multi-vehicle collaborative trajectories, which helps to improve the accuracy and reliability of planning and reduce unreasonable trajectory planning. At the same time, step S100 takes into account the starting position and end position information of each vehicle and the environment layout information, so that the generated multi-vehicle collaborative trajectory is more in line with the actual situation. The sampling method that satisfies the laws of kinematics also ensures the rationality and feasibility of vehicle driving.
[0018] Further, the step S200 specifically includes the following steps:
[0019] S210: Import the set SSS, create and initialize the variable current_best_value to positive infinity, create and initialize the variable current_best_solution to null;
[0020] S220: Take an element solution_i from the set SSS, extract fields from the multi-vehicle collaborative trajectory planning result corresponding to the element, and record the extracted fields as solution_i.tf. If:
[0021] current_best_value>solution_i.tf, then current_best_value=solution_i.tf,
[0022] current_best_solution=solution_i;
[0023] S230, repeat step S220 until all elements in the set SSS are taken, and output current_best_solution as the global optimal multi-vehicle collaborative trajectory planning result in the shortest time.
[0024] Step S200 takes out elements from the set SSS one by one for comparison. This iterative method can efficiently traverse all the multi-vehicle collaborative trajectory planning results. Each comparison only requires simple numerical comparison and assignment operations, with low computational complexity, and can complete the screening process in a short time. At the same time, by traversing the entire set SSS, the algorithm can take into account all possible results, thus avoiding the problem of local optimal solutions.
[0025] Further, the module A comprises the following steps:
[0026] S121: The length of the vehicle passing through i and width W i Find a circumscribed circle with its center at the geometric center of the vehicle and its surface covering the vehicle body, and record the radius of the circumscribed circle as R i ;
[0027] S122: Expand the circumscribed circle, and record the radius of the expanded circumscribed circle as R 膨胀i , through R i and R 膨胀i Obtain the expansion parameter γ i , through γ i and the inertial mass M of vehicle i i The parameter vector P is obtained. The basic structure formula (1) of the parameter vector P is as follows:
[0028] P≡{M1,…,M i ,…,M N ,γ1,…,γ i , …, γ N}∈R 1×2N (1)
[0029] Where N is the number of vehicles involved in the calculation;
[0030] S123: Receive the value num specified by the user, and select num values uniformly from 0.1 to 10 for each dimension of the N dimensions of the vector P. Then the inertial mass M i and the expansion parameter γ i The values of each are num N By taking the inertial mass M i and the expansion parameter γ i By permutation and combination, we can calculate the num of parameter vector P 2N different combinations, and num 2NThe combinations are stored in the set SS.
[0031] Further, the module B comprises the following steps:
[0032] S131: Import the adjustable parameter vector P and initialize the clock time clock = 0 seconds. At the same time, for each vehicle, obtain the position coordinates of vehicle i at clock = 0 seconds, marked as (x i (0),y i (0)), the attitude angle is denoted as θ i (0), the speed is recorded as v i (0);
[0033] S132: When clock=0s, for each vehicle i, call the preset module C to determine the driving acceleration a of vehicle i at the current moment i (0), the minimum time interval unit of the simulated motion process is recorded as ΔT, and clock = (clock + ΔT)s is set. Based on a at time 0 i (0) and v i (0) Derived car speed v at clock = ΔTs i (ΔT), and the displacement ΔS of vehicle i from clock = 0s to clock = ΔTs i :
[0034] S133: Repeat step S132, calculate the acceleration of each vehicle at each moment, accumulate the clock moments, and simulate the movement process of each vehicle until all vehicles have completely arrived at the end of their respective predetermined paths path_i;
[0035] S134: construct a structure solution, record the current accumulated time clock as solution.tf, and store the position, posture, speed and state information of each vehicle at each discrete moment in the structure solution in an orderly manner.
[0036] The operation of expanding the circumscribed circle (step S122) takes into account the safety margin or other factors of the vehicle in actual driving, making the model more in line with the actual situation. By introducing the expansion parameter, the effective range of the vehicle can be flexibly adjusted, thereby improving the adaptability of the model. In step S123, by uniformly selecting values within a specific range, a large number of different combinations of parameter vectors P can be generated. num 2N Different combinations of solutions provide rich possibilities for multi-vehicle collaborative trajectory planning, and this rich solution space increases the chance of finding the optimal or near-optimal solution.
[0037] Further, the module C comprises the following steps:
[0038] S132-1: Read in the adjustable parameter vector P and the number i of the currently focused vehicle;
[0039] S132-2: Analyze the positional relationship between the center of any other vehicle j in the vehicle formation except i and the center of vehicle i through vehicle i, and calculate the repulsive force F generated by vehicle j on vehicle i j→i , and then through F j→i Calculate the acceleration a corresponding to the repulsive force initiated by vehicle j on vehicle i j→i , and a j→i Projecting in the tangent direction of the vehicle i’s forward direction, the projected acceleration is Finally, traverse all possible j and calculate the synthetic acceleration caused by the repulsion of all surrounding vehicles to the current vehicle i
[0040] S132-3: Applying acceleration in the forward direction of vehicle i to drive the vehicle forward
[0041] S132-4: Calculate the resultant acceleration a according to formula (2) i , formula (2) is as follows:
[0042]
[0043] A i Treated as the return value of the entire module C.
[0044] Module B applies acceleration in the vehicle's forward direction to drive the vehicle forward, ensuring that the vehicle has the power to move forward in the planned trajectory; at the same time, the synthetic acceleration generated by the repulsive force of the surrounding vehicles is combined to achieve a balance between drive and constraint. Formula (2) comprehensively considers the synthetic acceleration caused by the driving acceleration and the repulsive force, enabling the vehicle to dynamically adjust according to the situation of the surrounding vehicles while moving forward, thereby improving the stability and coordination of multi-vehicle cooperative driving.
[0045] Furthermore, the inertial mass M of vehicle i in module A i It is not the actual weight of the vehicle, but an adjustable virtual parameter used to reflect the inertia of the vehicle. i , R 膨胀i and γ i The relationship between is defined as shown in formula (3):
[0046] R 膨胀i =R i ×γ i ,i=1,2,…,N (3)
[0047] Each expansion parameter must satisfy γi ≥1.
[0048] The virtual inertial mass can better reflect the size of the vehicle's inertia, rather than just the actual vehicle weight. This is very important for multi-vehicle collaborative trajectory planning, because the vehicle's inertia is not only related to weight, but also affected by other factors such as speed and acceleration. By adjusting the inertial mass, module A can more accurately simulate the inertial characteristics of the vehicle under different driving conditions, thereby improving the accuracy and reliability of trajectory planning.
[0049] Furthermore, module B calculates ΔS during step S132. i When , use the following formula (4):
[0050] v i (ΔT)=min(max(0,v i (0)+ΔT·a i (0)),v max )
[0051] ΔS i =0.5×v i (ΔT)+v i (0) (4)
[0052] Among them, v max Represents the maximum allowed speed of each vehicle. The nesting of min() and max() means that we require that the speed of the forward simulation cannot exceed v max , and cannot be lower than 0; after obtaining ΔS i After that, vehicle i moves forward ΔS along the given path path_i from the position at time 0. i Distance; the above operations must be performed for each vehicle i.
[0053] In module B, clear speed limits and movement rules make the algorithm calculation more stable and reliable, avoiding calculation errors and instability caused by unlimited speed or unreasonable movement. By operating each vehicle separately, module B can better consider the differences and individual needs between vehicles, improving the adaptability and accuracy of the algorithm.
[0054] Furthermore, in step S132-2 of module C, the repulsive force F of vehicle j on vehicle i is j→i The definition formula (5) is as follows:
[0055]
[0056] Where k1>0 is a fixed parameter specified by the user;
[0057] The repulsive force F initiated by vehicle j on vehicle i j→i The corresponding acceleration a j→i The definition formula (6) is as follows:
[0058]
[0059] The definition formula (7) is as follows:
[0060]
[0061] Formula (6) converts repulsion into acceleration, so that the interaction between vehicles can directly affect the vehicle's motion state. This conversion helps to more accurately simulate the dynamic behavior of vehicles in a multi-vehicle cooperative environment; at the same time, the calculation of acceleration takes into account the distance between vehicles and the magnitude of repulsion, which can reflect the acceleration or deceleration trend of vehicles when approaching or moving away from other vehicles, thereby achieving more natural and coordinated multi-vehicle driving. Formula (7) projects the repulsive acceleration in the tangent direction, further clarifying the impact on the vehicle in the driving direction. This projection can ensure that the vehicle is mainly affected by the force related to the forward direction in multi-vehicle cooperation, avoiding unnecessary lateral interference, and helping the vehicle to make appropriate adjustments according to the position and repulsion of surrounding vehicles while maintaining forward movement, thereby improving the stability and safety of multi-vehicle driving. The combined effect of the formulas in module C can enable module C to more effectively calculate the interaction between vehicles, thereby providing more accurate information for multi-vehicle cooperative trajectory planning. By accurately controlling repulsion, acceleration and projection direction, module C can achieve more optimized vehicle spacing control, avoid collisions, improve traffic flow and other goals, and overall improve the performance of the multi-vehicle cooperative system.
[0062] Further, module C in step S132-3 The definition formula (8) is as follows:
[0063]
[0064] The existence of formula (8) enables module C to adapt to different vehicle characteristics and driving requirements. Different vehicles may have different maximum speeds, acceleration capabilities, etc. By adjusting the parameters in the formula, the forward power requirements of various vehicles can be met. BRIEF DESCRIPTION OF THE DRAWINGS
[0065] Figure 1 Flowchart of the global optimal multi-vehicle collaborative trajectory planning method based on parameter enumeration simulation.
[0066] Figure 2 Schematic diagram of the Reeds-Sheep curve between the starting and ending positions of each vehicle in the embodiment of this solution.
[0067] Figure 3 is the radius R of the circumscribed circle of vehicle i in this embodiment i Related schematic diagrams.
[0068] Figure 4 is the R of vehicle i in this embodiment i and R 膨胀i Related schematic diagrams. DETAILED DESCRIPTION
[0069] The following will clearly and completely describe the concept and technical effects of the present invention in combination with the embodiments, so as to fully understand the purpose, features and effects of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of them. Based on the embodiments of the present invention, other embodiments obtained by those skilled in the art without creative work are all within the scope of protection of the present invention:
[0070] The embodiment is basically as shown in the attached Figure 1-4 As shown:
[0071] The global optimal multi-vehicle collaborative trajectory planning method based on parameter enumeration simulation is shown in the attached Figure 1 As shown, it mainly includes two steps S100 and S200, and these S100 and S200 specifically include the following steps:
[0072] S100: The starting position, end position information and environment layout information of each vehicle are used to generate a driving path that does not consider obstacle avoidance factors, and then a preset module A and module B are used to generate a large number of multi-vehicle collaborative trajectory planning alternative solutions through driving path enumeration.
[0073] S110: Import the multi-vehicle collaborative trajectory planning problem, including the starting position (position and posture) and terminal position information of each vehicle, as well as the environment layout information, initialize three empty sets S, SS, SSS, and use a sampling method that meets the vehicle kinematics law to generate a driving path for each vehicle that does not consider obstacle avoidance factors (note that it is a path rather than a trajectory). For example, the present invention uses a Reeds-Sheep curve to connect the starting and terminal positions of each vehicle. For the convenience of description, the path of vehicle i is now agreed to be path_i (as shown in the attached figure). Figure 2 shown).
[0074] S120: Use a preset module A to process the driving path into various parameter vectors P.
[0075] S121: Let’s assume that there are N smart cars that need to perform multi-car collaborative trajectory planning. Assume that the length and width of each car are given, and let’s assume that the length of the i-th car is L i , width is W i, then if we now use a circumscribed circle with its center at the geometric center of the vehicle to cover the vehicle body, then the radius of the circle (denoted by R i ) can be determined by formula (9), which is as follows:
[0076]
[0077] As attached Figure 3 As shown, there is no doubt that the radius R of the circumscribed circle of each vehicle i i is a fixed parameter.
[0078] S122: Define the inertial mass of each vehicle i as M i (i=1,…,N), the mass here is not the actual weight of the vehicle, but an adjustable virtual parameter used to reflect the size of the vehicle's motion inertia.
[0079] Define the expansion parameter of each vehicle i as γ i (i=1,…,N), the actual disk radius of the vehicle body mentioned above is R i , now we require the actual disk area in R i Further multiply by γ i ≥1, in order to reflect the expansion of the disk area, the radius of the expanded disk is called the expansion radius, which is defined as follows (Formula 3):
[0080] R 膨胀i =R i ×γ i ,i=1,2,…,N (3)
[0081] It is required that each expansion parameter must satisfy γ i ≥1, γ i ≥1 means that no disc shrinkage occurs.
[0082] After defining the above adjustable parameters, an adjustable parameter vector P is now constructed. The basic structure formula (1) of the parameter vector P is as follows:
[0083] P≡{M1,…,M i ,…,M N ,γ1,…,γ i ,…,γ N}∈R 1×2N (1)
[0084] S123: With regard to the structure of P described in step S122 above, we now let each of the first N dimensions of vector P take num values uniformly (arithmetic progression) between 0.1 and 10 (for example, if num=3, then each one can take one of the three values in the set).
[0085] Similarly, we let each dimension of the last N dimensions of the P vector take num values evenly (arithmetic progression) between 1.0 and 10.0. In short, each dimension of the 2N dimensions of the P vector can take num values, so the P vector will have a total of num 2N There are different combinations of solutions, all of which are stored in SS.
[0086] S130: Take a parameter vector P from the set SS and record it as P i , and then the parameter vector P i Import the preset module B.
[0087] S131: Module B receives parameter vector P i , and the currently focused vehicle number i; initialize the clock time clock = 0sec (sec refers to seconds, according to the International System of Units, hereinafter referred to as s). Import the position and posture information of each vehicle at clock = 0s. In addition, we agree that the position coordinates of each vehicle i at clock = 0s are (x i (0),y i (0)), the attitude angle is denoted as θ i (0), the speed is recorded as v i (0), the acceleration is a i (0). x i (0), y i (0), v i (0) The information should come from the planning task, that is, the motion state of each vehicle at time 0, but a i (0)Unknown.
[0088] S132: At clock time clock=0s, call the preset module C to determine the driving acceleration a of vehicle i at the current time i (0) (Note that the current P parameter and vehicle number i are passed to module C). The above operation is performed for each vehicle i (i=1, ..., N).
[0089] Assuming that the minimum unit of the time interval of the simulated motion process is ΔT (for example, ΔT is 0.001 seconds), the clock time is increased by ΔT, that is, clock=(0+ΔT)s.
[0090] Then based on time 0, a i (0) and v i (0) The vehicle speed v at clock = ΔTs can be deduced using the following formula i (ΔT) and the displacement ΔS of vehicle i from clock = 0 seconds to clock = ΔT seconds i :
[0091] v i (ΔT)=min(max(0,v i (0)+ΔT·a i (0)),v max )
[0092] ΔS i =0.5×v i (ΔT)+v i (0) (4)
[0093] Among them, v max represents the maximum allowed speed of each vehicle (set by the user in advance, such as 10m / s). The nesting of min() and max() means that we require that the speed of the vehicle simulated in the forward direction cannot exceed v max , and cannot be lower than 0. i After that, vehicle i moves forward ΔS along the given path path_i from the position at time 0. i Distance; the above operation must be performed for each vehicle i (i=1,…,N).
[0094] S133: Repeat step S132, calculate the acceleration of each vehicle at each moment, accumulate the clock moments, and simulate the movement process of each vehicle until all vehicles have completely arrived at the end of their respective predetermined paths path_i.
[0095] S134: Construct a structure solution, record the current accumulated time clock as solution.tf (for use in greedy screening). The position, posture, speed and other state information of each vehicle at each discrete moment are stored in the structure solution in an orderly manner, so as to fully and necessarily reflect a set of multi-vehicle collaborative trajectory planning results.
[0096] Among them, module C is a submodule of module B. Module C is responsible for calculating the current motion acceleration of a vehicle i under a specified parameter vector P. Module C includes the following steps:
[0097] S132-1: Module C should first read in the adjustable parameter vector P and the number of the currently focused vehicle i. Read in the locations of all numbered vehicles at the current moment.
[0098] S132-2: The current focus is on vehicle i. Then, a ray is connected from the center of the circle of other vehicles except i to the center of vehicle i in the vehicle formation. Other vehicles will generate a repulsive force on the current focus vehicle i along the direction of the ray to attach Figure 4 For example, the repulsive force of vehicle j on vehicle i is defined as two cases (as shown in Formula 5):
[0099]
[0100] Among them, k1>0 is a fixed parameter specified by the user, so it is recommended to set k1=1.0.
[0101] As attached Figure 4 As shown, the repulsive force F initiated by vehicle j to vehicle i j→i The corresponding acceleration a j→i The definition formula (6) is as follows:
[0102]
[0103] A j→i Projecting in the tangent direction of vehicle i’s forward direction, the projected acceleration is a j→i .
[0104] Traversing all possible j, the synthetic acceleration caused by the repulsion of all surrounding vehicles to the current vehicle i can be defined as The definition formula (7) is as follows:
[0105]
[0106] S132-3: Applying acceleration in the forward direction of vehicle i to drive the vehicle forward It is now defined as formula (8), which is as follows:
[0107]
[0108] Among them, k2>0 is a fixed parameter, and k2=1.0 is generally taken. Obviously, the forward driving acceleration When the current speed of vehicle i is small, the value is large, and vice versa. In addition, the above formula means The value will not exceed the allowed a max (This parameter has been mentioned before).
[0109] S132-4: Calculate the resultant acceleration a according to formula (2) i , formula (2) is as follows:
[0110]
[0111] A i Treated as the return value of the entire module C.
[0112] S140: Repeat step S130, and execute the other elements in the SSS set one by one in a similar manner to S130. For the i-th element, that is, the i-th group of multi-vehicle collaborative trajectory planning result solution_i, extract it, if current_best_value>solution_i.tf, then update the current_best_value variable value to solution_i.tf, and update the current_best_solution variable to solution_i.
[0113] The current_best_solution is regarded as the globally optimal multi-vehicle collaborative trajectory planning result, and then outputted.
[0114] S210: Import SSS, initialize the current_best_value variable to positive infinity, and initialize the current_best_solution variable to empty.
[0115] S220: Extract the first element from the SSS set, i.e., the first set of multi-vehicle collaborative trajectory planning results solution_1, and then extract the field tf, i.e., solution_1.tf. If current_best_value>solution_1.tf, then update the current_best_value variable value to solution_1.tf, and update the current_best_solution variable to solution_1.
[0116] S230: Repeat the above steps and execute the other elements in the SSS set one by one in a similar manner to S220. For the i-th element, that is, the i-th group of multi-vehicle collaborative trajectory planning result solution_i, extract it, if current_best_value>solution_i.tf, then update the current_best_value variable value to solution_i.tf, and update the current_best_solution variable to solution_i.
[0117] The current_best_solution is regarded as the globally optimal multi-vehicle collaborative trajectory planning result, and then outputted.
[0118] In the specific implementation, the multi-vehicle collaborative trajectory planning problem is introduced, including the starting position (position and posture) and terminal position information of each vehicle (vehicle 1, vehicle 2, vehicle 3), as well as the environment layout information, etc., to initialize three empty sets S, SS, SSS, and the starting position, terminal position information and environment layout information of vehicle 1, vehicle 2, and vehicle 3 are used to generate the driving paths path_1, path_2, and path_3 without considering obstacle avoidance factors using the Reeds-Sheep curve. The length and width of vehicle 1, vehicle 2, and vehicle 3 (such as the length of vehicle 1 is L1 and the width is W1) are used to calculate the radii R1, R2, and R3 of the circumscribed circle (such as the radius R1 of the circumscribed circle of vehicle 1), and the expansion parameters of each vehicle i are defined as γ1, γ2, and γ3, and R is calculated. 膨胀1 , R 膨胀2 , R 膨胀3 . Construct P through M2, M3, M1 and γ1, γ2, γ3, take num = 3, and calculate the 3 2*3 There are 729 different combinations, and the 729 combinations are stored in a set SS.
[0119] Take out the first parameter vector P from the set SS and record it as P1, then import the parameter vector P1 into the preset module B. Module B receives the parameter vector P1 and the currently focused vehicle number 1; initializes clock = 0s, and imports the position and attitude information of each vehicle at clock = 0s. The position coordinates of vehicle 1 at clock = 0s are (x1(0), y1(0)), the attitude angle is θ1(0), the speed is v1(0), and the acceleration is a1(0). Vehicles 2 and 3 also perform the same operation as vehicle 1. Take ΔT=0.001s, and deduce v1(0.001) and ΔS1 after 0.001s based on a1(0) and v1(0) at time 0, and then calculate the displacement of clock from 0.001s to 0.002s... until all three vehicles have completed their respective predetermined paths. Vehicle 1, vehicle 2, and vehicle 3 take 13s, 15s, and 18s to complete paths path_1, path_2, and path_3, respectively. Construct structure solution_1, and record the current accumulated time clock(18s) as solution_1.tf. Store the position, posture, speed and other state information of each vehicle at each discrete moment in order in the structure solution_1, and store solution_1 in the set SSS. Repeat the above operation until all 729 solutions in the set SS are exhausted.
[0120] Traverse the set SSS, extract the field tf, find out the multi-vehicle collaborative trajectory planning result with the smallest extracted field tf (that is, the shortest time), which is solution_524, and output solution_524 as current_best_solution.
[0121] The above is only an embodiment of the present invention, and the common knowledge such as the known specific structure and characteristics in the scheme is not described in detail here. It should be pointed out that for those skilled in the art, several deformations and improvements can be made without departing from the structure of the present invention, which should also be regarded as the protection scope of the present invention, and these will not affect the effect of the implementation of the present invention and the practicality of the patent. The scope of protection required by this application shall be based on the content of its claims, and the specific implementation methods and other records in the specification can be used to interpret the content of the claims.
Claims
1. A global optimal multi-vehicle collaborative trajectory planning method based on parameter enumeration simulation, characterized in that: The following steps are involved: S100: Generate a driving path without considering obstacle avoidance factors based on the starting position and end position information of each vehicle and the environment layout information, and then use the preset modules A and B to generate a large number of multi-vehicle collaborative trajectory planning alternative solutions through driving path enumeration; S200: Screening the numerous multi-vehicle collaborative trajectory planning alternative solutions enumerated, considering the most reasonable set of results as the globally optimal collaborative trajectory planning result, and outputting it as the final output of the entire algorithm.
2. The global optimal multi-vehicle collaborative trajectory planning method based on parameter enumeration simulation according to claim 1 is characterized in that: The step S100 specifically includes the following steps: S110: Import the multi-vehicle collaborative trajectory planning problem, including the starting position and end position information of each vehicle and the environment layout information, initialize three empty sets S, SS, SSS, and use a sampling method that satisfies the kinematic law to generate a driving path for each vehicle without considering obstacle avoidance factors; S120: using a preset module A to process the driving path into various parameter vectors P, and storing them in a set SS; S130: Take a parameter vector P from the set SS and record it as P i , then in the parameter vector P i Under the setting, the preset module B is called to generate the multi-vehicle collaborative trajectory result and recorded as solution_i, and solution_i is stored in the set SSS; S140: Repeat the above S130 until all parameter vectors P in the set SS are taken, and output SSS.
3. The global optimal multi-vehicle collaborative trajectory planning method based on parameter enumeration simulation according to claim 2 is characterized in that: The step S200 specifically includes the following steps: S210: Import the set SSS, create and initialize the variable current_best_value to positive infinity, create and initialize the variable current_best_solution to null; S220: Take an element solution_i from the set SSS, extract fields from the multi-vehicle collaborative trajectory planning result corresponding to the element, and record the extracted fields as solution_i.tf. If: current_best_value>solution_i.tf, then current_best_value=solution_i.tf, current_best_solution=solution_i; S230: Repeat step S220 until all elements in the set SSS are taken, and output current_best_solution as the global optimal multi-vehicle collaborative trajectory planning result in the shortest time.
4. The global optimal multi-vehicle collaborative trajectory planning method based on parameter enumeration simulation according to claim 3 is characterized in that: The module A comprises the following steps: S121: The length of the vehicle passing through i and width W i Find a circumscribed circle with its center at the geometric center of the vehicle and its surface covering the vehicle body, and record the radius of the circumscribed circle as R i ; S122: Expand the circumscribed circle, and record the radius of the expanded circumscribed circle as R 膨胀i , through R i and R 膨胀 i obtains the expansion parameter γ i , through γ i and the inertial mass M of vehicle i i The parameter vector P is obtained. The basic structure formula (1) of the parameter vector P is as follows: P≡{M1,…,M i ,…,m n ,γ1,…γ i ,…,c N }∈R 1×2N (1) Where N is the number of vehicles involved in the calculation; S123: Receive the value num specified by the user, and select num values uniformly from 0.1 to 10 for each dimension of the N dimensions of the vector P. Then the inertial mass M i and the expansion parameter γ i The values of each are num N By taking the inertial mass M i and the expansion parameter γ i By permutation and combination, we can calculate the num of parameter vector P 2N different combinations, and num 2N The combinations are stored in the set SS.
5. The global optimal multi-vehicle collaborative trajectory planning method based on parameter enumeration simulation according to claim 4 is characterized in that: The module B comprises the following steps: S131: Import the adjustable parameter vector P and initialize the clock time clock = 0 seconds. At the same time, for each vehicle, obtain the position coordinates of vehicle i at clock = 0 seconds, marked as (x i (0),y i (0)), the attitude angle is denoted as θ i (0), the speed is recorded as v i (0); S132: When clock=0s, for each vehicle i, call the preset module C to determine the driving acceleration a of vehicle i at the current moment i (0), the minimum time interval unit of the simulated motion process is recorded as ΔT, and clock = (clock + ΔT)s is set. Based on a at time 0 i (0) and v i (0) Deducing the vehicle speed v at clock = ΔTs i (ΔT), and the displacement ΔS of vehicle i from clock = 0s to clock = ΔTs i : S133: Repeat step S132, calculate the acceleration of each vehicle at each moment, accumulate the clock moments, and simulate the movement process of each vehicle until all vehicles have completely arrived at the end of their respective predetermined paths path_i; S134: construct a structure solution, record the current accumulated time clock as solution.tf, and store the position, posture, speed and state information of each vehicle at each discrete moment in the structure solution in an orderly manner.
6. The global optimal multi-vehicle collaborative trajectory planning method based on parameter enumeration simulation according to claim 5 is characterized in that: The module C comprises the following steps: S132-1: Read in the adjustable parameter vector P and the number i of the currently focused vehicle; S132-2: Analyze the positional relationship between the center of any other vehicle j in the vehicle formation except i and the center of vehicle i through vehicle i, and calculate the repulsive force F generated by vehicle j on vehicle i j→i , and then through F j→i Calculate the acceleration a corresponding to the repulsive force initiated by vehicle j on vehicle i j→i , and a j→i Projecting in the tangent direction of the vehicle i’s forward direction, the projected acceleration is Finally, traverse all possible j and calculate the synthetic acceleration caused by the repulsion of all surrounding vehicles to the current vehicle i S132-3: Applying acceleration in the forward direction of vehicle i to drive the vehicle forward S132-4: Calculate the resultant acceleration a according to formula (2) i , formula (2) is as follows: A i Treated as the return value of the entire module C.
7. The global optimal multi-vehicle collaborative trajectory planning method based on parameter enumeration simulation according to claim 6 is characterized in that: The inertial mass M of vehicle i in module A i It is not the actual weight of the vehicle, but an adjustable virtual parameter used to reflect the inertia of the vehicle. i , R 膨胀i and γ i The relationship between is defined as shown in formula (3): R 膨胀i =R i ×γ i ,i=1,2,…,N (3) Each expansion parameter must satisfy γ i ≥1.
8. The global optimal multi-vehicle collaborative trajectory planning method based on parameter enumeration simulation according to claim 7 is characterized in that: Module B calculates ΔS in step S132 i When , use the following formula (4): v i (ΔT)=min(max(0,v i (0)+ΔT·a i (0)),v max ) ΔS i =0.5×v i (ΔT)+v i (0) (4) Among them, v max Represents the maximum allowed speed of each vehicle. The nesting of min() and max() means that we require that the speed of the forward simulation cannot exceed v max , and cannot be lower than 0; after obtaining ΔS i After that, vehicle i moves forward ΔS along the given path path_i from the position at time 0. i Distance; the above operations must be performed for each vehicle i.
9. The global optimal multi-vehicle cooperative trajectory planning method based on parameter enumeration simulation according to claim 8 is characterized in that: In step S132-2 of module C, the repulsive force F of vehicle j on vehicle i is j→i The definition formula (5) is as follows: Where k1>0 is a fixed parameter specified by the user; The repulsive force F initiated by vehicle j on vehicle i j→i The corresponding acceleration a j→i The definition formula (6) is as follows: The definition formula (7) is as follows:
10. The global optimal multi-vehicle cooperative trajectory planning method based on parameter enumeration simulation according to claim 9 is characterized in that: Module C in step S132-3 The definition formula (8) is as follows:
