CN112572443B - Real-time collision-avoidance trajectory planning method and system for lane changing of vehicles on highway - Google Patents

Real-time collision-avoidance trajectory planning method and system for lane changing of vehicles on highway Download PDF

Info

Publication number
CN112572443B
CN112572443B CN202011527130.2A CN202011527130A CN112572443B CN 112572443 B CN112572443 B CN 112572443B CN 202011527130 A CN202011527130 A CN 202011527130A CN 112572443 B CN112572443 B CN 112572443B
Authority
CN
China
Prior art keywords
track
vehicle
lane
time
potential collision
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202011527130.2A
Other languages
Chinese (zh)
Other versions
CN112572443A (en
Inventor
张雷
王震坡
张志强
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Bitnei Corp ltd
Beijing Institute of Technology BIT
Original Assignee
Beijing Bitnei Corp ltd
Beijing Institute of Technology BIT
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Bitnei Corp ltd, Beijing Institute of Technology BIT filed Critical Beijing Bitnei Corp ltd
Priority to CN202011527130.2A priority Critical patent/CN112572443B/en
Publication of CN112572443A publication Critical patent/CN112572443A/en
Application granted granted Critical
Publication of CN112572443B publication Critical patent/CN112572443B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units, or advanced driver assistance systems for ensuring comfort, stability and safety or drive control systems for propelling or retarding the vehicle
    • B60W30/18—Propelling the vehicle
    • B60W30/18009—Propelling the vehicle related to particular drive situations
    • B60W30/18163—Lane change; Overtaking manoeuvres
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units, or advanced driver assistance systems for ensuring comfort, stability and safety or drive control systems for propelling or retarding the vehicle
    • B60W30/08—Active safety systems predicting or avoiding probable or impending collision or attempting to minimise its consequences
    • B60W30/09—Taking automatic action to avoid collision, e.g. braking and steering
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
    • B60W2050/0001—Details of the control system
    • B60W2050/0019—Control system elements or transfer functions

Abstract

The invention relates to a real-time collision-avoidance track planning method and a real-time collision-avoidance track planning system for lane changing of vehicles on an expressway, wherein a first reference lane changing track, reference lane changing time and reference lane changing longitudinal displacement are obtained according to the speed, the acceleration and the position of a vehicle to be changed; the vehicle to be lane-changed runs according to the first reference track and judges whether a potential collision barrier exists or not; if so, obtaining a first track set according to the time, the position and the time sampling point set of the potential collision; judging whether a feasible solution exists in the first track set; if so, changing tracks according to the tracks corresponding to the feasible solutions, and if not, obtaining a second track set according to the time and position of the potential collision, the time and the displacement sampling point set; judging whether a feasible solution exists in the second track set; if so, the lane changing is carried out on the vehicle to be changed along the track corresponding to the feasible solution from the time of the potential collision; if not, the vehicle to be changed returns to the original lane for running. The invention can improve the safety of lane change driving.

Description

Real-time collision-avoidance trajectory planning method and system for lane changing of vehicles on highway
Technical Field
The invention relates to the field of intelligent vehicle planning control, in particular to a method and a system for planning a lane changing real-time collision-avoiding track of vehicles on a highway.
Background
The rapid development of the Chinese transportation industry and the automobile industry brings great convenience to people's travel life on one hand, and brings more traffic accidents and casualties on the other hand. The harshness of road traffic safety situations has led to increased emphasis on vehicle safety. The passive safety technology can only be started after the vehicle collides so as to reduce the injury of a driver to the maximum extent, while the active safety technology takes accident prevention as a core, senses the motion state and key parameters of the vehicle in real time through vehicle-mounted sensor equipment, and controls a vehicle execution mechanism based on an active safety algorithm so as to improve the safety of the vehicle in the motion process.
Lane changing is one of the most common behaviors of the expressway, and is beneficial to improving traffic efficiency and guaranteeing traffic safety. With the rapid development of intelligent automobiles and automatic driving automobiles, a safe, reliable and trackable automatic lane changing technology gets more attention. The automatic lane changing is composed of a perception decision layer, a planning layer and a control layer. The perception decision layer utilizes vehicle sensors (radar, camera, V2V and the like) to perceive the surrounding environment and the motion state of the vehicle and evaluates the track changing time to make a track changing decision instruction; the planning layer plans a lane changing track which is free of collision and meets vehicle dynamics constraint based on a perception decision result; the control layer controls actuators (steering, brake driving, etc.) based on the planned lane change trajectory to reduce errors between the actual trajectory and the planned trajectory of the vehicle. The planning layer is used as an intermediate layer and is an important guarantee for safe lane changing behavior. Therefore, the research of the method suitable for the high-speed vehicle collision-free track change planning is of great significance.
The existing scheme I is as follows: a dynamic lane change track planning method for an unmanned vehicle based on a Frenet coordinate system. The method comprises the following steps: establishing a cubic polynomial path generation model based on discrete global track points based on a Frenet coordinate system; sensing the environment and the motion state of the surrounding vehicle based on V2V communication; the behavior decision layer sends out a lane change instruction; planning an alternative track cluster set by the path generation model; selecting an optimal path according to the optimization index and the cost function; and the optimal track switching track is sent to a lower-layer track tracking controller. The basic principle is shown in figure 1;
the existing scheme is as follows: an intelligent vehicle lane change track planning method under a dynamic driving environment. An intelligent vehicle lane change track planning method in a dynamic environment is provided. The basic principle is that a longitudinal and lateral reference track is determined firstly, and if collision risk is detected, the reference track is corrected (track re-planning) and tracked. And if no risk exists, continuing to drive according to the reference track. The basic principle is shown in fig. 2;
the prior art has the following disadvantages:
aiming at the scheme one: the lane changing process is a dynamic process, and the moving states of the lane changing vehicle and surrounding vehicles change all the time. In the existing method, lane changing is generally regarded as a static process, the problem of real-time collision avoidance is not considered in the lane changing process under the assumption that surrounding vehicles run at a constant speed or the motion state (acceleration) is kept unchanged in the lane changing process, and vehicle collision is easily caused. In a real driving environment, the driver behavior has large uncertainty, and is related to the driver style, the environmental traffic density and the like. Meanwhile, the highway traffic flow is large, the emergency is more, and the condition that the acceleration of the surrounding vehicles suddenly changes in the lane changing process cannot be ignored. Some of the potential lane-change collisions may be avoided by re-planning the speed of the waypoints, and prior methods have not applied speed planning to the lane-change process. In the method, a cubic polynomial is adopted to represent the lane change track, and theoretically, only one speed extreme point exists, so that the method cannot cope with complex collision avoidance working conditions.
Aiming at the scheme II: the scheme does not fully consider the action of speed planning in lane changing, longitudinally and laterally decouples during lane changing track re-planning, and does not fully consider vehicle dynamics constraints (such as acceleration). This may cause the planned trajectory to be unable to be tracked.
Disclosure of Invention
The invention aims to provide a method and a system for planning a lane change real-time collision avoidance track of vehicles on a highway, aiming at the scientific problem that the acceleration sudden change of surrounding vehicles in the lane change process is not fully considered in the conventional method. The collision avoidance in the lane changing process can be realized, and the traffic safety in the lane changing process is greatly ensured on the premise of meeting the road environment and vehicle dynamics constraints.
In order to achieve the purpose, the invention provides the following scheme:
a real-time collision avoidance trajectory planning method for lane changing of vehicles on a highway comprises the following steps:
acquiring the current speed, acceleration and position of a vehicle to be changed;
based on an SQP method, the speed, the acceleration and the position are used as constraint conditions to obtain a first reference lane changing track, reference lane changing time and reference lane changing longitudinal displacement;
the vehicle to be lane-changed runs according to the first reference track and judges whether a potential collision barrier exists or not;
if not, the lane of the vehicle to be lane changed is changed according to the first reference track;
if so, acquiring the time of the potential collision and the position of the potential collision of the vehicle to be lane-changed;
uniformly sampling the reference channel changing time to obtain a first sampling point set;
based on a QP method, taking the time of the potential collision, the position of the potential collision and the first sampling point set as constraint conditions to obtain a first track set;
judging whether a feasible solution exists in the first track set or not;
if so, acquiring a track corresponding to the feasible solution, recording the track as a second reference track, and switching the track of the vehicle to be switched along the second reference track from the time when the potential collision is monitored;
if not, uniformly sampling the reference channel-changing longitudinal displacement to obtain a second sampling point set;
based on a QP method, taking the time of the potential collision, the position of the potential collision, the first sampling point set and the second sampling point set as constraint conditions to obtain a second track set;
judging whether a feasible solution exists in the second track set or not;
if so, acquiring a track corresponding to the feasible solution, recording the track as a third reference track, and starting to switch the track of the vehicle to be switched along the third reference track from the time when the potential collision is monitored;
if not, the vehicle to be changed returns to the original lane for running.
Optionally, the position of the potential collision of the vehicle to be lane-changed is represented in the ST diagram.
Optionally, when a plurality of feasible solutions exist in the first trajectory set, the feasible solutions are screened based on a cost function to obtain an optimal solution, and a trajectory corresponding to the optimal solution is a second reference trajectory.
Optionally, when a plurality of feasible solutions exist in the second trajectory set, the feasible solutions are screened based on a cost function to obtain an optimal solution, and a trajectory corresponding to the optimal solution is a third reference trajectory.
Optionally, it is determined that a potential collision obstacle exists when the acceleration of the vehicle around the vehicle to be changed is changed.
Optionally, the vehicle to be lane changed judges whether the acceleration of the surrounding vehicle is changed through the V2V.
A real-time collision avoidance trajectory planning system for lane changing of vehicles on a highway comprises:
the first information acquisition module is used for acquiring the current speed, acceleration and position of the vehicle to be changed;
the reference lane changing track module is used for obtaining a first reference lane changing track, reference lane changing time and reference lane changing longitudinal displacement by taking the speed, the acceleration and the position as constraint conditions based on an SQP method;
the first judgment module is used for enabling the vehicle to be changed to run according to the first reference track and judging whether a potential collision barrier exists or not; when no potential collision barrier exists, the lane of the vehicle to be lane changed is changed according to the first reference track;
the second information acquisition module is used for acquiring the potential collision time and the potential collision position of the vehicle to be lane changed when a potential collision obstacle exists;
the first sampling module is used for uniformly sampling the reference channel changing time to obtain a first sampling point set;
a first track set module, configured to obtain a first track set by using the time of the potential collision, the location of the potential collision, and the first sample point set as constraints based on a QP method;
the second judgment module is used for judging whether a feasible solution exists in the first track set or not; when a feasible solution exists, acquiring a track corresponding to the feasible solution, recording the track as a second reference track, and switching the track of the vehicle to be switched along the second reference track from the time when the potential collision is monitored;
the second sampling module is used for uniformly sampling the reference channel-changing longitudinal displacement to obtain a second sampling point set when no feasible solution exists;
a second track set module, configured to obtain a second track set by taking the time of the potential collision, the position of the potential collision, the first sampling point set, and the second sampling point set as constraint conditions based on a QP method;
a third judging module, configured to judge whether a feasible solution exists in the second trajectory set; when a feasible solution exists, acquiring a track corresponding to the feasible solution, recording the track as a third reference track, and switching the track of the vehicle to be switched along the third reference track from the time when the potential collision is monitored; and when no feasible solution exists, returning the vehicle to be changed to the original lane for running.
Optionally, the system further comprises an ST map module, which is used for displaying the position of the potential collision of the vehicle to be lane changed.
Optionally, the system further includes an optimization screening module, configured to, when multiple feasible solutions exist in the first trajectory set, screen the multiple feasible solutions based on a cost function to obtain an optimal solution, where a trajectory corresponding to the optimal solution is a second reference trajectory.
Optionally, the lane changing system further comprises a V2V communication module, configured to determine whether there is an acceleration change in vehicles around the lane changing vehicle.
According to the specific embodiment provided by the invention, the invention discloses the following technical effects:
the invention provides a method and a system for planning lane change real-time collision avoidance tracks of vehicles on an expressway, which consider factors such as motion state uncertainty of acceleration sudden change of vehicles running on the expressway, and the like.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings needed to be used in the embodiments will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings without inventive exercise.
FIG. 1 is a first schematic diagram of a track-change trajectory planning method in the prior art;
FIG. 2 is a second schematic diagram of a track-change trajectory planning method in the prior art;
FIG. 3 is a schematic diagram of a three-level controller architecture and control logic according to the present invention;
FIG. 4 is a schematic diagram of a real-time collision avoidance trajectory planning method for lane change of vehicles on a highway according to the present invention;
FIG. 5 is a schematic diagram of the velocity re-planning of the present invention;
FIG. 6 is a schematic diagram of path re-planning according to the present invention;
FIG. 7 is a block diagram of the system of the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
The invention aims to provide a method and a system for planning a lane change real-time collision avoidance track of a vehicle on an expressway, which can realize real-time collision avoidance in the lane change process and greatly ensure traffic safety in the lane change process on the premise of meeting the constraints of road environment and vehicle dynamics.
In order to make the aforementioned objects, features and advantages of the present invention comprehensible, embodiments accompanied with figures are described in further detail below.
The controller of the vehicle real-time lane changing collision avoidance trajectory planning method and system provided by the invention comprises three layers: the upper layer is a reference track generation layer, and a track meeting road boundary constraint and vehicle dynamics constraint is planned based on the assumption that the current surrounding vehicle state is unchanged; the middle layer is a speed re-planning layer, in each control period, the obstacle vehicle position prediction module (in the decision layer) collects state information of real-time positions, speeds, accelerations and the like of surrounding vehicles through V2V, predicts the positions of the surrounding vehicles through an algorithm and evaluates the collision risk of the surrounding vehicles and a lane-changing vehicle, and when the collision risk is detected, the middle speed re-planning layer is triggered. The speed re-planning layer generates a plurality of track clusters meeting multiple constraint conditions based on a QP (quantization parameter) method in an ST (test sequence) diagram on the basis of the current vehicle state and on the premise that the end point position is kept unchanged, and selects an optimal path based on a cost optimization function; if the speed re-planning cannot generate a collision-free trajectory, the lower path re-planning layer is triggered. The method samples the path arc length (space) and the time (time) respectively, and solves the collision-free track based on a QP method. If the speed re-planning and the path re-planning cannot solve the lane change track meeting the collision avoidance and vehicle dynamics constraints, a track returning to the original lane is planned, and the logical relationship among the layers of the controller is shown in fig. 3.
In general, the present invention is composed of the following three parts, as shown in fig. 4:
1.1: reference trajectory generation layer: and based on the assumption that the acceleration of the surrounding vehicle does not change, converting the track changing track solving problem into a constraint optimization problem to obtain a reference track.
1.2: speed re-planning layer: if the acceleration of the surrounding vehicle changes, a potential collision is detected, and the zone is triggered. Converting the speed re-planning problem into a QP problem (the QP problem does not necessarily have a feasible solution), if the QP has the solution, generating a feasible track cluster (a plurality of candidate collision-free tracks, see the detailed diagram in FIG. 5) by the layer, and then screening out the optimal track by considering a cost function (equation 15); if the QP has no solution, then part 1.3 content is triggered.
1.3: path re-planning layer: if no feasible solution exists in the speed re-planning, the path re-planning is carried out by simultaneously sampling the arc length and the time, and the path re-planning is also converted into a QP problem. If the QP has a solution, the layer generates a feasible track cluster (a plurality of candidate collision-free tracks, see the detailed figure 6), and then screens out the optimal track by considering a cost function (equation 15); if not, returning to the original lane.
Namely: if the acceleration of the surrounding vehicle is constant all the time in the lane changing process, only 1.1 is triggered and the reference track is the final lane changing track; if the acceleration sudden change has potential collision, 1.2 is triggered, and the optimal track in 1.2 is the final track changing track; if the QP problem in 1.2 is unsolved (the optimal trajectory cannot be obtained from 1.2), 1.3 is triggered.
The overall logic of the algorithm is as follows:
step 1: the decision layer makes a lane change decision, and the system of the invention calculates the reference lane change time t based on the SQP methodrefAnd a reference lane change longitudinal displacement xrefFurther generating a reference track-changing track (see the content of part 1.1 in detail);
step 2: the lane changing vehicle runs according to the first reference track generated in the step 1, potential collision detection is carried out in real time in the running process, and if the lane changing potential collision is not detected all the time, the reference track generated in the step 1 is the final lane changing track; if t is in the process of traveling along the reference trajectoryiniDetecting that potential collision exists at any moment, and skipping to the step 3;
and step 3: t is tiniWhen the potential collision in the lane changing process is detected at any momentThe velocity re-programming layer is triggered first (1.2 part of the content). The potential collision location of an obstacle may be represented in the ST diagram. Based on t in step 1refIt is uniformly sampled, thus forming a sampling point set tend. With t in step 2iniVehicle position s of timeiniVelocity viniIs the initial constraint condition, the sampling point set t in step 3endEach element in the path is an end point constraint condition, and based on a QP method, obstacle avoidance is realized by changing the time for reaching an end point (namely changing a speed profile curve in the path changing process and speed re-planning). If feasible solution exists, after the optimal track is selected based on the cost function, the vehicle is driven from tiniAnd starting to drive along the updated optimal track. If no feasible solution exists in the speed re-planning, skipping to the step 4;
and 4, step 4: if at tiniAnd (3) detecting a lane change potential collision at the moment, and if the speed re-planning in the step (3) has no feasible solution (the collision-free track cannot be updated), the path re-planning in the step (4) is triggered (part 1.3). Based on x in step 1refAnd sampling the data to obtain a series of arc length sampling point sets s in the ST graphend. With t in step 2iniVehicle position s of timeiniVelocity viniIs the initial constraint condition, the time sampling point set t in step 3endArc length sampling point set s in step 4endRespectively, the end point constraint conditions, and based on the QP method, the obstacle avoidance is realized by simultaneously changing the time of reaching the end point and the position of the end point. If feasible solution exists, after the optimal track is selected based on the cost function, the vehicle is driven from tiniAnd starting to drive along the updated optimal track. And if no feasible solution exists in the path re-planning, returning to the original lane.
Namely: a complete collision-free lane change behavior may be represented by steps 1 and 2 (no potential collision is detected during driving); steps 1, 2 and 3 (potential collision is detected in the driving process and can be avoided only by means of speed re-planning); steps 1, 2, 3, 4 (potential collision is detected during driving and unavoidable depending on speed re-planning).
The following explains the specific calculation process of the present invention:
1.1 reference Path Generation layer
And decoupling the vehicle motion situation into longitudinal and lateral motion under the geodetic coordinate system. A fifth-order polynomial is adopted to represent the lane changing track, and the speed, the acceleration and the like can be obtained by derivation. Boundary conditions and constraints are solved by defining a polynomial, and a collision-free track can be solved based on an SQP method. (reference trajectory generation is based on the assumption that the acceleration of the surrounding vehicle is constant)
Figure BDA0002851145840000081
Where t is time, x is longitudinal displacement, y is lateral displacement, pi,qjIs a polynomial coefficient. The above equation satisfies the following boundary constraints:
Figure BDA0002851145840000082
Figure BDA0002851145840000083
wherein x0,y0,vx0,vy0,ax0,ay0Longitudinal and lateral position, velocity, acceleration, respectively, at an initial time, where xref,yref,vxf,vyf,axf,ayfFor the end of track change (t)ref) Longitudinal and lateral position, velocity, acceleration.
By setting a cost function and its constraints:
Figure BDA0002851145840000084
s.t.
Figure BDA0002851145840000085
Figure BDA0002851145840000086
Figure BDA0002851145840000087
Figure BDA0002851145840000088
-κmax≤κ(t)≤κmax
\*MERGEFORMAT(4)
wherein jaccIs the derivative of acceleration (jerk), κ is curvature, acc is acceleration, w1-w4Are weight coefficients. The optimal reference time t can be found based on the constrained optimization problem described aboverefOptimum reference longitudinal lane change displacement xrefAnd then a reference track is obtained. The velocity and the acceleration can be obtained by respectively calculating the first derivative and the second derivative of the track function. The potential collision is detected in real time during the running process of the vehicle along the reference track, and if the potential collision exists due to sudden change of the acceleration of the surrounding vehicle, the speed re-planning is triggered firstly (part 1.2).
1.2 speed reprogramming tier
The speed re-planning algorithm is schematically shown in fig. 5: the obstacle vehicle position prediction module collects the motion state information of surrounding vehicles through V2V, predicts the positions of the surrounding vehicles in real time in each control period and evaluates the collision risks of the surrounding vehicles and the lane changing vehicles, and if no collision risks exist, the vehicles run along a reference path; if there is a collision risk, based on the current vehicle state, based on t in the ST diagramrefSampling time to obtain a series of sampling points tendBased on tendAnd xrefMultiple curves are generated, i.e. collision avoidance is achieved by varying the profile of the velocity. By a certain time sample length to trefSampling to obtain a series of point sets tend：
tend＝{t|t＝tref±kt·tsample,kt∈} \*MERGEFORMAT(5)
Wherein t issampleIs the length of the time sample, ktIs a coefficient, tendFor sampling a set of time points
The reference path in the x-y coordinate system is converted into an arc length trajectory in the ST diagram:
Figure BDA0002851145840000091
where s is the arc length travelled along the reference path, t1，t2At any two time points during the lane change. Arc length s corresponding to reference pathrefIs composed of
Figure BDA0002851145840000092
The collision avoidance curve in ST is also constructed using a fifth order polynomial:
S＝f(t)＝a0+b0t+c0t2+d0t3+e0t4+f0t5 \*MERGEFORMAT(8)
wherein S is the arc length of the path, a0-f0As a coefficient, the following constraints are satisfied:
(1) boundary condition constraints
Figure BDA0002851145840000093
Send＝sref＝f(tend) \*MERGEFORMAT(10)
Figure BDA0002851145840000094
Wherein t isiniFor when speed re-planning is triggeredTime, Sini，viniIs tiniCorresponding path arc length and speed, SendThe arc length is corresponding to the lane changing end point.
(2) Velocity and acceleration constraints
v∈(vmin,vmax) \*MERGEFORMAT(12)
a∈(amin,amax) \*MERGEFORMAT(13)
Where v, a are velocity and acceleration, respectively, needed to meet environmental and vehicle dynamics constraints.
After appropriate warping, the constraint optimization problem can be transformed into the QP problem as follows:
Figure BDA0002851145840000101
wherein x is the coefficient of the trajectory (a)0-f0) H is the Hessian matrix, f is the input, ai，biFor equality to constrain the vector, aj，bjThe vectors are constrained by inequalities.
If the formula (14) has a feasible solution, after a plurality of candidate track clusters are generated, the optimal track is screened out based on the cost function
Figure BDA0002851145840000102
Wherein w1-w3Are weight coefficients.
If equation (14) has no feasible solution, path re-planning is triggered (1.3 part of the content).
1.3 Path re-planning layer
The path re-planning diagram is shown in fig. 6: if the speed re-planning layer cannot plan a collision-free trajectory (equation 14 has no feasible solution), the path re-planning layer will be triggered. The method comprises the steps of simultaneously sampling the path arc length (space) and the time (time) in an ST graph, generating an alternative track cluster based on a QP method, and screening out an optimal track by combining a cost optimization function. The arc length of the updated path and the corresponding timestamp information are both changed compared to the original reference path. If the speed re-planning and the path re-planning can not solve the track meeting the constraints of no collision and vehicle dynamics, the controller updates a track returning to the original lane by taking the current position state of the vehicle as the initial state. Sampling path arc length and time:
send＝{s|s＝sref±ks·ssample,ks∈} \*MERGEFORMAT(16)
wherein s isendIs based on srefSet of sampling points of, ssampleThe arc length sample length.
Updating the constraint conditions:
Send＝send＝f(tend) \*MERGEFORMAT(17)
the remaining constraints are the same as in section 1.2.
If neither the speed re-planning nor the path re-planning can update the collision-free track, the controller will update a path for returning to the original lane. The track returning to the original lane is partially symmetrical to the reference track.
The invention also provides a system for planning the lane-changing real-time collision-avoidance track of the vehicles on the highway, as shown in fig. 7, comprising:
the first information acquisition module 701 is used for acquiring the current speed, acceleration and position of a vehicle to be lane changed;
a reference lane change trajectory module 702, configured to obtain a first reference lane change trajectory, a reference lane change time, and a reference lane change longitudinal displacement based on the SQP method by using the speed, the acceleration, and the position as constraint conditions;
a first judging module 703, configured to enable the vehicle to be driven according to the first reference trajectory and judge whether a potential collision obstacle exists; when no potential collision barrier exists, the lane of the vehicle to be lane changed is changed according to the first reference track;
a second information obtaining module 704, configured to obtain a time of a potential collision and a location of the potential collision of the vehicle to be lane changed when there is a potential collision obstacle;
a first sampling module 705, configured to perform uniform sampling on the reference channel change time to obtain a first sampling point set;
a first trajectory set module 706, configured to obtain a first trajectory set by taking the time of the potential collision, the location of the potential collision, and the first sampling point set as constraints based on a QP method;
a second determining module 707, configured to determine whether a feasible solution exists in the first trajectory set; when a feasible solution exists, acquiring a track corresponding to the feasible solution, recording the track as a second reference track, and changing the track of the vehicle to be changed along the second reference track from the time of the potential collision;
a second sampling module 708, configured to, when there is no feasible solution, perform uniform sampling on the reference lane change longitudinal displacement to obtain a second sampling point set;
a second trajectory set module 709, configured to obtain a second trajectory set by using the time of the potential collision, the position of the potential collision, the first sample point set, and the second sample point set as constraint conditions based on a QP method;
a third determining module 710, configured to determine whether a feasible solution exists in the second trajectory set; when a feasible solution exists, acquiring a track corresponding to the feasible solution, recording the track as a third reference track, and changing the track of the vehicle to be changed along the third reference track from the time of the potential collision; and when no feasible solution exists, returning the vehicle to be changed to the original lane for running.
The invention also discloses the following technical effects:
aiming at the complex driving environment of the expressway, the invention provides a real-time collision avoidance strategy in the lane changing process, a non-collision track cluster is obtained by representing potential collision positions in an ST (ST) diagram based on a QP (quantum dot) method, and the optimal track is selected for tracking based on a cost optimization function. The method combines speed re-planning and path re-planning, designs appropriate switching logic between the speed re-planning and the path re-planning, and is suitable for lane changing behaviors under complex vehicle conditions of an expressway.
The embodiments in the present description are described in a progressive manner, each embodiment focuses on differences from other embodiments, and the same and similar parts among the embodiments are referred to each other. For the system disclosed by the embodiment, the description is relatively simple because the system corresponds to the method disclosed by the embodiment, and the relevant points can be referred to the method part for description.
The principles and embodiments of the present invention have been described herein using specific examples, which are provided only to help understand the method and the core concept of the present invention; meanwhile, for a person skilled in the art, according to the idea of the present invention, the specific embodiments and the application range may be changed. In view of the above, the present disclosure should not be construed as limiting the invention.

Claims (10)

1. A real-time collision avoidance trajectory planning method for lane changing of vehicles on a highway is characterized by comprising the following steps:
acquiring the current speed, acceleration and position of a vehicle to be changed;
based on an SQP method, the speed, the acceleration and the position are used as constraint conditions to obtain a first reference lane changing track, reference lane changing time and reference lane changing longitudinal displacement;
the vehicle to be lane-changed runs according to the first reference lane-changing track and judges whether a potential collision obstacle exists or not;
if not, the vehicle to be lane-changed carries out lane change according to the first reference lane change track;
if so, acquiring the time of the potential collision and the position of the potential collision of the vehicle to be lane-changed;
uniformly sampling the reference channel changing time to obtain a first sampling point set;
based on a QP method, taking the time of the potential collision, the position of the potential collision and the first sampling point set as constraint conditions to obtain a first track set;
judging whether a feasible solution exists in the first track set or not;
if so, acquiring a track corresponding to the feasible solution, recording the track as a second reference track, and switching the track of the vehicle to be switched along the second reference track from the time when the potential collision is monitored;
if not, uniformly sampling the reference channel-changing longitudinal displacement to obtain a second sampling point set;
based on a QP method, taking the time of the potential collision, the position of the potential collision, the first sampling point set and the second sampling point set as constraint conditions to obtain a second track set;
judging whether a feasible solution exists in the second track set or not;
if so, acquiring a track corresponding to the feasible solution, recording the track as a third reference track, and starting to switch the track of the vehicle to be switched along the third reference track from the time when the potential collision is monitored;
if not, the vehicle to be changed returns to the original lane for running.
2. The method for planning a lane change real-time collision avoidance trajectory of a highway vehicle according to claim 1, wherein the position of a potential collision of the vehicle to be changed is represented in an ST diagram.
3. The method for planning the lane changing real-time collision avoidance track of the expressway vehicle as claimed in claim 1, wherein when a plurality of feasible solutions exist in the first track set, the feasible solutions are screened based on a cost function to obtain an optimal solution, and the track corresponding to the optimal solution is a second reference track.
4. The method for planning the lane changing real-time collision avoidance trajectory of the expressway vehicle according to claim 1, wherein when a plurality of feasible solutions exist in the second trajectory set, the feasible solutions are screened based on a cost function to obtain an optimal solution, and a trajectory corresponding to the optimal solution is a third reference trajectory.
5. The method for planning the lane change real-time collision avoidance trajectory of the expressway vehicle according to claim 1, wherein the potential collision obstacle is determined to exist when the acceleration of the vehicle around the vehicle to be changed changes.
6. The method for planning the real-time collision avoidance track of the expressway vehicle as claimed in claim 5, wherein the vehicle to be lane-changed judges whether acceleration change exists in surrounding vehicles through V2V.
7. A real-time collision avoidance trajectory planning system for lane changing of vehicles on a highway is characterized by comprising:
the first information acquisition module is used for acquiring the current speed, acceleration and position of the vehicle to be changed;
the reference lane changing track module is used for obtaining a first reference lane changing track, reference lane changing time and reference lane changing longitudinal displacement by taking the speed, the acceleration and the position as constraint conditions based on an SQP method;
the first judgment module is used for enabling the vehicle to be subjected to lane changing to run according to the first reference lane changing track and judging whether a potential collision barrier exists or not; when no potential collision barrier exists, the vehicle to be lane-changed is enabled to change the lane according to the first reference lane-changing track;
the second information acquisition module is used for acquiring the potential collision time and the potential collision position of the vehicle to be lane changed when a potential collision obstacle exists;
the first sampling module is used for uniformly sampling the reference channel changing time to obtain a first sampling point set;
a first track set module, configured to obtain a first track set by using the time of the potential collision, the location of the potential collision, and the first sample point set as constraints based on a QP method;
the second judgment module is used for judging whether a feasible solution exists in the first track set or not; when a feasible solution exists, acquiring a track corresponding to the feasible solution, recording the track as a second reference track, and switching the track of the vehicle to be switched along the second reference track from the time when the potential collision is monitored;
the second sampling module is used for uniformly sampling the reference channel-changing longitudinal displacement to obtain a second sampling point set when no feasible solution exists;
a second track set module, configured to obtain a second track set by taking the time of the potential collision, the position of the potential collision, the first sampling point set, and the second sampling point set as constraint conditions based on a QP method;
a third judging module, configured to judge whether a feasible solution exists in the second trajectory set; when a feasible solution exists, acquiring a track corresponding to the feasible solution, recording the track as a third reference track, and switching the track of the vehicle to be switched along the third reference track from the time when the potential collision is monitored; and when no feasible solution exists, returning the vehicle to be changed to the original lane for running.
8. The system for planning the real-time collision-avoidance trajectory for a vehicle on a highway according to claim 7, further comprising an ST map module for displaying the potential collision position of the vehicle to be lane-changed.
9. The system for planning the lane changing real-time collision avoidance trajectory of the highway vehicle as claimed in claim 7, further comprising an optimization screening module, configured to screen a plurality of feasible solutions based on a cost function to obtain an optimal solution when the plurality of feasible solutions exist in the first trajectory set, where a trajectory corresponding to the optimal solution is a second reference trajectory.
10. The system for planning the lane change real-time collision avoidance trajectory of the expressway vehicle of claim 7, further comprising a V2V communication module for determining whether there is an acceleration change in the surrounding vehicles of the vehicle to be lane changed.
CN202011527130.2A 2020-12-22 2020-12-22 Real-time collision-avoidance trajectory planning method and system for lane changing of vehicles on highway Active CN112572443B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011527130.2A CN112572443B (en) 2020-12-22 2020-12-22 Real-time collision-avoidance trajectory planning method and system for lane changing of vehicles on highway

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011527130.2A CN112572443B (en) 2020-12-22 2020-12-22 Real-time collision-avoidance trajectory planning method and system for lane changing of vehicles on highway

Publications (2)

Publication Number Publication Date
CN112572443A CN112572443A (en) 2021-03-30
CN112572443B true CN112572443B (en) 2021-12-07

Family

ID=75138823

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011527130.2A Active CN112572443B (en) 2020-12-22 2020-12-22 Real-time collision-avoidance trajectory planning method and system for lane changing of vehicles on highway

Country Status (1)

Country Link
CN (1) CN112572443B (en)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112937608B (en) * 2021-03-31 2022-06-21 吉林大学 Track prediction-based integrated rolling decision method and device for unmanned vehicle in ice and snow environment and storage medium
CN113276848B (en) * 2021-04-09 2022-09-13 江苏大学 Intelligent driving lane changing and obstacle avoiding track planning and tracking control method and system
CN113744563B (en) * 2021-08-02 2023-03-10 北京工业大学 Road-vehicle risk real-time estimation method based on track data
CN113815615B (en) * 2021-09-14 2023-07-04 上汽通用五菱汽车股份有限公司 Overtaking control method, vehicle-mounted terminal and computer readable storage medium

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2848487A1 (en) * 2013-09-12 2015-03-18 Volvo Car Corporation Manoeuvre generation for automated drive
EP3001272A1 (en) * 2014-09-26 2016-03-30 Volvo Car Corporation Method of trajectory planning for yielding manoeuvres
CN109375632A (en) * 2018-12-17 2019-02-22 清华大学 Automatic driving vehicle real-time track planing method
CN109813328A (en) * 2019-02-22 2019-05-28 百度在线网络技术（北京）有限公司 A kind of driving path planing method, device and vehicle
CN109960261A (en) * 2019-03-22 2019-07-02 北京理工大学 A kind of dynamic barrier preventing collision method based on collision detection
CN110329263A (en) * 2019-07-05 2019-10-15 西南交通大学 The adaptive lane-change method for planning track of automatic driving vehicle
CN110466516A (en) * 2019-07-11 2019-11-19 北京交通大学 A kind of curved road automatic vehicle lane-change method for planning track based on Non-Linear Programming
CN110597245A (en) * 2019-08-12 2019-12-20 北京交通大学 Automatic driving track-changing planning method based on quadratic planning and neural network
CN111258323A (en) * 2020-03-30 2020-06-09 华南理工大学 Intelligent vehicle trajectory planning and tracking combined control method
CN111489578A (en) * 2020-04-01 2020-08-04 北京理工大学 Unmanned highway decision planning method based on time and space gaps of lanes

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9669828B2 (en) * 2012-06-01 2017-06-06 Toyota Motor Engineering & Manufacturing North America, Inc. Cooperative driving and collision avoidance by distributed receding horizon control
US9278713B2 (en) * 2013-12-11 2016-03-08 GM Global Technology Operations LLC Collision avoidance control integrated with EPS controller
US10754339B2 (en) * 2017-09-11 2020-08-25 Baidu Usa Llc Dynamic programming and quadratic programming based decision and planning for autonomous driving vehicles
US10571921B2 (en) * 2017-09-18 2020-02-25 Baidu Usa Llc Path optimization based on constrained smoothing spline for autonomous driving vehicles
US10816990B2 (en) * 2017-12-21 2020-10-27 Baidu Usa Llc Non-blocking boundary for autonomous vehicle planning
US20190315357A1 (en) * 2018-04-17 2019-10-17 Baidu Usa Llc Novel method for speed adjustment of autonomous driving vehicles prior to lane change
JP2020111302A (en) * 2019-01-17 2020-07-27 マツダ株式会社 Vehicle driving support system and method

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2848487A1 (en) * 2013-09-12 2015-03-18 Volvo Car Corporation Manoeuvre generation for automated drive
EP3001272A1 (en) * 2014-09-26 2016-03-30 Volvo Car Corporation Method of trajectory planning for yielding manoeuvres
CN109375632A (en) * 2018-12-17 2019-02-22 清华大学 Automatic driving vehicle real-time track planing method
CN109813328A (en) * 2019-02-22 2019-05-28 百度在线网络技术（北京）有限公司 A kind of driving path planing method, device and vehicle
CN109960261A (en) * 2019-03-22 2019-07-02 北京理工大学 A kind of dynamic barrier preventing collision method based on collision detection
CN110329263A (en) * 2019-07-05 2019-10-15 西南交通大学 The adaptive lane-change method for planning track of automatic driving vehicle
CN110466516A (en) * 2019-07-11 2019-11-19 北京交通大学 A kind of curved road automatic vehicle lane-change method for planning track based on Non-Linear Programming
CN110597245A (en) * 2019-08-12 2019-12-20 北京交通大学 Automatic driving track-changing planning method based on quadratic planning and neural network
CN111258323A (en) * 2020-03-30 2020-06-09 华南理工大学 Intelligent vehicle trajectory planning and tracking combined control method
CN111489578A (en) * 2020-04-01 2020-08-04 北京理工大学 Unmanned highway decision planning method based on time and space gaps of lanes

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
A dynamic lane-changing trajectory planning scheme;Hanbing Jia等;《2020 IEEE 9th International Power Electronics and Motion Control Conference》;20201202;全文 *
A Real-Time Dynamic Trajectory Planning for Autonomous Driving Vehicles;Mingqiang Wang等;《2019 3rd Conference on Vehicle Control and Intelligence》;20190922;全文 *
Real Time Trajectory Re-planning for Autonomous Vehicle Lane Changing in Uncertain Traffic;Bangjun Qiao等;《2019 IEEE 28th International Symposium on Industrial Electronics 》;20190614;全文 *
Spatially-Partitioned Environmental Representation and Planning;Wei Zhan等;《2017 IEEE Intelligent Vehicles Symposium》;20170614;全文 *
基于MPC的汽车自适应巡航控制策略研究;李世豪;《长安大学 硕士》;20190415;全文 *

Also Published As

Publication number Publication date
CN112572443A (en) 2021-03-30

Similar Documents

Publication Publication Date Title
CN112572443B (en) Real-time collision-avoidance trajectory planning method and system for lane changing of vehicles on highway
WO2021077725A1 (en) System and method for predicting motion state of surrounding vehicle based on driving intention
CN110488802B (en) Decision-making method for dynamic behaviors of automatic driving vehicle in internet environment
Khodayari et al. A historical review on lateral and longitudinal control of autonomous vehicle motions
CN110187639B (en) Trajectory planning control method based on parameter decision framework
CN110471408B (en) Unmanned vehicle path planning method based on decision process
Do et al. Human drivers based active-passive model for automated lane change
JP3714258B2 (en) Recommended operation amount generator for vehicles
Wei et al. A behavioral planning framework for autonomous driving
Bosetti et al. On curve negotiation: From driver support to automation
US20220080961A1 (en) Control system and control method for sampling based planning of possible trajectories for motor vehicles
CN108011947A (en) A kind of vehicle cooperative formula formation driving system
CN110053619A (en) Controller of vehicle
CN106740864A (en) A kind of driving behavior is intended to judge and Forecasting Methodology
JP2017154554A (en) Vehicle control device and vehicle control method
EP3919336B1 (en) Travel control method and travel control device for vehicle
WO2022053026A1 (en) Automatic driving meeting scene processing method and apparatus, vehicle, and storage medium
CN111770865B (en) Vehicle travel control device
CN109195846A (en) Vehicle control system, control method for vehicle and vehicle control program
CN102139696A (en) Grid unlock
EP3854620A1 (en) Vehicle controller, and vehicle
CN109643118A (en) The function of vehicle is influenced based on the information relevant to function of the environment about vehicle
EP3869342A1 (en) System and method for generating simulation scenario definitions for an autonomous vehicle system
CN113496189A (en) Sensing method and system based on static obstacle map
CN113844465A (en) Automatic driving method and system

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant