A multi-objective trajectory optimization method and device based on a forward sequential lane-changing strategy

Abstract

This invention provides a multi-target trajectory optimization method and apparatus based on a forward sequential lane-changing strategy, relating to the field of intelligent connected vehicle cooperative control technology. The method includes: acquiring first information; calculating the desired merging position of the vehicles based on the first information to obtain a first calculation result; obtaining a longitudinal motion control command sequence for the target vehicle and the assisting vehicle based on the first calculation result; determining the lateral motion trigger time based on the longitudinal motion control command sequence to obtain a lateral motion initiation command sequence and an optimal lateral trajectory; performing rolling time-domain optimization updates based on the longitudinal motion control command sequence, the lateral motion initiation command sequence, and the optimal lateral trajectory to obtain longitudinal-lateral motion update commands; and performing forward sequential lane-changing cyclic control based on the longitudinal-lateral motion update commands to obtain a merging result. This invention provides an efficient, energy-saving, and safe cooperative solution for intelligent connected vehicle fleets in scenarios such as ramp merging and bottleneck sections.

Description

Technical Field

[0001] This invention relates to the field of intelligent connected vehicle cooperative control technology, and more specifically, to a multi-objective trajectory optimization method and apparatus based on a forward sequential lane-changing strategy. Background Technology

[0002] With the development of connected vehicle (CAV) technology, collaborative control between vehicles offers new possibilities for improving traffic efficiency, safety, and fuel economy. However, in the transitional phase of CAV technology development, the mixed traffic environment on the road, where both manually driven vehicles (HDVs) and CAVs coexist, presents challenges to platoon collaborative control. Especially when platoons need to perform lane-changing and merging operations, such as at ramp entrances or bottleneck sections, traditional methods often struggle to simultaneously achieve efficiency, safety, and fuel economy.

[0003] In existing technologies, fleet cooperative control strategies are mainly divided into two categories: centralized decision-making and distributed decision-making. Centralized decision-making relies on the fleet leader vehicle to make decisions based on global information, while distributed decision-making involves each vehicle making independent decisions. Furthermore, although single-vehicle trajectory optimization methods such as dynamic lane-changing models and reinforcement learning methods have made some progress, their application at the fleet level remains insufficient in mixed traffic environments, especially when facing the unpredictability of manually driven vehicles.

[0004] Based on the shortcomings of the existing technology, there is an urgent need for a multi-target trajectory optimization method and device based on a forward sequential lane-changing strategy. Summary of the Invention

[0005] The purpose of this invention is to provide a multi-objective trajectory optimization method based on a forward sequential lane-changing strategy to improve the aforementioned problems. To achieve this objective, the technical solution adopted by this invention is as follows:

[0006] Firstly, this application provides a multi-objective trajectory optimization method based on a forward sequential lane-changing strategy, including:

[0007] The first information includes the real-time position, speed, and acceleration data of the target vehicle fleet and the manually driven vehicles and intelligent connected vehicles in the current lane, the lane width parameter of the target lane, and the headway constant, stationary safety distance constant, and vehicle length in the safe following distance constraint.

[0008] Based on the first information, the expected meeting position of the vehicles is calculated to obtain a first calculation result, which includes the expected meeting position of the target vehicle and the assisting vehicle and the longitudinal movement end time.

[0009] Based on the first calculation result, a longitudinal acceleration control sequence is generated to obtain a longitudinal motion control command sequence for the target vehicle and the assisting vehicle.

[0010] Based on the longitudinal motion control command sequence, the lateral motion trigger time is determined to obtain the lateral motion start command sequence and the optimal lateral trajectory.

[0011] Based on the longitudinal motion control command sequence, the lateral motion start command sequence, and the optimal lateral trajectory, a rolling time-domain optimization update is performed to obtain the longitudinal-lateral motion update command.

[0012] Based on the longitudinal-lateral motion update command, forward sequential lane change loop control is performed to obtain the merging result.

[0013] Secondly, this application also provides a multi-target trajectory optimization device based on a forward sequential lane-changing strategy, comprising:

[0014] The acquisition module is used to acquire first information, which includes the real-time position, speed, and acceleration data of the target vehicle fleet and the manually driven vehicles and intelligent connected vehicles in the current lane, the lane width parameter of the target lane, and the headway constant, stationary safety distance constant, and vehicle length in the safe following distance constraint.

[0015] The calculation module is used to calculate the expected meeting position of the vehicles based on the first information and obtain a first calculation result, which includes the expected meeting position of the target vehicle and the assisting vehicle and the longitudinal movement terminal time.

[0016] The generation module is used to generate a longitudinal acceleration control sequence based on the first calculation result, so as to obtain a longitudinal motion control command sequence for the target vehicle and the assisting vehicle.

[0017] The determination module is used to determine the lateral movement trigger time based on the longitudinal motion control command sequence, and to obtain the lateral movement start command sequence and the optimal lateral trajectory.

[0018] The optimization module is used to perform rolling time-domain optimization updates based on the longitudinal motion control command sequence, the lateral motion start command sequence, and the optimal lateral trajectory to obtain longitudinal-lateral motion update commands.

[0019] The control module is used to perform forward sequential lane-changing cyclic control based on longitudinal-lateral motion update commands to obtain the merging result.

[0020] The beneficial effects of this invention are as follows:

[0021] This invention decomposes the single-vehicle cooperative merging process into two stages: longitudinal trajectory optimization and lateral motion control. While ensuring safety, compared to non-optimized control strategies, it can reduce the total merging time of the fleet by up to 7.16% and lower total fuel consumption by 61.47%. Furthermore, addressing the dynamic uncertainties of manually driven vehicles, a backward-threshold rolling optimization framework is designed. Combined with terminal state constraints and real-time trajectory updates, this significantly improves the system's anti-interference capability, ensuring safe and smooth lane changing ("leave-change-leave" cycle) in dynamic traffic flow and maintaining queue stability. This provides an efficient, energy-saving, and safe cooperative solution for intelligent connected vehicle fleets in scenarios such as ramp merging and bottleneck sections. Attached Figure Description

[0022] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings used in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of the present invention and should not be regarded as a limitation on the scope. For those skilled in the art, other related drawings can be obtained based on these drawings without creative effort.

[0023] Figure 1 This is a schematic diagram of a multi-target trajectory optimization method based on a forward sequential lane-changing strategy as described in an embodiment of the present invention;

[0024] Figure 2 This is a schematic diagram of a multi-target trajectory optimization device based on a forward sequential lane-changing strategy, as described in an embodiment of the present invention.

[0025] Figure 3 This is a schematic diagram of a single vehicle model as described in an embodiment of the present invention.

[0026] The diagram is labeled as follows: 901, Acquisition Module; 902, Calculation Module; 903, Generation Module; 904, Judgment Module; 905, Optimization Module; 906, Control Module. Detailed Implementation

[0027] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. The components of the embodiments of the present invention described and shown in the accompanying drawings can generally be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present invention provided in the accompanying drawings is not intended to limit the scope of the claimed invention, but merely to illustrate selected embodiments of the invention. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without inventive effort are within the scope of protection of the present invention.

[0028] It should be noted that similar reference numerals and letters in the following figures indicate similar items; therefore, once an item is defined in one figure, it does not need to be further defined and explained in subsequent figures. Furthermore, in the description of this invention, terms such as "first," "second," etc., are used only to distinguish descriptions and should not be construed as indicating or implying relative importance.

[0029] Example 1:

[0030] This embodiment provides a multi-target trajectory optimization method based on a forward sequential lane-changing strategy.

[0031] See Figure 1 The figure shows that the method includes steps S100 to S600.

[0032] Step S100: Obtain first information, which includes the real-time position, speed, and acceleration data of the target vehicle fleet and the manually driven vehicles and intelligent connected vehicles in the current lane, the lane width parameter of the target lane, and the headway constant, stationary safety distance constant, and vehicle length in the safe following distance constraint.

[0033] Understandably, in mixed traffic flow environments, a unified input interface for multi-source heterogeneous data is constructed by simultaneously collecting the three-dimensional motion states (position / velocity / acceleration) and road structure parameters of both manually driven vehicles and intelligent connected vehicles. This step addresses the uncertainty of vehicle behavior in dynamic scenarios such as highway ramp merging by transforming physical world parameters into mathematically processable objects, providing a basic state-space mapping for collaborative control, and preventing subsequent optimization from deviating from the real traffic environment due to data gaps.

[0034] Step S200: Calculate the expected merging position of the vehicles based on the first information to obtain a first calculation result. The first calculation result includes the expected merging position of the target vehicle and the assisting vehicle, as well as the longitudinal motion terminal time. This step addresses the random disturbance characteristics of artificial vehicle trajectories in the mixed traffic flow during the transition period, transforming the traditional merging decision-making based on empirical rules into a dynamic optimization problem: it analyzes the balance point between vehicle dynamics constraints and safe distance using the minimum principle, ensuring that the expected merging position meets both the physical capacity limit of the target lane and the speed coordination requirements of different driving subjects (CAV / HDV). Further, step S200 includes steps S210 to S230.

[0035] Step S210: Based on the first information, construct and process the optimal control problem of the assisted vehicle. By defining the second-order dynamic equation of the vehicle and integrating the safety following constraint, the terminal speed consistency condition and the minimum safety distance constraint, construct an objective function with the goal of minimizing the joint time-fuel consumption.

[0036] Step S220: Perform costate equation analysis based on the objective function to obtain parameterized functions for vehicle acceleration, velocity, and position;

[0037] Step S230: Solve the terminal position and time according to the parameterized function to obtain the expected meeting position of the target vehicle and the assisting vehicle and the longitudinal movement terminal time.

[0038] Step S300: Generate a longitudinal acceleration control sequence based on the first calculation result to obtain a longitudinal motion control command sequence for the target vehicle and the assisting vehicle;

[0039] It should be noted that, addressing the inherent contradiction between fuel economy and traffic efficiency, a sequential discrete strategy is employed to transform the continuous control problem into nonlinear programming. While ensuring safe car following, a sharp reduction in energy consumption is achieved through smooth control of the acceleration sequence. Furthermore, step S300 includes steps S310 to S330.

[0040] Step S310: Based on the expected rendezvous position and terminal time in the first calculation result, construct a multi-objective optimization model. Define a joint optimization objective function by integrating the time cost weight coefficient and the fuel consumption weight coefficient to obtain the longitudinal trajectory optimization model.

[0041] Step S320: Discretize the continuous optimal control problem according to the longitudinal trajectory optimization model. Divide the time domain into equal intervals using the direct collocation method. Use the third-order Simpson integral formula to numerically discretize the state equation and objective function to obtain the discrete nonlinear programming problem.

[0042] Step S330: Based on the discrete nonlinear programming problem, the control sequence is solved by iteratively solving the acceleration values ​​at each discrete time node and verifying the feasibility of the constraints, thereby obtaining the longitudinal motion control command sequence of the target vehicle and the assisting vehicle.

[0043] Step S400: Determine the lateral motion trigger time based on the longitudinal motion control command sequence to obtain the lateral motion start command sequence and the optimal lateral trajectory;

[0044] It should be noted that this step, taking into account the fluctuating characteristics of the following distance of manually driven vehicles, designs a triple safety time threshold fusion algorithm: through the correlation analysis of the relationship chain between the preceding vehicle, the current vehicle, and the following vehicle, the optimal entry timing is captured in the merging zone of lane change opportunities. Furthermore, step S400 includes steps S410 to S430.

[0045] Step S410: Perform minimum safe time point calculation processing based on the longitudinal motion control command sequence. By predicting the future trajectories of the target vehicle, the target preceding vehicle, the assisting vehicle, and the following vehicles in the convoy, the minimum safe time points between the target vehicle and the target preceding vehicle, the target vehicle and the assisting vehicle, and the following vehicles in the convoy and the target vehicle are calculated respectively to obtain three types of minimum safe time points.

[0046] Step S420: Determine the earliest start time of lateral movement based on the three types of minimum safe time points. The theoretical earliest start time is determined by taking the maximum value of the three, and the theoretical trigger time of lateral movement is obtained.

[0047] Step S430: Based on the triggering time of the lateral motion theory, perform triggering feasibility verification and trajectory generation processing to obtain the lateral motion start command and the optimal lateral trajectory.

[0048] Step S500: Perform rolling time-domain optimization and update based on the longitudinal motion control command sequence, the lateral motion start command sequence and the optimal lateral trajectory to obtain the longitudinal-lateral motion update command;

[0049] This step uses a fixed time window slicing method to extract the optimization interval in real time, and achieves coordinated iteration of the longitudinal acceleration sequence and the lateral trigger time through terminal status feedback closed-loop verification. Further, step S500 includes steps S510 to S530.

[0050] Step S510: Perform optimized time domain dynamic interception processing based on the longitudinal motion control command sequence and the lateral motion start command sequence. By setting a fixed time step and rolling the time window backward, the initial optimized time domain is updated to a sub-interval from the current time to the terminal time, and the optimized time domain interval after rolling update is obtained.

[0051] Step S520: Perform terminal state constraint verification processing based on the optimized time domain interval to obtain the verification result. The verification process involves calculating whether the vehicle speed deviation and position deviation meet the maximum allowable threshold to determine the merging completion condition. If the condition is met, the optimization is terminated; otherwise, the rolling update continues.

[0052] Step S530: Perform collaborative update processing based on the verification results and the optimal lateral trajectory. By resolving the longitudinal acceleration sequence and dynamically adjusting the lateral trigger time, the terminal state constraints are integrated to generate longitudinal-lateral motion update instructions.

[0053] Step S600: Perform forward sequential lane change loop control according to the longitudinal-lateral motion update command to obtain the merging result.

[0054] Understandably, this step decomposes complex group collaboration into individual vehicle sequential execution modules through a cyclical operation chain of "leave empty - change lanes - leave empty". This step addresses emergency scenarios such as sudden changes in the number of lanes on construction sections by replacing the ambiguous collaboration of human vehicle fleets with precise robotic arm-like operations. While maintaining the turbulence intensity of traffic flow on the main road, it allows the entire merging process to proceed step-by-step until completion, providing an efficient, energy-saving, and safe collaborative solution for intelligent connected vehicle fleets in scenarios such as ramp merging and bottleneck sections. Furthermore, step S600 includes steps S610 to S630.

[0055] Step S610: According to the longitudinal control sequence in the longitudinal-lateral motion update instruction, perform the first vehicle safety clearance creation process, and control the assist vehicle to decelerate so that the distance between the target vehicle and the assist vehicle is expanded to the safe following distance threshold, and obtain the first vehicle safety clearance completion signal.

[0056] Step S620: Based on the first vehicle's safety clearance completion signal and lateral motion update instruction, perform the target vehicle's lateral lane change execution processing, generate the optimal steering trajectory through the lateral dynamics model and verify the lane change completion status in real time, and obtain the target vehicle's lane change completion signal.

[0057] Step S630: Based on the target vehicle's lane change completion signal and longitudinal motion update instruction, perform subsequent vehicle filling and cyclic advancement processing. By controlling the subsequent vehicles to accelerate and fill the gaps and update the convoy order, until the last vehicle in the convoy merges into the target lane, the convoy merging completion result is obtained.

[0058] Specifically, the overall planning process is as follows:

[0059] First, the motion of a connected vehicle (CAV) is decomposed into longitudinal motion and lateral motion, and a trajectory planning problem for the longitudinal motion is constructed. Vehicle dynamics and range constraints, safe following constraints, safe merging constraints, and initial state constraints are then established.

[0060] definition Any vehicle The longitudinal position coordinates on the current lane (measured from the foremost position of the vehicle body). and They are vehicles The velocity and acceleration. For any vehicle The longitudinal dynamics of a vehicle can be expressed as:

[0061]

[0062] Among them, S j L, F, and P represent vehicle identifiers, respectively; j represents the vehicle ID. Represents the vehicle dynamics formula; This represents the vehicle's speed at time t; and Both represent the vehicle's acceleration at time t; This represents the speed of the vehicle at time t.

[0063] The actual driving speed and acceleration of real vehicles are limited by range. For simplicity, this paper does not consider the differences in this parameter range among different vehicles, i.e., the constraint is satisfied:

[0064]

[0065] Among them, v min ,a min These are the minimum speed and minimum acceleration limits, respectively. max With a max These are the maximum speed and maximum acceleration limits, respectively.

[0066] Before the target vehicle merges into the target vehicle, all participating vehicles in the formation should maintain a safe following distance. The safe following distance between the following vehicle j and the preceding vehicle k is primarily related to the speed v of the following vehicle j. j (t) is related to, and is defined as

[0067]

[0068] Where φ is the headway constant between vehicle j and vehicle k (taken as 1.5s), δ is the safe distance constant when the vehicles are stationary (taken as 2m), and l veh This refers to the length of the vehicle body (taken as 4.5m).

[0069] Before a vehicle merges into another lane, the two adjacent vehicles in the same lane must meet the safe following distance constraints. For assisting vehicles, i.e., the current lead vehicle of the target convoy, such as... Figure 3 As shown, the following conditions are met:

[0070]

[0071] Where, x P (t) represents the position of the vehicle in front at time t; It is vehicle S i The start time of longitudinal motion; It is vehicle S i The end time of the longitudinal motion; x F (t) represents the position of the following vehicle at time t; d PF (v F (t) represents the safe following distance between the following car F and the preceding car P as a function of the speed of the following car F; t represents time t. For the target car, as the last car in the target convoy, during the longitudinal movement phase, it still maintains the same dynamics (the same speed v) as the other cars in the current convoy. S (t) and control acceleration aS (t)), therefore the main consideration is the target team's lead car (denoted as S). 0 The safe following distance must meet the following requirements:

[0072]

[0073] Where, x L (t) represents the position of vehicle L; d LS (v S (t) represents the safe following distance between the following vehicle S and the preceding vehicle L as a function of the speed of the following vehicle S; It is an intelligent connected vehicle S 0 The location can be obtained through communication technology in an intelligent connected environment.

[0074] The relative positions of the target vehicle when it merges in are shown in the figure. Vehicle S and vehicles F and P satisfy the corresponding safety distance constraints to avoid collisions with assisting vehicles or the target vehicle in front after merging in lane. That is, the safety distance constraint between the target vehicle in front and the target vehicle is:

[0075]

[0076] The safety distance constraint between the target vehicle and the assisting vehicle is:

[0077]

[0078] in, Indicates that vehicle P is at time... Location; Indicates that S is at time... The location.

[0079] The starting state of a vehicle refers to the moment when coordinated lane changing is triggered. For the first lead vehicle in the convoy, the starting moment is the moment when the convoy combination intention is satisfied. For the subsequent i-th lead vehicle, the starting moment is the moment when the (i-1)-th vehicle completes its lateral movement. The starting state is constrained by:

[0080]

[0081] in, The dynamic formula representing vehicle S; Indicates time The position of vehicle S; Indicates time The speed of vehicle S; The dynamic formula representing vehicle F; Indicates time The position of vehicle F; Indicates time The speed of vehicle F; These are known vehicle motion constants.

[0082] The goal of this problem is to minimize the cooperative lane-changing time of the target vehicle and the fuel consumption of all intelligent vehicles. The intelligent vehicles considered refer to all vehicles in both the target fleet and the current fleet. For stage i... This represents the time domain from the triggering of the coordinated lane change of the i-th CAV to the triggering of the (i+1)-th CAV. The objective function for this stage is:

[0083]

[0084] Where, α t >0,α a >0 represents the weights related to time and fuel consumption. The larger the value, the higher the requirement for saving operating time; the smaller the value, the more stringent the requirement for reducing fuel consumption.

[0085] In summary, the optimal trajectory planning problem M1 in the longitudinal motion phase is described as follows:

[0086]

[0087] Problem M1 is a complex model characterized by nonlinearity, multiple objectives, and multiple constraints, making it slow and difficult to solve using general methods. This section employs a two-stage approach to plan the longitudinal trajectory of the intelligent vehicle, aiming to accelerate the solution. The first stage is a position planning problem, planning the desired position of the merging vehicle, which is then passed as input to the second stage. The second stage is a trajectory planning problem with free time and a determined terminal position, generating the "control acceleration-time" curves for the target vehicle and the assisting vehicle.

[0088] Because the final meeting point of the vehicles is flexible, it is difficult to find a globally optimal solution for problem M1. Therefore, in the first stage of the model, the meeting point of the vehicles can be planned first, so that the problem in the next stage incorporates the constraint of a fixed final position, making it easier to find a local optimum more quickly. If the motion of the target vehicle is ignored, the longitudinal motion can be viewed as the assisting vehicle F actively detecting the state of the target vehicle P ahead and decelerating to leave a gap large enough to accommodate one vehicle. Therefore, the final meeting point is mainly determined by the assisting vehicle. Thus, the desired meeting point can be obtained by separately considering the optimal trajectory of the assisting vehicle.

[0089] Question M2:

[0090]

[0091] Among them, l veh These represent the lengths of the vehicles. δ and δ are model parameters.

[0092] Equation (12-1) is the objective function, which includes two objectives: minimizing travel time and minimizing the indirect fuel consumption of vehicle F. α t and α a / 2 represents the weights of these two terms. Equation (12-2) is the constraint of the second-order dynamic model of the vehicle system. Equation (12-3) is the initial state constraint. Equations (12-4) and (12-5) are the speed and spacing constraints under the ideal state, respectively. The former indicates that the speed of the assisting vehicle is consistent with that of the target vehicle in front. The latter indicates that the distance between the front ends of the two vehicles is just enough to accommodate a vehicle with the same speed. The use of "=" is to solve for the desired meeting position, and does not mean that the actual requirement is that the vehicles strictly conform to the ideal state.

[0093] The optimal control problem M2 is a functional extremum problem with equality constraints. It exhibits characteristics such as a time-varying system, an integral objective function, terminal freedom, and terminal state constraints. It can be solved using the minimum principle. Based on the necessary condition for the optimal solution, let the Hamiltonian function be:

[0094]

[0095] Where H represents the Hamiltonian function; λ T Let λt represent the Lagrange matrix; λ1(t) and λ2(t) represent the Lagrange parameters; and λ = [λ1(t), λ2(t)] represent the costate variables. T It satisfies the canonical equation:

[0096]

[0097] in, and Let k1 and k2 be the derivatives of the Lagrange parameters. k1 and k2 are the integral constants to be solved. Combining the terminal state constraints into a vector Π, we have:

[0098]

[0099] Therefore, the boundary conditions are:

[0100]

[0101] Where γ1 and γ2 are both constants, the extremum condition is:

[0102]

[0103] The Hamiltonian function should satisfy the following at the end of the optimal trajectory curve:

[0104]

[0105] Substituting (14)-(19) into (20), we can derive the following equation:

[0106]

[0107] Therefore:

[0108]

[0109] By combining the initial and final states with equation (21), we can solve for k1, k2, k3, k4. variable.

[0110]

[0111] Since the state of car P is unknown, the above formula regarding car F at the terminal time... The speed and position of vehicle F can only be estimated based on the current state value of vehicle P, because vehicle F always tracks vehicle P in real time and intends to maintain the same speed and the minimum time difference with it, that is:

[0112]

[0113] in, For a moment The expected location of vehicle F; For a moment The position of vehicle P; It represents the safe following distance between the following vehicle S and the preceding vehicle P, and the speed of the following vehicle S.

[0114] Solving the simultaneous equations yields the desired position S of car F. F As shown in equation (30). Furthermore, if the target vehicle has a forced lane-changing position, such as the starting position of a lane reduction, its longitudinal position is represented by S. M If it means:

[0115]

[0116] In the next stage of optimization, the expected terminal time obtained this time will be used. Alternate terminal time To distinguish the actual end time of the final solution.

[0117] Based on the expected vehicle merging locations provided in the first phase, the terminal location can be fixed. In addition to problem M1, the following terminal location constraints are added:

[0118]

[0119] Therefore, longitudinal trajectory planning problems for the main vehicle S and the assisting vehicle F are constructed respectively, with fuel consumption and time joint optimization models as M3 and M4 respectively:

[0120]

[0121] For ease of mathematical expression, problems M3 or M4 can be abbreviated as:

[0122]

[0123] Where z(t) = [x F (t),x S (t),v F (t),v S (t)] T It is a state variable, u(t) = [a F (t),a S (t)] T These are the control variables, B(X) represents all equality constraints, C(X) represents all inequality constraints, and the integrand of the objective function is expressed as L(u(t)) = α. a a F (t) 2 +α a a S (t) 2 +α t .

[0124] The above problem is a continuous optimal control problem (COCP), which can be transformed into a discrete nonlinear programming problem (DNLP) through discretization. This paper adopts the direct collocation method: firstly, the continuous time interval is dispersed into a finite time series, that is, the time is divided at equal intervals. K sub-intervals The time length of the subinterval is denoted as h. j Where j = 0, 1, ..., K-1, thus obtaining K+1 time nodes. Therefore, the time constraint after discretization is:

[0125]

[0126] Then, the state variables (i.e., position and velocity) and the control variables (i.e., acceleration) are expressed as collocational values ​​with respect to discrete time. Specifically, in the time domain... Inside, take the midpoint. As collocation points, there are a total of 2K+1 discrete time points and corresponding state variable and control variable values, where j∈{0,0.5,1,…,K-0.5,K}. For ease of representation, all variables are described as...

[0127]

[0128] From the preceding vehicle longitudinal dynamics equations, the differential of the state function can be expressed as: Using the third-order Simpson method, at the midpoint of the subinterval The state variable values ​​of (i.e., collocation points) can be obtained from the states of each node through Hermite interpolation:

[0129]

[0130] Similarly, the control input of this invention is considered as an acceleration constant, and thus... Therefore, the control variable value at the collocation point is:

[0131]

[0132] The state equations can be obtained by applying the third-order Simpson integral formula over the entire subinterval:

[0133]

[0134] Finally, according to the Simpson integral formula rule, the objective function can be estimated as:

[0135]

[0136] Therefore, the transformed nonlinear programming problem is:

[0137]

[0138] This paper uses the sequential quadratic programming method to solve the lower-level programming problem.

[0139] This paper assumes that the speed of all vehicles in the system is constant during the lateral movement. Therefore, as long as the vehicles meet the longitudinal safety constraints at the start of the lateral movement, collision avoidance during the lateral movement is guaranteed, and collision safety constraints are no longer considered. This is a reasonable assumption because the duration of the lateral movement is very short compared to the duration of the longitudinal movement phase.

[0140] To describe the kinematics of a vehicle changing lanes during the lane change, a single-vehicle model is used, such as... Figure 3 As shown, the two front wheels and two rear wheels are each represented by a virtual wheel at their center, and the vehicle's position is represented by the midpoint between two virtual wheels. The front virtual wheels represent the vehicle's steering. The vehicle's lateral dynamics can be represented by a second-order dynamics model:

[0141]

[0142] in and Let ω represent the start and end times of the lateral lane-changing movement of the i-th vehicle, respectively. s (t) represents the angular velocity of the front wheel at time t, φ s (t) represents the angle of rotation of the front wheel at time t.

[0143] During the lateral movement phase, this paper does not consider the differences in vehicle boundary parameters, and the target vehicle also satisfies the following boundary constraints:

[0144] θ min ≤θ s (t)≤θ max (43)

[0145] ω min ≤ω s (t)≤ω max (44)

[0146] φ min ≤φ s (t)≤φ max (45)

[0147] In addition, the vehicle's lateral lane change satisfies the initial and final state constraints:

[0148]

[0149] Among them l la It represents the width of a single lane, indicating the lateral displacement of a vehicle from the centerline of the current lane to the target lane.

[0150] Similar to the longitudinal motion phase, the objective function considers minimizing the lateral motion completion time. With fuel consumption estimate ω s (t) 2 Therefore, the objective function for the lateral optimal trajectory planning is:

[0151]

[0152] Assumption This refers to the start time of the vehicle's lateral movement. It's easy to see that the most conservative approach is to set the start time equal to the time when the merging conditions are met. in This can be obtained from the results calculated in Section 3. Because the merging condition requires that the speeds and spacing of cars F, S, and P meet relatively strict standards, the moment when the merging condition is met is not the ideal earliest start time for lateral movement. If the target car S is not the lead car of the original convoy, i.e., the last car to change lanes, the start time of the next cycle can be advanced to the end time of vehicle S's lateral movement, i.e., when the merging condition is met...

[0153] Therefore, this paper proposes a method for determining the earliest lateral movement start time, which makes... Therefore T i Get as close to the shortest time as possible Assuming that no vehicle in the longitudinal direction collides with the target lane-changing vehicle S, the earliest time can be described as:

[0154]

[0155] in Representative and The safe distance is related to the vehicle's speed. Therefore, it is set as follows: The earliest time of lateral movement can be obtained:

[0156]

[0157] Assuming that the speeds of all participating vehicles remain constant during lateral movement, the actual speeds of the manually driven vehicles P and L may change slightly, and the longitudinal velocity of vehicle S is less than its actual vector velocity (which has a lateral velocity component). Therefore, there is still a risk of collision during lateral lane changes, and it cannot be guaranteed that there will be no collision. It is strictly feasible. Find the optimal longitudinal trajectory at the current moment, and define the safety constraint SC condition as follows:

[0158]

[0159] Where g pred The g value represents the estimated optimal longitudinal trajectory predicted based on the current moment. and This represents the estimated value under the condition of maintaining a constant speed.

[0160] Preferably, in this embodiment, the following process is used to check whether the start time of the lateral movement meets the safety constraints:

[0161] Input: Expected time t e Search step size Δt;

[0162] Output: Whether the movement is lateral, and the optimal trajectory for lateral movement;

[0163] If the current time t≥t e And flag == 1;

[0164] calculate Estimates of lateral motion programming and And the predicted value of x(t)+d(v(t)) under longitudinal motion programming;

[0165] If the flag value is equal to 1, check if the SC condition is met: if it is met, flag = 0; This condition is not met, so delay Δt.

[0166] Get the command to start lateral movement, and Optimal trajectory within and

[0167] Therefore, with Gradually approaching the endpoint of the longitudinal movement The probability of satisfying safety constraint SC also increases.

[0168] The dynamics of manually driven vehicles are uncertain. If the trajectory of the intelligent vehicle is planned according to the initial state, it may lead to certain safety risks. Therefore, trajectory planning must be carried out dynamically.

[0169] This paper designs a backtracking time-domain algorithm. To ensure that the calculated trajectory can adapt to the current vehicle speed, the endpoint time in the model is adjusted. It should be updated dynamically. The specific method is as follows: first determine a small time step Δt, and start time... Rolling backwards with a step size Δt, the current optimization time domain is: The first stage of the model is based on the states of the preceding vehicle L, the target preceding vehicle P, and the intelligent connected vehicle. Solving the model yields the desired convergence position. The second stage of the model then uses the desired convergence position and... Generate optimal control acceleration This allows us to obtain the state of the intelligent vehicle at the next moment. Similarly, the optimization time domain for the next time step is: The state of the intelligent vehicle was obtained after two stages of optimization. And so on, until the vehicle status... The rendezvous conditions are met, at this time No further updates are needed, indicating the end of dynamic optimization. The optimal terminal time is...

[0170] Next, consider the criteria for determining when vehicles have successfully merged. Assume an ideal merging state, which can be interpreted as follows: the task is for a convoy of n vehicles to change lanes to the target lane; for the nth vehicle, at the moment of merging completion… Regarding speed, this vehicle and all other vehicles in the convoy, including assisting vehicles, maintain the same speed as the manually driven vehicle to ensure traffic flow stability and efficiency. In terms of longitudinal spacing, this vehicle maintains a minimum safe distance from assisting vehicles and the target vehicle ahead. Within permissible limits, when considering... The conditions for determining whether vehicle merging is complete are as follows:

[0171]

[0172] in These represent the maximum permissible values ​​for speed and distance deviation, respectively. The above formula indicates that the error between the vehicle merging speed and position and the ideal state is not higher than the maximum permissible value.

[0173] Example 2:

[0174] like Figure 2 As shown, this embodiment provides a multi-target trajectory optimization device based on a forward sequential lane-changing strategy. The device includes:

[0175] The acquisition module 901 is used to acquire first information, which includes the real-time position, speed, and acceleration data of the target vehicle fleet and the manually driven vehicles and intelligent connected vehicles in the current lane, the lane width parameters of the target lane, and the headway constant, stationary safety distance constant, and vehicle length in the safe following distance constraint.

[0176] The calculation module 902 is used to calculate the expected meeting position of the vehicles based on the first information and obtain a first calculation result. The first calculation result includes the expected meeting position of the target vehicle and the assisting vehicle and the longitudinal movement terminal time.

[0177] The generation module 903 is used to generate a longitudinal acceleration control sequence based on the first calculation result, so as to obtain a longitudinal motion control command sequence for the target vehicle and the assisting vehicle.

[0178] The determination module 904 is used to determine the lateral motion trigger time based on the longitudinal motion control command sequence, and to obtain the lateral motion start command sequence and the optimal lateral trajectory.

[0179] The optimization module 905 is used to perform rolling time-domain optimization updates based on the longitudinal motion control command sequence, the lateral motion start command sequence and the optimal lateral trajectory to obtain longitudinal-lateral motion update commands.

[0180] The control module 906 is used to perform forward sequential lane change cyclic control based on the longitudinal-lateral motion update command to obtain the merging result.

[0181] In one specific embodiment of this application, the computing module 902 includes:

[0182] The first calculation unit is used to construct and process the optimal control problem of the assisted vehicle based on the first information. By defining the second-order dynamic equation of the vehicle and integrating the safe following constraint, the terminal speed consistency condition and the minimum safe distance constraint, an objective function is constructed with the goal of minimizing the joint time-fuel consumption.

[0183] The second calculation unit is used to perform analytical processing of the costate equation based on the objective function to obtain parameterized functions of vehicle acceleration, velocity and position;

[0184] The third calculation unit is used to solve for the terminal position and time according to the parameterized function, so as to obtain the expected meeting position of the target vehicle and the assistance vehicle and the longitudinal movement terminal time.

[0185] In one specific embodiment of this application, the generation module 903 includes:

[0186] The first generation unit is used to construct a multi-objective optimization model based on the expected rendezvous position and terminal time in the first calculation result. By integrating the time cost weight coefficient and the fuel consumption weight coefficient, a joint optimization objective function is defined to obtain the longitudinal trajectory optimization model.

[0187] The second generation unit is used to discretize the continuous optimal control problem based on the longitudinal trajectory optimization model. It divides the time domain into equal intervals using the direct collocation method and uses the third-order Simpson integral formula to numerically discretize the state equation and objective function to obtain the discrete nonlinear programming problem.

[0188] The third generation unit is used to solve the control sequence based on the discrete nonlinear programming problem. It iteratively solves the acceleration values ​​at each discrete time node and verifies the feasibility of the constraints to obtain the longitudinal motion control command sequence of the target vehicle and the assisting vehicle.

[0189] In one specific embodiment of this application, the determination module 904 includes:

[0190] The first determination unit is used to calculate the minimum safe time point based on the longitudinal motion control command sequence. It predicts the future trajectories of the target vehicle, the vehicle in front of the target, the assisting vehicle, and the following vehicles in the convoy, and calculates the minimum safe time points between the target vehicle and the vehicle in front of the target, between the target vehicle and the assisting vehicle, and between the following vehicles in the convoy and the target vehicle, respectively, to obtain three types of minimum safe time points.

[0191] The second determination unit is used to determine the earliest start time of lateral movement based on three types of minimum safe time points. The theoretical earliest start time is determined by taking the maximum value of the three, and the theoretical trigger time of lateral movement is obtained.

[0192] The third determination unit is used to verify the feasibility of triggering and generate the trajectory based on the triggering time of the lateral motion theory, so as to obtain the lateral motion start command and the optimal lateral trajectory.

[0193] In one specific embodiment of this application, the optimization module 905 includes:

[0194] The first optimization unit is used to perform dynamic time domain extraction processing based on the longitudinal motion control command sequence and the lateral motion start command sequence. By setting a fixed time step and rolling the time window backward, the initial optimization time domain is updated to a sub-interval from the current time to the terminal time, thus obtaining the optimized time domain interval after rolling update.

[0195] The second optimization unit is used to perform terminal state constraint verification processing based on the optimization time domain interval to obtain the verification result. The verification process involves calculating whether the vehicle speed deviation and position deviation meet the maximum allowable threshold to determine the merging completion condition: if they are met, the optimization is terminated; otherwise, rolling updates continue.

[0196] The third optimization unit is used to perform collaborative update processing based on the verification results and the optimal lateral trajectory. It generates longitudinal-lateral motion update instructions by resolving the longitudinal acceleration sequence and dynamically adjusting the lateral trigger time, and integrating the terminal state constraints.

[0197] In one specific embodiment of this application, the control module 906 includes:

[0198] The first control unit is used to perform the first vehicle safety clearance creation process according to the longitudinal control sequence in the longitudinal-lateral motion update instruction, and to increase the distance between the target vehicle and the assist vehicle to the safe following distance threshold by controlling the assist vehicle to decelerate, and to obtain the first vehicle safety clearance completion signal.

[0199] The second control unit is used to perform lateral lane change execution processing of the target vehicle based on the safety clearance completion signal of the first vehicle and the lateral movement update command, generate the optimal steering trajectory through the lateral dynamics model and verify the lane change completion status in real time to obtain the target vehicle lane change completion signal.

[0200] The third control unit is used to perform subsequent vehicle filling and cyclical advancement processing based on the target vehicle's lane change completion signal and longitudinal movement update command. It controls the subsequent vehicles to accelerate and fill the gaps and update the convoy order until the last vehicle in the convoy merges into the target lane, thus obtaining the convoy merging completion result.

[0201] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention.

Claims

1. A multi-objective trajectory optimization method based on a forward sequential lane-changing strategy, characterized in that, include: The first information includes the real-time position, speed, and acceleration data of the target vehicle fleet and the manually driven vehicles and intelligent connected vehicles in the current lane, the lane width parameter of the target lane, and the headway constant, stationary safety distance constant, and vehicle length in the safe following distance constraint. Based on the first information, the expected meeting position of the vehicles is calculated to obtain a first calculation result, which includes the expected meeting position of the target vehicle and the assisting vehicle and the longitudinal movement end time. Based on the first calculation result, a longitudinal acceleration control sequence is generated to obtain a longitudinal motion control command sequence for the target vehicle and the assisting vehicle. Based on the longitudinal motion control command sequence, the lateral motion trigger time is determined to obtain the lateral motion start command sequence and the optimal lateral trajectory. Based on the longitudinal motion control command sequence, the lateral motion start command sequence, and the optimal lateral trajectory, a rolling time-domain optimization update is performed to obtain the longitudinal-lateral motion update command. Based on the longitudinal-lateral motion update command, perform forward sequential lane change cyclic control to obtain the merging result; Specifically, the determination of the lateral movement trigger time based on the longitudinal motion control command sequence yields the lateral movement initiation command sequence and the optimal lateral trajectory, including: The minimum safe time point is calculated based on the longitudinal motion control command sequence. By predicting the future trajectories of the target vehicle, the vehicle in front of the target, the assisting vehicle, and the following vehicles in the convoy, the minimum safe time points between the target vehicle and the vehicle in front of the target, between the target vehicle and the assisting vehicle, and between the following vehicles in the convoy and the target vehicle are calculated respectively, resulting in three types of minimum safe time points. The earliest start time of lateral movement is determined based on the three types of minimum safe time points. The theoretical earliest start time is determined by taking the maximum value of the three, and the theoretical trigger time of lateral movement is obtained. Based on the theoretical triggering time of the lateral motion, the feasibility of triggering and trajectory generation are verified to obtain the lateral motion start command and the optimal lateral trajectory.

2. The multi-objective trajectory optimization method based on a forward sequential lane-changing strategy according to claim 1, characterized in that, Based on the first information, the expected merging position of the vehicles is calculated to obtain a first calculation result, including: Based on the first information, the optimal control problem of the assisted vehicle is constructed and processed. By defining the second-order dynamic equation of the vehicle and integrating the safe following constraint, the terminal speed consistency condition and the minimum safe distance constraint, an objective function is constructed with the goal of minimizing the joint time-fuel consumption. Based on the objective function, the costate equation is analyzed to obtain parameterized functions for vehicle acceleration, velocity, and position. The terminal position and time are calculated based on the parameterized function to obtain the expected meeting position of the target vehicle and the assisting vehicle and the longitudinal movement terminal time.

3. The multi-objective trajectory optimization method based on a forward sequential lane-changing strategy according to claim 1, characterized in that, Based on the first calculation result, a longitudinal acceleration control sequence is generated to obtain a longitudinal motion control command sequence for the target vehicle and the assisting vehicle, including: Based on the expected meeting point and terminal time in the first calculation result, a multi-objective optimization model is constructed. By integrating the time cost weight coefficient and the fuel consumption weight coefficient, a joint optimization objective function is defined to obtain the longitudinal trajectory optimization model. Based on the longitudinal trajectory optimization model, the continuous optimal control problem is discretized. The time domain is divided into equal intervals by the direct collocation method. The state equation and objective function are numerically discretized by the third-order Simpson integral formula to obtain the discrete nonlinear programming problem. The control sequence is solved based on the discrete nonlinear programming problem. The acceleration values ​​at each discrete time node are solved iteratively, and the feasibility of the constraints is verified to obtain the longitudinal motion control command sequence of the target vehicle and the assisting vehicle.

4. The multi-objective trajectory optimization method based on a forward sequential lane-changing strategy according to claim 1, characterized in that, Based on the longitudinal motion control command sequence, the lateral motion initiation command sequence, and the optimal lateral trajectory, a rolling time-domain optimization update is performed to obtain longitudinal-lateral motion update commands, including: Based on the longitudinal motion control command sequence and the lateral motion start command sequence, the time domain dynamic interception process is optimized. By setting a fixed time step and rolling the time window backward, the initial optimized time domain is updated to a sub-interval from the current time to the terminal time, and the optimized time domain interval after rolling update is obtained. The terminal state constraint verification process is performed based on the optimized time domain interval to obtain the verification result. The verification process involves calculating whether the vehicle speed deviation and position deviation meet the maximum allowable threshold to determine the merging completion condition. If the condition is met, the optimization is terminated; otherwise, the rolling update continues. Based on the verification results and the optimal lateral trajectory, a collaborative update process is performed. By resolving the longitudinal acceleration sequence and dynamically adjusting the lateral trigger time, the terminal state constraints are integrated to generate a longitudinal-lateral motion update instruction.

5. A multi-target trajectory optimization device based on a forward sequential lane-changing strategy, characterized in that, include: The acquisition module is used to acquire first information, which includes the real-time position, speed, and acceleration data of the target vehicle fleet and the manually driven vehicles and intelligent connected vehicles in the current lane, the lane width parameter of the target lane, and the headway constant, stationary safety distance constant, and vehicle length in the safe following distance constraint. The calculation module is used to calculate the expected meeting position of the vehicles based on the first information and obtain a first calculation result, which includes the expected meeting position of the target vehicle and the assisting vehicle and the longitudinal movement terminal time. The generation module is used to generate a longitudinal acceleration control sequence based on the first calculation result, so as to obtain a longitudinal motion control command sequence for the target vehicle and the assisting vehicle. The determination module is used to determine the lateral movement trigger time based on the longitudinal motion control command sequence, and to obtain the lateral movement start command sequence and the optimal lateral trajectory. The optimization module is used to perform rolling time-domain optimization updates based on the longitudinal motion control command sequence, the lateral motion start command sequence, and the optimal lateral trajectory to obtain longitudinal-lateral motion update commands. The control module is used to perform forward sequential lane-changing cyclic control based on longitudinal-lateral motion update commands to obtain the merging result; The determination module includes: The first determination unit is used to perform minimum safe time point calculation processing according to the longitudinal motion control command sequence. By predicting the future trajectories of the target vehicle, the target preceding vehicle, the assisting vehicle and the following vehicle in the convoy, and calculating the minimum safe time points between the target vehicle and the target preceding vehicle, the target vehicle and the assisting vehicle, and the following vehicle in the convoy and the target vehicle respectively, three types of minimum safe time points are obtained. The second determination unit is used to determine the earliest start time of lateral movement based on the three types of minimum safe time points. The theoretical earliest start time is determined by taking the maximum value of the three, and the theoretical trigger time of lateral movement is obtained. The third determination unit is used to perform triggering feasibility verification and trajectory generation processing based on the triggering time of the lateral motion theory, so as to obtain the lateral motion start command and the optimal lateral trajectory.

6. The multi-target trajectory optimization device based on a forward sequential lane-changing strategy according to claim 5, characterized in that, The computing module includes: The first calculation unit is used to construct and process the optimal control problem of the assisted vehicle based on the first information. By defining the second-order dynamic equation of the vehicle and integrating the safe following constraint, the terminal speed consistency condition and the minimum safe distance constraint, an objective function is constructed with the goal of minimizing the joint time-fuel consumption. The second calculation unit is used to perform costate equation analytical processing based on the objective function to obtain parameterized functions of vehicle acceleration, velocity and position; The third calculation unit is used to perform terminal position and time calculation based on the parameterized function to obtain the expected meeting position of the target vehicle and the assisting vehicle and the longitudinal movement terminal time.

7. The multi-target trajectory optimization device based on a forward sequential lane-changing strategy according to claim 5, characterized in that, The generation module includes: The first generation unit is used to construct a multi-objective optimization model based on the expected rendezvous position and terminal time in the first calculation result, and to define a joint optimization objective function by integrating the time cost weight coefficient and the fuel consumption weight coefficient to obtain the longitudinal trajectory optimization model. The second generation unit is used to discretize the continuous optimal control problem according to the longitudinal trajectory optimization model. The time domain is divided into equal intervals by the direct collocation method, and the state equation and objective function are numerically discretized by the third-order Simpson integral formula to obtain the discrete nonlinear programming problem. The third generation unit is used to solve the control sequence according to the discrete nonlinear programming problem. It iteratively solves the acceleration value at each discrete time node and verifies the feasibility of the constraints to obtain the longitudinal motion control command sequence of the target vehicle and the assisting vehicle.

8. The multi-target trajectory optimization device based on a forward sequential lane-changing strategy according to claim 5, characterized in that, The optimization module includes: The first optimization unit is used to perform dynamic time domain truncation processing based on the longitudinal motion control command sequence and the lateral motion start command sequence. By setting a fixed time step and rolling the time window backward, the initial optimization time domain is updated to a sub-interval from the current time to the terminal time, and the updated optimization time domain interval is obtained. The second optimization unit is used to perform terminal state constraint verification processing according to the optimization time domain interval to obtain the verification result. The verification process is to determine whether the vehicle speed deviation and position deviation meet the maximum allowable threshold and determine the merging completion condition: if they meet the threshold, the optimization is terminated; otherwise, the rolling update continues. The third optimization unit is used to perform collaborative update processing based on the verification results and the optimal lateral trajectory. By resolving the longitudinal acceleration sequence and dynamically adjusting the lateral trigger time, it integrates the terminal state constraints to generate longitudinal-lateral motion update instructions.

Images