Multi-constraint multi-target coupled multi-vehicle cooperative safe lane changing trajectory planning method

Abstract

The invention discloses a multi-constraint multi-target coupled multi-vehicle cooperative safe lane changing trajectory planning method, which belongs to the field of intelligent traffic vehicle trajectory optimization, and comprises the following steps: S1, establishing a kinematics model of an automatic driving vehicle and discretizing the kinematics model; s2, on the basis of the discretized kinematics model, constraint conditions which need to be met during trajectory planning of the autonomous vehicle are established; s3, building a multi-target cost function of multiple vehicles by considering external factors, so as to convert a trajectory planning problem of the multiple vehicles into an optimization problem for solving a plurality of equality constraints and inequality constraints; and S4, under the condition that constraint conditions are met, the multi-target cost function constructed in the step S3 is minimized, and the optimal track of multi-vehicle cooperative lane changing is obtained. According to the multi-constraint multi-target coupling multi-vehicle cooperative safe lane changing trajectory planning method, the optimal trajectory is provided for the vehicles of the current road section, and multi-vehicle trajectory planning in a complex dynamic environment is achieved.

Description

Technical Field

[0001] The present invention relates to the field of intelligent transportation vehicle trajectory optimization, and in particular to a multi-vehicle collaborative safe lane change trajectory planning method with multi-constraint and multi-objective coupling. Background Art

[0002] With the rapid development of autonomous driving and Internet of Vehicles technologies, multi-vehicle collaborative trajectory planning has become a core key technology in the field of intelligent transportation systems and autonomous driving. This technology involves close coordination and cooperation between multiple vehicles, aiming to improve the efficiency and safety of road use. At the same time, in complex and changing traffic environments, vehicles need to perform operations such as lane changes and speed adjustments, so it is required that vehicles can share information in real time and coordinate their action strategies.

[0003] In the process of multi-vehicle collaborative trajectory planning, each vehicle not only needs to optimize its own driving trajectory, but also needs to consider the dynamic changes of surrounding vehicles to achieve efficient and safe collaboration. By considering the real-time status information of the entire system, the driving trajectory of the entire fleet is optimized, thereby improving the overall traffic efficiency and safety.

[0004] In addition, in multi-vehicle collaborative trajectory planning, it is necessary to achieve the global optimality of the entire system, rather than the optimal trajectory of a single individual, to ensure the operating efficiency and safety of the entire fleet. Based on the idea of ​​the overall optimality of collaborative planning, multi-vehicle collaborative trajectory planning technology can significantly improve the stability and smoothness of traffic flow, while reducing the incidence of traffic accidents, and provide strong technical support for the long-term development of intelligent transportation systems.

[0005] The existing literature on multi-vehicle cooperative lane change trajectory planning is as follows: Li B, Zhang Y, Feng Y et al. proposed a two-stage multi-vehicle motion planning (MVMP) framework for cooperative lane changes in "Balancing computation speed and quality: A decentralized motion planning method for cooperative lane changes of connected and automated vehicles". In the first stage, the vehicles are transformed from the original formation to a sufficiently sparse formation, involving only longitudinal motion, simplifying the constraints of collision avoidance. In the second stage, based on the sparse formation, all vehicles change lanes simultaneously, implicitly ensuring collision avoidance. Through staged processing, this method can quickly calculate a near-optimal solution while maintaining the ability of vehicles to cooperate with each other. Wang Z, Zhao X, Chen Z et al. proposed a dynamic cooperative lane-changing model in "A dynamic cooperative lane-changing model for connected and autonomous vehicles with possible accelerations of a preceding vehicle", which takes into account the acceleration possibility of the preceding vehicle, uses a linearized vehicle kinematic model, and dynamically designs the longitudinal and lateral trajectories of the SV (subject vehicle) and surrounding vehicles to reduce the negative impact of lane changes on surrounding traffic.

[0006] It can be seen that the existing trajectory planning methods perform well in static or quasi-static traffic environments, but their adaptability and robustness may be insufficient in dynamically changing traffic environments, such as rapid changes in vehicle density or emergencies. At the same time, since multi-vehicle collaborative trajectory planning usually involves complex optimization problems, especially in complex interactive environments with multiple lanes and multiple vehicles, the above methods may suffer from poor real-time performance due to high computational complexity. Summary of the invention

[0007] To solve the above problems, the present invention provides a multi-vehicle collaborative safe lane change trajectory planning method with multi-constraint and multi-objective coupling, which provides the optimal trajectory for vehicles on the current road section and realizes multi-vehicle trajectory planning in a complex dynamic environment.

[0008] To achieve the above object, the present invention provides a multi-vehicle cooperative safe lane change trajectory planning method involving multi-constraint and multi-objective coupling, comprising the following steps: S1. Establish the kinematic model of the autonomous driving vehicle and discretize it; S2. Based on the kinematic model discretized in step S1, establish the constraints that the autonomous driving vehicle needs to meet when planning the trajectory; S3. Considering external factors, a multi-objective cost function for multiple vehicles is constructed to transform the trajectory planning problem of multiple vehicles into an optimization problem with multiple equality constraints and inequality constraints. S4. Under the constraints established in step S2, the multi-objective cost function constructed in step S3 is minimized to obtain the optimal trajectory for multi-vehicle coordinated lane change.

[0009] Preferably, step S1 specifically includes the following steps: S11. Assume that in the Cartesian coordinate system, The autonomous driving vehicles (CAVs) form a coordinated lane change system and number the autonomous driving vehicles. , ; S12, using Ackerman's bicycle kinematics model to describe the driving process of the autonomous vehicle CAV, and obtaining a kinematic model of the autonomous vehicle; S13. Use the forward Euler method to discretize the kinematic model of the continuous-time autonomous driving vehicle to plan a trajectory consisting of a set of spatiotemporal state points, thereby obtaining a trajectory that satisfies both the vehicle kinematic constraints and real-time traffic.

[0010] Preferably, step S12 specifically includes the following steps: The bicycle kinematic model is used to simplify the two wheels of the front and rear axles of the autonomous vehicle CAV into a single wheel at the center of the axle, so that the autonomous vehicle CAV can be regarded as a bicycle motion model along the central axis, and the following kinematic model of the autonomous vehicle is obtained: (1) In the formula, Respectively indicate that in the current Moment The longitudinal position coordinates and lateral position coordinates of the center point of the rear axle of the autonomous driving vehicle; Indicates that at present Moment The speed of the autonomous vehicle; Indicates that at present Moment The acceleration of an autonomous vehicle; Indicates that at present Moment The rate of change of acceleration of an autonomous vehicle, Indicates that at present Moment The direction angle of the autonomous vehicle; Indicates that at present Moment The front wheel steering angle of the autonomous vehicle; Indicates the distance from the front axle to the rear axle of the autonomous vehicle; Based on the kinematic equations, the front wheel steering angle of the autonomous vehicle is determined and the change in acceleration , get the State variables of the autonomous vehicle CAV , input variable ; The discretization expression described in step S13 is as follows: (2) In the formula, ( , ) are respectively represented in Time step The longitudinal position coordinates and lateral position coordinates of the center point of the rear axle of the autonomous driving vehicle; ( , ) are respectively represented in Time step The longitudinal position coordinates and lateral position coordinates of the center point of the rear axle of the autonomous driving vehicle; Indicated in Time step The speed of the autonomous vehicle; Indicated in Time step The acceleration of an autonomous vehicle; Indicated in Time step The rate of change of acceleration of an autonomous vehicle, Indicated in Time step The direction angle of the autonomous vehicle; Indicated in Time step The front wheel steering angle of the autonomous vehicle; is the time step.

[0011] Preferably, the constraints described in step S2 include the kinematic constraints that the vehicle needs to satisfy in step S1, the safety collision avoidance constraints between the boundary of the autonomous driving vehicle and the boundary of the road, the safety collision avoidance constraints between two adjacent autonomous driving vehicles, and the initial and terminal state constraints before and after lane changing.

[0012] Preferably, step S2 specifically includes the following steps: S21. Kinematic constraints required to construct the vehicle motion trajectory: Formula (2) is set as the kinematic constraint that trajectory planning needs to satisfy, and the boundaries of the variables in the kinematic equation are solved as constraint conditions, where the boundary conditions of the variables are as follows: (3) In the formula, The maximum speed that an autonomous vehicle can reach during driving. is the maximum acceleration of the autonomous vehicle, is the maximum heading angle of the autonomous vehicle; is the maximum steering angle of the front wheels of the autonomous vehicle, is the maximum acceleration rate of the autonomous vehicle; S22. Safety collision avoidance constraints between the autonomous vehicle boundary and the road boundary: S221. Integrated safety collision avoidance constraints: Assume that each autonomous vehicle is rectangular, the road guardrail is straight, and the upper and lower bounds of the road boundary are and , if the autonomous vehicle does not collide with the road boundary, the four vertices of the rectangle The vertical coordinate Within the limits of the road boundaries, the expression is as follows: (4) In the formula, The ordinates of the vertices of the vehicle rectangle; S222: according to the determined coordinates of the rear axle center point of the autonomous driving vehicle And heading angle , and geometric relationships to determine the four corners of the autonomous vehicle Coordinates: (5) (6) (7) (8) In the formula, Represent the four vertices of the rectangle The horizontal axis of Respectively represent The longitudinal position coordinates and lateral position coordinates of the center point of the rear axle of the autonomous driving vehicle; Indicates the horizontal distance from the center of the front wheel axle to the front of the autonomous vehicle; Indicates the horizontal distance from the center of the rear axle of the autonomous vehicle to the rear of the vehicle; is the width of the autonomous vehicle; S223, using formula (5) to formula (8) to solve the coordinates of the four vertices of the rectangular outline of the autonomous driving vehicle at any time; S23. Perform safety collision avoidance constraints on two adjacent autonomous driving vehicles during the lane change process: S231. Introducing the triangle area criterion to describe the conditions for non-overlap between two irregularly placed rectangles, we have: (9) In the formula, represents the area of ​​the triangle formed by point Q and vertices A and B in rectangle ABCD; represents the area of ​​the triangle formed by point Q and vertices B and C in rectangle ABCD; represents the area of ​​the triangle formed by point Q and vertices C and D in rectangle ABCD; represents the area of ​​the triangle formed by point Q and vertices D and A in rectangle ABCD; represents the area of ​​rectangle ABCD; According to the triangle area criterion, if point Q satisfies formula (9), then point Q is determined to be outside rectangle ABCD; S232, When the self-driving car The four vertices All in self-driving cars outside the rectangle, and the self-driving car The four vertices All in self-driving cars When the self-driving car is outside the rectangle and self-driving cars Satisfy the safety collision avoidance constraint, therefore, any two autonomous vehicles and self-driving cars The collision avoidance constraint is described as: (10) In the formula, Self-driving car A point about self-driving cars Vertex and vertices The area of ​​the enclosed triangle; Self-driving cars A point about self-driving cars Vertex and vertices The area of ​​the enclosed triangle; Self-driving cars A point about self-driving cars Vertex and vertices The area of ​​the enclosed triangle; Self-driving cars A point about self-driving cars Vertex and vertices The area of ​​the enclosed triangle; Self-driving car The four vertices The area of ​​the enclosed rectangle; Self-driving car A point about self-driving cars Vertex and vertices The area of ​​the enclosed triangle; Self-driving cars A point about self-driving cars Vertex and vertices The area of ​​the enclosed triangle; Self-driving cars A point about self-driving cars Vertex and vertices The area of ​​the enclosed triangle; Self-driving cars A point about self-driving cars Vertex and vertices The area of ​​the enclosed triangle; Self-driving car The four vertices The area of ​​the enclosed rectangle; Expanding formula (10) to obtain the safety collision avoidance constraints between multiple vehicles, the autonomous driving vehicle The specific expression of the collision avoidance constraint between is: (11); S24. Building a self-driving car Initial and final state constraints before and after lane change: Assuming a self-driving car In lane at the start of a lane change The top is driving along the center line of the road at a uniform speed. , the acceleration and heading angle are both 0, and the autonomous driving car The initial state is ; After the lane change is completed, assuming that the lane change completion time is , the discrete time step is , so after discretization we get , self-driving cars In the driveway The top is driving along the center line of the road at a uniform speed. , the acceleration and heading angle are both 0, and the autonomous driving car The terminal state is ; Self-driving cars Initial state and self-driving car The terminal state of is used as the initial and terminal state constraints before and after lane change.

[0013] Preferably, the external factors described in step S3 include passenger comfort, vehicle traffic efficiency and safety factors.

[0014] Preferably, step S3 specifically includes the following steps: S31, the change in acceleration As an indicator to measure passenger comfort, a cost function related to passenger comfort is designed: (12) In the formula, For the The comfort cost function of an autonomous vehicle, It is The acceleration rate of the autonomous vehicle, is the maximum acceleration rate of the autonomous vehicle, is the maximum acceleration of the autonomous vehicle, is the time step; S32. Cost function related to design efficiency: (13) In the formula, For the The efficiency cost function of an autonomous vehicle, For a time step of Self-driving car speed, For the The expected speed of an autonomous vehicle, is the maximum speed; S33. Take the front wheel steering angle of the autonomous driving vehicle as the safety cost function and minimize the front wheel steering angle: (14) In the formula, For the Safety cost function for an autonomous vehicle; S34, based on the comfort cost function designed in step S31, the efficiency cost function designed in step S32, and the safety cost function designed in step S33, obtain the cost function of the entire system composed of N autonomous driving vehicles CAV for: (15) In the formula, are all weight coefficients.

[0015] Preferably, step S4 specifically includes the following steps: S41: Based on the constraints established in step S2 and the cost function of the entire system established in step S3 , we get the constrained nonlinear optimization problem of multi-vehicle coordinated lane change: (16) S42. Using CasADi under the MATLAB platform as a solving tool, the optimal solution obtained by solving formula (16) is used as the optimal trajectory of N autonomous driving vehicles.

[0016] Preferably, step S5 is also included after step S4: by using a rolling time domain optimization method, the state of the current autonomous driving vehicle is updated at fixed intervals, and the established optimization problem is re-optimized and solved based on the current latest state.

[0017] Preferably, in step S5, at the beginning of each predetermined time step, the optimal driving trajectory of the vehicle is recalculated and generated according to the current traffic conditions and vehicle status, and the cycle is iterated until the entire planning time range is covered.

[0018] The present invention has the following beneficial effects: 1. Comprehensively consider safety, comfort and efficiency, and establish the optimization problem of the overall system composed of multiple vehicles. The final planned trajectory can make the overall system optimal and improve the traffic efficiency of the current road; 2. By using the rolling time domain optimization strategy to iteratively update the trajectory of each vehicle, the overall system can adjust the trajectory of the vehicle in real time in a constantly changing and complex traffic environment to meet safe and efficient lane changes.

[0019] The technical solution of the present invention is further described in detail below through the accompanying drawings and embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] Figure 1 is a flow chart of a multi-vehicle cooperative safe lane change trajectory planning method involving multi-constraint and multi-objective coupling of the present invention; Figure 2 It is a schematic diagram of an automatic driving vehicle and its parameters in a multi-vehicle cooperative safe lane change trajectory planning method involving multi-constraint and multi-objective coupling of the present invention; Figure 3 is a motion trajectory diagram of a multi-vehicle lane-changing maneuver obtained by optimization under scenario 1 in the simulation experiment of the present invention; Figure 4 is the yaw angle of each vehicle in the multi-vehicle lane-changing process optimized under scenario 1 in the simulation experiment of the present invention. Simulation diagram with the change of simulation time step; Figure 5 is the speed of each vehicle in the multi-vehicle lane-changing process optimized in scenario 1 in the simulation experiment of the present invention. Simulation diagram with the change of simulation time step; Figure 6 is the acceleration of each vehicle in the multi-vehicle lane-changing process optimized under scenario 1 in the simulation experiment of the present invention. Simulation diagram with the change of simulation time step; Figure 7 It is the motion trajectory diagram of the multi-vehicle lane-changing maneuver obtained by optimization under scenario 2 in the simulation experiment of the present invention; Figure 8 is the yaw angle of each vehicle in the multi-vehicle lane-changing process optimized under scenario 2 in the simulation experiment of the present invention. Simulation diagram with the change of simulation time step; Fig. 9 is the speed of each vehicle in the multi-vehicle lane-changing process optimized in scenario 2 in the simulation experiment of the present invention. Simulation diagram with the change of simulation time step; Fig.10 is the acceleration of each vehicle in the multi-vehicle lane-changing process optimized in scenario 2 in the simulation experiment of the present invention. Simulation plot as the simulation time step changes. DETAILED DESCRIPTION

[0021] In order to make the purpose, technical scheme and advantages disclosed in the embodiments of the present invention clearer, the embodiments of the present invention are further described in detail in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the embodiments of the present invention and are not intended to limit the embodiments of the present invention. Based on the embodiments in this application, all other embodiments obtained by ordinary technicians in this field without making creative work are 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 throughout represent the same or similar elements or elements with the same or similar functions.

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

[0023] Like reference numerals and letters denote similar items in the following drawings, and thus, once an item is defined in one drawing, further definition and explanation thereof is not required in subsequent drawings.

[0024] The multi-vehicle lane-changing trajectory planning should take into account all vehicles in the entire system, and at the same time, various factors need to be comprehensively integrated to improve the safety, comfort, and efficiency of lane-changing maneuvers.

[0025] Based on the above analysis, the present invention is designed as follows: Figure 1 and Figure 2As shown, the multi-vehicle cooperative safe lane change trajectory planning method involving multi-constraint and multi-objective coupling includes the following steps: S1. Establish the kinematic model of the autonomous driving vehicle and discretize it; Step S1 specifically includes the following steps: S11. Assume that in the Cartesian coordinate system, The autonomous driving vehicles (CAVs) form a coordinated lane change system and number the autonomous driving vehicles. , ; S12. Ackerman's bicycle kinematic model is used to describe the driving process of the autonomous driving vehicle CAV, and a kinematic model of the autonomous driving vehicle is obtained; with the help of the bicycle kinematic model, a mathematical description of the vehicle motion can be provided without considering the forces that affect the motion.

[0026] Step S12 specifically includes the following steps: The bicycle kinematic model is used to simplify the two wheels of the front and rear axles of the autonomous vehicle CAV into a single wheel at the center of the axle, so that the autonomous vehicle CAV can be regarded as a bicycle motion model along the central axis, and the following kinematic model of the autonomous vehicle is obtained: (1) In the formula, Respectively indicate that in the current Moment The longitudinal position coordinates and lateral position coordinates of the center point of the rear axle of the autonomous driving vehicle; Indicates that at present Moment The speed of the autonomous vehicle; Indicates that at present Moment The acceleration of an autonomous vehicle; Indicates that at present Moment The rate of change of acceleration of an autonomous vehicle, Indicates that at present Moment The direction angle of the autonomous vehicle; Indicates that at present Moment The front wheel steering angle of the autonomous vehicle; Indicates the distance from the front axle to the rear axle of the autonomous vehicle; Based on the kinematic equations, the front wheel steering angle of the autonomous vehicle is determined and the change in acceleration , get the State variables of the autonomous vehicle CAV , input variable ; The discretization expression described in step S13 is as follows: (2) In the formula, ( , ) are respectively represented in Time step The longitudinal position coordinates and lateral position coordinates of the center point of the rear axle of the autonomous driving vehicle; ( , ) are respectively represented in Time step The longitudinal position coordinates and lateral position coordinates of the center point of the rear axle of the autonomous driving vehicle; Indicated in Time step The speed of the autonomous vehicle; Indicated in Time step The acceleration of an autonomous vehicle; Indicated in Time step The rate of change of acceleration of an autonomous vehicle, Indicated in Time step The direction angle of the autonomous vehicle; Indicated in Time step The front wheel steering angle of the autonomous vehicle; is the time step.

[0027] S13. In the field of trajectory planning for autonomous vehicles, continuous-time trajectory planning is extremely challenging due to its complexity. In order to simplify the problem and improve computational efficiency, the forward Euler method is used to discretize the kinematic model of the continuous-time autonomous vehicle to plan a trajectory consisting of a set of spatiotemporal state points, thereby obtaining a trajectory that satisfies both the vehicle kinematic constraints and real-time traffic. Through this strategy, a feasible trajectory that satisfies both the vehicle kinematic constraints and the real-time traffic environment can be effectively planned.

[0028] S2. Based on the kinematic model discretized in step S1, establish the constraints that the autonomous driving vehicle needs to meet when planning the trajectory; The constraints described in step S2 include the kinematic constraints that the vehicle needs to satisfy in step S1, the safety collision avoidance constraints between the boundary of the autonomous driving vehicle and the boundary of the road, the safety collision avoidance constraints between two adjacent autonomous driving vehicles, and the initial and terminal state constraints before and after lane change.

[0029] Step S2 specifically includes the following steps: S21. Kinematic constraints required to construct the vehicle motion trajectory: Formula (2) is set as the kinematic constraint that trajectory planning needs to satisfy, and the boundaries of the variables in the kinematic equation are solved as constraint conditions, where the boundary conditions of the variables are as follows: (3) In the formula, The maximum speed that an autonomous vehicle can reach during driving. is the maximum acceleration of the autonomous vehicle, is the maximum heading angle of the autonomous vehicle. The vehicle should move forward when driving on the road, so Generally set to ; is the maximum steering angle of the front wheels of the autonomous vehicle, is the maximum acceleration rate of the autonomous vehicle; S22. Safety collision avoidance constraints between the autonomous vehicle boundary and the road boundary: S221. When changing lanes, autonomous vehicles must comply with the safety collision avoidance principle, ensure a safe distance from surrounding vehicles, and avoid collisions with road guardrails to ensure the safety of vehicles and road users. To this end, the trajectory planning algorithm needs to integrate safety collision avoidance constraints to ensure that autonomous vehicles can achieve safe and efficient lane change operations while following kinematic and physical constraints: Assume that each autonomous vehicle is rectangular and the road guardrail is straight. If the four corners of the vehicle do not exceed the boundary of the road, the vehicle will not collide with the road guardrail. The upper and lower boundaries of the road boundary are respectively and , if the autonomous vehicle does not collide with the road boundary, the four vertices of the rectangle The vertical coordinate Within the limits of the road boundaries, the expression is as follows: (4) In the formula, The ordinates of the vertices of the vehicle rectangle; S222: according to the determined coordinates of the rear axle center point of the autonomous driving vehicle And heading angle , and geometric relationships to determine the four corners of the autonomous vehicle Coordinates: (5) (6) (7) (8) In the formula, Represent the four vertices of the rectangle The horizontal axis of Respectively represent The longitudinal position coordinates and lateral position coordinates of the center point of the rear axle of the autonomous driving vehicle; Indicates the horizontal distance from the center of the front axle of the autonomous vehicle to the front of the vehicle (front overhang length); Indicates the horizontal distance from the center of the rear axle to the rear of the autonomous vehicle (rear overhang length); is the width of the autonomous vehicle; S223, using formula (5) to formula (8) to solve the coordinates of the four vertices of the rectangular outline of the autonomous driving vehicle at any time; S23. Perform safety collision avoidance constraints on two adjacent autonomous driving vehicles during the lane change process: S231. In the lane change strategy of an autonomous vehicle, in addition to ensuring that the geometric boundary of the vehicle does not conflict with the road boundary, it is also necessary to ensure that the vehicle does not collide with adjacent vehicles during the lane change process. To this end, the triangle area criterion is introduced to describe the condition that two irregularly placed rectangles do not overlap, and then: (9) In the formula, represents the area of ​​the triangle formed by point Q and vertices A and B in rectangle ABCD; represents the area of ​​the triangle formed by point Q and vertices B and C in rectangle ABCD; represents the area of ​​the triangle formed by point Q and vertices C and D in rectangle ABCD; represents the area of ​​the triangle formed by point Q and vertices D and A in rectangle ABCD; represents the area of ​​rectangle ABCD; According to the triangle area criterion, if point Q satisfies formula (9), it is determined that point Q is outside rectangle ABCD, thus ensuring the collision avoidance safety of the vehicle when changing lanes. This provides a mathematically rigorous collision avoidance criterion for the trajectory planning of the autonomous vehicle, which helps to achieve safe and efficient lane change operations. S232, When the self-driving car The four vertices All in self-driving cars outside the rectangle, and the self-driving car The four vertices All in self-driving cars When the self-driving car is outside the rectangle and self-driving cars Satisfy safety collision avoidance constraints (autonomous driving vehicle and self-driving cars There will be no collision,), so any two autonomous vehicles and self-driving cars The collision avoidance constraint is described as: (10) In the formula, Self-driving car A point about self-driving cars Vertex and vertices The area of ​​the enclosed triangle; Self-driving cars A point about self-driving cars Vertex and vertices The area of ​​the enclosed triangle; Self-driving cars A point about self-driving cars Vertex and vertices The area of ​​the enclosed triangle; Self-driving cars A point about self-driving cars Vertex and vertices The area of ​​the enclosed triangle; Self-driving car The four vertices The area of ​​the enclosed rectangle; Self-driving car A point about self-driving cars Vertex and vertices The area of ​​the enclosed triangle; Self-driving cars A point about self-driving cars Vertex and vertices The area of ​​the enclosed triangle; Self-driving cars A point about self-driving cars Vertex and vertices The area of ​​the enclosed triangle; Self-driving cars A point about self-driving cars Vertex and vertices The area of ​​the enclosed triangle; Self-driving car The four vertices The area of ​​the enclosed rectangle; In this embodiment, the area of ​​each triangle is calculated by the vertices. For example, the vertex coordinates are , , triangle The area is: ; Expanding formula (10) to obtain the safety collision avoidance constraints between multiple vehicles, the autonomous driving vehicle The specific expression of the collision avoidance constraint between is: (11); S24. Building a self-driving car Initial and final state constraints before and after lane change: Assuming a self-driving car In lane at the start of a lane change The top is driving along the center line of the road at a uniform speed. , the acceleration and heading angle are both 0, and the autonomous driving car The initial state is ; After the lane change is completed, assuming that the lane change completion time is , the discrete time step is , so after discretization we get , self-driving cars In the driveway The top is driving along the center line of the road at a uniform speed. , the acceleration and heading angle are both 0, and the autonomous driving car The terminal state is ; Self-driving cars Initial state and self-driving car The terminal state of is used as the initial and terminal state constraints before and after lane change.

[0030] S3. Considering external factors, a multi-objective cost function for multiple vehicles is constructed to transform the trajectory planning problem of multiple vehicles into an optimization problem with multiple equality constraints and inequality constraints. The external factors described in step S3 include passenger comfort, vehicle traffic efficiency and safety factors.

[0031] Preferably, step S3 specifically includes the following steps: S31, the change in acceleration As an indicator of passenger comfort, when the vehicle is moving at a constant speed, the passengers do not feel any external force. However, when the acceleration changes, the passengers will lose their balance. Therefore, in order to improve riding comfort, the longitudinal and lateral It should be as small as possible. Based on this, the cost function related to passenger comfort is designed: (12) In the formula, For the The comfort cost function of an autonomous vehicle, It is The acceleration rate of the autonomous vehicle, is the maximum acceleration rate of the autonomous vehicle, is the maximum acceleration of the autonomous vehicle, is the time step; S32. Since lane change involves at least two adjacent lanes, it will affect the movement of vehicles on the two lanes, thereby reducing traffic efficiency. In order to reduce the negative impact of lane change on traffic efficiency, the deviation between the vehicle speed and the expected speed during the lane change process is minimized so that the lane change maneuver can be completed as quickly as possible. Based on this, the efficiency-related cost function is designed: (13) In the formula, For the The efficiency cost function of an autonomous vehicle, For a time step of Self-driving car speed, For the The expected speed of an autonomous vehicle, is the maximum speed; S33. Lane change maneuvers will have a certain negative impact on the current traffic flow. It is expected that in the overall system composed of multiple vehicles, vehicles without lane change requirements are not encouraged to perform lane change maneuvers, and these vehicles are driven along the current lane as much as possible. Therefore, the front wheel steering angle of the autonomous driving vehicle is used as a safety cost function, and the front wheel steering angle is minimized: (14) In the formula, For the Safety cost function for an autonomous vehicle; S34, based on the comfort cost function designed in step S31, the efficiency cost function designed in step S32, and the safety cost function designed in step S33, obtain the cost function of the entire system composed of N autonomous driving vehicles CAV for: (15) In the formula, They are all weight coefficients used to balance the proportions of different parts. In this embodiment, the weight coefficients can be adjusted according to actual needs.

[0032] S4. Under the constraints established in step S2, the multi-objective cost function constructed in step S3 is minimized to obtain the optimal trajectory for multi-vehicle coordinated lane change.

[0033] Step S4 specifically includes the following steps: S41: Based on the constraints established in step S2 and the cost function of the entire system established in step S3 , we get the constrained nonlinear optimization problem of multi-vehicle coordinated lane change: (16) S42. Using CasADi under the MATLAB platform as the solving tool (CasADi is a powerful open source software designed specifically for efficiently solving numerical optimization problems, especially in the field of optimal control. It can provide a flexible modeling environment and support a variety of optimization problem forms), the optimal solution obtained by solving formula (16) is used as the optimal trajectory of N autonomous driving vehicles. It can be seen that the optimal trajectory not only strictly follows all the preset constraints, but also realizes the optimal trajectory selection in a dynamic traffic environment.

[0034] Step S4 is followed by step S5: by means of a rolling time domain optimization method, the current state of the autonomous driving vehicle is updated at fixed intervals, and the established optimization problem is re-optimized and solved based on the latest current state.

[0035] In step S5, at the beginning of each predetermined time step, the optimal driving trajectory of the vehicle is recalculated and generated according to the current traffic conditions and vehicle status, and the cycle is iterated until the entire planning time range is covered.

[0036] Through this iterative trajectory update method, CAV can dynamically adapt to the complexity and uncertainty of the traffic environment. At each interval, CAV will execute the trajectory planning algorithm based on the latest traffic information and its own status to generate a new driving trajectory. This dynamic planning strategy enables CAV to effectively respond to challenges in a dynamic traffic environment and improve driving safety and efficiency.

[0037] Simulation experiment The following simulation verification is performed on the MATLAB simulation software: two lane change scenarios (Scenario 1 and Scenario 2) are set up, each with three autonomous vehicles , these three autonomous vehicles are used as a whole system to establish the optimization problem of multi-vehicle lane change trajectory planning.

[0038] Table 1 The initial state (position, speed) of each vehicle before changing lanes and the terminal state (position, speed) after changing lanes in scenario 1 ;

[0039] In scenario 1, all three autonomous vehicles need to change lanes. If a reasonable trajectory is not planned, there is a risk of collision between the three vehicles during the lane change process. Figure 3 and Figure 4 As shown, three vehicles of the present invention are used They all successfully completed the lane change maneuvers, and the lane change trajectories were very smooth, meeting the requirements of ride comfort.

[0040] like Figure 5 and Figure 6 As shown in the figure, in order to complete the lane change maneuver as quickly as possible, the vehicle changes lanes at the desired speed, and in order to reduce , the vehicle maintains a constant speed.

[0041] Table 2 The initial state (position, speed) of each vehicle before changing lanes and the terminal state (position, speed) after changing lanes in scenario 2 ;

[0042] ; In scenario 2, only vehicle 3 needs to change lanes to the left lane, while vehicle 2 is driving in the left lane. Figure 7-Figure 10 As shown, vehicle 3 overtook vehicle 2 at a higher speed and entered the left lane. In order to maintain an appropriate safety distance from vehicle 3 during the lane change process, vehicle 2 slightly moved to the left for a distance and then resumed normal driving, successfully completing the lane change maneuver.

[0043] In these two example scenarios, multiple vehicles successfully completed the lane change maneuver, which demonstrated the effectiveness of the proposed multi-objective and multi-constraint optimization method.

[0044] Therefore, the present invention adopts the above-mentioned multi-constraint and multi-objective coupled multi-vehicle collaborative safe lane change trajectory planning method, and constructs the nonlinear optimization problem of the entire fleet through real-time information collection and interaction of vehicles. On the premise of ensuring that the lane change process meets multiple constraints, the lane change trajectory of the entire system vehicles is obtained by minimizing the cost function. In addition, this method adopts a rolling time domain optimization strategy to cope with complex and changeable traffic environments, thereby realizing efficient, comfortable and safe lane change maneuvers for vehicles.

[0045] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that they can still modify or replace the technical solution of the present invention with equivalents, and these modifications or equivalent replacements cannot cause the modified technical solution to deviate from the spirit and scope of the technical solution of the present invention.

Claims

1. A multi-vehicle cooperative safe lane change trajectory planning method involving multi-constraint and multi-objective coupling, characterized by: The following steps are involved: S1. Establish the kinematic model of the autonomous driving vehicle and discretize it; S2. Based on the kinematic model discretized in step S1, establish the constraints that the autonomous driving vehicle needs to meet when planning the trajectory; S3. Considering external factors, a multi-objective cost function for multiple vehicles is constructed to transform the trajectory planning problem of multiple vehicles into an optimization problem with multiple equality constraints and inequality constraints. S4. Under the constraints established in step S2, the multi-objective cost function constructed in step S3 is minimized to obtain the optimal trajectory for multi-vehicle coordinated lane change.

2. The multi-vehicle cooperative safe lane change trajectory planning method involving multi-constraint and multi-objective coupling according to claim 1 is characterized in that: Step S1 specifically includes the following steps: S11. Assume that in the Cartesian coordinate system, The autonomous driving vehicles (CAVs) form a coordinated lane change system and number the autonomous driving vehicles. , ; S12, using Ackerman's bicycle kinematics model to describe the driving process of the autonomous vehicle CAV, and obtaining a kinematic model of the autonomous vehicle; S13. Use the forward Euler method to discretize the kinematic model of the continuous-time autonomous driving vehicle to plan a trajectory consisting of a set of spatiotemporal state points, thereby obtaining a trajectory that satisfies both the vehicle kinematic constraints and real-time traffic.

3. The multi-vehicle cooperative safe lane change trajectory planning method involving multi-constraint and multi-objective coupling according to claim 2 is characterized in that: Step S12 specifically includes the following steps: The bicycle kinematic model is used to simplify the two wheels of the front and rear axles of the autonomous vehicle CAV into a single wheel at the center of the axle, so that the autonomous vehicle CAV can be regarded as a bicycle motion model along the central axis, and the following kinematic model of the autonomous vehicle is obtained: (1) In the formula, Respectively indicate that at the current Moment The longitudinal position coordinates and lateral position coordinates of the center point of the rear axle of the autonomous driving vehicle; Indicates that at present Moment The speed of the autonomous vehicle; Indicates that at present Moment The acceleration of an autonomous vehicle; Indicates that at present Moment The rate of change of acceleration of an autonomous vehicle, Indicates that at present Moment The direction angle of the autonomous vehicle; Indicates that at present Moment The front wheel steering angle of the autonomous vehicle; Indicates the distance from the front axle to the rear axle of the autonomous vehicle; Based on the kinematic equations, the front wheel steering angle of the autonomous vehicle is determined and the change in acceleration , get the State variables of the autonomous vehicle CAV , input variable ; The discretization expression described in step S13 is as follows: (2) In the formula, ( , ) are respectively expressed in Time step The longitudinal position coordinates and lateral position coordinates of the center point of the rear axle of the autonomous driving vehicle; ( , ) are respectively expressed in Time step The longitudinal position coordinates and lateral position coordinates of the center point of the rear axle of the autonomous driving vehicle; Indicated in Time step The speed of the autonomous vehicle; Indicated in Time step The acceleration of an autonomous vehicle; Indicated in Time step The rate of change of acceleration of an autonomous vehicle, Indicated in Time step The direction angle of the autonomous vehicle; Indicated in Time step The front wheel steering angle of the autonomous vehicle; is the time step.

4. The multi-vehicle cooperative safe lane change trajectory planning method involving multi-constraint and multi-objective coupling according to claim 3 is characterized in that: The constraints described in step S2 include the kinematic constraints that the vehicle needs to satisfy in step S1, the safety collision avoidance constraints between the boundary of the autonomous driving vehicle and the boundary of the road, the safety collision avoidance constraints between two adjacent autonomous driving vehicles, and the initial and terminal state constraints before and after lane change.

5. The multi-vehicle cooperative safe lane change trajectory planning method involving multi-constraint and multi-objective coupling according to claim 4 is characterized in that: Step S2 specifically includes the following steps: S21. Kinematic constraints required to construct the vehicle motion trajectory: Formula (2) is set as the kinematic constraint that trajectory planning needs to satisfy, and the boundaries of the variables in the kinematic equation are solved as constraint conditions, where the boundary conditions of the variables are as follows: (3) In the formula, The maximum speed that an autonomous vehicle can reach during driving. is the maximum acceleration of the autonomous vehicle, is the maximum heading angle of the autonomous vehicle; is the maximum steering angle of the front wheels of the autonomous vehicle, is the maximum acceleration rate of the autonomous vehicle; S22. Safety collision avoidance constraints between the autonomous vehicle boundary and the road boundary: S221. Integrated safety collision avoidance constraints: Assume that each autonomous vehicle is rectangular, the road guardrail is straight, and the upper and lower bounds of the road boundary are and , if the autonomous vehicle does not collide with the road boundary, the four vertices of the rectangle The vertical coordinate Within the limits of the road boundaries, the expression is as follows: (4) In the formula, The ordinates of the vertices of the vehicle rectangle; S222: according to the determined coordinates of the rear axle center point of the autonomous driving vehicle And heading angle , and geometric relationships to determine the four corners of the autonomous vehicle Coordinates: (5) (6) (7) (8) In the formula, Represent the four vertices of the rectangle The horizontal axis of Respectively represent The longitudinal position coordinates and lateral position coordinates of the center point of the rear axle of the autonomous driving vehicle; Indicates the horizontal distance from the center of the front wheel axle to the front of the autonomous vehicle; Indicates the horizontal distance from the center of the rear axle of the autonomous vehicle to the rear of the vehicle; is the width of the autonomous vehicle; S223, using formula (5) to formula (8) to solve the coordinates of the four vertices of the rectangular outline of the autonomous driving vehicle at any time; S23. Perform safety collision avoidance constraints on two adjacent autonomous driving vehicles during the lane change process: S231. Introducing the triangle area criterion to describe the conditions for non-overlap between two irregularly placed rectangles, we have: (9) In the formula, represents the area of ​​the triangle formed by point Q and vertices A and B in rectangle ABCD; represents the area of ​​the triangle formed by point Q and vertices B and C in rectangle ABCD; represents the area of ​​the triangle formed by point Q and vertices C and D in rectangle ABCD; represents the area of ​​the triangle formed by point Q and vertices D and A in rectangle ABCD; represents the area of ​​rectangle ABCD; According to the triangle area criterion, if point Q satisfies formula (9), then point Q is determined to be outside rectangle ABCD; S232, When the self-driving car The four vertices All in self-driving cars outside the rectangle, and the self-driving car The four vertices All in self-driving cars When the self-driving car is outside the rectangle and self-driving cars Satisfy the safety collision avoidance constraint, therefore, any two autonomous vehicles and self-driving cars The collision avoidance constraint is described as: (10) In the formula, Self-driving car A point about self-driving cars Vertex and vertices The area of ​​the enclosed triangle; Self-driving cars A point about self-driving cars Vertex and vertices The area of ​​the enclosed triangle; Self-driving cars A point about self-driving cars Vertex and vertices The area of ​​the enclosed triangle; Self-driving cars A point about self-driving cars Vertex and vertices The area of ​​the enclosed triangle; Self-driving car The four vertices The area of ​​the enclosed rectangle; Self-driving car A point about self-driving cars Vertex and vertices The area of ​​the enclosed triangle; Self-driving cars A point about self-driving cars Vertex and vertices The area of ​​the enclosed triangle; Self-driving cars A point about self-driving cars Vertex and vertices The area of ​​the enclosed triangle; Self-driving cars A point about self-driving cars Vertex and vertices The area of ​​the enclosed triangle; Self-driving car The four vertices The area of ​​the enclosed rectangle; Expanding formula (10) to obtain the safety collision avoidance constraints between multiple vehicles, the autonomous driving vehicle The specific expression of the collision avoidance constraint between is: （11）； S24. Building a self-driving car Initial and final state constraints before and after lane change: Assuming a self-driving car In lane at the start of a lane change The top is moving in a straight line along the center line of the road at a constant speed. , the acceleration and heading angle are both 0, and the autonomous driving car The initial state is ; After the lane change is completed, assuming that the lane change completion time is , the discrete time step is , so after discretization we get , self-driving cars In the driveway The top is moving in a straight line along the center line of the road at a constant speed. , the acceleration and heading angle are both 0, and the autonomous driving car The terminal state is ; Self-driving cars Initial state and self-driving car The terminal state is used as the initial and terminal state constraints before and after lane change.

6. The multi-vehicle cooperative safe lane change trajectory planning method involving multi-constraint and multi-objective coupling according to claim 5 is characterized in that: The external factors described in step S3 include passenger comfort, vehicle traffic efficiency and safety factors.

7. The multi-vehicle cooperative safe lane change trajectory planning method involving multi-constraint and multi-objective coupling according to claim 6 is characterized in that: Step S3 specifically includes the following steps: S31, the change in acceleration As an indicator to measure passenger comfort, a cost function related to passenger comfort is designed: （12） In the formula, For the The comfort cost function of an autonomous vehicle, It is The acceleration rate of the autonomous vehicle, is the maximum acceleration rate of the autonomous vehicle, is the maximum acceleration of the autonomous vehicle, is the time step; S32. Cost function related to design efficiency: （13） In the formula, For the The efficiency cost function of an autonomous vehicle, For a time step of Self-driving car speed, For the The expected speed of an autonomous vehicle, is the maximum speed; S33. Take the front wheel steering angle of the autonomous driving vehicle as the safety cost function and minimize the front wheel steering angle: （14） In the formula, For the Safety cost function for an autonomous vehicle; S34, based on the comfort cost function designed in step S31, the efficiency cost function designed in step S32, and the safety cost function designed in step S33, obtain the cost function of the entire system composed of N autonomous driving vehicles CAV for: （15） In the formula, are all weight coefficients.

8. The multi-vehicle cooperative safe lane change trajectory planning method involving multi-constraint and multi-objective coupling according to claim 7 is characterized in that: Step S4 The specific steps include: S41: Based on the constraints established in step S2 and the cost function of the entire system established in step S3 , we get the constrained nonlinear optimization problem of multi-vehicle coordinated lane change: （16） S42. Using CasADi under the MATLAB platform as a solving tool, the optimal solution obtained by solving formula (16) is used as the optimal trajectory of N autonomous driving vehicles.

9. The multi-vehicle cooperative safe lane change trajectory planning method involving multi-constraint and multi-objective coupling according to claim 8 is characterized in that: Step S4 is followed by step S5: by means of a rolling time domain optimization method, the current state of the autonomous driving vehicle is updated at fixed intervals, and the established optimization problem is re-optimized and solved based on the latest current state.

10. The multi-vehicle cooperative safe lane change trajectory planning method involving multi-constraint and multi-objective coupling according to claim 9, characterized in that: In step S5, at the beginning of each predetermined time step, the optimal driving trajectory of the vehicle is recalculated and generated according to the current traffic conditions and vehicle status, and the cycle is iterated until the entire planning time range is covered.

Images