A ramp merging trajectory planning method based on optimal control theory and regional multi-agent collaborative decision-making technology

Abstract

The application relates to a ramp merging trajectory planning method based on optimal control theory and regional multi-agent collaborative decision-making technology, characterized in that the method is realized through the following steps: step one, establishing a basic constraint relationship of an optimal control model for vehicle trajectory planning; step two, constructing a cost function for finding an optimal solution; and step three, obtaining an optimal ramp merging trajectory by minimizing the cost function.

Description

Technical Field

[0001] This invention relates to a vehicle ramp merging trajectory planning method based on optimal control theory and multi-agent cooperative decision-making technology within a region, belonging to the interdisciplinary technical field of control optimization and cooperative decision-making. Background Technology

[0002] Against the backdrop of rapid development in computer-related technologies, intelligent and connected vehicles (ICVs) have become a hot research topic in the field of transportation science and engineering. Based on onboard sensors, signal transmitters, signal receivers, and execution terminals, ICVs can exchange and utilize information in a networked state. They typically possess one or more functions such as obstacle detection, environmental risk assessment, individual vehicle planning and control, and multi-agent collaborative decision-making, promising to achieve safe, convenient, comfortable, and low-carbon intelligent driving. With continuous advancements in related theories and technologies, Level 5 autonomous driving (i.e., a level of autonomous driving completely without a human driver) is expected to be achieved, fundamentally liberating humans from the tiring and tedious act of driving.

[0003] The key technologies involved in intelligent connected vehicles mainly include environmental perception technology, planning and decision-making technology, control and execution technology, and information interaction technology. Environmental perception technology refers to the precise capture of key information about the surrounding environment through onboard sensors such as LiDAR, millimeter-wave radar, sonar, and vehicle-mounted cameras, providing the vehicle with a basis for decision-making. It can be figuratively described as the "eyes" and "ears" of the intelligent connected vehicle. Decision-making and planning technology refers to the vehicle autonomously judging based on the information obtained, selecting an appropriate plan, and planning a safe driving trajectory accordingly, equivalent to the "brain" of the intelligent vehicle. Control and execution technology refers to the vehicle's ability to maintain a planned trajectory within a certain margin of error even under interference from the real environment; it can be understood as the "hands and feet" of the intelligent connected vehicle. Information interaction technology generally includes vehicle-to-everything (V2X) communication technology, cloud computing technology, big data technology, network communication encryption technology, and anti-network attack technology, ensuring the real-time performance, security, and privacy of the intelligent vehicle's collaborative perception, group decision-making, and collaborative planning and control capabilities.

[0004] Currently, while intelligent connected vehicles perform well on closed, structured roads such as highways, effectively handling tasks like lane departure detection and reducing the burden on human drivers, they still face numerous challenges in more complex situations, such as ramp merging. Ramps merging refers to vehicles entering a main road from a short auxiliary road adjacent to a main road (highway, elevated road, bridge, etc.), or from an overpass / ramp / access road connecting to another main road, or from some ancillary connecting roads. Ramps merging is a very common scenario in the real world, and statistics show that ramp merging points are among the most accident-prone locations. Therefore, enabling intelligent connected vehicles to handle these situations with ease is crucial for the practical application of intelligent driving technology. However, current domestic and international research has certain shortcomings. For example, intelligent connected vehicles on main roads cannot effectively interact with merging vehicles and other vehicles on the main road, thus failing to accurately determine the presence, timing, and route of merging vehicles. Furthermore, the indicators for evaluating the quality of merging trajectory planning are too rigid and do not fully consider the complexity of the real world.

[0005] Optimal control theory refers to a mathematical model that achieves open-loop optimal control by minimizing a constructed cost function based on the relationships between the physical quantities of vehicle motion and the constraints under specific scenarios, and solves for the optimal trajectory through numerical optimization. Its advantage lies in its intuitive and concise description of vehicle motion planning tasks, and the constructed models generally have good generalization ability. Multi-agent cooperative decision-making technology, in this context, refers to multiple intelligent connected vehicles within a region—that is, multiple intelligent agents with their own perception, information transmission and reception, and decision-making reasoning capabilities—determining their respective trajectories through information exchange and collaborative planning decisions. Undoubtedly, this technical approach is beneficial for convenient traffic flow and safe driving on an overall scale. However, current research on the application of optimal control theory and cooperative decision-making theory in specific scenarios is somewhat lacking both domestically and internationally. Existing research often suffers from drawbacks such as slow model calculation speed, low efficiency, and excessive scenario assumptions that differ significantly from real-world environments. Combining optimal control theory with multi-agent cooperative decision-making techniques can make the assumptions more realistic while improving the solution efficiency, interpretability, and operability of ramp merging trajectory planning, which has strong practical significance. Of course, how to ensure the robustness and security of the trajectory planning results still needs further exploration in subsequent related research. Summary of the Invention

[0006] The purpose of this invention is to solve the above problems by proposing a vehicle ramp merging trajectory planning method based on optimal control theory and multi-agent collaborative decision-making technology within the region. This method achieves information interaction through onboard communication units and dedicated short-range communication technology, and uses the local computing resources of each vehicle unit to perform collaborative calculations and plan the vehicle merging trajectory on the ramp, thus enabling the safe and efficient merging of intelligent connected vehicles at the ramp entrance.

[0007] This invention is a vehicle ramp merging trajectory planning method based on optimal control theory and multi-agent cooperative decision-making technology within a region, implemented through the following steps:

[0008] Step 1: Establish the basic constraints for the optimal control model of vehicle trajectory planning;

[0009] Step 2: Construct the cost function for finding the optimal solution;

[0010] Step 3: By minimizing the cost function, the optimal ramp merging trajectory is obtained.

[0011] The advantages of this invention are:

[0012] (1) This invention makes full use of the dedicated short-range communication signal resources in the intelligent connected vehicle cooperative system. Under the premise of fully considering price and convenience, it lays the foundation for using group decision-making theory and related technologies to achieve safe passage of vehicles at road intersections and smooth traffic flow as a whole.

[0013] (2) Based on optimal control theory and multi-agent cooperative decision-making technology in the region, this invention establishes an innovative scenario hypothesis that is more in line with the actual situation, expands the existing research on multi-vehicle cooperative ramp merging motion planning decision, opens up a new scenario of multi-vehicle cooperative passage through complex traffic environment, making it more generalizable, and effectively improving the efficiency of traffic facilities while ensuring traffic safety. Attached Figure Description

[0014] Figure 1 This is a flowchart of the vehicle cooperative positioning method of the present invention. Detailed Implementation

[0015] The present invention will now be described in further detail with reference to the accompanying drawings and embodiments.

[0016] This invention is a vehicle ramp merging trajectory planning method based on optimal control theory and multi-agent cooperative decision-making technology within a region, such as... Figure 1 As shown, this can be achieved through the following steps:

[0017] Step 1: Establish the basic constraints for the optimal control model of vehicle trajectory planning;

[0018] A. Vehicle kinematic constraints

[0019] For a specific intelligent connected vehicle (ICV), L b L represents the width of the vehicle body. w L represents the wheelbase between the front and rear wheels. f and L r These represent the front overhang distance and the rear overhang distance, respectively.

[0020] Assume there are a total of n+1 vehicles in the intersection area represented by a Cartesian coordinate system (the origin of the coordinate system is at the intersection of the center lines of the main lane and the ramp), each labeled with a number, forming a set N = {ICV0, ICV1, ..., ICV...} n}. Among them, (x i (t),y i (t) represents ICV i Position coordinates, v i (t) represents ICV i The vehicle speed along the longitudinal axis of the vehicle body, θ i (t) represents ICV i The yaw angle φ relative to the positive x-axis direction of the established coordinate system. i (t) represents ICV i Front wheel steering angle (defined here, without considering the difference in steering angle between the front wheels), a i (t) represents ICV i The acceleration along the longitudinal axis of the vehicle body, ω i (t) represents ICV i Front wheel yaw rate. Specifically, assume that initially, vehicles numbered k (1, 2, 3, ..., n) are on the main road, preparing to cross the intersection, and therefore do not need to turn; the corresponding θ... i and φ i There is θ i ≡0 and φ i ≡0 (the direction of traffic flow on the main road is defined as the positive x-axis); while vehicle number 0 is located on the ramp at an angle θ to the main road, at a distance L from the origin, and is preparing to merge into the main road.

[0021] Then the motion of any ICV (i∈N) in the time domain will be subject to a set of motion differential equations, as shown below:

[0022]

[0023] B. Two-point boundary constraints corresponding to the initial and final states

[0024] For trajectory planning, clear starting and ending points or corresponding judgment indicators should be proposed. This is discussed below.

[0025] (I) Initially, for car number 0, the position is

[0026] x0(0)=L*cosθ0(0)

[0027] The attitude angle is the lane angle:

[0028] θ0(0)=θ

[0029] The speed is v0(0); the front wheel deflection angle is φ0(0);

[0030] At the end time t = tf, for the attitude angle, we should have:

[0031] θ0(tf)=0

[0032] The corresponding constraints are expressed as

[0033] sinθ0(tf)=0

[0034] cosθ0(tf)=1

[0035] Regarding speed:

[0036] v0(tf)=vj(tf)

[0037] Where j is the previous vehicle number at the time of insertion, and:

[0038] a0(tf)=0

[0039] If the front wheels are deflecting, you should stop steering.

[0040]

[0041] ω0(tf)=0

[0042] (II) Initially, for traffic on the main road, it can be assumed that the traffic flow is steady at a speed v. For i = 1, 2, 3, ..., n, that is:

[0043] v i (0)=v

[0044] a i (0)=0

[0045] End time t = t f At that time, the traffic flow should return to its original state, that is:

[0046] v i (t f ) = v

[0047] a i (t f ) = 0

[0048] The constraints for attitude angles and steering angles are constructed in the same way as in (I).

[0049] C. Limitations on motor skills

[0050] In actual motion, the body's movement capacity is inevitably limited. Correspondingly, state variables and control variables have ranges of values ​​to meet vehicle performance requirements and ensure the generated trajectory has realistic significance. The expression is as follows:

[0051]

[0052] Max(|a i (t)|)≤a max

[0053] Max(|v i (t)|)≤v max

[0054] MAx(|ω i (t)|)≤ω max

[0055] Where t can be any point in time within the entire time domain, and i can be any natural number from 0 to n. Since the steering angle and heading angle of vehicles on the main road are always 0, for these two physical quantities, only the case of vehicle number 0 is examined to see if it satisfies the motion capability constraints.

[0056] D. Avoid collision constraints

[0057] For each ICV, a circle is used (the radius of the circle is half the length of the diagonal of the corresponding vehicle body, denoted as r). i Let's represent this. Then, the condition for avoiding collisions can be expressed as: any two adjacent circles do not intersect, that is:

[0058] d j =((x) j (t)-x j+1 (t)) 2 +(y j (t)-y j+1 (t)) 2 ≤r j +r j+1

[0059]

[0060] Here, t can be any point in time within the entire time domain, and j = 0, 1, ..., n-1. However, j is not exactly the same as the initial vehicle number because it involves vehicle merging. After merging on the ramp, the x-component of each ICV's position coordinates needs to be reassigned in descending order, with j = 0, j = 1, ..., j = n.

[0061] E. Boundary constraints under specific scenarios during ramp merging

[0062] Boundary constraints are used to prevent vehicles from veering off the lane during movement, thus avoiding traffic accidents.

[0063] As explained above, for traffic in the main lane, we can assume that they will not take overtaking strategies. Therefore, as long as they are initially in the main lane, they naturally satisfy the boundary constraint conditions.

[0064] For vehicle number 0 merging into the ramp, assume the ramp width is D. z The main lane is D wide. m From geometric relationships, we know that the initial coordinates are:

[0065] x0(0)=-L*cosθ

[0066] y0(0)=L*sinθ

[0067] First, vehicle number 0 needs to merge, so it should not cross the outer boundary line of the ramp or its extended straight line, that is:

[0068]

[0069] Similarly, the vehicle should not cross the outer edge line of the main road, that is:

[0070]

[0071] Furthermore, the inner boundary line is approximated by multiple circles (since in reality, the merging points of ramps are often connected in the form of arcs, this approximation is reasonable). The x and y coordinates of the center of this series of circles can be expressed as:

[0072]

[0073]

[0074] Where i = 2 0 ,2 1 , ..., 2 7 .

[0075] To ensure that car number 0 does not cross the inner boundary line, the circle represented by car number 0 should not intersect with any of the other circles:

[0076]

[0077] In summary, the boundary constraint equations can be written as:

[0078]

[0079] Step 2: Construct the cost function for finding the optimal solution;

[0080] By simultaneously applying the constraints described in step one, we obtain the constraint matrix for this problem, and then the feasible region in the solution space can be calculated. Generally, the feasible region will not contain only one feasible solution; that is, there are often multiple paths that satisfy the above conditions. Therefore, it is necessary to design a cost function, which is a polynomial that can serve as an indicator of the quality of each optional path, and thereby select the most desirable trajectory (the path with the smallest function value). The cost function designed here includes the following performance indicators, which are explained in turn:

[0081] (1) Final value performance index

[0082] The final performance index is used to describe the need to improve traffic flow efficiency at ramp entrances, let t i Let represent the time taken for car i to travel. Then:

[0083]

[0084] (2) Integral performance index

[0085] Common cumulative metrics include trajectory smoothness and the degree to which the trajectory deviates from obstacles. Trajectory smoothness is related to the change in front wheel steering angle, therefore the following settings are used:

[0086]

[0087] It represents the range of wheel angle change of car number i throughout the entire process. Smaller values ​​result in smoother trajectories.

[0088] Because only car number 0 experiences a change in wheel angle during the merging process, therefore, specifically:

[0089]

[0090] Similar examples include:

[0091]

[0092] It represents the speed change range of each vehicle throughout the entire process.

[0093] Also:

[0094]

[0095] It represents the degree of distance between each vehicle and the vehicle in front throughout the entire process.

[0096] By summing the cost function terms for each vehicle with weights, we can obtain the cost function for each vehicle:

[0097]

[0098] In the formula Each performance metric for each vehicle is assigned a weighting coefficient, and all coefficients are non-negative. The specific values ​​should be assigned based on the actual situation. For example, if vehicle i is an ambulance carrying a critically ill patient, then... A sufficiently large value should be given, meaning the car in front should pass as quickly as possible so that car number i can reach the hospital as soon as possible.

[0099] Among them, for A more general assignment formula can still be given:

[0100]

[0101] In the formula, This represents the total mass of car number i. This indicates the unloaded mass of vehicle number i. On the one hand, as the additional mass of a vehicle increases, its deceleration ability weakens, requiring vehicles in front and behind to maintain a greater safe distance. On the other hand, if the vehicle itself is very heavy, it indicates that it is a large vehicle. Maintaining a distance from it helps the occupants of the vehicles in front and behind to have better visibility and avoid blind spots, which is beneficial for the occupants to identify environmental risks and intervene in a timely manner, thus preventing traffic accidents.

[0102] Finally, for a specific ramp merging trajectory, quantitative evaluation metrics can be provided:

[0103]

[0104] Step 3: By minimizing the cost function, the optimal ramp merging trajectory is obtained.

[0105] The cost function C contains an integral term and is a nonlinear function. For convenience, AMPL is used to solve it, as it has powerful solving capabilities and speed for nonlinear problems. Mathematically, the solution that minimizes the cost function in the solution domain is clearly the optimal ramp merging trajectory. Therefore, the final optimal solution is obtained.

Claims

1. A ramp merge trajectory planning method based on optimal control theory and regional multi-agent collaborative decision-making technology, characterized in that, The method is realized by the following steps: Step one, establishing basic constraint relations of optimal control model for vehicle trajectory planning; Step two, constructing a cost function for finding an optimal solution; Step three, solving an optimal ramp merging trajectory by minimizing the cost function; The step one of establishing basic constraint relations of optimal control model for vehicle trajectory planning comprises: A, vehicle kinematics constraint For intelligent connected vehicle ICV, denotes the vehicle body width, denotes the front and rear wheel track, and denote the front and rear suspension distance, respectively, In the intersection area represented by a Cartesian coordinate system, i.e., the origin of the coordinate system at the intersection of the main lane and the ramp centerline, there are a total of n+1 vehicles, each labeled with a number, forming a set N={ICV0,ICV1,⋯, },in,( , )express Location coordinates, express Vehicle speed along the longitudinal axis of the vehicle body. express The yaw angle relative to the positive x-axis direction of the established coordinate system. express Front wheel steering angle, express Acceleration along the longitudinal axis of the vehicle body, express The front wheel yaw rate is as follows: Initially, vehicles numbered k (front wheel) and 1, 2, 3, ..., n are located on the main road, preparing to cross the intersection. Therefore, they do not need to turn. and have ≡0 and ≡0, the direction of traffic flow on the main road is defined as the positive x-axis; and the vehicle numbered 0 is located at an angle to the main road. On the ramp, at a distance L from the origin, it is preparing to merge into the main road. Then, for any Motion in the time domain is constrained by a set of motion differential equations, as follows: B, two-point boundary constraint corresponding to initial and terminal states For trajectory planning, (I) initially, for the 0th vehicle, the position is where the attitude angle is the lane angle: Speed is ; front wheel deflection angle is ; End time t At the end, for the attitude angle, there should be The corresponding constraint is expressed as For the speed: where j is the previous vehicle number at the time of insertion, and For the front wheel deflection, it should not be turned again, so (II) initially, for the oncoming vehicles on the main road, it is assumed that the traffic flows smoothly at a speed v, for i = 1, 2, 3,..., n, i.e.: End time t At this time, the traffic flow should return to the original state, i.e.: C, motion capability constraint In the actual motion process, the motion capability of the machine body is necessarily limited, and accordingly, the state variables and control variables have a range of values to meet the performance requirements of the vehicle and ensure that the generated trajectory has practical significance, and the expression is as follows: where t can be any time point in the entire time domain, i can be any natural number from 0 to n, and in the expression, since the steering angle and the heading angle of the vehicles on the main road are always 0, only the case of the 0th vehicle is considered to meet the motion capability constraint; D, collision avoidance constraint For each ICV, a circle is used, with a radius equal to half the length of the diagonal of the vehicle body, denoted as is performed; then, the condition to avoid collisions can be expressed as the non-intersection of any two adjacent circles, i.e. where t can be any time point in the entire time domain, and j = 0, 1,..., n-1; However, j is not completely consistent with the initial number of vehicles, because it involves the merging of vehicles; After the ramp merging, the x component of the position coordinates of each ICV is re-assigned from large to small, i.e. j = 0, j = 1,..., j = n; E, boundary constraint under specific situations during ramp merging The boundary constraint is used to regulate that the vehicles will not run off the lane during the motion process and cause traffic accidents; For the traffic flow on the main road, as explained above, it is assumed that they will not adopt the overtaking strategy, and therefore, as long as they are in the main lane at the initial time, they naturally meet the boundary constraint condition, For the 0th vehicle merging into the ramp, assuming the ramp width is , and the main lane width is , by geometric relations, the initial coordinates are: Firstly, the 0th vehicle needs to perform the merging action, so it should not cross the outer boundary line of the ramp and its extended straight line, i.e. Similarly, the vehicle should not cross the outer boundary line of the main road, i.e. In addition, for the inner boundary line, a plurality of circles are used to approximate it, and since the ramp merging place is often connected in a circular arc form in reality, this approximation is reasonable, and the x and y coordinates of the centers of the series of circles can be expressed as: Where i= , ,......, ; To ensure that the 0th vehicle does not cross the inner boundary line, the 0th vehicle represented circle should not intersect with the several circles: In summary, the boundary constraint equation set can be written as: 。 2. The planning method of claim 1, wherein, Step two, constructing a cost function for finding an optimal solution; With the constraints described in step one, the constraint matrix of this problem is obtained, and then the feasible region in the solution space can be solved. Generally, the feasible region may not contain only one feasible solution, that is, there are often multiple paths that meet the above conditions, so it is necessary to design a cost function, that is, a polynomial that can be used as an indicator to evaluate the quality of each optional path, and then filter the most desirable trajectory, that is, the path with the smallest function value. The cost function designed here contains the following performance indicators, which are described in turn: (1) End value performance index The last performance index is used to describe the demand for improving the traffic flow efficiency at the ramp, which is defined as Let T i represent the travel time of the i th vehicle, then: (2) Integral performance index Common cumulative indicators include trajectory smoothness and distance from obstacles; trajectory smoothness is related to the change in front wheel angle, so set: which characterizes the amplitude of the variation of the angle of the wheels of the vehicle during the whole process of the i-th vehicle, The smaller the value, the smoother the trajectory. Since only the 0-th vehicle has a variation of the angle of the wheels during the merging process, we have, in particular: Similarly, there are: which represents the speed change of each vehicle throughout the process, And also: which represents the distance between each vehicle and the vehicle in front throughout the process, The weighted sum of each vehicle's cost function term gives the cost function of each vehicle: In the formula is the weight coefficient corresponding to each performance index of each vehicle, and is a non-negative number. The specific assignment should be based on the actual situation; the i-th vehicle is an ambulance carrying a critically ill patient, and a sufficiently large value should be given, that is, the preceding vehicle should pass as soon as possible to allow the i-th vehicle to arrive at the hospital as soon as possible. where, for , a more general assignment formula can still be given: In the formula, represents the total mass of the i-th vehicle, represents the empty mass of the i-th vehicle, on the one hand, as the additional mass of the vehicle increases, the deceleration ability of the vehicle weakens, and the front and rear vehicles need to maintain a longer safety distance from it; on the other hand, if the mass of the vehicle itself is large, it means that the vehicle is a large vehicle, and the distance between the front and rear vehicles and it helps the vision of the personnel on the front and rear vehicles and avoids the blind spots of the personnel on the large vehicle, which is conducive to the identification of environmental risks and timely intervention of the personnel on the vehicle, and avoids the occurrence of traffic accidents, Finally, for a specific ramp merging trajectory, a quantitative evaluation index can be given: Step three, the optimal ramp merging trajectory is obtained by minimizing the cost function; The integral term in the cost function C is a nonlinear function. For convenience, AMPL is used for solving. AMPL has strong solving ability and speed for nonlinear problems; mathematically, the solution that minimizes the cost function in the solution domain is obviously the optimal ramp merging trajectory; therefore, the final optimal solution is obtained.

Images