Multi-vehicle collaborative trajectory planning method for urban roads based on roadside guidance

Abstract

The present invention relates to a method for multi-vehicle collaborative trajectory planning on urban roads based on roadside guidance, and belongs to the field of intelligent transportation technology. It solves the technical problem of low efficiency of single-vehicle trajectory planning under the existing technology. The key points of its technical solution are to generate networked vehicle control instructions through a multi-vehicle collaborative trajectory planning algorithm deployed on a roadside collaborative control unit, and send them to the vehicle end for execution. Specifically, the multi-vehicle collaborative trajectory planning algorithm projects the vehicles within the collaborative range into a relative coordinate system, and then decomposes the trajectory planning into two processes: path planning and speed planning. In the control stage, the vehicle's collaborative driving rules, strategies, collaborative groups, surrounding vehicle status and other information are comprehensively considered to obtain the multi-vehicle collaborative optimal trajectory that meets the requirements of safety, comfort, efficiency and the like. The present invention can realize collaborative trajectory planning of networked vehicles in a mixed traffic environment, and has strong practicality and broad commercial application scenarios.

Description

Technical Field

[0001] The present invention belongs to the field of intelligent transportation technology and relates to a method for multi-vehicle collaborative trajectory planning on urban roads based on roadside guidance. Background Art

[0002] With the acceleration of urbanization and the continued growth of motor vehicle ownership, traffic congestion and accidents are becoming increasingly prominent, seriously impacting the quality of life of urban residents and the efficiency of urban operations. Traditional traffic management methods, such as signal control and traffic signs and markings, are no longer able to cope with increasingly complex traffic conditions. Intelligent Transportation Systems (ITS) have emerged to address this issue, aiming to achieve intelligent, efficient, and safe transportation systems through advanced information technology, communication technology, and control technology.

[0003] In intelligent transportation systems, vehicle trajectory planning is a key technology for achieving autonomous driving and coordinated traffic control. Traditional vehicle trajectory planning methods rely primarily on onboard sensors and computing platforms, sensing the surrounding environment and generating vehicle trajectories. However, in complex urban traffic environments, relying solely on onboard sensing suffers from issues such as incomplete information acquisition and limited computing resources, making efficient and safe vehicle trajectory planning difficult.

[0004] In recent years, the development of vehicle-to-everything (V2X) technology has provided new solutions to these problems. Through information exchange between vehicles (V2V), vehicles and infrastructure (V2I), and vehicles and pedestrians (V2P), V2X enables collaborative perception, decision-making, and control between vehicles and road infrastructure, a concept often referred to as vehicle-infrastructure collaboration. Trajectory planning methods based on V2X can fully leverage the rich environmental information captured by roadside sensing devices (such as cameras, radars, lidars, and integrated radar and vision cameras), compensating for the shortcomings of on-board sensing and improving the accuracy and safety of trajectory planning.

[0005] Furthermore, multi-vehicle collaborative trajectory planning is an important tool for addressing urban traffic congestion. Through multi-vehicle collaboration, dynamic path allocation, speed coordination, and trajectory optimization can be achieved between vehicles, effectively reducing conflicts and delays, and improving road efficiency. However, traditional multi-vehicle collaborative trajectory planning methods typically utilize a centralized control architecture, which suffers from high computational complexity and significant communication latency, making them difficult to meet real-time requirements. Summary of the Invention

[0006] In view of this, the purpose of the present invention is to provide a multi-vehicle collaborative trajectory planning method for urban roads based on roadside guidance, aiming to make full use of vehicle-road collaborative technology, combine roadside perception information and multi-vehicle collaborative strategies, and achieve efficient and safe urban traffic operation.

[0007] In order to achieve the above object, the present invention provides the following technical solutions:

[0008] The method for multi-vehicle collaborative trajectory planning on urban roads based on roadside guidance includes the following steps:

[0009] S1. Deploy roadside sensing equipment, roadside collaborative control units, and roadside units (RSUs) on urban roads. Vehicles on the roads include connected vehicles and human-driven vehicles (HDVs).

[0010] Among them, connected vehicles include L4 autonomous driving vehicles with autonomous driving and connected communication functions and L2 autonomous driving vehicles with assisted driving and connected communication functions;

[0011] S2. Access external input data: cooperative driving rules, surrounding vehicle intentions, surrounding vehicle trajectory information, cooperative vehicle and key vehicle information, behavioral decision information, high-precision maps, and global path planning information;

[0012] S3. Establish a relative coordinate system based on key vehicles, global paths, static risk fields, and dynamic risk fields according to external input data;

[0013] S4. Plan the lane change trajectory using a quintic polynomial fitting method, where target constraint 1 includes safety constraints (static obstacles), regulations (time on the lane, continuous lane change), and comfort constraints (avoiding sharp turns).

[0014] S5. Use the improved Jump Point Search (JPS) algorithm to perform speed planning on the planned lane change path. Target Constraint 2 includes safety constraints (dynamic obstacles), regulations (speed limits and traffic rules), comfort constraints (acceleration limits), and efficiency constraints (time to traffic control allowed value less than or equal to 3 seconds).

[0015] S6. The planned path and speed are combined into a planned trajectory, and the trajectory information is sent to the vehicle controller to achieve multi-vehicle coordinated lane change.

[0016] Furthermore, in said S2, external input data is analyzed: cooperative driving rules refer to the constraints on the cooperative driving behavior of connected vehicles based on existing traffic laws and regulations, including what kind of following distance the connected vehicle maintains during stable following, and in what range the pre-collision time (Time to Collision, TTC) between the connected vehicle and the vehicle in front is maintained during maneuvers, i.e., lane changing, overtaking and following; the intentions and trajectory information of surrounding vehicles are predicted through the vehicle's historical trajectory data; cooperative vehicles refer to connected vehicles that participate in cooperative control in this cooperative process; key vehicles refer to HDVs that do not participate in the cooperative process but have an impact on the trajectory planning of connected vehicles; behavioral decision information represents the recommended behavior and priority of each connected vehicle; the global path is a road center reference line with a starting point and end point determined in advance based on a high-precision map, and the vehicle travels along the reference line during normal driving.

[0017] Furthermore, the step S3 includes the following steps:

[0018] S31. Build a vehicle collection:

[0019] Vehicle C k is a cooperative vehicle, k∈K represents the vehicle number; H l is the key vehicle, l∈L represents the vehicle number;

[0020] S32. Constructing a vehicle risk field for collaborative vehicles and key vehicles:

[0021] First, a static risk field for the vehicle is constructed. The relative distance and approach direction between the vehicle and the obstacle are considered to characterize the risk caused by the static obstacle. The formula is as follows:

[0022]

[0023] Among them, U s is the risk field generated by the stationary obstacle vehicle, and the position of the vehicle is (x obs ,y obs ); w represents the field strength coefficient greater than 0; ρ xobs and ρ yobs Represent the shape equations of the obstacle in the longitudinal and lateral directions, ρ xobs =k x L xobs ,ρ yobs =k y L yobs ;k x and k y Respectively represent coefficients greater than 0, L xobs and L yobs Respectively represent the horizontal and longitudinal lengths of the obstacle vehicle;

[0024] Then, a vehicle dynamic risk field is constructed to characterize the risk caused by moving obstacles that may collide with the ego vehicle. It is related to the relative distance, relative speed, and approach direction of the ego vehicle. The formula is as follows:

[0025]

[0026] Among them, U d is the risk field generated by the movement of the obstacle vehicle; ρ v represents the relative velocity equation, ρ v =k v |vv obs |;k v represents a coefficient greater than 0; Indicates the direction of relative motion; when v≥v obs hour, When v <v obs hour,

[0027] S33. Construct relative coordinate system:

[0028] First, define the lane centerline as:

[0029] P i =[(x 1,i ,y 1,i ),(x 2,i ,y 2,i ),…,(x j,i ,y j,i ),…,(x M,i ,y M,i )] (3)

[0030] Among them, P i ∈P, i=0,1,2…,N is the lane number, the innermost lane i=0, arranged from the inside to the outside, N is the total number of lanes; (x j,i ,y j,i ) is the lane center line P i The coordinates of the points on ; P is the lane centerline set;

[0031] Then, take the last vehicle in the current driving direction of the main vehicle or cooperative vehicle as the starting point of the X-axis boundary and the outer boundary of the road as the starting point of the Y-axis to establish a Cartesian relative coordinate system;

[0032] Secondly, the center line of the lane where the main vehicle is located is used as the reference curve to establish the Frenet coordinate system. The construction method is as follows:

[0033] d(s)=a5s 5 +a4s 4 +a3s 3 +a2s 2 +a1s+a0,s∈[sini ,s fin ] (4)

[0034] The following coordinate transformation is achieved by formula (4):

[0035] Lane line P i Converted to Frenet coordinates, recorded as And satisfy the following formula:

[0036]

[0037] st s left ≤s j,i ≤s right +s pre (5-1)

[0038]

[0039] in, is the lane centerline coordinate in the Frenet coordinate system; s left and s right Represent the leftmost and rightmost cooperative vehicles C on the road respectively k The horizontal coordinate is for the east-west lane; s pre Represents the cooperative vehicle C k The distance of a single planning, Δt is the time of forward planning, Indicates vehicle C k speed;

[0040] The coordinates of the cooperative vehicles Transformed into Frenet coordinates, denoted as The key vehicle coordinates Transformed into Frenet coordinates, denoted as

[0041] Further, the S4 includes the following steps:

[0042] S41. Based on the relative coordinate system, the ego vehicle, key vehicle, and cooperative vehicle are projected into SL, and then the paths of the vehicles are planned in descending order of vehicle priority. In the Frenet coordinate system, the relationship between the lane centerline arc length s and the lateral offset is given by formula (4) in S33.

[0043] S42, determine the boundary conditions, solve the undetermined coefficients of the quintic polynomial, and determine the lane change path; the initial state is where d ini , v ini and a ini are the initial lateral offset, initial velocity and initial acceleration of the vehicle respectively; the final state is where d fin , v fin and a fin are the final lateral offset, final velocity and final acceleration of the vehicle respectively; the length of the path is s pre ; Then formula (4) is solved by the following matrix:

[0044]

[0045] The planned path of the vehicle is solved by formulas (4) to (7).

[0046] Furthermore, the step S5 includes the following steps:

[0047] S51, based on the lane change path planned in S4 and the information of vehicles around the vehicle, construct the ST space, rasterize the ST space, and use the position s(t0), velocity v(t0) and acceleration a(t0) of the obstacle at the current time t0 to calculate the obstacle at any future time t k location;

[0048]

[0049] S52, the speed feasible domain and the search target are determined. After obtaining the ST search space, some grids are unavailable due to being occupied by obstacles, and the remaining unoccupied grids form free areas. These free areas together constitute the feasible space for speed planning;

[0050] S53, speed curve planning based on JPS algorithm;

[0051] When using the A* algorithm for path search, the cost function of each node usually takes the following form:

[0052] f(n)=g(n)+h(n) (9)

[0053] Where f(n) represents the total cost of the current node n; g(n) represents the cumulative cost from the starting node to the current node n; h(n) represents the estimated cost from the current node to the target node;

[0054] The JPS algorithm is used to constrain the expansion direction of the node. In the ST space, the vehicle's motion state is restricted to moving only to the right, upward, or along the upper right diagonal direction. During the algorithm's iteration process, the cost of the current node is the cost of its parent node plus the cost from the parent node to the current node, that is:

[0055]

[0056] Among them, the cost from the parent node to the current node is:

[0057]

[0058] Among them, s i Indicates the vertical position of the current node, s target is the longitudinal position of the target point; parameter ω s Represents the weight of the longitudinal position, which is used to encourage the vehicle to move from the current position to the target position as quickly as possible; s′ i represents the current longitudinal velocity, and s′ target It represents the expected longitudinal velocity of the target point; ω v is the weight of the longitudinal speed, guiding the vehicle to gradually approach the desired speed; s″ i represents the current longitudinal acceleration, ω a is the weight of the longitudinal acceleration, which is used to constrain the acceleration to improve driving comfort; in addition, d obs Indicates the minimum longitudinal distance between the vehicle and the obstacle on the ST map; set a value δ = 0.01; ω obs The weight representing the security cost;

[0059] S54. Speed ​​curve optimization based on quadratic programming. Since the speed curve generated by the JPS algorithm is composed of broken line segments, the sudden change of speed will significantly affect the riding comfort. The speed curve generated in the ST space is optimized. In the ST graph, the speed curve is represented as a mapping function relative to the timestamp and the stations along the lane, represented as v i =f(t i ,s i ), where t i and s i represents the time and arc length along the path; during the optimization process, ride comfort, acceleration smoothness, and traffic efficiency are comprehensively considered to achieve a balanced performance. The time series t=[t0,t1,…,t n ] T remains unchanged, and for the position sequence s=[s0,s1,…,s n ] T Adjust and optimize; for any point on the speed curve (t i ,s i ), position s is obtained by vertical expansion i The feasible space range (up i ,bottom i );

[0060] After determining the feasible region of speed planning, the discrete position sequence [s0,s1,…,s n ] T As the core goal of optimization; at this time, the optimization goal of the speed planning problem is expressed as follows:

[0061]

[0062] When optimizing the objective function, driving efficiency and ride comfort are comprehensively considered, and the deviation index between the rough solution of the speed curve is introduced. The specific objective function is expressed as:

[0063] f(s)=cost ref +cost exp +cost a +cost j (13)

[0064] Among them, cost ref represents the deviation cost between the optimized velocity curve and the rough solution, aiming to make the optimized curve as close as possible to the section where the rough solution is located; cost exp is the expected speed cost, which is used to make the optimized vehicle speed close to the expected driving speed; cost a and cost j They represent the cost of acceleration and jerk, respectively, and are used to improve the smoothness of the speed curve, thereby improving ride comfort. The calculation formulas for these costs are as follows:

[0065]

[0066]

[0067] When optimizing the speed curve, ensure that the vehicle does not collide with obstacles during driving and meet the vehicle's dynamic constraints;

[0068] Constrain the longitudinal position of the vehicle to avoid interference with obstacles:

[0069] bottom i ≤s i ≤up i (18)

[0070] Satisfy the vehicle's stability constraints:

[0071]

[0072] Among them, a y,max is the maximum lateral acceleration allowed when the vehicle is moving, k max is the maximum curvature of the path, v limit The speed limit for the road;

[0073] The longitudinal acceleration and jerk must not exceed their respective upper and lower limits:

[0074] a x,min ≤s″ i ≤a x,max (20)

[0075] j x,min ≤s″′ i ≤j x,max (twenty one)

[0076] Ensure that the starting point of the speed curve complies with the current state constraints of the vehicle to avoid sudden speed changes when the vehicle is driving along the speed curve; restrict the state of the starting point of the speed curve;

[0077] s0=s start ,s′0=s′ start ,s″0=s″ start (twenty two).

[0078] The beneficial effects of the present invention are: through the multi-vehicle collaborative trajectory planning method based on roadside guidance, it is possible to achieve efficient collaboration between connected vehicles and manually driven vehicles in complex urban traffic environments, reduce conflicts and delays between vehicles, and significantly improve road traffic efficiency. At the same time, the rich environmental information obtained by roadside sensing equipment is used to make up for the shortcomings of on-board perception, improve the accuracy and safety of trajectory planning, effectively reduce the incidence of traffic accidents, and optimize the speed curve through secondary planning, reduce speed mutations, improve acceleration smoothness, and thus improve ride comfort. This method can adapt to traffic scenarios with different connected vehicle penetration rates. As the connected vehicle penetration rate increases, the intersection traffic efficiency can be further improved, giving full play to the advantages of information interaction and collaborative traffic of connected vehicles. It has strong practicality in mixed traffic environments, can be widely used in urban traffic management, and has broad commercial application prospects.

[0079] Other advantages, objects, and features of the present invention will be described in part in the following description and, in part, will be apparent to those skilled in the art upon examination of the following description or may be learned from practice of the present invention. The objects and other advantages of the present invention may be realized and obtained through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0080] In order to make the purpose, technical solutions and advantages of the present invention more clear, the present invention will be described in detail below with reference to the accompanying drawings, in which:

[0081] Figure 1 is the algorithm flow chart;

[0082] Figure 2 Schematic diagram of static and dynamic risk field;

[0083] Figure 3 is a schematic diagram of the relative coordinate system;

[0084] Figure 4 This is a schematic diagram of multi-vehicle path planning;

[0085] Figure 5 Schematic diagram of feasible space for speed planning;

[0086] Figure 6 A schematic diagram for speed planning;

[0087] Figure 7 Initialize the position diagram of the vehicle in Carla;

[0088] Figure 8 This is a schematic diagram of the algorithm running results in Carla. DETAILED DESCRIPTION

[0089] The following describes the embodiments of the present invention by means of specific examples, and those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed in various ways based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments are only schematic illustrations of the basic concept of the present invention, and the following embodiments and features in the embodiments can be combined with each other without conflict.

[0090] Among them, the accompanying drawings are only for illustrative purposes and represent only schematic diagrams rather than actual pictures, and should not be understood as limiting the present invention. In order to better illustrate the embodiments of the present invention, some parts of the accompanying drawings may be omitted, enlarged or reduced, and do not represent the dimensions of actual products. For those skilled in the art, it is understandable that some well-known structures and their descriptions may be omitted in the accompanying drawings.

[0091] The same or similar numbers in the drawings of the embodiments of the present invention correspond to the same or similar parts; in the description of the present invention, it should be understood that if there are terms such as "upper", "lower", "left", "right", "front", "back", etc. indicating directions or positional relationships, they are based on the directions or positional relationships shown in the drawings. They are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific direction, be constructed and operate in a specific direction. Therefore, the terms describing the positional relationship in the drawings are only used for illustrative purposes and cannot be understood as limiting the present invention. For ordinary technicians in this field, the specific meanings of the above terms can be understood according to specific circumstances.

[0092] This example is based on the following assumptions:

[0093] (1) Deploy roadside sensing equipment, roadside cooperative control units, and roadside units on urban roads. Vehicles on the roads include intelligent connected vehicles and manually driven vehicles. Connected vehicles include Level 4 autonomous driving vehicles with autonomous driving and connected communication functions and Level 2 autonomous driving vehicles with assisted driving and connected communication functions.

[0094] (2) Externally accessed data: collaborative driving rules, surrounding vehicle intentions, surrounding vehicle trajectory information, collaborative vehicle and key vehicle information, behavioral decision information, high-precision maps, and global path planning information can all be directly applied.

[0095] like Figure 1 The figure shows a multi-vehicle collaborative trajectory planning method for urban roads based on roadside guidance in an intelligent connected environment. The specific contents of this method are as follows:

[0096] S1. Deploy roadside sensing equipment, roadside collaborative control units, and roadside units on urban roads. Vehicles on these roads include both connected vehicles and manually driven vehicles.

[0097] Among them, connected vehicles include L4 autonomous driving vehicles with autonomous driving and connected communication functions and L2 autonomous driving vehicles with assisted driving and connected communication functions;

[0098] S2. Access external input data: cooperative driving rules, surrounding vehicle intentions, surrounding vehicle trajectory information, cooperative vehicle and key vehicle information, behavioral decision information, high-precision maps, and global path planning information;

[0099] S3. Establish a relative coordinate system based on key vehicles, global paths, static risk fields, and dynamic risk fields according to external input data;

[0100] S31. Construct a vehicle set, record vehicle C k is a cooperative vehicle, k∈K represents the vehicle number; H l is the key vehicle, l∈L represents the vehicle number;

[0101] S32. Construct a vehicle risk field for cooperative vehicles and key vehicles. First, construct a static vehicle risk field. Consider the relative distance and approach direction between the ego vehicle and the obstacle to characterize the risk caused by static obstacles. The formula is as follows:

[0102]

[0103] Among them, U s is the risk field generated by the obstacle vehicle (stationary), and the position of the vehicle is (x obs ,y obs ); w represents the field strength coefficient greater than 0; ρ xobs and ρ yobsRepresent the shape equations of the obstacle in the longitudinal and lateral directions, ρ xobs =k x L xobs , ρ yobs =k y L yobs ;k x and k y Respectively represent coefficients greater than 0, L xobs and L yobs Respectively represent the horizontal and longitudinal lengths of the obstacle vehicle.

[0104] Then, a vehicle dynamic risk field is constructed, which characterizes the risk caused by moving obstacles that may collide with the ego vehicle. It is related to the relative distance, relative speed, and approach direction of the ego vehicle. The formula is as follows:

[0105]

[0106] Among them, U d is the risk field generated by the obstacle vehicle (movement); ρ v represents the relative velocity equation, ρ v =k v |vv obs |;k v represents a coefficient greater than 0; Indicates the direction of relative motion; when v≥v obs hour, When v <v obs hour, The static and dynamic obstacle risk field in S32 is as follows Figure 2 shown.

[0107] S33. Construct a relative coordinate system. First, define the lane centerline as:

[0108] P i =[(x 1,i ,y 1,i ),(x 2,i ,y 2,i ),…,(x j,i ,y j,i ),…,(x M,i ,y M,i )], (3)

[0109] Among them, P i ∈P, i=0,1,2…,N is the lane number, the innermost lane i=0, arranged from the inside to the outside, N is the total number of lanes; (x j,i ,y j,i ) is the lane center line P i The coordinates of the points on ; P is the lane centerline set.

[0110] Then, take the last vehicle in the current driving direction of the main vehicle or cooperative vehicle as the starting point of the X-axis boundary and the outer boundary of the road as the starting point of the Y-axis to establish a Cartesian relative coordinate system;

[0111] Secondly, the center line of the lane where the main vehicle is located is used as the reference curve to establish the Frenet coordinate system. The construction method is as follows:

[0112] d(s)=a5s 5 +a4s 4 +a3s 3 +a2s 2 +a1s+a0,s∈[s ini ,s fin ], (4)

[0113] The following coordinate transformation is achieved by formula (4):

[0114] Lane line P i Converted to Frenet coordinates, recorded as And satisfy the following formula:

[0115]

[0116] st s left ≤s j,i ≤s right +s pre , (5-1)

[0117]

[0118] in, is the lane centerline coordinate in the Frenet coordinate system; s left and s right Represent the leftmost and rightmost cooperative vehicles C on the road respectively k The horizontal coordinate (taking the east-west lane as an example); s pre Represents the cooperative vehicle C k The distance of a single planning, Δt is the time of forward planning, v Ck Indicates vehicle C k speed.

[0119] The coordinates of the cooperative vehicles Transformed into Frenet coordinates, denoted as The key vehicle coordinates Transformed into Frenet coordinates, denoted as The relative coordinate system established in S33 is as follows Figure 3 shown.

[0120] S4. Plan the lane change trajectory using a quintic polynomial fitting method, where target constraint 1 includes safety constraints (static obstacles), regulations (time on the lane, continuous lane change), and comfort constraints (avoiding sharp turns).

[0121] S41. Based on the relative coordinate system, the ego vehicle, key vehicle, and cooperative vehicle are projected into SL, and then the paths of the vehicles are planned in descending order of vehicle priority. In the Frenet coordinate system, the relationship between the lane centerline arc length s and the lateral offset is given by equation (4) in S33.

[0122] S42, determine the boundary conditions, solve the undetermined coefficients of the quintic polynomial, and determine the lane change path; the initial state is where d ini , v ini and a ini are the initial lateral offset, initial velocity and initial acceleration of the vehicle respectively; the final state is where d fin , v fin and a fin are the final lateral offset, final velocity and final acceleration of the vehicle respectively; the length of the path is s pre ; Then formula (4) can be solved by the following matrix:

[0123]

[0124] The planned path of the vehicle can be solved by formulas (4), (6), and (7). The path planning based on the fifth-order polynomial in S42 is as follows: Figure 4 shown.

[0125] S5. Use the improved Jump Point Search (JPS) algorithm to perform speed planning on the planned lane change path. Target Constraint 2 includes safety constraints (dynamic obstacles), regulations (speed limits and traffic rules), comfort constraints (acceleration limits), and efficiency constraints (time to traffic control allowed value less than or equal to 3 seconds).

[0126] S51, based on the lane change path planned in S4 and the information of vehicles around the vehicle, construct the ST space, rasterize the ST space, and use the position s(t0), velocity v(t0) and acceleration a(t0) of the obstacle at the current time t0 to calculate the obstacle at any future time t k location.

[0127]

[0128] S52, speed feasible domain and search target are determined. After obtaining the ST search space, some grids are unavailable due to obstacles, and the remaining unoccupied grids form free areas. These free areas together constitute the feasible space for speed planning. The speed planning drivable space constructed in S51 is as follows: Figure 5 shown.

[0129] S53. Speed ​​curve planning is performed based on the JPS algorithm. As an advanced variant of the A* algorithm, the JPS algorithm not only inherits the graph search capability of A*, but also breaks the path symmetry in a systematic way.

[0130] When using the A* algorithm for path search, the cost function of each node usually takes the following form:

[0131] f(n)=g(n)+h(n), (9)

[0132] Among them, f(n) represents the total cost of the current node n; g(n) represents the cumulative cost from the starting node to the current node n; h(n) represents the estimated cost from the current node to the target node.

[0133] The JPS algorithm requires constraints on the expansion direction of the node. In ST space, the vehicle's motion state should be limited to moving only to the right, upward, or along the upper right diagonal direction. During the algorithm's iteration process, the cost of the current node is the cost of its parent node plus the cost from the parent node to the current node, that is:

[0134]

[0135] Among them, the cost from the parent node to the current node is:

[0136]

[0137] Among them, s i Indicates the vertical position of the current node, s target is the longitudinal position of the target point. Parameter ω s Represents the weight of the longitudinal position, which is used to encourage the vehicle to move from the current position to the target position as quickly as possible; s′ i represents the current longitudinal velocity, and s′ target Then it represents the expected longitudinal velocity of the target point. v is the weight of the longitudinal speed, which can guide the vehicle to gradually approach the desired speed; s″ i represents the current longitudinal acceleration, ω a is the weight of the longitudinal acceleration, which is used to constrain the acceleration to improve driving comfort. In addition, d obsIndicates the minimum longitudinal distance between the vehicle and the obstacle on the ST map. To ensure the validity of this item, a small value δ = 0.01 is set. Finally, ω obs The weight representing the security cost.

[0138] S54. Speed ​​curve optimization based on quadratic programming. Since the speed curve generated by the JPS algorithm is composed of broken line segments, a sudden change in speed will significantly affect the ride comfort. Therefore, it is necessary to optimize the speed curve generated in the ST space. In the ST graph, the speed curve can be expressed as a mapping function relative to the timestamp and the stations along the lane, which can be expressed as v i =f(t i ,s i ), where t i and s i Indicates the time and arc length along the path. During the optimization process, ride comfort, acceleration smoothness, and traffic efficiency should be comprehensively considered to achieve a balanced performance. The time series t=[t0,t1,…,t n ] T remains unchanged, and for the position sequence s=[s0,s1,…,s n ] T Adjust and optimize. For any point on the speed curve (t i ,s i ), position s can be obtained by vertical expansion i The feasible space range (up i ,botom i ).

[0139] After determining the feasible region of speed planning, the discrete position sequence [s0,s1,…,s n ] T It can be used as the core goal of optimization. At this time, the optimization goal of the speed planning problem can be formally expressed as follows:

[0140]

[0141] When optimizing the objective function, it is necessary not only to comprehensively consider driving efficiency and ride comfort, but also to introduce indicators such as the deviation from the rough solution of the speed curve. The specific objective function can be expressed as:

[0142] f(s)=cost ref +cost exp +cost a +cost j , (13)

[0143] Among them, cost ref represents the deviation cost between the optimized velocity curve and the rough solution, aiming to make the optimized curve as close as possible to the section where the rough solution is located; costexp is the expected speed cost, which is used to make the optimized vehicle speed close to the expected driving speed; cost a and cost j Represents the cost of acceleration and jerk, respectively, used to improve the smoothness of the speed curve, thereby improving ride comfort. The calculation formulas for these costs are as follows:

[0144]

[0145]

[0146] When optimizing the speed curve, it is necessary to ensure that the vehicle does not collide with obstacles while meeting the vehicle's dynamic constraints. The former focuses on driving safety, while the latter focuses on improving comfort and driving stability.

[0147] To this end, the longitudinal position of the vehicle needs to be constrained to avoid interference with obstacles:

[0148] bottom i ≤s i ≤up i , (18)

[0149] The vehicle's stability constraints also need to be met:

[0150]

[0151] Among them, a y,max is the maximum lateral acceleration allowed when the vehicle is traveling, κ max is the maximum curvature of the path, v limit The speed limit for the road.

[0152] In addition, longitudinal acceleration and jerk must also be kept within reasonable limits and must not exceed their respective upper and lower limits:

[0153] a x,min ≤s″ i ≤a x,max , (20)

[0154] j x,min ≤s″′ i ≤j x,max (twenty one)

[0155] It is also necessary to ensure that the starting point of the speed curve complies with the vehicle's current state constraints to avoid sudden speed changes when the vehicle follows the speed curve. Therefore, it is necessary to restrict the state of the starting point of the speed curve.

[0156] s0=s start ,s′0=s′ start,s″0=s″ start , (twenty two)

[0157] Based on S53 and S54, the JPS algorithm is used to generate the speed planning curve of the broken line segment, and then the speed planning curve after optimization by the secondary planning is as follows: Figure 6 shown.

[0158] S6. The planned path and speed are combined into a planned trajectory, and the trajectory information is sent to the vehicle controller to achieve multi-vehicle coordinated lane change.

[0159] The entire example runs in Carla as follows Figure 7 and Figure 8 shown.

[0160] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not limiting. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solutions of the present invention can be modified or replaced by equivalents without departing from the purpose and scope of the technical solutions, which should all be included in the scope of the claims of the present invention.

Claims

1. A multi-vehicle collaborative trajectory planning method for urban roads based on roadside guidance, characterized by: The method comprises the following steps: S1. Deploy roadside sensing equipment, roadside cooperative control units, and roadside units (RSUs) on urban roads. Vehicles on these roads include connected vehicles and human-driven vehicles (HDVs). Among them, connected vehicles include L4 autonomous driving vehicles with autonomous driving and connected communication functions and L2 autonomous driving vehicles with assisted driving and connected communication functions; S2. Access external input data: cooperative driving rules, surrounding vehicle intentions, surrounding vehicle trajectory information, cooperative vehicle and key vehicle information, behavioral decision information, high-precision maps, and global path planning information; S3. Establish a relative coordinate system based on key vehicles, global paths, static risk fields, and dynamic risk fields according to external input data; S4. Plan the lane change path using a quintic polynomial fitting method. Target constraint 1 includes safety constraints (static obstacles), regulations (time spent on the lane, continuous lane change), and comfort constraints (avoiding sharp turns). S5. Use the improved Jump Point Search (JPS) algorithm to perform speed planning on the planned lane change path. Target Constraint 2 includes safety constraints (dynamic obstacles), regulations (speed limits and traffic rules), comfort constraints (acceleration limits), and efficiency constraints (TTC values ​​less than or equal to 3 seconds). Specifically, the following steps are included: S51, based on the lane change path planned in S4 and the information of vehicles around the vehicle, construct the ST space, rasterize the ST space, and use the position s(t0), velocity v(t0) and acceleration a(t0) of the obstacle at the current time t0 to calculate the obstacle at any future time t k location; S52, the speed feasible domain and the search target are determined. After obtaining the ST search space, some grids are unavailable due to being occupied by obstacles, and the remaining unoccupied grids form free areas. These free areas together constitute the feasible space for speed planning; S53, speed curve planning based on JPS algorithm; When using the A* algorithm for path search, the cost function of each node usually takes the following form: f(n)=g(n)+h(n) (9) Where f(n) represents the total cost of the current node n; g(n) represents the cumulative cost from the starting node to the current node n; h(n) represents the estimated cost from the current node to the target node; The JPS algorithm is used to constrain the expansion direction of the node. In the ST space, the vehicle's motion state is restricted to moving only right, upward, or along the upper right diagonal direction. During the algorithm's iteration process, the cost of the current node is the cost of its parent node plus the cost from the parent node to the current node. S54. Speed ​​curve optimization based on quadratic programming. Since the speed curve generated by the JPS algorithm is composed of broken line segments, the sudden change of speed will significantly affect the riding comfort. The speed curve generated in the ST space is optimized. In the ST graph, the speed curve is represented as a mapping function relative to the timestamp and the stations along the lane, represented as v i =f(t i , s i ), where t i and s i represents the time and arc length along the path; during the optimization process, ride comfort, acceleration smoothness, and traffic efficiency are comprehensively considered to achieve a balanced performance. The time series t = [t0, t1, ..., t n ] T remains unchanged, and for the position sequence s=[s0,s1,...,s n ] T Adjust and optimize; for any point on the speed curve (t i , s i ), position s is obtained by vertical expansion i The feasible space range (up i , bottom i ); After determining the feasible region of speed planning, the discrete position sequence [s0,s1,...,s n ] T As the core goal of optimization; at this time, the optimization goal of the speed planning problem is expressed as follows: When optimizing the objective function, driving efficiency and ride comfort are comprehensively considered, and the deviation index between the rough solution of the speed curve is introduced. The specific objective function is expressed as: f(s)=cost ref +cost exp +cost a +cost j (13) Among them, cost ref represents the deviation cost between the optimized velocity curve and the rough solution, aiming to make the optimized curve as close as possible to the section where the rough solution is located; cost exp is the expected speed cost, which is used to make the optimized vehicle speed close to the expected driving speed; cost a and cost j They represent the cost of acceleration and jerk, respectively, and are used to improve the smoothness of the speed curve, thereby improving ride comfort. The calculation formulas for these costs are as follows: i is the lane number, the innermost lane i=0, and the lanes are arranged from the inside to the outside. N is the total number of lanes; s′ i Represents the current longitudinal speed, s″ i Indicates the current longitudinal acceleration; When optimizing the speed curve, ensure that the vehicle does not collide with obstacles during driving and meet the vehicle's dynamic constraints; Constrain the longitudinal position of the vehicle to avoid interference with obstacles: bottom i ≤s i ≤up i (18) Satisfy the vehicle's stability constraints: Among them, a y，max is the maximum lateral acceleration allowed when the vehicle is traveling, κ max is the maximum curvature of the path, v limit The speed limit for the road; The longitudinal acceleration and jerk must not exceed their respective upper and lower limits: a x，min ≤s″ i ≤a x，max (20) j x，min ≤s″′ i ≤j x，max (21) Ensure that the starting point of the speed curve complies with the current state constraints of the vehicle to avoid sudden speed changes when the vehicle is driving along the speed curve; restrict the state of the starting point of the speed curve; s0=s start ,s′0=s′ start ,s″0=s″ start (22) S6. The planned lane change path and speed are combined into a planned trajectory, and the trajectory information is sent to the vehicle controller to achieve multi-vehicle coordinated lane change.

2. The method for multi-vehicle collaborative trajectory planning on urban roads based on roadside guidance according to claim 1 is characterized by: In S2, external input data analysis: cooperative driving rules refer to the constraints on the cooperative driving behavior of connected vehicles based on existing traffic laws and regulations, including the following distance maintained by the connected vehicle during stable following, and the range of the pre-collision time (TTC) between the connected vehicle and the preceding vehicle during maneuvers, i.e., lane changing, overtaking, and following; the intentions and trajectory information of surrounding vehicles are predicted based on the vehicle's historical trajectory data; cooperative vehicles refer to connected vehicles that participate in cooperative control during this cooperative process; key vehicles refer to HDVs that do not participate in the cooperative process but have an impact on the trajectory planning of connected vehicles; The behavioral decision information indicates the recommended behavior and priority of each connected vehicle. The global path is a road center reference line with the starting and ending points determined in advance based on the high-precision map. During normal driving, the vehicle travels along the reference line.

3. The method for multi-vehicle collaborative trajectory planning on urban roads based on roadside guidance according to claim 1 is characterized by: The S3 includes the following steps: S31. Build a vehicle collection: Vehicle C k is a cooperative vehicle, k∈K represents the vehicle number; H l is the key vehicle, l∈L represents the vehicle number; S32. Constructing a vehicle risk field for collaborative vehicles and key vehicles: First, a static risk field for the vehicle is constructed. The relative distance and approach direction between the vehicle and the obstacle are considered to characterize the risk caused by the static obstacle. The formula is as follows: Among them, U s is the risk field generated by the stationary obstacle vehicle, and the position of the vehicle is (x obs ,y obs ); w represents the field strength coefficient greater than 0; ρ xobs and ρ yobs Represent the shape equations of the obstacle in the longitudinal and lateral directions, ρ xobs =k x L xobs , ρ yobs =k y L yobs ;k x and k y Respectively represent coefficients greater than 0, L xobs and L yobs Respectively represent the horizontal and longitudinal lengths of the obstacle vehicle; Then, a vehicle dynamic risk field is constructed to characterize the risk caused by moving obstacles that may collide with the ego vehicle. It is related to the relative distance, relative speed, and approach direction of the ego vehicle. The formula is as follows: Among them, U d is the risk field generated by the movement of the obstacle vehicle; ρ v represents the relative velocity equation, ρ v =k v |vv obs |;k v represents a coefficient greater than 0; Indicates the direction of relative motion; when v≥v obs hour, When v <v obs hour, S33. Construct relative coordinate system: First, define the lane centerline as: P i =[(x 1,i ,and 1,i ),(x 2,i ,and 2,i ),...,(x j，i ,and j，i ),...,(x M,i ,and M，i )] (3) Among them, P i ∈P, i=0, 1, 2…, N is the lane number, the innermost lane i=0, arranged from the inside to the outside, N is the total number of lanes; (x j，i ,y j，i ) is the lane center line P i The coordinates of the points on ; P is the lane centerline set; Then, take the last vehicle in the current driving direction of the main vehicle or cooperative vehicle as the starting point of the X-axis boundary and the outer boundary of the road as the starting point of the Y-axis to establish a Cartesian relative coordinate system; Secondly, the center line of the lane where the main vehicle is located is used as the reference curve to establish the Frenet coordinate system. The construction method is as follows: d(s)=a5s 5 +a4s 4 +a3s 3 +a2s 2 +a1s+a0,s∈[s ini ,s fin ] (4) The following coordinate transformation is achieved by formula (4): Lane line P i Converted to Frenet coordinates, recorded as And satisfy the following formula: s.t s left ≤s j，i ≤s right +s pre (5-1) in, is the lane centerline coordinate in the Frenet coordinate system; s left and s right Represent the leftmost and rightmost cooperative vehicles C on the road respectively k The horizontal coordinate is for the east-west lane; s pre Represents the cooperative vehicle C k The distance of a single planning, Δt is the time of forward planning, Indicates vehicle C k speed; The coordinates of the cooperative vehicles Transformed into Frenet coordinates, denoted as The key vehicle coordinates Transformed into Frenet coordinates, denoted as 4. The method for multi-vehicle collaborative trajectory planning on urban roads based on roadside guidance according to claim 3 is characterized by: The S4 comprises the following steps: S41. Based on the relative coordinate system, the ego vehicle, key vehicle, and cooperative vehicle are projected into SL, and then the paths of the vehicles are planned in descending order of vehicle priority. In the Frenet coordinate system, the relationship between the lane centerline arc length s and the lateral offset is given by formula (4) in S33. S42, determine the boundary conditions, solve the undetermined coefficients of the quintic polynomial, and determine the lane change path; the initial state is where d ini , v ini and a ini are the initial lateral offset, initial velocity and initial acceleration of the vehicle respectively; the final state is where d fin , v fin and a fin are the final lateral offset, final velocity and final acceleration of the vehicle respectively; the length of the path is s pre ; Then formula (4) is solved by the following matrix: The planned path of the vehicle is solved by formulas (4) to (7).

Images