Multi-vehicle cooperative trajectory planning method based on high-order polynomial fitting in confluence scene

Abstract

The invention discloses a multi-vehicle cooperative trajectory planning method based on high-order polynomial fitting in a confluence scene, and belongs to the field of intelligent traffic, and the method comprises the following steps: S1, obtaining the positions and operation states of a plurality of vehicles entering a confluence area in the confluence scene; s2, grouping the vehicles based on the vehicle positions; s3, based on a grouping result, establishing an expected trajectory function of each vehicle; s4, establishing constraint conditions satisfied by the multi-vehicle cooperative trajectory planning process; s5, converting a multi-vehicle cooperative trajectory planning problem into an optimization solution problem under a multi-constraint condition; and S6, based on the vehicle running state and the constraint condition, minimizing the multi-vehicle cooperative trajectory planning target function to obtain a multi-vehicle cooperative trajectory planning result. According to the multi-vehicle cooperative trajectory planning method based on high-order polynomial fitting in the confluence scene, the calculation complexity in multi-vehicle cooperative trajectory planning is reduced, the calculation efficiency is improved, and meanwhile the safety, comfort and high efficiency of the confluence process are guaranteed.

Description

Technical Field

[0001] The present invention relates to the field of intelligent transportation, and particularly to a multi-vehicle cooperative trajectory planning method based on high-order polynomial fitting in a merging scenario. Background Art

[0002] With the rapid development of autonomous driving technology, intelligent transportation systems have gradually become an important part of future urban transportation. In intelligent transportation systems, cooperative control and trajectory planning of vehicles are one of the key technologies to achieve efficient and safe traffic flow. Especially in complex traffic scenarios, such as merging scenarios, cooperative operations among multiple vehicles are crucial for avoiding collisions, reducing congestion, and improving road use efficiency.

[0003] Existing methods often focus on the trajectory planning of individual vehicles, while ignoring the mutual influence and cooperative effects among vehicles, which is not conducive to ensuring driving safety and improving traffic efficiency. Summary of the Invention

[0004] Aiming at the problems of insufficient vehicle cooperation, frequent safety accidents, poor driving comfort, low traffic efficiency, and slow planning response in the existing merging area, the present invention provides a multi-vehicle cooperative trajectory planning method based on high-order polynomial fitting in a merging scenario, which reduces the computational complexity in multi-vehicle cooperative trajectory planning, improves the computational efficiency, and at the same time ensures the safety, comfort, and efficiency of the merging process.

[0005] To achieve the above object, the present invention provides a multi-vehicle cooperative trajectory planning method based on high-order polynomial fitting in a merging scenario, including the following steps: S1. Obtain the positions and operating states of multiple vehicles entering the merging area in the merging scenario; S2. Group the vehicles based on the vehicle positions obtained in step S1; S3. Based on the grouping result of step S2, according to the initial states and target states of each vehicle, use high-order polynomial fitting to establish the expected trajectory function of each vehicle; S4. On the premise of considering limiting factors, establish the constraint conditions satisfied in the multi-vehicle cooperative trajectory planning process; S5. Establish a multi-vehicle cooperative trajectory planning objective function, and convert the multi-vehicle cooperative trajectory planning problem into an optimization problem with multiple constraint conditions; S6. Based on the vehicle operating states obtained in step S1 and the constraint conditions established in step S4, minimize the multi-vehicle cooperative trajectory planning objective function established in step S5, solve the coefficients of the expected trajectory function of each vehicle, and obtain the multi-vehicle cooperative trajectory planning result.

[0006] Preferably, in step S1, the merging scenario is set as a two-lane to single-lane scenario, and the length of the merging area is , all vehicles in the merging area are numbered in descending order of the ordinate, and the number of vehicles is ; The obtained vehicle positions include the vehicle's abscissa and ordinate, and the obtained vehicle operating states include the vehicle's lateral speed, longitudinal speed, and acceleration.

[0007] Preferably, the specific steps of step S2 include the following steps: S21. Define the vehicles entering the merging area in sequence as vehicle one, vehicle two,..., vehicle , and define the maximum number of vehicles in each group as , S22. Set the vehicle one with the maximum ordinate as the first group. If the distance between vehicle two and vehicle one is less than the desired distance , and the total number of vehicles in the first group is less than , then vehicle two belongs to the first group; otherwise, vehicle two belongs to the second group. S23. If the distance between vehicle three and vehicle two is less than the desired distance , and the total number of vehicles in the group where vehicle two is located is less than , then vehicle three and vehicle two are in the same group; otherwise, vehicle three belongs to a new group. S24. Repeat step S23 for vehicle four, vehicle five,..., vehicle in sequence until all vehicles in the merging area belong to a specific group, and the grouping ends. Among them, the desired distance is calculated using the intelligent driver model , : (1) In the formula, is the static safety distance; is the safety time headway; is the vehicle at the longitudinal speed at the moment; is the vehicle at the difference in longitudinal speed from the preceding vehicle at the moment; is the maximum acceleration; is the comfortable deceleration.

[0008] Preferably, in step S3, the desired trajectory functions in the direction and direction of each vehicle are respectively fitted with a fifth-degree polynomial : (2) (3) In the formula, Represents time; respectively represent Coefficients of the desired trajectory polynomial in the direction; respectively represent Coefficients of the desired trajectory polynomial in the direction; And in step S3, the initial state of each vehicle and the target state are respectively: (4) (5) In the formula, represents the longitudinal position of the vehicle at the initial moment of trajectory planning ; represents the lateral position of the vehicle at the initial moment of trajectory planning ; represents the longitudinal speed of the vehicle at the initial moment of trajectory planning ; represents the lateral speed of the vehicle at the initial moment of trajectory planning ; represents the longitudinal acceleration of the vehicle at the initial moment of trajectory planning ; represents the lateral acceleration of the vehicle at the initial moment of trajectory planning ; represents the longitudinal position of the vehicle at the final moment of trajectory planning ; represents the lateral position of the vehicle at the final moment of trajectory planning ; represents the longitudinal speed of the vehicle at the final moment of trajectory planning ; represents the lateral speed of the vehicle at the final moment of trajectory planning ; represents the longitudinal acceleration of the vehicle at the final moment of trajectory planning ; represents the lateral acceleration of the vehicle at the final moment of trajectory planning ;

[0009] Preferably, the limiting factors described in step S4 include traffic rules, vehicle dynamic characteristics, intra-group trajectory conflicts, and inter-group trajectory conflicts, and the constraint conditions include traffic rule constraints, vehicle dynamic constraints, intra-group trajectory conflicts, and inter-group trajectory conflict constraints; Among them, to satisfy the intra-group trajectory conflict and inter-group trajectory conflict constraints, the road area occupied by the vehicle is defined as a two-dimensional convex polygon 𝒫, and the initial occupied area of the vehicle is , when the vehicle travels along the road, the area occupied after undergoing an affine transformation process including rotation and translation The calculation formula is as follows: (6) In the formula, represents the rotation matrix, which is a function of the vehicle heading angle ; represents the coordinates of the vehicle at time; represents the translation vector, which is a function of the vehicle's horizontal and vertical coordinates; and is an orthogonal rotation matrix, is the translation vector, is the coordinate vector dimension, is a constant, and the area occupied by the transformation process of each vehicle is regarded as a time-varying polyhedron set: (7) In the formula, are the horizontal and vertical coordinates of the points in the polyhedron set in the two-dimensional space respectively; (8) In the formula, represents the rotation matrix, ; represent the length and width of the vehicle respectively, represents the vehicle heading angle; represents the vertical coordinate of the vehicle center at time t; represents the horizontal coordinate of the vehicle center at time t; the time-varying polyhedron set the distance between The calculation formula is as follows: (9) In the formula, and it is set that when , is the set minimum distance, it means that the time-varying polyhedron set intersects, otherwise it does not intersect; Based on the strong duality theory, formula (9) is expressed as a conflict constraint, and the dual problem is expressed as: (10) In the formula, both represent dual variables; Therefore, the constraint of the optimal value of the dual problem is equivalent to: (11) After sorting out, the constraint conditions are described as follows: (12) In the formula, , , respectively represent the longitudinal speed, acceleration, and acceleration derivative of the vehicle at time ; , , respectively represent the lateral speed, acceleration, and acceleration derivative of the vehicle at time ; , , respectively represent the maximum speed, acceleration, and acceleration derivative in the direction; , respectively represent the maximum speed, acceleration, and acceleration derivative in the direction; is a function of and represents the polyhedron set of the vehicle at time ; represents the position coordinates of the vehicle and are functions of and represent the polyhedron set of the vehicle at time ; represents the position coordinates of the vehicle ; represents the dual variable between the vehicle and the vehicle at time .

[0010] Preferably, in step S5, a multi-vehicle cooperative trajectory planning objective function is established with the goals of driving comfort and driving efficiency; Specifically, it includes the following steps: S5. Taking the acceleration change amount as an index to measure driving comfort, design the driving comfort objective function as follows: (13) In the formula, is the driving comfort objective function of the vehicle ; are all weight coefficients; , are respectively the acceleration derivatives in the direction and the direction at time , are respectively Direction The maximum value of the derivative of the acceleration in the direction; and are respectively Direction The maximum value of the acceleration in the direction; S52. Taking the speed as an index to measure the driving efficiency, the driving efficiency objective function is designed as follows: (14) Wherein, is the vehicle Driving efficiency function; is the weight coefficient; is At time The actual driving speed of the vehicle; is the preset desired speed; S53. Based on the driving comfort objective function designed in S51 and the driving efficiency objective function designed in S52, a single-vehicle trajectory planning objective function is established: (15) In the formula, represents the vehicle Trajectory planning objective function; represents the vehicle Weight coefficient of the driving comfort objective function; represents the vehicle Weight coefficient of the driving efficiency function; S54. Based on the single-vehicle trajectory planning objective function established in S53, a multi-vehicle cooperative trajectory planning objective function is established: (16) In the formula, represents the multi-vehicle cooperative trajectory planning objective function.

[0011] Preferably, step S6 specifically includes the following steps: S61. Based on the vehicle operating state obtained in step S1 and the constraint conditions established in step S4, minimizing the multi-vehicle cooperative trajectory planning objective function established in step S5, and transforming the multi-vehicle cooperative trajectory planning problem into a non-linear optimization problem: (17) In the formula, represents the initial state of the vehicle; represents the target state of the vehicle; S62. Using the interior point method to solve the non-linear optimization problem constructed in step S61 to obtain the coefficient of the desired trajectory function of each vehicle; S63. Substitute the coefficients of the desired trajectory function of each vehicle into the high-order polynomial described in step S3 to obtain the multi-vehicle collaborative trajectory planning result.

[0012] Preferably, after step S6, there is further step S7: Divide the entire multi-vehicle collaborative trajectory planning task into several progressive sliding windows, update in each fixed time interval, determine the trajectory starting point by the real-time trajectory coordinates, and update the multi-vehicle collaborative trajectory planning result to the completion of merging through replanning.

[0013] Preferably, step S7 specifically includes the following steps: Set the simulation time of the entire multi-vehicle collaborative trajectory planning task as , starting from when the vehicle enters the merging area, update once every fixed time interval, determine the trajectory starting point of each vehicle by the real-time trajectory coordinates, and update the multi-vehicle collaborative trajectory planning result to the completion of merging through replanning.

[0014] The present invention has the following beneficial effects: 1. By grouping the vehicles in the merging area and decomposing the problem into several sub-problems for solution, the computational complexity in multi-vehicle collaborative trajectory planning is reduced, and the computational efficiency is improved; 2. Comprehensively consider driving safety (constraint conditions), comfort, and traffic efficiency to construct a multi-vehicle collaborative trajectory planning model, establish the lateral and longitudinal desired trajectory functions of vehicle driving based on high-order polynomial fitting, ensure the safety, comfort, and efficiency of the merging process, thereby providing a safe driving trajectory for the vehicles in the merging area and improving the traffic efficiency; 3. Use a dynamic update strategy to adjust the vehicle driving trajectory in real time, which is beneficial to the safe and efficient completion of merging.

[0015] The technical solution of the present invention will be further described in detail below through the accompanying drawings and embodiments. Description of the Drawings

[0016] Figure 1 is the flowchart of the multi-vehicle collaborative trajectory planning method based on high-order polynomial fitting in the merging scenario of the present invention; Figure 2 is the multi-vehicle collaborative trajectory schematic diagram of the simulation experiment of the present invention; Figure 3 is the schematic diagram of the lateral speed change of the vehicle in the simulation experiment of the present invention; Figure 4 is the schematic diagram of the longitudinal speed change of the vehicle in the simulation experiment of the present invention. Detailed Embodiments

[0017] To make the objectives, technical solutions, and advantages of the embodiments of the present invention more clearly understood, the following further elaborates on the embodiments of the present invention in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely used to explain the embodiments of the present invention and are not intended to limit the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments in this application without creative efforts fall within the scope of protection of this application. Examples of the embodiments are shown in the accompanying drawings, where the same or similar reference numerals represent the same or similar elements or elements with the same or similar functions throughout.

[0018] It should be noted that the terms "comprising" and "having" and any variations thereof are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or server that includes a series of steps or units does not necessarily have to be limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or are inherent to these processes, methods, products, or devices.

[0019] Similar reference numerals and letters in the following drawings represent similar items. Therefore, once an item is defined in one drawing, it does not need to be further defined and explained in subsequent drawings.

[0020] As Figure 1 shown, the multi-vehicle collaborative trajectory planning method based on high-order polynomial fitting in the confluence scenario of the present invention includes the following steps: S1. Obtain the positions and operating states of multiple vehicles entering the confluence area in the confluence scenario; In step S1, set the confluence scenario as a two-lane to single-lane scenario, the length of the confluence area is , all vehicles in the confluence area are numbered in descending order of the ordinate, and the number of vehicles is ; The vehicle positions obtained include the vehicle abscissa and ordinate, and the vehicle operating states obtained include the vehicle lateral speed, longitudinal speed, and acceleration.

[0021] S2. Group the vehicles based on the vehicle positions obtained in step S1; The specific steps of step S2 include the following steps: S21. Define the vehicles entering the confluence area in sequence as vehicle one, vehicle two,..., vehicle , and define the maximum number of vehicles in each group as , S22. Set vehicle one with the maximum ordinate as the first group. If the distance between vehicle two and vehicle one is less than the expected distance , and the total number of vehicles in the first group is less than , then Vehicle 2 belongs to the first group; otherwise, Vehicle 2 belongs to the second group; S23. If the distance between Vehicle 3 and Vehicle 2 is less than the expected distance , and the total number of vehicles in the group where Vehicle 2 is located is less than , then Vehicle 3 and Vehicle 2 are in the same group; otherwise, Vehicle 3 belongs to a new group; S24. Repeat step S23 for Vehicle 4, Vehicle 5,..., Vehicle in sequence for grouping until all vehicles in the merging area belong to a specific group, and the grouping ends; Among them, the expected distance is calculated using the intelligent driver model , : (1) In the formula, is the static safety distance; is the safety time headway; is the vehicle at the longitudinal speed at the moment; is the vehicle at the difference in longitudinal speed from the leading vehicle at the moment; is the maximum acceleration; is the comfortable deceleration.

[0022] S3. Based on the grouping results of step S2, according to the initial states and target states of each vehicle, use high-order polynomial fitting to establish the expected trajectory function of each vehicle; In step S3, use a fifth-order polynomial to fit the expected trajectory functions of each vehicle in the direction and direction respectively : (2) (3) In the formula, represents time; respectively represent the coefficients of the expected trajectory polynomial in the direction respectively; the coefficients of the expected trajectory polynomial in the direction respectively; and the target state of each vehicle in step S3 are respectively: (4) (5) In the formula, represents the initial moment of trajectory planning The longitudinal position of the vehicle at a certain time; Indicates the initial moment of trajectory planning The lateral position of the vehicle at a certain time; Indicates the initial moment of trajectory planning The longitudinal speed of the vehicle at a certain time; Indicates the initial moment of trajectory planning The lateral speed of the vehicle at a certain time; Indicates the initial moment of trajectory planning The longitudinal acceleration of the vehicle at a certain time; Indicates the initial moment of trajectory planning The lateral acceleration of the vehicle at a certain time; Indicates the final moment of trajectory planning The longitudinal position of the vehicle at a certain time; Indicates the final moment of trajectory planning The lateral position of the vehicle at a certain time; Indicates the final moment of trajectory planning The longitudinal speed of the vehicle; Indicates the final moment of trajectory planning The lateral speed of the vehicle at a certain time; Indicates the final moment of trajectory planning The longitudinal acceleration of the vehicle at a certain time; Indicates the final moment of trajectory planning The lateral acceleration of the vehicle at a certain time.

[0023] S4. On the premise of considering the limiting factors, establish the constraint conditions satisfied by the multi-vehicle cooperative trajectory planning process; The limiting factors described in step S4 include traffic rules, vehicle dynamic characteristics, intra-group trajectory conflicts, and inter-group trajectory conflicts. The constraint conditions include traffic rule constraints, vehicle dynamic constraints, intra-group trajectory conflicts, and inter-group trajectory conflict constraints; Among them, to satisfy the intra-group trajectory conflict and inter-group trajectory conflict constraints, the road area occupied by the vehicle is defined as a two-dimensional convex polygon 𝒫, and the initial occupied area of the vehicle is , when the vehicle travels along the road, The area occupied after an affine transformation process including rotation and translation The calculation formula is as follows: (6) In the formula, Represents the rotation matrix, which is a function of the vehicle's heading angle ; Represents the coordinates of the vehicle at moment; Represents the translation vector, which is a function of the vehicle's horizontal and vertical coordinates; And is an orthogonal rotation matrix, is a translation vector, is a coordinate vector of dimension, is a constant, in this embodiment the value of is 2, and the area occupied by the transformation process of each vehicle is regarded as a time-varying polyhedron set: (7) In the formula, are respectively the horizontal and vertical coordinates of the points in the polyhedron set in the two-dimensional space; (8) In the formula, represents the rotation matrix, ; represent the length and width of the vehicle respectively, represents the vehicle heading angle; represents the vertical coordinate of the vehicle center at time t; represents the horizontal coordinate of the vehicle center at time t; the time-varying polyhedron set the distance between The calculation formula is as follows: (9) In the formula, and it is set that when , is the set minimum distance, it means that the time-varying polyhedron set intersects, otherwise it does not intersect; Based on the strong duality theory, formula (9) is expressed as a conflict constraint, where the dual problem is expressed as: (10) In the formula, both represent dual variables; Therefore, the constraint of the optimal value of the dual problem is equivalent to: (11) After sorting, the constraint conditions are described as follows: (12) In the formula, , , respectively represent at time the longitudinal speed, acceleration, and acceleration derivative of the vehicle; , , respectively represent at time Lateral velocity, acceleration, acceleration derivative; , , respectively represent the maximum velocity, acceleration, acceleration derivative in the , , respectively represent the maximum velocity, acceleration, acceleration derivative in the is a function of and represents the polyhedron set of the vehicle at time ; represents the position coordinates of the vehicle ; and are functions of and represent the polyhedron set of the vehicle at time ; represents the position coordinates of the vehicle ; represents the dual variable between the vehicle and the vehicle at time .

[0024] S5. Establish a multi-vehicle cooperative trajectory planning objective function and transform the multi-vehicle cooperative trajectory planning problem into an optimization problem with multiple constraints; In step S5, establish a multi-vehicle cooperative trajectory planning objective function with driving comfort and driving efficiency as the goals; Specifically, it includes the following steps: S5. Take the acceleration change as an index to measure driving comfort and design the driving comfort objective function as follows: (13) In the formula, is the driving comfort objective function of the vehicle ; are all weight coefficients; are respectively the acceleration derivatives in the direction and direction at time ; are respectively the maximum values of the acceleration derivatives in the , are respectively the acceleration maximum values in the direction; S52. Take the velocity as an index to measure driving efficiency and design the driving efficiency objective function as follows: (14) Wherein, is the vehicle driving efficiency function; is the weight coefficient; is the actual driving speed of the vehicle at time is the preset desired speed; S53. Based on the driving comfort objective function designed in S51 and the driving efficiency objective function designed in S52, establish a single-vehicle trajectory planning objective function: (15) In the formula, represents the vehicle trajectory planning objective function; represents the weight coefficient of the vehicle driving comfort objective function; represents the weight coefficient of the vehicle driving efficiency function; S54. Based on the single-vehicle trajectory planning objective function established in S53, establish a multi-vehicle cooperative trajectory planning objective function: (16) In the formula, represents the multi-vehicle cooperative trajectory planning objective function.

[0025] S6. Based on the vehicle operating states obtained in step S1 and the constraint conditions established in step S4, minimize the multi-vehicle cooperative trajectory planning objective function established in step S5, and solve the coefficients of the desired trajectory function of each vehicle to obtain the multi-vehicle cooperative trajectory planning result.

[0026] Step S6 specifically includes the following steps: S61. Based on the vehicle operating states obtained in step S1 and the constraint conditions established in step S4, minimize the multi-vehicle cooperative trajectory planning objective function established in step S5, and transform the multi-vehicle cooperative trajectory planning problem into a non-linear optimization problem: (17) In the formula, represents the initial state of the vehicle; represents the target state of the vehicle; S62. Use the interior point method to solve the non-linear optimization problem constructed in step S61 to obtain the coefficients of the desired trajectory function of each vehicle; S63. Substitute the coefficients of the desired trajectory function of each vehicle into the high-order polynomial described in step S3 to obtain the multi-vehicle cooperative trajectory planning result.

[0027] After step S6, there is also step S7. To achieve dynamic collaborative trajectory planning, the entire multi-vehicle collaborative trajectory planning task is divided into several progressive sliding windows, which are updated at each fixed time interval. The starting point of the trajectory is determined by the real-time trajectory coordinates, and the multi-vehicle collaborative trajectory planning result is updated through replanning until the confluence is completed.

[0028] And step S7 specifically includes the following steps: Set the simulation time of the entire multi-vehicle collaborative trajectory planning task as , starting from when the vehicle enters the confluence area, update is performed once at each fixed time interval. The starting point of each vehicle's trajectory is determined by the real-time trajectory coordinates, and the multi-vehicle collaborative trajectory planning result is updated through replanning until the confluence is completed.

[0029] Simulation experiment This simulation experiment is verified on the MATLAB simulation software as follows: The confluence scenario is a two-lane to single-lane scenario.

[0030] Table 1 Road parameters ;

[0031] Table 2 Vehicle parameters ;

[0032] Table 3 Vehicle positions and states ;

[0033] As can be seen from Table 3, in the confluence area, Vehicle 1 and Vehicle 3 keep going straight, and Vehicle 2 needs to perform a lane change operation. The simulation time is 9 s, and update is performed every 3 s. As Figures 2 - 4 shown, vehicle confluence is completed within 9 s, and the vehicles accelerate to 20 m / s under safe conditions, improving the traffic efficiency and verifying the effectiveness of the present invention.

[0034] Therefore, the present invention adopts the multi-vehicle collaborative trajectory planning method based on high-order polynomial fitting in the above confluence scenario, reducing the computational complexity in multi-vehicle collaborative trajectory planning, improving the computational efficiency, and at the same time ensuring the safety, comfort and efficiency of the confluence process.

[0035] Finally, it should be noted that: The above embodiments are only used to illustrate the technical solutions of the present invention rather than to limit them. Although the present invention has been described in detail with reference to the preferred embodiments, those of ordinary skill in the art should understand that: They can still modify or equivalently replace the technical solutions of the present invention, and these modifications or equivalent replacements cannot make the modified technical solutions deviate from the spirit and scope of the technical solutions of the present invention.

Claims

1. A multi-vehicle collaborative trajectory planning method based on high-order polynomial fitting in a merging scenario, characterized by: The following steps are involved: S1. Obtaining the positions and operating states of multiple vehicles entering the merging area in a merging scenario; S2, grouping the vehicles based on the vehicle positions obtained in step S1; S3, based on the grouping result of step S2, according to the initial state and target state of each vehicle, using high-order polynomial fitting to establish the expected trajectory function of each vehicle; S4. Under the premise of considering the limiting factors, establish the constraints satisfied by the multi-vehicle collaborative trajectory planning process; S5. Establish a multi-vehicle collaborative trajectory planning objective function and convert the multi-vehicle collaborative trajectory planning problem into an optimization problem under multiple constraints; S6. Based on the vehicle operating status obtained in step S1 and the constraints established in step S4, the multi-vehicle collaborative trajectory planning objective function established in step S5 is minimized, the expected trajectory function coefficient of each vehicle is solved, and the multi-vehicle collaborative trajectory planning result is obtained.

2. The multi-vehicle collaborative trajectory planning method based on high-order polynomial fitting in a merging scenario according to claim 1, characterized in that: In step S1, the merging scene is set to a two-lane merging scene, and the merging area length is , all vehicles in the merging area are numbered in descending order of the ordinate, and the number of vehicles is ; The acquired vehicle position includes the vehicle's horizontal coordinate and vertical coordinate, and the acquired vehicle running state includes the vehicle's lateral speed, longitudinal speed and acceleration.

3. The multi-vehicle collaborative trajectory planning method based on high-order polynomial fitting in a merging scenario according to claim 2 is characterized by: Step S2 specifically includes the following steps: S21. Define the vehicles that enter the merging area in sequence as vehicle 1, vehicle 2, ..., vehicle , and define the maximum number of vehicles in each group to be ; S22, set the vehicle 1 with the largest ordinate as the first group, if the distance between vehicle 2 and vehicle 1 is less than the expected distance , and the total number of vehicles in the first group is less than , then vehicle 2 belongs to the first group, otherwise vehicle 2 belongs to the second group; S23: If the distance between vehicle 3 and vehicle 2 is less than the expected distance , and the total number of vehicles in the group where vehicle 2 belongs is less than , then vehicle 3 and vehicle 2 are in the same group, otherwise vehicle 3 belongs to a new group; S24, repeat step S23 for vehicle 4, vehicle 5, ..., vehicle The grouping is performed sequentially until all vehicles in the merging area belong to a specific group, and the grouping is completed; Among them, the expected distance is calculated using the intelligent driver model ， : （1） In the formula, It is the static safety distance; For safe time interval; For vehicles exist Longitudinal velocity at the moment; For vehicles exist The difference between the longitudinal speed of the vehicle at the time and the preceding vehicle; is the maximum acceleration; Deceleration for comfort.

4. The multi-vehicle collaborative trajectory planning method based on high-order polynomial fitting in a merging scenario according to claim 3 is characterized by: In step S3, a fifth-order polynomial is used to fit each vehicle Direction and Expected trajectory function of direction : （2） （3） In the formula, Indicates time; Respectively Directional expected trajectory polynomial coefficients; Respectively Directional expected trajectory polynomial coefficients; And in step S3, the initial state of each vehicle and target state They are: （4） （5） In the formula, represents the initial time of trajectory planning The longitudinal position of the vehicle at represents the initial time of trajectory planning The lateral position of the vehicle at the time represents the initial time of trajectory planning The longitudinal speed of the vehicle at ; represents the initial time of trajectory planning The lateral speed of the vehicle at ; represents the initial time of trajectory planning The longitudinal acceleration of the vehicle at ; represents the initial time of trajectory planning The lateral acceleration of the vehicle at Represents the final moment of trajectory planning The longitudinal position of the vehicle at Represents the final moment of trajectory planning The lateral position of the vehicle at the time Represents the final moment of trajectory planning The longitudinal speed of the vehicle at ; Represents the final moment of trajectory planning The lateral speed of the vehicle at ; Represents the final moment of trajectory planning The longitudinal acceleration of the vehicle at ; Represents the final moment of trajectory planning The lateral acceleration of the vehicle.

5. The multi-vehicle collaborative trajectory planning method based on high-order polynomial fitting in a merging scenario according to claim 4 is characterized in that: The limiting factors described in step S4 include traffic rules, vehicle dynamics characteristics, intra-group trajectory conflicts and inter-group trajectory conflicts, and the constraint conditions include traffic rule constraints, vehicle dynamics constraints, intra-group trajectory conflicts and inter-group trajectory conflict constraints; In order to satisfy the intra-group and inter-group trajectory conflict constraints, the road area occupied by the vehicle is defined as a two-dimensional convex polygon 𝒫, and the initial area occupied by the vehicle is , when the vehicle is traveling along the road, The area occupied by the affine transformation process including rotation and translation The calculation formula is as follows: （6） In the formula, Represents the rotation matrix, which is the vehicle heading angle Function of Indicates that the vehicle is The coordinates of the moment; represents the translation vector, which is a function of the vehicle's horizontal and vertical coordinates; and is an orthogonal rotation matrix, is the translation vector, is the coordinate vector The dimension of As a constant, the area occupied by the transformation process of each vehicle Viewed as a collection of time-varying polyhedra: （7） In the formula, are the vertical and horizontal coordinates of the points in the polyhedron set in two-dimensional space; （8） In the formula, represents the rotation matrix, ; Represent the length and width of the vehicle respectively, Indicates the vehicle heading angle; express The vertical coordinate of the center of the vehicle at the moment; express The horizontal coordinate of the center of the vehicle at the moment; Time-varying polyhedron set The distance between The calculation formula is as follows: （9） In the formula, And set when ， When the minimum distance is set, it represents a time-varying polyhedron set intersect, otherwise they do not intersect; Based on the strong duality theory, formula (9) is expressed as a conflict constraint, where the dual problem is expressed as: （10） In the formula, Both represent dual variables; Therefore, the constraint on the optimal value of the dual problem is equivalent to: （11） The constraints are described as follows: （12） In the formula, , , Respectively Time Vehicle Longitudinal velocity, acceleration, acceleration derivative; , , Respectively Time Vehicle Lateral velocity, acceleration, acceleration derivative; , , Respectively Directional maximum velocity, acceleration, acceleration derivative; , , Respectively Directional maximum velocity, acceleration, acceleration derivative; for The function of Time Vehicle The set of polyhedra of ; Indicates vehicle The location coordinates of and for The function of Time Vehicle The set of polyhedra of ; Indicates vehicle The location coordinates of Indicates vehicle With vehicle Between The dual variable of the moment.

6. The multi-vehicle collaborative trajectory planning method based on high-order polynomial fitting in a merging scenario according to claim 5, characterized in that: In step S5, a multi-vehicle collaborative trajectory planning objective function is established based on driving comfort and driving efficiency objectives; It specifically includes the following steps: S5. Taking the acceleration change as an indicator to measure driving comfort, the driving comfort objective function is designed as follows: （13） In the formula, For vehicles Driving comfort objective function; All are weight coefficients; 、 They are time direction, Directional derivative of acceleration; , They are direction, The maximum value of the directional acceleration derivative; , They are direction, Maximum directional acceleration; S52. Taking speed as an indicator to measure driving efficiency, the driving efficiency objective function is designed as follows: （14） in, For vehicles Driving efficiency function; is the weight coefficient; for Time Vehicle Actual driving speed; To preset the expected speed; S53. Based on the driving comfort objective function designed in S51 and the driving efficiency objective function designed in S52, a single vehicle trajectory planning objective function is established: （15） In the formula, Indicates vehicle Trajectory planning objective function; Indicates vehicle The weight coefficient of the driving comfort objective function; Indicates vehicle The weight coefficient of the driving efficiency function; S54. Based on the single-vehicle trajectory planning objective function established in S53, a multi-vehicle collaborative trajectory planning objective function is established: （16） In the formula, Represents the objective function of multi-vehicle collaborative trajectory planning.

7. The multi-vehicle collaborative trajectory planning method based on high-order polynomial fitting in a merging scenario according to claim 6, characterized in that: Step S6 specifically includes the following steps: S61, based on the vehicle running status obtained in step S1 and the constraint conditions established in step S4, minimize the multi-vehicle collaborative trajectory planning objective function established in step S5, and transform the multi-vehicle collaborative trajectory planning problem into a nonlinear optimization problem: （17） In the formula, Indicates the initial state of the vehicle; Indicates the target state of the vehicle; S62, using the interior point method to solve the nonlinear optimization problem constructed in step S61, and obtain the coefficient of the expected trajectory function of each vehicle; S63. Substitute the coefficients of the expected trajectory function of each vehicle into the high-order polynomial described in step S3 to obtain the multi-vehicle collaborative trajectory planning result.

8. The multi-vehicle collaborative trajectory planning method based on high-order polynomial fitting in a merging scenario according to claim 7, characterized in that: Step S6 also includes step S7, dividing the entire multi-vehicle collaborative trajectory planning task into several progressive sliding windows, updating them at each fixed time interval, the trajectory starting point is determined by the real-time trajectory coordinates, and the multi-vehicle collaborative trajectory planning results are updated by re-planning until the merging is completed.

9. The multi-vehicle collaborative trajectory planning method based on high-order polynomial fitting in a merging scenario according to claim 8, characterized in that: Step S7 specifically includes the following steps: setting the simulation time of the entire multi-vehicle collaborative trajectory planning task to ,After the vehicle enters the merging area, an update is performed at a fixed time interval. The starting point of each vehicle trajectory is determined by the real-time trajectory coordinates. The multi-vehicle collaborative trajectory planning results are updated through re-planning until the merging is completed.

Images