Urban road section multi-vehicle cooperative trajectory planning method based on roadside guidance

Abstract

The invention relates to an urban road section multi-vehicle cooperative trajectory planning method based on roadside guidance, and belongs to the technical field of intelligent traffic. The technical problem that in the prior art, single vehicle track planning passing efficiency is not high is solved, and the technical scheme is characterized in that a network connection vehicle control instruction is generated through a multi-vehicle cooperative track planning algorithm deployed on a roadside cooperative control unit and is issued to a vehicle end to be executed. Specifically, according to a multi-vehicle cooperative trajectory planning algorithm, vehicles in a cooperative range are projected into a relative coordinate system, trajectory planning is decomposed into two processes of path planning and speed planning, and information of cooperative driving rules, strategies, cooperative groups, surrounding vehicle states and the like of the vehicles is comprehensively considered in a regulation and control stage. And a multi-vehicle cooperative optimal track meeting the requirements of safety, comfort, high efficiency and the like is obtained. According to the invention, cooperative trajectory planning of the networked vehicles in a mixed traffic environment can be realized, and the method has high practicability and wide 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 an urban road section based on roadside guidance. Background Art

[0002] With the acceleration of urbanization and the continuous growth of the number of motor vehicles, problems such as traffic congestion and traffic accidents have become increasingly prominent, seriously affecting the quality of life of urban residents and the efficiency of urban operations. Traditional traffic management methods, such as signal light control, traffic signs and markings, have been unable to cope with increasingly complex traffic conditions. Intelligent Transportation System (ITS) came into being, aiming to achieve intelligent, efficient and safe transportation systems through advanced information technology, communication technology, control technology, etc.

[0003] In intelligent transportation systems, vehicle trajectory planning is one of the key technologies to achieve autonomous driving and traffic coordinated control. Traditional vehicle trajectory planning methods mainly rely on on-board sensors and on-board computing platforms to generate vehicle trajectories by sensing the surrounding environment. However, in complex urban traffic environments, relying solely on on-board perception has problems such as incomplete information acquisition and limited computing resources, making it difficult to achieve efficient and safe vehicle trajectory planning.

[0004] In recent years, the development of vehicle-to-everything (V2X) technology has provided a new approach to solving the above problems. Through information exchange between vehicles (V2V), vehicles and infrastructure (V2I), and vehicles and pedestrians (V2P), V2X technology realizes collaborative perception, collaborative decision-making, and collaborative control between vehicles and road infrastructure, referred to as vehicle-road collaboration. The trajectory planning method based on vehicle-road collaboration can make full use of the rich environmental information obtained by roadside perception equipment (such as cameras, radars, lidars, integrated radar and vision devices, etc.), make up for the shortcomings of vehicle-mounted perception, and improve the accuracy and safety of trajectory planning.

[0005] In addition, multi-vehicle collaborative trajectory planning is also an important means to solve the problem of urban traffic congestion. Through multi-vehicle collaboration, dynamic path allocation, speed coordination and trajectory optimization between vehicles can be achieved, effectively reducing conflicts and delays between vehicles and improving road traffic efficiency. However, traditional multi-vehicle collaborative trajectory planning methods usually adopt a centralized control architecture, which has problems such as high computational complexity and large communication delays, and it is 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] A method for multi-vehicle collaborative trajectory planning on urban roads based on roadside guidance, the method comprising the following steps:

[0009] S1. Deploy roadside sensing equipment, roadside cooperative control units, and roadside units (RSU) on urban roads. Vehicles on the roads include connected vehicles and human-driven vehicles (HDV);

[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, behavior 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. The lane change trajectory is planned by using a fifth-order polynomial fitting method, where target constraint 1 includes safety category: static obstacles; regulations category: line-crossing time, continuous lane change; comfort category: avoid sharp turns;

[0014] S5. Use the improved Jump Point Search (JPS) algorithm to plan the speed of the planned lane change path, where target constraint 2 includes safety category: dynamic obstacles; regulations category: speed limit, traffic rules; comfort category: acceleration limit; efficiency category: TTC allowed value is 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 S2, external input data is analyzed: collaborative driving rules refer to constraints on the collaborative driving behavior of connected vehicles based on existing traffic regulations, including what kind of following distance the connected vehicle maintains during stable following, and in maneuvering operations, namely lane changing, overtaking and following, the pre-collision time (Time to Collision, TTC) between the connected vehicle and the vehicle in front is maintained in what range; the intentions and trajectory information of surrounding vehicles are predicted through the vehicle's historical trajectory data; collaborative vehicles represent connected vehicles that participate in collaborative control during this collaborative process; key vehicles represent HDVs that do not participate in collaboration during this collaborative 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] Further, the S3 comprises the following steps:

[0018] S31. Construct 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. Construct vehicle risk field for cooperative vehicles and key vehicles:

[0021] First, a static risk field of the vehicle is constructed, and 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 Respectively 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 lateral 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 relative motion direction; when v ≥ v obs hour, When v <v obs hour,

[0027] S33. Construct a relative coordinate system:

[0028] First, define the center line of the lane 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 inside to 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, the last vehicle in the current driving direction of the main vehicle or the cooperative vehicle is taken as the starting point of the X-axis boundary, and the outer boundary of the road is taken 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 a 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 through formula (4):

[0035] Lane line P i Converted to Frenet coordinates, denoted 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 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 comprises the following steps:

[0042] S41, based on the relative coordinate system, SL projection is performed on the ego vehicle, the key vehicle and the cooperative vehicle, and then the path planning is performed on the vehicles in descending order of vehicle priority. In the Frenet coordinate system, the relationship between the arc length s of the lane centerline 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 deviation, 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] Further, the S5 comprises 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 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] 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;

[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 iteration of the algorithm, 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 diagram; 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 diagram, 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 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 ), and obtain the position s by vertical expansion i The feasible space range (up i ,bottom i );

[0060] After determining the feasible domain 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 riding 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 traveling, 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 meets 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, and effectively reduce the incidence of traffic accidents. The speed curve is optimized through secondary planning, speed mutations are reduced, and acceleration smoothness is improved, thereby improving ride comfort. This method can adapt to traffic scenarios with different connected vehicle penetration rates. With the increase in the penetration rate of connected vehicles, the traffic efficiency of intersections 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, objectives and features of the present invention will be described in the following description to some extent, and to some extent, will be obvious to those skilled in the art based on the following examination and study, or can be taught from the practice of the present invention. The objectives and other advantages of the present invention can 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 in conjunction with the accompanying drawings, wherein:

[0081] Figure 1 is the algorithm flow chart;

[0082] Figure 2 It is a schematic diagram of static and dynamic risk field;

[0083] Figure 3 It 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 It is a schematic diagram of speed planning;

[0087] Figure 7 Schematic diagram of the initialization position 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 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 only illustrate the basic concept of the present invention in a schematic manner, and the following embodiments and features in the embodiments can be combined with each other without conflict.

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

[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 the terms "upper", "lower", "left", "right", "front", "rear", etc. indicate the orientation or position relationship, they are based on the orientation or position relationship shown in the drawings, which is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operate in a specific orientation. Therefore, the terms describing the position 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 As shown in the figure, a multi-vehicle collaborative trajectory planning method for urban roads based on roadside guidance in an intelligent network environment is shown. The specific contents of the method are as follows:

[0096] S1. Deploy roadside sensing equipment, roadside cooperative control units, and roadside units on urban roads. Vehicles on the roads include 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, behavior 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 vehicle static risk field. Consider the relative distance and approach direction between the ego vehicle and the obstacle to characterize the risk caused by the static obstacle. 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 ρ yobsRespectively 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 lateral and longitudinal lengths of the obstacle vehicle.

[0104] Then, the 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 relative motion direction; when v ≥ v obs hour, When v <v obs hour, The static and dynamic obstacle risk fields in S32 are as follows Figure 2 shown.

[0107] S33. Construct a relative coordinate system. First, define the center line of the lane 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 inside to 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 set of lane centerlines.

[0110] Then, the last vehicle in the current driving direction of the main vehicle or the cooperative vehicle is taken as the starting point of the X-axis boundary, and the outer boundary of the road is taken 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 a 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 through formula (4):

[0114] Lane line P i Converted to Frenet coordinates, denoted 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 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 forward planning time, 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. The lane change trajectory is planned by using a fifth-order polynomial fitting method, where target constraint 1 includes safety category: static obstacles; regulations category: line-crossing time, continuous lane change; comfort category: avoid sharp turns;

[0121] S41, based on the relative coordinate system, SL projection is performed on the ego vehicle, key vehicle and cooperative vehicle, and then the path planning is performed on the vehicles 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 deviation, 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 quintic polynomial in S42 is as follows: Figure 4 shown.

[0125] S5. Use the improved Jump Point Search (JPS) algorithm to plan the speed of the planned lane change path, where target constraint 2 includes safety category: dynamic obstacles; regulations category: speed limit, traffic rules; comfort category: acceleration limit; efficiency category: TTC allowed value is 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 needs to constrain the expansion direction of the node. In the 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 iteration of the algorithm, 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 obsrepresents 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, the sudden change of speed will significantly affect the riding 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 i represents the time and arc length along the path. In the optimization process, ride comfort, acceleration smoothness, and traffic efficiency should be considered comprehensively 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 For any point on the speed curve (t i ,s i ), the position s can be obtained by vertical expansion i The feasible space range (up i ,botom i ).

[0139] After determining the feasible domain 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 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:

[0144]

[0145]

[0146] When optimizing the speed curve, it is necessary to ensure that the vehicle will not collide with obstacles during driving and meet 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, the longitudinal acceleration and jerk must also be kept within a reasonable range 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 meets the current state constraints of the vehicle to avoid sudden speed changes when the vehicle is driving according to 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 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 the technical solution of the present invention can be modified or replaced by equivalents without departing from the purpose and scope of the technical solution, which should 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 (RSU) on urban roads. Vehicles on the roads include connected vehicles and human-driven vehicles (HDV); 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, behavior 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. The lane change trajectory is planned by using a fifth-order polynomial fitting method, where target constraint 1 includes safety category: static obstacles; regulations category: line-crossing time, continuous lane change; comfort category: avoid sharp turns; S5. Use the improved Jump Point Search (JPS) algorithm to plan the speed of the planned lane change path, where target constraint 2 includes safety category: dynamic obstacles; regulations category: speed limit, traffic rules; comfort category: acceleration limit; efficiency category: TTC allowed value is less than or equal to 3 seconds; 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.

2. The method for multi-vehicle collaborative trajectory planning on urban roads based on roadside guidance according to claim 1 is characterized in that: 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 regulations, including what kind of following distance the connected vehicle maintains during stable following, and in maneuvering operations, that is, lane changing, overtaking and following, the pre-collision time (Time to Collision, TTC) between the connected vehicle and the preceding vehicle is maintained in what range; the intention and trajectory information of surrounding vehicles are predicted through the historical trajectory data of the vehicle; cooperative vehicles refer to the connected vehicles participating in the cooperative control in this cooperative process; key vehicles refer to HDVs that do not participate in the cooperation in this cooperative process but have an impact on the trajectory planning of the 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 a starting point and an end point determined in advance based on a 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 in that: The S3 comprises the following steps: S31. Construct 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. Construct vehicle risk field for cooperative vehicles and key vehicles: First, a static risk field of the vehicle is constructed, and 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 Respectively 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 lateral 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 relative motion direction; when v ≥ v obs hour, When v <v obs hour, S33. Construct a relative coordinate system: First, define the center line of the lane 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 inside to 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, the last vehicle in the current driving direction of the main vehicle or the cooperative vehicle is taken as the starting point of the X-axis boundary, and the outer boundary of the road is taken 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 a 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 through formula (4): Lane line P i Converted to Frenet coordinates, denoted 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 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 1 is characterized in that: The S4 comprises the following steps: S41, based on the relative coordinate system, SL projection is performed on the ego vehicle, the key vehicle and the cooperative vehicle, and then the path planning is performed on the vehicles in descending order of vehicle priority. In the Frenet coordinate system, the relationship between the arc length s of the lane centerline 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 deviation, 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).

5. The method for multi-vehicle collaborative trajectory planning on urban roads based on roadside guidance according to claim 1 is characterized in that: The S5 comprises the following steps: 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 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) 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; 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 iteration of the algorithm, 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: Among them, the cost from the parent node to the current node is: 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 diagram; set a value δ = 0.01; ω obs The weight representing the security cost; 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 diagram, 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 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 ), and obtain the position s by vertical expansion i The feasible space range (up i ,bottom i ); After determining the feasible domain 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 riding 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 The expected speed cost 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: 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 meets 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)。

Images