Expressway vehicle ramp convergence decision-making method and system based on insertable gap

Abstract

The invention discloses a highway vehicle ramp convergence decision-making method and system based on an insertable gap, and relates to the technical field of intelligent traffic control. According to the method, an expressway vehicle ramp confluence scene is constructed, a to-be-planned automatic driving vehicle is selected as a main vehicle, vehicle information of all vehicles and the main vehicle on a lane outside a main road in the expressway vehicle ramp confluence scene is acquired, and the main vehicle confluence process is divided into two stages. According to the method, a main vehicle confluence trajectory planning scheme is formulated by combining whether a main vehicle triggers a preset cut-in condition or not, a longitudinal trajectory planning scheme before the main vehicle cuts into a ramp opening from a confluence lane and a two-dimensional trajectory planning scheme when the main vehicle cuts into the ramp opening are obtained, and the main vehicle is guided to safely confluence into a lane on the outer side of an expressway from the ramp. The problems that the lane changing space of the automatic driving vehicle is narrow, the safety is low and the influence on the traffic flow is large when the vehicle is converged at the highway ramp are effectively solved, and the driving safety of the vehicle when the highway ramp is cut in is ensured.

Description

Technical Field

[0001] The present invention relates to the technical field of intelligent traffic control, and in particular to a highway vehicle ramp merging decision method and system based on insertable gaps. Background Art

[0002] The highway ramp merging scenario is a highway driving scenario with an autonomous driving lane and a ramp on the inner side. Its characteristics are: 1. Due to the existence of the autonomous driving lane, it is assumed that all autonomous driving vehicles drive on the inner lane of the main road of the highway, while only manually driven vehicles drive on the outer lane of the main road of the highway; 2. There will be both manually driven vehicles and autonomous driving vehicles on the merging lane, that is, for autonomous driving vehicles, the front and rear vehicles driving on the ramp may not be able to cooperate in operation; 3. Due to the limitation that manually driven vehicles do not have communication capabilities, the planning scheme of the autonomous driving vehicle cannot communicate with the environmental vehicles, and can only obtain the historical status information and current status information of the environmental vehicles through communication with the road test unit.

[0003] Since the highway vehicle ramp merging scenario has the above characteristics, there are at least the following problems:

[0004] (1) The gap selection problem formed by manually driven vehicles on the outer lanes of the main highway.

[0005] (2) The trajectory planning problem of vehicles merging into and cutting into highway ramps.

[0006] For the first question, the choice of gap significantly affects the efficiency and success rate of vehicle lane changes, and thus affects the efficiency of the entire traffic flow. At present, research on gap selection is mostly focused on gap selection in all intelligent network environments, in which environmental vehicles can actively cooperate in lane changes, and there is no problem of competition between main road and ramp vehicles; few studies have discussed the choice of gaps when autonomous driving vehicles face manual driving environments, and have ignored the driving behavior of manual driving vehicles when facing the main vehicle cutting in. They are more based on absolute safety considerations on whether the gap can be selected rather than whether the gap selection is optimal, which has a negative impact on traffic flow efficiency.

[0007] For the second question, after the gap selection is completed, the main vehicle performs trajectory planning to complete the cut-in. At present, when solving trajectory planning problems, most of them are based on the method of first sampling, then optimization, and then evaluation to simplify the problem and then solve the trajectory. This method has significant problems: if the number of samples is small, the interval between feature data is large and the solution accuracy is low; on the contrary, if fine sampling is performed, there will be multiple parallel sub-problems to be solved, and the solution efficiency is low. Although parallel computing can solve this problem to a certain extent, it will greatly increase the cost for single-vehicle intelligence.

[0008] Therefore, it is urgent to propose a highway vehicle ramp merging decision method and system based on insertable gaps to balance the trajectory planning calculation accuracy and efficiency, and to achieve precise control of insertable gap identification, selection and trajectory planning when the vehicle merges from the ramp into the outer lane of the highway. Summary of the invention

[0009] The object of the present invention is to provide a highway vehicle ramp merging decision method based on an insertable gap, aiming to solve at least one of the technical problems existing in the above-mentioned prior art. To achieve the above-mentioned object, the technical solution adopted is as follows:

[0010] The highway vehicle ramp merging decision method based on insertable gap includes the following steps:

[0011] Obtain vehicle information of vehicles on the outer lane of the main road in the highway vehicle ramp merging scenario, as well as the main vehicle in the lane to be merged, wherein the main vehicle is the autonomous driving vehicle to be planned, and the vehicle information includes the displacement, speed and acceleration of the current vehicle;

[0012] Determining whether the main vehicle has reached a decision planning trigger point based on the vehicle information of the main vehicle;

[0013] When the main vehicle reaches the decision planning trigger point, the first stage trajectory planning is iteratively executed until the main vehicle meets the cut-in condition, and the main vehicle is controlled to complete the longitudinal driving before cutting into the ramp entrance from the merging lane, wherein the cut-in condition is set based on the insertable gap of the main vehicle;

[0014] Iteratively executing the second-stage trajectory planning, and controlling the main vehicle to complete the ramp merging based on the second-stage trajectory planning, wherein the second-stage trajectory planning is a two-dimensional trajectory planning scheme for determining the main vehicle when entering the ramp entrance;

[0015] The first-stage trajectory planning and the second-stage trajectory planning are respectively executed based on the MPC framework.

[0016] Preferably, when the main vehicle reaches the decision planning trigger point, iteratively executing the first stage trajectory planning includes:

[0017] Under the MPC framework, the main vehicle's driving trajectory within a preset time period is iteratively planned, including:

[0018] Determine the reachable range of the host vehicle within a preset time period based on the vehicle information on the outer lane of the main road and the host vehicle to be merged into the lane, and identify the insertable gap;

[0019] Based on the pre-built first MILP problem, the optimal insertion gap, state during insertion, and trajectory of the main vehicle before insertion are calculated;

[0020] Determine whether the first MILP problem has a feasible solution. If so, set the calculated pre-insertion vehicle trajectory as a first-stage trajectory planning scheme, execute the first-stage trajectory planning scheme and update the time step to plan the main vehicle driving trajectory for the next round of preset time period; if there is no feasible solution, control the main vehicle to decelerate and update the time step to plan the main vehicle driving trajectory for the next round of preset time period;

[0021] Among them, the pre-construction of the first MILP problem includes setting the objective function of the first-stage trajectory planning scheme based on minimizing traffic impact and minimum lane change time, combining vehicle dynamics equations, kinematic equations, safety indicator threshold range when cutting in, vehicle following behavior, and acceleration length at the ramp to set constraints to establish the first MILP problem.

[0022] Preferably, the iterative execution of the second stage trajectory planning includes:

[0023] Based on the pre-built second-stage trajectory planning model, determine the longitudinal displacement termination state equation and the lateral displacement termination state equation of the main vehicle;

[0024] Establishing a second-stage optimization problem, under the MPC framework, iteratively solving the longitudinal displacement termination state equation and the lateral displacement termination state equation of the main vehicle based on the second-stage optimization problem, obtaining the longitudinal displacement termination state parameters and the lateral displacement termination state parameters of the main vehicle in each planning cycle, until the main vehicle completes the cut-in, including:

[0025] t end Produce several references by uniform sampling within a feasible setting range;

[0026] The environmental vehicle information at the current time step is compared with each reference t end As input, the pre-built second-stage trajectory planning model is iteratively solved based on the MPC framework to obtain each reference t end The corresponding optimal trajectory, based on each reference t end The corresponding best trajectories form a trajectory cluster;

[0027] According to the objective function in the second-stage optimization problem, the objective function value of each trajectory in the trajectory cluster is evaluated, the trajectory cluster is screened based on the objective function value, and the execution trajectory of the second stage is determined based on the screening result, the execution trajectory includes the longitudinal displacement termination state parameter and the lateral displacement termination state parameter of the main vehicle in each planning cycle,

[0028] The second-stage optimization problem is established based on a time-varying risk field model, and the establishment method includes:

[0029] The objective function is determined with the goal of minimizing lane change time, comfort and risk field intensity, and constraints are set by combining vehicle dynamics, kinematics, and road speed limit constraints to construct the second-stage optimization problem.

[0030] Preferably, the highway vehicle ramp merging scene is used to simulate the process of the main vehicle merging into the outer lane of the highway via the ramp, and the highway vehicle ramp merging scene includes a main road and a merging lane, wherein the main road is provided with multiple lanes, and the vehicles traveling on the outer lane of the main road are all manually driven vehicles, the merging lane is located on one side of the outer lane of the main road, and the merging area between the merging lane and the outer lane of the main road is the ramp entrance, and autonomous driving vehicles and manually driven vehicles are traveling on the merging lane.

[0031] Preferably, the vehicle information on the outer lane of the main road is Among them, s i =[x i , v i , a i ], i∈{1, 2, .., N m}, i is the serial number of the manually driven vehicle, N m is the total number of vehicles on the outer lane, s i is the vehicle information of the i-th vehicle, x i is the displacement of the ith vehicle, v i is the speed of the i-th vehicle, a i is the acceleration of the i-th vehicle;

[0032] The vehicle information of the host vehicle is s ego =[x ego , v ego , a ego ], where x ego is the displacement of the main vehicle, v ego is the speed of the main vehicle, a ego is the acceleration of the main vehicle;

[0033] The insertable gap is defined as the gap between the front vehicle and the rear vehicle. The cut-in condition is determined according to the length of the insertable gap, which is determined according to the reachable range of the main vehicle. The maximum reachable displacement of the main vehicle is for:

[0034]

[0035] In the formula, v ub is the maximum speed limit; a ub is the maximum acceleration limit; T is the preset time period;

[0036] The minimum achievable displacement of the main vehicle for:

[0037]

[0038] In the formula, v lb is the minimum speed limit; a lb is the minimum acceleration limit;

[0039] Based on the reachable range of the main vehicle Determine the insertable gap as:

[0040]

[0041] In the formula, is the position that the i-th manually driven vehicle on the outer lane can reach at the end of the preset time period, L is the length of the vehicle; d is the safe distance between adjacent vehicles.

[0042] Preferably, the preset time period is discretized into a plurality of time steps t at equal intervals, and vehicle information of the host vehicle and other vehicles on the highway is obtained in each time step, and the first MILP problem is established by combining vehicle dynamics equations, kinematic equations, safety index threshold range during cut-in, vehicle following behavior, and acceleration length at the ramp to set constraints;

[0043] The first MILP problem is expressed as:

[0044]

[0045] Where MinJ is the objective function of the first MILP problem; ω1, ω2, ω3 are all weight coefficients; t is the time step number; a t is the acceleration at time step t, v t is the velocity at time step t, s t is the vehicle information at time step t, which is a vector including position, velocity, and acceleration; a t+1 is the acceleration at time step t+1, s t+1 is the vehicle information at time step t+1; is the speed of the ith human-driven vehicle at time step t, is the speed of the i-1th manually driven vehicle at time step t; is the acceleration of the ith human-driven vehicle at time step t, is the acceleration of the i-th manually driven vehicle at time step t+1; They are all intermediate variables used for linearization; is the zero-one variable at time step t, is a zero-one variable at time step t+1, which is used to indicate whether the vehicle has changed lanes. After the vehicle changes lanes M is a preset parameter; is the intermediate variable of the ith manually driven vehicle at time step t, is the intermediate variable of the ith manually driven vehicle at time step t+1; s 0 is the vehicle information of the main vehicle at time 0; s init is the initial vehicle information of the main vehicle; is the ordinate of the main vehicle displacement at time step t, is the inflow indicator variable of the main vehicle at time step t, is the speed of the main vehicle at time step t; L acc is the length of the acceleration zone in the ramp entrance; N ac The number of linear programming problems transformed from MILP problems; γ i,i+1 is the insertable gap between vehicle i and vehicle i+1, γ i-1，i is the insertable gap between vehicle i-1 and vehicle i, is the insertable gap between vehicle i-1 and vehicle i at time step t; is the displacement of the ith human-driven vehicle at time step t, is the displacement of the i-1th human-driven vehicle at time step t; ∈ TTC is the threshold of TTC risk index; Δt is the time interval; is the acceleration calculated by the i-th vehicle for the following behavior of the preceding vehicle at time step t+1; is the acceleration calculated by the i-th vehicle for the following behavior of the cut-in vehicle at time step t+1.

[0046] Preferably, the pre-built second-stage trajectory planning model is a two-dimensional trajectory, expressed as:

[0047]

[0048] Wherein, tm is the moment during the cutting process; x(·) is the longitudinal position of the two-dimensional trajectory; y(·) is the lateral position of the two-dimensional trajectory; P = [p0, p1, p2, p3, p4, p5], p0, p1, p2, p3, p4, p5 are all longitudinal displacement control variables; Q = [q0, q1, q2, q3, q4, q5], q0, q1, q2, q3, q4, q5 are all lateral displacement control variables; T = [1, tm, tm 2 ,tm 3 ,tm 4 ,tm 5 ];

[0049] Based on the movement of the main vehicle in the x-axis and y-axis directions, the initial state and final state of the main vehicle meet the conditions:

[0050]

[0051] Where t0 is the start time of the second stage trajectory planning; the initial state of the main vehicle is Among them, x0 is the initial vertical coordinate, is the initial longitudinal velocity, is the initial longitudinal acceleration; y0 is the initial horizontal coordinate, is the initial lateral velocity, is the initial lateral acceleration; the final state of the main vehicle is Among them, x end To end the vertical axis, is the termination speed, is the termination acceleration, t end is the end time; tar is the ending horizontal axis;

[0052] The equation for determining the terminal state of the longitudinal displacement of the main vehicle is:

[0053]

[0054] The equation for determining the terminal state of the lateral displacement of the main vehicle is:

[0055]

[0056] Preferably, the objective function in the second MILP problem is set to:

[0057] minJ=ω1×J acc +ω2×J jerk +ω3×t end +ω4×J risk

[0058] in,

[0059]

[0060] In the formula, ω1, ω2, ω3, and ω4 are weight coefficients; J acc is the parameter affecting the comfort level due to acceleration; J jerk is the parameter affecting the acceleration change rate on comfort; J risk For environmental risk sites;

[0061] The environmental risk field risk for:

[0062] J risk =U front +U behind

[0063] in,

[0064]

[0065] U behind = k × U sta +(1-k)×U mov ;

[0066]

[0067] Where T″ is the second-order derivative of T; T″′ is the third-order derivative of T; U front is the risk field of the preceding vehicle; x front is the distance to the vehicle in front; σ x is the control coefficient in the longitudinal direction; σ y is the horizontal axis control coefficient; A is the field intensity amplitude; β is the risk field shape control parameter; U behind is the risk field intensity of the following vehicle; k is the weight coefficient; U sta is the static risk field intensity; U mov is the dynamic risk field intensity; exp(·) is the exponential function;

[0068] The speed, acceleration, and lateral acceleration along the x-axis are set based on vehicle dynamics constraints as follows:

[0069] v lb ≤P·T′≤v ub

[0070] a lb ≤P·T″≤a ub

[0071] -λ≤Q·T″≤λ

[0072] Where f′ is the first-order derivative of T; λ is the limiting threshold of centripetal acceleration.

[0073] The present invention also proposes a highway vehicle ramp merging decision system based on an insertable gap, which is applied to a highway vehicle ramp merging decision method based on an insertable gap as described in any one of claims 1 to 6, wherein the decision system comprises: a vehicle information acquisition device and a server;

[0074] The vehicle information acquisition device is used to acquire vehicle information of vehicles on the outer lane of the main road in the highway vehicle ramp merging scene, and the host vehicle in the lane to be merged, wherein the host vehicle is the autonomous driving vehicle to be planned, and the vehicle information includes the displacement, speed and acceleration of the current vehicle;

[0075] The server is used to determine whether the main vehicle has reached a decision planning trigger point based on the vehicle information of the main vehicle; when the main vehicle reaches the decision planning trigger point, iteratively execute the first stage trajectory planning until the main vehicle meets the cut-in condition, and control the main vehicle to execute the first stage trajectory based on the first stage trajectory planning, so as to control the main vehicle to complete the longitudinal driving before cutting into the ramp from the merging lane, wherein the cut-in condition includes identifying that the main vehicle has an insertable gap; and iteratively execute the second stage trajectory planning, and control the main vehicle to complete the ramp merging based on the second stage trajectory planning, wherein the second stage trajectory planning is a two-dimensional trajectory planning scheme for determining when the main vehicle cuts into the ramp; wherein the first stage trajectory planning and the second stage trajectory planning are respectively executed based on the MPC framework;

[0076] Wherein, the vehicle information acquisition device is communicatively connected to the server.

[0077] Preferably, the server comprises:

[0078] A judgment module, configured to judge whether the main vehicle has reached a decision planning trigger point based on the vehicle information of the main vehicle;

[0079] a gap recognition module, configured to iteratively execute the first stage trajectory planning when the main vehicle reaches the decision planning trigger point, until the main vehicle meets the cut-in condition, and then control the main vehicle to execute the first stage trajectory based on the first stage trajectory planning, so as to control the main vehicle to complete the longitudinal travel before cutting into the ramp entrance from the merging lane, wherein the cut-in condition includes recognizing that the main vehicle has an insertable gap;

[0080] The merging decision module is configured to iteratively execute the second-stage trajectory planning, and control the main vehicle to complete the ramp merging based on the second-stage trajectory planning, wherein the second-stage trajectory planning is to determine the two-dimensional trajectory planning scheme when the main vehicle enters the ramp entrance; wherein the first-stage trajectory planning and the second-stage trajectory planning are respectively executed based on the MPC framework.

[0081] Compared with the prior art, the present invention has the following beneficial effects:

[0082] The present invention discloses a highway vehicle ramp merging decision method and system based on insertable gaps. The highway vehicle ramp cutting process is divided into a first stage and a second stage. By identifying and selecting insertable gaps, a longitudinal trajectory planning scheme is formulated for the main vehicle before it cuts into the ramp entrance from the merging lane, and a two-dimensional trajectory planning scheme is formulated for the main vehicle when it cuts into the ramp entrance. The influence of the front and rear vehicles on the ramp cutting process of the automatic driving vehicle is comprehensively considered, and the problems of narrow lane changing space, low safety and great impact on traffic flow of the automatic driving vehicle when merging at the highway ramp are effectively solved, and a basis is provided for guiding the ramp cutting of the automatic driving vehicle on the highway.

[0083] At the same time, the highway vehicle ramp merging decision method based on insertable gaps in the embodiments of the present invention optimizes the calculation process of the trajectory planning scheme, thereby improving the calculation efficiency while ensuring the accuracy of the trajectory planning calculation, effectively realizing precise control of the trajectory of the autonomous driving vehicle and reducing the trajectory planning cost of the autonomous driving vehicle. BRIEF DESCRIPTION OF THE DRAWINGS

[0084] Figure 1 This is a flow chart of the highway vehicle ramp merging decision method based on insertable gaps according to Example 1.

[0085] Figure 2 This is a flow chart of iteratively planning the driving trajectory of the main vehicle within a preset time period in Example 1.

[0086] Figure 3 The second stage trajectory planning flowchart is iteratively executed in Example 1.

[0087] Figure 4 Schematic diagram of the method for iteratively solving the second-stage trajectory planning model in Example 1.

[0088] Figure 5 This is a flow chart of the trajectory planning entry method in the second stage of Example 1.

[0089] Figure 6 This is a schematic diagram of the structure of a highway vehicle ramp merging decision system based on insertable gaps according to Example 2.

[0090] Figure 7 This is a schematic diagram of the structure of the server in the system of Example 2. DETAILED DESCRIPTION

[0091] The present disclosure is further described in detail below in conjunction with the accompanying drawings and embodiments.

[0092] Example 1

[0093] The embodiment of the present disclosure proposes a highway vehicle ramp merging decision method based on an insertable gap, such as Figure 1 As shown, the following steps are included:

[0094] S1. Construct a highway vehicle ramp merging scenario, select the autonomous driving vehicle to be planned as the main vehicle, and obtain vehicle information of all vehicles on the outer lane of the main road in the highway vehicle ramp merging scenario and the main vehicle in the merging lane, wherein the vehicle information includes the displacement, speed and acceleration of the current vehicle.

[0095] For example, the highway vehicle ramp merging scenario is used to simulate the process of the main vehicle merging into the outer lane of the highway via the ramp. The highway vehicle ramp merging scenario includes a main road and a merging lane, wherein the main road is provided with multiple lanes, and the vehicles traveling on the outer lane of the main road are all manually driven vehicles. The merging lane is located on one side of the outer lane of the main road, and the merging area between the merging lane and the outer lane of the main road is the ramp entrance. Connected and Autonomous Vehicle, CAV for short, and Human-Driven Vehicle, HDV for short, are traveling on the merging lane.

[0096] In the highway vehicle ramp merging scenario, a monitoring system for acquiring manually driven vehicles and autonomous driving vehicles is provided on one side of the main road. The monitoring range of the monitoring system is the control area. The monitoring system is connected to the central controller and is used to acquire vehicle information on the highway in real time, including vehicle information of all vehicles on the outer lane of the main road and the main vehicle, such as the displacement (obtained through position coordinates), speed and acceleration of the vehicle.

[0097] S2. Determine whether the host vehicle has reached a decision planning trigger point based on the vehicle information of the host vehicle. According to an embodiment of the present disclosure, the trigger point may be an entrance to a ramp acceleration zone.

[0098] S3. When the main vehicle reaches the decision-making planning trigger point, start to build the main vehicle merging trajectory planning plan, and complete the merging based on the main vehicle merging trajectory planning plan.

[0099] In an embodiment of the present disclosure, the main vehicle merging trajectory planning scheme includes two stages, wherein the first stage trajectory planning is a longitudinal trajectory planning scheme before the main vehicle cuts into the ramp from the merging lane. When the main vehicle reaches the decision planning trigger point, the first stage trajectory planning is iteratively executed until the main vehicle meets the cutting-in condition, and the main vehicle is controlled to execute the first stage trajectory based on the first stage trajectory planning to control the main vehicle to complete the longitudinal travel before cutting into the ramp from the merging lane. The second stage trajectory planning is a two-dimensional trajectory planning scheme (i.e., a lateral and longitudinal trajectory planning scheme) when the main vehicle cuts into the ramp. In an embodiment of the present disclosure, the second stage trajectory planning can be iteratively executed, and the main vehicle is controlled to complete the ramp merging based on the second stage trajectory planning.

[0100] In the embodiment of the present disclosure, the cut-in condition is set based on the insertable gap of the host vehicle. It should be understood that when the host vehicle merges, it can merge only when the gap between the vehicle in front of the host vehicle and the vehicle behind the host vehicle reaches a certain distance. The gap between the vehicle in front and the vehicle behind that can meet the requirements of the safety and operability of the host vehicle merging is defined as the insertable gap. Further, the cut-in condition can be set based on the length of the insertable gap of the host vehicle.

[0101] In the embodiments of the present disclosure, the first stage trajectory planning and the second stage trajectory planning are respectively executed based on the MPC framework. In the MPC framework, for a complete planning cycle, only the first few seconds of operation are executed each time, and the planning of the next cycle is re-performed after execution. For example, the duration of vehicle merging may be 3-12s. Assuming that the planning is performed with a time period of 10s, under the MPC framework, only the first 3 seconds of planning are executed each time. After execution, the real-time vehicle information is used to re-enter the planning of the next 10s. In this way, the vehicle data obtained in real time can be fully utilized to make the two-stage trajectory planning more accurate, and improve the accuracy, safety and efficiency of the main vehicle trajectory planning scheme.

[0102] In some embodiments, when the host vehicle reaches the decision planning trigger point, iteratively executing the first stage trajectory planning includes: iteratively planning the host vehicle driving trajectory within a preset time period under the MPC framework.

[0103] Specifically, Figure 2 As shown, iterative planning of the main vehicle's driving trajectory within a preset time period may include the following steps:

[0104] Step S301, based on the vehicle information of the vehicle on the outer lane of the main road and the host vehicle to be merged into the lane, the reachable range of the host vehicle within a preset time period is determined, and an insertable gap is identified.

[0105] For example, the vehicle information on the outer lane of the main road is Among them, s i =[x i , v i , a i ], i∈{1, 2, .., N m}, i is the serial number of the manually driven vehicle, N m is the total number of vehicles on the outer lane, s i is the vehicle information of the i-th vehicle, x i is the displacement of the ith vehicle, v i is the speed of the i-th vehicle, a i is the acceleration of the i-th vehicle.

[0106] The vehicle information of the host vehicle is s ego =[xego , v ego , a ego ], where x ego is the displacement of the main vehicle, v ego is the speed of the main vehicle, a ego The acceleration of the main vehicle.

[0107] Define Gap γ i,i+1 is the gap between vehicle i and the vehicle behind. The identification of the insertable gap is essentially to determine the relationship between the reachable position of the main vehicle in the planning cycle and the reachable position of the vehicle on the target lane. In general, if the main road vehicle of the environment vehicle is not affected by the environment, it will maintain a relatively stable driving state. Assuming that each planning cycle is T, the final state is:

[0108]

[0109] The cut-in condition may be determined according to the length of the insertable gap, and the length of the insertable gap is determined according to the reachable range of the host vehicle.

[0110] In the MPC framework, this step will continuously receive new state information and repeat the execution to respond to the impact of environmental changes. For the autonomous vehicle on the ramp, the maximum position during the planning cycle is divided into two cases: the first case is that due to the speed limit of the acceleration lane, the autonomous vehicle has reached the maximum speed allowed before the end of the planning cycle; the second case is that the autonomous vehicle has not reached the maximum speed until the end of the planning cycle, so the maximum achievable displacement of the main vehicle is for:

[0111]

[0112] In formula (1), v ub is the maximum speed limit; a ub is the maximum acceleration limit; T is the preset time period;

[0113] Similarly, the minimum reachable range considers that the autonomous vehicle is constantly decelerating, but cannot fall below the minimum speed limit. lb and the minimum acceleration limit a lb The minimum achievable displacement that the vehicle can achieve is for:

[0114]

[0115] In formula (2), v lb is the minimum speed limit; a lb is the minimum acceleration limit;

[0116] Therefore, the reach of the main vehicle is From this, the insertable gap can be determined as:

[0117]

[0118] In formula (3), is the position that the i-th manually driven vehicle on the outer lane can reach at the end of the preset time period, L is the length of the vehicle; d is the safe distance between adjacent vehicles.

[0119] It should be understood that due to factors such as the length of the acceleration lane, some gaps are only theoretically possible to cut into but not feasible in reality. The above strategy can ensure that the infeasible gaps are pruned as much as possible while ensuring that no gap is missed, so as to reduce the complexity of subsequent calculations.

[0120] Step S302 , calculating the optimal insertion gap, the state during insertion, and the vehicle trajectory before insertion of the host vehicle based on the pre-constructed first MILP problem.

[0121] In an embodiment of the present disclosure, pre-constructing the first MILP problem includes setting the objective function of the first stage trajectory planning scheme based on minimizing traffic impact and minimum lane change time, combining vehicle dynamics equations, kinematic equations, safety index threshold ranges during cut-in, vehicle following behavior, and acceleration length at the ramp to set constraints, and establish the first MILP problem. Among them, minimizing traffic impact can be achieved by minimizing the speed change and / or acceleration change of all affected vehicles. The goal of minimizing lane change time is to minimize the time required for the main vehicle to complete the lane change. There is a safety distance requirement for cut-in to ensure that there is enough safety distance between the main vehicle and the front and rear vehicles, and it is necessary to ensure that other vehicles have enough time to react to the actions of the main vehicle to avoid collision. In some embodiments, the safety index threshold includes the thresholds of the two safety indicators of absolute safety distance and collision time (time to collision). The vehicle following behavior can be used to establish a behavioral model that describes how other vehicles follow or avoid the main vehicle. After converting the above objectives and constraints into mathematical expressions, a formal description of a mixed integer nonlinear programming (MINLP) problem is obtained, and the traffic impact and lane change time are minimized by optimizing the objective function while ensuring safety and complying with physical constraints. Through pruning and linearization, the MINLP problem is transformed into a series of linear programming problems, that is, the first MILP problem is obtained, which accelerates the solution efficiency while ensuring the accuracy of the solution and ensures the timeliness of the planning.

[0122] It should be understood that the selection of the insertable gap and the longitudinal trajectory planning before the cut-in are coupled to each other. The selection of the gap will directly guide the longitudinal driving behavior of the host vehicle before the cut-in, and the longitudinal driving behavior of the host vehicle will affect the difficulty and cost of each cut-in into each insertable gap. The embodiment of the present disclosure directly constructs an integrated model, taking into account the selection of the insertable gap and the longitudinal trajectory planning of the ramp autonomous driving vehicle at the same time.

[0123] For the convenience of calculation, all time-related variables are discretized according to a fixed time step, and the time step is represented by t∈{1, 2, ..., T}. The states of the autonomous driving main vehicle and other vehicles are set to and Since the lateral process is not considered, the lane change of the autonomous vehicle is modeled as an instantaneous process with zero-one variables. Indicates whether the lane change is executed. The model for constructing the first stage trajectory planning is described as follows:

[0124]

[0125] Among them, MinJ is the objective function of the first stage trajectory planning, ω1, ω2, ω3 are weight coefficients to balance different objectives. The goal is to minimize delay. In the embodiment of the present disclosure, delay is defined as the difference between the time it takes for a vehicle to pass a road section in free flow and the time it actually takes to pass a road section. The difference is linearly related to the difference between the free flow speed and the actual speed. Therefore, the first term uses the difference between the lane speed limit and the actual speed to represent the delay. The second term It is used to smooth the trajectory of the vehicle, with the goal of minimizing acceleration, to reduce the rate of change of speed and alleviate the shock of traffic flow. The goal is to maximize the efficiency of cutting in. 0, after changing lanes is 1, so if the lane-changing efficiency is higher, it will appear earlier in the planning cycle. is 1, The value will be larger.

[0126] Since there is a term in the objective function It is nonlinear and difficult to solve, so we linearize it by adding two intermediate variables: make In the original objective function The item changes to At this point, linearization is complete.

[0127] Both manually driven vehicles and autonomous vehicles need to satisfy the vehicle's own kinematic and dynamic constraints:

[0128]

[0129] v lb ≤v t ≤v ub

[0130] a lb ≤a t ≤a ub Formula (5)

[0131] Formula (5) is the state s t In the iterative process, both the autonomous vehicle and the manually driven vehicle are known in their initial state. In the merging scenario of the embodiment of the present disclosure, the autonomous driving vehicle will only perform one cut-in operation, and the variable y ego The constraints that need to be met are:

[0132]

[0133] Assume the length of the acceleration lane is L acc , there are constraints:

[0134]

[0135] Under the premise of intelligently selecting an insertion gap, there are:

[0136]

[0137] In order to characterize the real-time response of the manually driven vehicle to the autonomous vehicle, the following car-following model described in equation (6) is constructed as the behavior model of the manual driving:

[0138]

[0139] In formula (6), α1, α2 are parameters, The speed of the vehicle in front of the manually driven vehicle. The vehicle may be a manually driven vehicle that was originally on the main road, or it may be an autonomous driving vehicle that has just entered the road.

[0140] The advantage of this model is that it can well represent the behavioral characteristics of human drivers when following a vehicle, that is, the acceleration of the vehicle at the next moment is affected by the speed difference with the vehicle in front and the relative safety distance with the vehicle in front. At the same time, the model ensures that the constraints are linear under the above discrete time step conditions, and the solution efficiency is high.

[0141] Therefore, for different main road vehicles, their following objects are affected by the variable and γ iThe influence of , only when the autonomous driving vehicle inserts into the i-th gap and starts to perform the cut-in operation, the i+1-th vehicle will change from following the front vehicle to the autonomous driving vehicle that follows and cuts in. In order to make the following object of the manually driven vehicle the corresponding vehicle, the acceleration of the self-driving vehicle to the non-following object is set to a maximum value, and the acceleration calculated for the following object is the normal calculation result. Finally, the smaller value of the acceleration calculated for the front vehicle and the autonomous driving vehicle is taken as the acceleration of the following model. Therefore, the formula (6) is adjusted, and the optimized following model is shown in formula (7):

[0142]

[0143] Taking into account the vehicle's own performance and the road speed limit, the expected acceleration of each step of the final manually driven vehicle is:

[0144]

[0145] In order to reduce the difficulty of solving the model, intermediate variables are introduced to relax and linearize the above equality constraints, and transform them into several linear inequality constraint methods as shown in formula (9):

[0146]

[0147]

[0148] The above constraints make the intermediate variables Strictly satisfy the constraint of being less than the minimum value, and ensure that the acceleration satisfies the lower bounds of acceleration and velocity while being greater than the intermediate variable. In addition, add a minimization item in the objective function MinJ To constrain the distance between decision variables and intermediate variables.

[0149] After cutting in, the autonomous driving vehicle needs to maintain a safe distance from the manually driven vehicles before and after the gap. In the embodiment of the present disclosure, not only the safety of the lane change moment is considered, but also the autonomous driving vehicle needs to have a feasible longitudinal trajectory when actually cutting in. Therefore, in the modeling, all moments Since the safety of vehicles should be affected by both the absolute distance and relative speed between vehicles, the absolute distance and TTC are used to construct dual constraints to ensure safety. The constructed constraints are shown in formula (10):

[0150]

[0151] In summary, the first MILP problem can be expressed as:

[0152]

[0153]

[0154] In formula (11), MinJ is the objective function of the first MILP problem; ω1, ω2, ω3 are all weight coefficients; t is the time step number; a t is the acceleration at time step t, v t is the velocity at time step t, s t is the vehicle information at time step t, which is a vector including position, velocity, and acceleration; a t+1 is the acceleration at time step t+1, s t+1 is the vehicle information at time step t+1; is the speed of the ith human-driven vehicle at time step t, is the speed of the i-1th manually driven vehicle at time step t; is the acceleration of the ith human-driven vehicle at time step t, is the acceleration of the i-th manually driven vehicle at time step t+1; They are all intermediate variables used for linearization; is the zero-one variable at time step t, is a zero-one variable at time step t+1, which is used to indicate whether the vehicle has changed lanes. After the vehicle changes lanes M is a preset parameter; is the intermediate variable of the ith manually driven vehicle at time step t, is the intermediate variable of the ith manually driven vehicle at time step t+1; s 0 is the vehicle information of the main vehicle at time 0; s init is the initial vehicle information of the main vehicle; is the ordinate of the main vehicle displacement at time step t, is the inflow indicator variable of the main vehicle at time step t, is the speed of the main vehicle at time step t; L acc is the length of the acceleration zone in the ramp entrance; N ac The number of linear programming problems transformed from MILP problems; γ i,i+1 is the insertable gap between vehicle i and vehicle i+1, γ i-1，i is the insertable gap between vehicle i-1 and vehicle i, is the insertable gap between vehicle i-1 and vehicle i at time step t; is the displacement of the ith human-driven vehicle at time step t, is the displacement of the i-1th human-driven vehicle at time step t; ∈ TTC is the threshold of TTC risk index; Δt is the time interval; is the acceleration calculated by the i-th vehicle for the following behavior of the preceding vehicle at time step t+1; is the acceleration calculated by the i-th vehicle for the following behavior of the cut-in vehicle at time step t+1.

[0155] Through the first MILP problem, we can calculate the optimal insertion gap, state during insertion, and trajectory of the main vehicle before insertion. i，i+1＝1 For the corresponding gap, the state at the time of insertion is the state s at the time of insertion, and the vehicle trajectory before insertion is the state set {s_0,…,s_t} from the current time to the time of insertion.

[0156] Step S303, determine whether the first MILP problem has a feasible solution. If a feasible solution exists, set the calculated pre-insertion driving trajectory as the first stage trajectory planning scheme, execute the first stage trajectory planning scheme and update the time step to plan the main vehicle driving trajectory for the next round of preset time period. It can be understood that when there is a feasible solution, there are decision variables that meet the objective function and all constraints of the first MILP problem in the current planning cycle time step, and the driving trajectory can be executed under the MPC framework based on the decision variables.

[0157] On the other hand, if there is no feasible solution, the main vehicle is controlled to decelerate and the time step is updated to plan the main vehicle driving trajectory for the next round of preset time period.

[0158] In the embodiment of the present disclosure, the preset time period can be discretized into multiple time steps t at equal intervals, and the vehicle information of the host vehicle and other vehicles on the highway is obtained in each time step. Thus, the representation of the first MILP problem can be further simplified and easy to solve.

[0159] According to the embodiments of the present disclosure, Figure 3 As shown, iterative execution of the second stage trajectory planning may include the following steps:

[0160] S401, based on the pre-built second-stage trajectory planning model, determine the longitudinal displacement termination state equation and the lateral displacement termination state equation of the main vehicle.

[0161] In the embodiment of the present disclosure, the second stage trajectory planning includes the planning of the lateral and longitudinal motion of the vehicle. The pre-built second stage trajectory planning model is a two-dimensional trajectory, which is expressed as shown in formula (12):

[0162]

[0163] In formula (12), tm is the time in the cutting process; x(·) is the longitudinal position of the two-dimensional trajectory; y(·) is the lateral position of the two-dimensional trajectory; P = [p0, p1, p2, p3, p4, p5], p0, p1, p2, p3, p4, p5 are all longitudinal displacement control variables; Q = [q0, q1, q2, q3, q4, q5], q0, q1, q2, q3, q4, q5 are all lateral displacement control variables; T = [1, tm, tm 2 ,tm 3 ,tm 4 ,tm 5 ], t is the time from the start of cut-in.

[0164] Based on the movement of the main vehicle in the x-axis and y-axis directions, the initial state and final state of the main vehicle meet the conditions:

[0165]

[0166] In formula (13), t0 is the start time of the second stage trajectory planning; the initial state of the main vehicle is Among them, x0 is the initial vertical coordinate, is the initial longitudinal velocity, is the initial longitudinal acceleration; y0 is the initial horizontal coordinate, is the initial lateral velocity, is the initial lateral acceleration; the final state of the main vehicle is Among them, x end To end the vertical axis, is the termination speed, is the termination acceleration, t end is the termination time, where termination means the lane change is completed; y tar The ending horizontal axis.

[0167] For the movement in the x-axis direction, the initial state Known, final state The equation for determining the terminal state of the lateral displacement of the main vehicle is unknown, as shown in equation (14):

[0168]

[0169] For the motion in the y-axis direction, the initial state It is known that due to y end is the position of the target lane centerline, It can be assumed that there is no longitudinal operation after the lane change is completed, so they are all 0. The control parameters in the y-axis direction are only related to the variable t end The equation for determining the terminal state of the lateral displacement of the main vehicle is shown in equation (15):

[0170]

[0171] S402, establishing a second-stage optimization problem, and iteratively solving the longitudinal displacement termination state equation and the lateral displacement termination state equation of the main vehicle based on the second-stage optimization problem under the MPC framework, and obtaining the longitudinal displacement termination state parameters and the lateral displacement termination state parameters of the main vehicle in each planning cycle until the main vehicle completes the cut-in.

[0172] The second-stage optimization problem is established based on a time-varying risk field model, and the establishment method includes:

[0173] The objective function is determined with the goal of minimizing lane change time, comfort and risk field intensity, and constraints are set by combining vehicle dynamics, kinematics, and road speed limit constraints to construct the second-stage optimization problem.

[0174] In the embodiment of the present disclosure, the objective function in the second MILP problem is set as:

[0175] minJ=ω1×J acc +ω2×J jerk +ω3×t end +ω4×J risk Formula (16)

[0176] in,

[0177]

[0178] In formulas (16)-(17), ω1, ω2, ω3, and ω4 are all weight coefficients; J acc is the parameter affecting the comfort level due to acceleration; J jerk is the parameter affecting the comfort level due to the acceleration change rate.

[0179] J risk is the environmental risk field, which is the potential risk indicator of the vehicle. Considering that the behavior characteristics of the environmental vehicle are mainly affected by two factors, one is its own driving style, and the other is the following behavior under the influence of the front vehicle. In the process of lane changing of the autonomous driving vehicle, the influence and influence source of the two vehicles in front and behind the target gap are different, so risk fields that adapt to different characteristics are constructed respectively.

[0180] Specifically, environmental risk areas risk for:

[0181] J risk =U front +U behind Formula (18)

[0182] Among them, the front car wind scene is:

[0183]

[0184] The rear wind scene is:

[0185] U behind = k × U sta +(1-k)×U mov ;

[0186]

[0187] In formula (16) to formula (20), T″ is the second-order derivative of T; T″′ is the third-order derivative of T; U front is the risk field of the preceding vehicle; x front is the distance to the vehicle in front; σ x is the control coefficient in the longitudinal direction; σ y is the horizontal axis control coefficient; A is the field intensity amplitude; β is the risk field shape control parameter; U behind is the risk field intensity of the following vehicle; k is the weight coefficient; U sta is the static risk field intensity; U mov is the dynamic risk field intensity; exp(·) is the exponential function;

[0188] Among them, considering that the speed limit on the highway is generally in the longitudinal direction, the lateral steering of the vehicle during the cut-in process during highway driving is ignored, and the lateral acceleration is constrained to prevent rollover. The constraints on the speed, acceleration and lateral acceleration along the x-axis direction are set based on vehicle dynamics as shown in formula (21):

[0189] v lb ≤P·T′≤v ub

[0190] a lb ≤P·T″≤a ub

[0191] -λ≤Q·T″≤λ

[0192] Formula (21)

[0193] In formula (21), T′ is the first-order derivative of T; λ is the limiting threshold of centripetal acceleration.

[0194] This establishes the complete second-stage optimization problem.

[0195] In the embodiment of the present disclosure, in order to increase the accuracy and robustness of the solution, by end Perform uniform sampling within the feasible range to generate several initial t end As the input of the solution algorithm, for each t endTo form a trajectory cluster, and then select the best trajectory from the trajectory cluster as the final execution trajectory.

[0196] Specifically, in an embodiment of the present disclosure, a method for iteratively solving the longitudinal displacement termination state equation and the lateral displacement termination state equation of the main vehicle based on the second stage optimization problem includes:

[0197] t end Uniform sampling is performed within a feasible setting range to generate several reference t end In this way, the optimal trajectory can be searched from multiple initial solutions, so the uniform sampling distribution is more even and the solution effect is more stable.

[0198] The environmental vehicle information at the current time step is compared with each reference t end As input, the pre-built second-stage trajectory planning model is iteratively solved based on the MPC framework to obtain each reference t end The corresponding optimal trajectory, based on each reference t end The corresponding best trajectories form a trajectory cluster;

[0199] According to the objective function in the second-stage optimization problem, the objective function value of each trajectory in the trajectory cluster is evaluated, the trajectory cluster is screened based on the objective function value, and the execution trajectory of the second stage is determined based on the screening result, and the execution trajectory includes the longitudinal displacement termination state parameters and the lateral displacement termination state parameters of the main vehicle in each planning cycle.

[0200] In the process of iteratively solving the second-stage trajectory planning model, since the lane change end time t end The existence of risk field related targets in the objective function makes the problem highly complicated. To simplify the solution, the variable t end With x end , v end , a end It is decomposed into two sub-problems, and the optimal solution is approached through continuous iteration of the sub-problems. In an example, it can be based on Figure 4 Solve using the method shown.

[0201] The first sub-problem considers setting t end As a known input, the objective function does not consider the risk field, the constraints remain unchanged, and directly optimizes x end , v end , a end ,Right now:

[0202]

[0203] This problem is a simple convex QP problem that can be solved exactly in a very short time. The purpose of establishing this subproblem is to find the search direction of decision variables that can improve the comfort and efficiency of the control trajectory. Because the above problem does not consider the impact of risk locations, the optimal solution to the original problem J' may lead to a significant increase in the risk level, so here we choose is the target search direction, and the initial search step size is set to α. During the search process, J is used as the evaluation function, and the step size α is continuously updated through the Armijo rule until a better solution with a lower target function value is found. The final output after calculation is If the step size is less than the threshold ∈ alpha If the objective function value still cannot be reduced after searching, it means that the local optimal value has been found.

[0204] The second sub-problem is to make the output x of the first sub-problem end , v end , a end As a known input, let t end is the decision variable, and J is the target for problem optimization. end The optimization problem is non-convex and nonlinear, but there is only one decision variable, so it can be solved quickly using line search. The search direction is the derivative of the objective function with respect to the decision variable. The initial search step size is η, and the Armijo rule is also used to continuously update the step size until the search is carried out in a direction that can reduce the objective function value.

[0205] It is worth noting that in the solution process, a known reference trajectory is required as the initial solution to start the search. For example, if the lane change time is between 3 and 12 seconds, the initial t end , and obtain x by solving the first subproblem end , v end , a end , forming the initial trajectory. Furthermore, the trajectory of the last execution can be used as the reference trajectory in this new round of iteration for iterative solution.

[0206] Figure 5 A flowchart of a second-stage trajectory planning cut-in method according to an example of the present disclosure is exemplarily shown.

[0207] like Figure 5 As shown in Figure 1, when the autonomous driving vehicle (main vehicle) reaches the ramp planning trigger point, the time step is updated to determine whether the main vehicle meets the cut-in condition. If the cut-in condition is met, the environmental vehicle information is obtained and several reference lane change times t are sampled. end , according to the second stage optimization problem, solve the problem corresponding to each reference lane changing time t endThe corresponding optimal solution forms a trajectory cluster. The optimal trajectory is selected from the trajectory cluster and the cut-in is iteratively performed until the cut-in is completed. On the other hand, if the cut-in condition is not met, the insertable gap is iteratively identified, and the optimal insertion gap, the state during insertion, and the trajectory before insertion are calculated until the cut-in condition is met.

[0208] Example 2

[0209] The present embodiment proposes a highway vehicle ramp merging decision system based on insertable gaps, which is applied to a highway vehicle ramp merging decision method based on insertable gaps as in the embodiment disclosed herein, and is used for insertable gap identification, insertion gap selection, and host vehicle merging trajectory planning decision making.

[0210] In this embodiment, the highway vehicle ramp merging decision system based on insertable gaps includes a vehicle information acquisition device and a server.

[0211] Figure 6 The structural diagram of the highway vehicle ramp merging decision system based on insertable gap according to this embodiment is schematically shown.

[0212] like Figure 6 As shown, the highway vehicle ramp merging decision system 600 based on insertable gaps in this embodiment includes a vehicle information acquisition device 601 and a server 602 .

[0213] Among them, the vehicle information acquisition device 601 is used to obtain vehicle information on the outer lane of the main road in the highway vehicle ramp merging scene, as well as the main vehicle in the lane to be merged, wherein the main vehicle is an autonomous driving vehicle to be planned, and the vehicle information includes the displacement, speed and acceleration of the current vehicle.

[0214] The server 602 is used to determine whether the main vehicle has reached a decision-making planning trigger point based on the vehicle information of the main vehicle; when the main vehicle reaches the decision-making planning trigger point, iteratively execute the first-stage trajectory planning until the main vehicle meets the entry condition, and control the main vehicle to execute the first-stage trajectory based on the first-stage trajectory planning, so as to control the main vehicle to complete the longitudinal driving before cutting into the ramp from the merging lane, wherein the cutting-in condition includes identifying that the main vehicle has an insertable gap; and iteratively execute the second-stage trajectory planning, and control the main vehicle to complete the ramp merging based on the second-stage trajectory planning, wherein the second-stage trajectory planning is a two-dimensional trajectory planning scheme for determining when the main vehicle cuts into the ramp; wherein the first-stage trajectory planning and the second-stage trajectory planning are respectively executed based on the MPC framework.

[0215] The vehicle information acquisition device 601 is communicatively connected to the server 602 .

[0216] Figure 7 The schematic diagram shows the structure of the server in the decision-making system according to this embodiment.

[0217] like Figure 7 As shown, the server 602 in the decision-making system of this embodiment includes a determination module 6021 , a gap identification module 6022 and an import decision module 6023 .

[0218] The judgment module 6021 is configured to judge whether the main vehicle reaches the decision planning trigger point based on the vehicle information of the main vehicle.

[0219] The gap recognition module 6022 is configured to iteratively execute the first-stage trajectory planning when the main vehicle reaches the decision planning trigger point until the main vehicle meets the cut-in condition, and then control the main vehicle to execute the first-stage trajectory based on the first-stage trajectory planning to control the main vehicle to complete the longitudinal travel before cutting into the ramp from the merging lane, wherein the cut-in condition includes recognizing that the main vehicle has an insertable gap.

[0220] The merging decision module 6023 is configured to iteratively execute the second-stage trajectory planning, and control the main vehicle to complete the ramp merging based on the second-stage trajectory planning, wherein the second-stage trajectory planning is to determine the two-dimensional trajectory planning scheme when the main vehicle enters the ramp entrance; wherein the first-stage trajectory planning and the second-stage trajectory planning are respectively executed based on the MPC framework.

[0221] The highway vehicle ramp merging decision system based on insertable gaps proposed in this embodiment can utilize the monitoring system to monitor the vehicle status parameters of all vehicles on the highway, accurately identify, select and plan the insertable gaps, and provide technical support for accurately guiding the ramp entry of autonomous driving vehicles at highway ramps and the driving safety of manually driven vehicles.

[0222] Of course, the above description is not a limitation of the present invention, and the present invention is not limited to the above examples. Changes, modifications, additions or substitutions made by technicians in this technical field within the essential scope of the present invention should also fall within the protection scope of the present invention.

Claims

1. A highway vehicle ramp merging decision method based on insertable gaps, characterized in that: The following steps are involved: Obtain vehicle information of vehicles on the outer lane of the main road in the highway vehicle ramp merging scenario, as well as the main vehicle in the lane to be merged, wherein the main vehicle is the autonomous driving vehicle to be planned, and the vehicle information includes the displacement, speed and acceleration of the current vehicle; Determining whether the main vehicle has reached a decision planning trigger point based on the vehicle information of the main vehicle; When the main vehicle reaches the decision planning trigger point, the first stage trajectory planning is iteratively executed until the main vehicle meets the cut-in condition, and the main vehicle is controlled to complete the longitudinal driving before cutting into the ramp entrance from the merging lane, wherein the cut-in condition is set based on the insertable gap of the main vehicle; Iteratively executing the second-stage trajectory planning, and controlling the main vehicle to complete the ramp merging based on the second-stage trajectory planning, wherein the second-stage trajectory planning is a two-dimensional trajectory planning scheme for determining the main vehicle when entering the ramp entrance; The first-stage trajectory planning and the second-stage trajectory planning are respectively executed based on the MPC framework.

2. The highway vehicle ramp merging decision method based on insertable gap according to claim 1 is characterized in that: When the main vehicle reaches the decision planning trigger point, iteratively executing the first stage trajectory planning includes: Under the MPC framework, the main vehicle's driving trajectory within a preset time period is iteratively planned, including: Determine the reachable range of the host vehicle within a preset time period based on the vehicle information on the outer lane of the main road and the host vehicle to be merged into the lane, and identify the insertable gap; Based on the pre-built first MILP problem, the optimal insertion gap, state during insertion, and trajectory of the main vehicle before insertion are calculated; Determine whether the first MILP problem has a feasible solution. If so, set the calculated pre-insertion vehicle trajectory as a first-stage trajectory planning scheme, execute the first-stage trajectory planning scheme and update the time step to plan the main vehicle driving trajectory for the next round of preset time period; if there is no feasible solution, control the main vehicle to decelerate and update the time step to plan the main vehicle driving trajectory for the next round of preset time period; Among them, the pre-construction of the first MILP problem includes setting the objective function of the first-stage trajectory planning scheme based on minimizing traffic impact and minimum lane change time, combining vehicle dynamics equations, kinematic equations, safety indicator threshold range when cutting in, vehicle following behavior, and acceleration length at the ramp to set constraints to establish the first MILP problem.

3. The highway vehicle ramp merging decision method based on insertable gap according to claim 1, characterized in that: The iterative execution of the second stage trajectory planning includes: Based on the pre-built second-stage trajectory planning model, determine the longitudinal displacement termination state equation and the lateral displacement termination state equation of the main vehicle; Establishing a second-stage optimization problem, under the MPC framework, iteratively solving the longitudinal displacement termination state equation and the lateral displacement termination state equation of the main vehicle based on the second-stage optimization problem, obtaining the longitudinal displacement termination state parameters and the lateral displacement termination state parameters of the main vehicle in each planning cycle, until the main vehicle completes the cut-in, including: t end Produce several references by uniform sampling within a feasible setting range; The environmental vehicle information at the current time step is compared with each reference t end As input, the pre-built second-stage trajectory planning model is iteratively solved based on the MPC framework to obtain each reference t end The corresponding optimal trajectory, based on each reference t end The corresponding best trajectories form a trajectory cluster; According to the objective function in the second-stage optimization problem, the objective function value of each trajectory in the trajectory cluster is evaluated, the trajectory cluster is screened based on the objective function value, and the execution trajectory of the second stage is determined based on the screening result, the execution trajectory includes the longitudinal displacement termination state parameter and the lateral displacement termination state parameter of the main vehicle in each planning cycle, The second-stage optimization problem is established based on a time-varying risk field model, and the establishment method includes: The objective function is determined with the goal of minimizing lane change time, comfort and risk field intensity, and constraints are set by combining vehicle dynamics, kinematics, and road speed limit constraints to construct the second-stage optimization problem.

4. The highway vehicle ramp merging decision method based on insertable gap according to claim 1, characterized in that: The highway vehicle ramp merging scenario is used to simulate the process of a main vehicle merging into the outer lane of a highway via a ramp. The highway vehicle ramp merging scenario includes a main road and a merging lane, wherein the main road is provided with multiple lanes, and vehicles traveling on the outer lane of the main road are all manually driven vehicles. The merging lane is located on one side of the outer lane of the main road, and the merging area between the merging lane and the outer lane of the main road is the ramp entrance, and autonomous driving vehicles and manually driven vehicles are traveling on the merging lane.

5. The highway vehicle ramp merging decision method based on insertable gap according to claim 2, characterized in that: The vehicle information on the outer lane of the main road is: Among them, s i =[x i ,v i ,a i ],i∈{1,2,..,N m }, i is the serial number of the manually driven vehicle, N m is the total number of vehicles on the outer lane, s i is the vehicle information of the i-th vehicle, x i is the displacement of the ith vehicle, v i is the speed of the i-th vehicle, a i is the acceleration of the i-th vehicle; The vehicle information of the host vehicle is s ego =[x ego ,v ego ,a ego ], where x ego is the displacement of the main vehicle, v ego is the speed of the main vehicle, a ego is the acceleration of the main vehicle; The insertable gap is defined as the gap between the front vehicle and the rear vehicle. The cut-in condition is determined according to the length of the insertable gap, which is determined according to the reachable range of the main vehicle. The maximum reachable displacement of the main vehicle is for: In the formula, v ub is the maximum speed limit; a ub is the maximum acceleration limit; T is the preset time period; The minimum achievable displacement of the main vehicle for: In the formula, v lb is the minimum speed limit; a lb is the minimum acceleration limit; Based on the reachable range of the main vehicle Determine the insertable gap as: In the formula, is the position that the i-th manually driven vehicle on the outer lane can reach at the end of the preset time period, L is the length of the vehicle; d is the safe distance between adjacent vehicles.

6. The highway vehicle ramp merging decision method based on insertable gap according to claim 5, characterized in that: The preset time period is discretized into multiple time steps t at equal intervals, and vehicle information of the host vehicle and other vehicles on the highway is obtained in each time step. The first MILP problem is established by combining vehicle dynamics equations, kinematic equations, safety index threshold ranges during cut-in, vehicle following behavior, and acceleration length constraints at the ramp. The first MILP problem is expressed as: Where, MinJ is the objective function of the first MILP problem; ω1, ω2, ω3 are all weight coefficients; t is the time step number; a t is the acceleration at time step t, v t is the velocity at time step t, s t is the vehicle information at time step t, which is a vector including position, velocity, and acceleration; a t+1 is the acceleration at time step t+1, s t+1 is the vehicle information at time step t+1; is the speed of the ith human-driven vehicle at time step t, is the speed of the i-1th manually driven vehicle at time step t; is the acceleration of the ith human-driven vehicle at time step t, is the acceleration of the i-th manually driven vehicle at time step t+1; They are all intermediate variables used for linearization; is the zero-one variable at time step t, is a zero-one variable at time step t+1, which is used to indicate whether the vehicle has changed lanes. After the vehicle changes lanes M is a preset parameter; is the intermediate variable of the ith manually driven vehicle at time step t, is the intermediate variable of the ith manually driven vehicle at time step t+1; s 0 is the vehicle information of the main vehicle at time 0; s init is the initial vehicle information of the main vehicle; is the ordinate of the main vehicle displacement at time step t, is the inflow indicator variable of the main vehicle at time step t, is the speed of the main vehicle at time step t; L acc is the length of the acceleration zone in the ramp entrance; N ac The number of linear programming problems transformed from MILP problems; γ i,i+1 is the insertable gap between vehicle i and vehicle i+1, γ i-1,i is the insertable gap between vehicle i-1 and vehicle i, is the insertable gap between vehicle i-1 and vehicle i at time step t; is the displacement of the ith human-driven vehicle at time step t, is the displacement of the i-1th human-driven vehicle at time step t; ∈ TTC is the threshold of TTC risk index; Δt is the time interval; is the acceleration calculated by the i-th vehicle for the following behavior of the preceding vehicle at time step t+1; is the acceleration calculated by the i-th vehicle for the following behavior of the cut-in vehicle at time step t+1.

7. The highway vehicle ramp merging decision method based on insertable gap according to claim 3, characterized in that: The pre-built second-stage trajectory planning model is a two-dimensional trajectory, expressed as: Where tm is the moment during the cutting process; x(·) is the longitudinal position of the two-dimensional trajectory; y(·) is the lateral position of the two-dimensional trajectory; P = [p0, p1, p2, p3, p4, p5], p0, p1, p2, p3, p4, p5 are all longitudinal displacement control variables; Q = [q0, q1, q2, q3, q4, q5], q0, q1, q2, q3, q4, q5 are all lateral displacement control variables; T = [1, tm, tm 2 ,tm 3 ,tm 4 ,tm 5 ]; Based on the movement of the main vehicle in the x-axis and y-axis directions, the initial state and final state of the main vehicle meet the conditions: Where t0 is the start time of the second stage trajectory planning; the initial state of the main vehicle is Among them, x0 is the initial vertical coordinate, is the initial longitudinal velocity, is the initial longitudinal acceleration; y0 is the initial horizontal coordinate, is the initial lateral velocity, is the initial lateral acceleration; the final state of the main vehicle is Among them, x end To end the vertical axis, is the termination speed, is the termination acceleration, t end is the end time; tar is the ending horizontal axis; The equation for determining the terminal state of the longitudinal displacement of the main vehicle is: The equation for determining the terminal state of the lateral displacement of the main vehicle is:

8. The highway vehicle ramp merging decision method based on insertable gap according to claim 7, characterized in that: The objective function in the second MILP problem is set as: minJ=ω1×J acc +ω2×J jerk +ω3×t end +ω4×J risk in, Where ω1, ω2, ω3, and ω4 are weight coefficients; J acc is the parameter affecting the comfort level due to acceleration; J jerk is the parameter affecting the acceleration change rate on comfort; J risk For environmental risk sites; The environmental risk field risj for: J risk =U front +U behind in, IN behind =k×U sta +(1-k)×U mov ; Where T'' is the second-order derivative of T; T''' is the third-order derivative of T; U front is the risk field of the preceding vehicle; x front is the distance to the vehicle in front; x is the control coefficient in the longitudinal direction; σ y is the horizontal axis control coefficient; A is the field intensity amplitude; β is the risk field shape control parameter; U brhind is the risk field intensity of the following vehicle; k is the weight coefficient; U sta is the static risk field intensity; U mov is the dynamic risk field intensity; exp(·) is the exponential function; The speed, acceleration, and lateral acceleration along the x-axis are set based on vehicle dynamics constraints as follows: v lb ≤P·T′≤v ub a lb ≤P·T″≤a ub -λ≤Q·T″≤λ Where T′ is the first-order derivative of T; λ is the limiting threshold of centripetal acceleration.

9. A highway vehicle ramp merging decision system based on insertable gaps, characterized in that: Applied to the highway vehicle ramp merging decision method based on insertable gap according to any one of claims 1 to 6, the decision system comprises: a vehicle information acquisition device and a server; The vehicle information acquisition device is used to acquire vehicle information of vehicles on the outer lane of the main road in the highway vehicle ramp merging scene, and the host vehicle in the lane to be merged, wherein the host vehicle is the autonomous driving vehicle to be planned, and the vehicle information includes the displacement, speed and acceleration of the current vehicle; The server is used to determine whether the main vehicle has reached a decision planning trigger point based on the vehicle information of the main vehicle; when the main vehicle reaches the decision planning trigger point, iteratively execute the first stage trajectory planning until the main vehicle meets the cut-in condition, and control the main vehicle to execute the first stage trajectory based on the first stage trajectory planning, so as to control the main vehicle to complete the longitudinal driving before cutting into the ramp from the merging lane, wherein the cut-in condition includes identifying that the main vehicle has an insertable gap; and iteratively execute the second stage trajectory planning, and control the main vehicle to complete the ramp merging based on the second stage trajectory planning, wherein the second stage trajectory planning is a two-dimensional trajectory planning scheme for determining when the main vehicle cuts into the ramp; wherein the first stage trajectory planning and the second stage trajectory planning are respectively executed based on the MPC framework; Wherein, the vehicle information acquisition device is communicatively connected to the server.

10. The highway vehicle ramp merging decision system based on insertable gap according to claim 9, characterized in that: The server comprises: A judgment module, configured to judge whether the main vehicle has reached a decision planning trigger point based on the vehicle information of the main vehicle; a gap recognition module, configured to iteratively execute the first stage trajectory planning when the main vehicle reaches the decision planning trigger point, until the main vehicle meets the cut-in condition, and then control the main vehicle to execute the first stage trajectory based on the first stage trajectory planning, so as to control the main vehicle to complete the longitudinal travel before cutting into the ramp entrance from the merging lane, wherein the cut-in condition includes recognizing that the main vehicle has an insertable gap; The merging decision module is configured to iteratively execute the second-stage trajectory planning, and control the main vehicle to complete the ramp merging based on the second-stage trajectory planning, wherein the second-stage trajectory planning is to determine the two-dimensional trajectory planning scheme when the main vehicle enters the ramp entrance; wherein the first-stage trajectory planning and the second-stage trajectory planning are respectively executed based on the MPC framework.

Images