CN112269384B - Vehicle dynamic trajectory planning method combining obstacle behavior intention - Google Patents

Vehicle dynamic trajectory planning method combining obstacle behavior intention Download PDF

Info

Publication number
CN112269384B
CN112269384B CN202011144751.2A CN202011144751A CN112269384B CN 112269384 B CN112269384 B CN 112269384B CN 202011144751 A CN202011144751 A CN 202011144751A CN 112269384 B CN112269384 B CN 112269384B
Authority
CN
China
Prior art keywords
longitudinal
vehicle
lateral
obstacle
moment
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
CN202011144751.2A
Other languages
Chinese (zh)
Other versions
CN112269384A (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.)
University of Electronic Science and Technology of China
Original Assignee
University of Electronic Science and Technology of China
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 University of Electronic Science and Technology of China filed Critical University of Electronic Science and Technology of China
Priority to CN202011144751.2A priority Critical patent/CN112269384B/en
Publication of CN112269384A publication Critical patent/CN112269384A/en
Application granted granted Critical
Publication of CN112269384B publication Critical patent/CN112269384B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0217—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with energy consumption, time reduction or distance reduction criteria
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Driving Devices And Active Controlling Of Vehicle (AREA)
  • Traffic Control Systems (AREA)

Abstract

The invention discloses a vehicle dynamic trajectory planning method combining obstacle behavior intention, which comprises the steps of obtaining current position state information of a vehicle by using a sensor, obtaining surrounding environment information of an obstacle in the current environment by using a sensing module, obtaining an expected position state of the vehicle at the next moment by using a decision module, and obtaining possible position state information of the obstacle at the next moment by using a prediction module. The acquired information of the vehicle and the barrier at the current moment and the next moment is used as constraints, and the vehicle and the barrier are decoupled in the transverse and longitudinal directions by respectively establishing a quintic polynomial and a quadritic polynomial in the transverse and longitudinal directions; and then, respectively carrying out collision detection on the transverse and longitudinal tracks of the vehicle and the obstacle, carrying out loss evaluation on the tracks without collision one by one, selecting the vehicle track with the minimum loss, and outputting the vehicle track.

Description

Vehicle dynamic trajectory planning method combining obstacle behavior intention
Technical Field
The invention belongs to the technical field of intelligent automobiles, and particularly relates to a vehicle dynamic trajectory planning method combining barrier behavior intentions.
Background
The vehicle trajectory planning is mainly responsible for generating a motion trajectory from a current state to a next target state, and the trajectory planning should take into account the constraint conditions of a driving space and meet the vehicle kinematics constraint and the riding comfort. The path planning algorithms of the unmanned vehicle mainly comprise algorithms based on random sampling and algorithms based on an optimization theory.
The random sampling method is mainly to update and iterate generated states through a probability method by randomly generating the states of the target possibility in space. The random sampling method is random de-sampling. That is, as long as there is a theoretical solution to the motion planning problem in the current state, the algorithm needs to find a theoretical solution after an infinite number of iterations. The random sampling method generally includes a PRM (probabilistic roadmap method) algorithm and an RRT (fast search random tree) algorithm.
However, the PRM algorithm cannot be used in a complex environment, such as a large number of obstacles, and the path generated by the PRM algorithm is generally poor in feasibility, so that many scholars perform improved variants on the PRM algorithm. The original RRT algorithm adopts a uniform sampling method, so that the method is poor in efficiency, the planning result is discontinuous, and many experts and scholars perform improvement and optimization on the method, and many variant algorithms are proposed, such as the RRT-Connect algorithm, by constructing a random tree at the starting point and the target at the same time, the knowledge is found when two trees meet. And the heuristic RRT algorithm guides the growth direction of the RRT node by using a heuristic function, so that the convergence rate of the algorithm is improved. The PCL-RRT algorithm is combined with the uncertainty of other vehicles in a traffic scene, and the collision probability is used as a heuristic function, so that the optimal path is found out. The RRT algorithm and the variant algorithm thereof improve the planning efficiency on the original basis, but have the defects of unstable solution result and the like.
One idea of an algorithm based on optimization theory is to divide the motion planning problem into two steps. First, a map is randomly sampled to obtain a rough path, and then a path satisfying the road geometric constraint is generated using the optimized path curvature. A trajectory planning based on an optimal control theory is provided in consideration of space and the like, and a trajectory evaluation function is improved, so that the trajectory under the current driving environment becomes an optimal trajectory. The motion planning algorithm based on the optimization theory can generate the optimal track suitable for the driving scene, but has the problem of low efficiency.
Although the safety and feasibility of the trajectory planning are considered by the vehicle-to-vehicle trajectory planning algorithm, only static information of pedestrians or other vehicle obstacles in the driving environment is directly input, dynamic information of the obstacles (such as the intention of the obstacles and possible future trajectories) which is not considered has certain defects (such as the fact that the optimal trajectory planned by the vehicle is different when the pedestrians are about to cross the road and the pedestrians are about to continue to walk or stop). At present, although methods such as a possible collision radius of an obstacle or a gravitational potential field are proposed to reduce the possibility of collision of a dynamic obstacle to an automobile, action intentions of pedestrians and other vehicles and possible future track situations are not considered. Aiming at the problems, the invention plans the track by a method of decomposing and then fusing the track of the vehicle and the barrier in the transverse and longitudinal directions based on a vehicle-mounted mobile computing platform and combining the action intention information of pedestrians and vehicles to reduce the possibility of collision, thereby realizing a track planning method combining the action intention of the barrier and improving the safety of unmanned driving.
Disclosure of Invention
The invention aims to overcome the defects of the prior art and provide a vehicle dynamic track planning method combined with obstacle action intention.
In order to achieve the above object, the present invention provides a vehicle dynamic trajectory planning method combining an intention of an obstacle, comprising the steps of:
(1) acquiring a global path in the vehicle driving process by using a global path module;
(2) vehicle decoupling
(2.1) vehicle lateral decoupling
(2.1.1) acquiring t by utilizing vehicle-mounted combined inertial navigation sensor of vehicle0The longitude and latitude of the moment are converted into position points under a Cartesian coordinate system, and the transverse driving distance is calculated by combining a global path and is recorded as d (t)0) (ii) a Directly obtaining t through vehicle-mounted combined inertial navigation sensor0The lateral running speed and the lateral running acceleration at the time are respectively recorded as
Figure BDA0002739358110000021
And
Figure BDA0002739358110000022
will t0The lateral travel distance, the lateral travel speed, and the lateral travel acceleration of the vehicle at the time are expressed by a fifth-order polynomial as:
Figure BDA0002739358110000031
wherein,
Figure BDA0002739358110000032
indicating a cross to be determinedTo the coefficients, superscript d denotes the transverse direction;
(2.1.2) providing t by a vehicle decision node1The desired lateral travel distance of the vehicle to the global path at time, denoted as d (t)1) (ii) a And t1The desired lateral travel speed and the desired lateral travel acceleration at that moment are respectively recorded as
Figure BDA0002739358110000033
And
Figure BDA0002739358110000034
wherein subscript e represents desired;
will t1The desired lateral travel distance, the desired lateral travel speed, and the desired lateral travel acceleration of the vehicle at the time are expressed by a fifth-order polynomial as:
Figure BDA0002739358110000035
(2.1.3) let t 00, T-T1-t0Jointly solving the formulas (1) and (2) to obtain the transverse coefficient
Figure BDA0002739358110000036
Figure BDA0002739358110000037
Figure BDA0002739358110000038
(2.1.4) constructing a transverse driving track equation of the vehicle:
Figure BDA0002739358110000039
(2.2) vehicle longitudinal decoupling
(2.2.1) vehicle-mounted device using vehicleCombined inertial navigation sensor acquisition t0The longitude and latitude of the moment are converted into position points under a Cartesian coordinate system, and the longitudinal driving distance is calculated by combining a global path and is recorded as s (t)0) (ii) a Directly obtaining t through vehicle-mounted combined inertial navigation sensor0The longitudinal running speed and the longitudinal running acceleration at the time are respectively recorded as
Figure BDA00027393581100000310
And
Figure BDA00027393581100000311
will t0The longitudinal travel distance, the longitudinal travel speed and the longitudinal travel acceleration of the vehicle at the time are expressed by a fourth-order polynomial as:
Figure BDA0002739358110000041
wherein,
Figure BDA0002739358110000042
representing the longitudinal coefficient to be determined, the superscript s representing the longitudinal direction;
(2.2.2) providing t by a vehicle decision node1The desired longitudinal running speed and longitudinal running acceleration of the vehicle at the moment are respectively recorded as
Figure BDA0002739358110000043
And
Figure BDA0002739358110000044
will t1The longitudinal travel distance, the longitudinal travel speed and the longitudinal travel acceleration of the vehicle at the time are expressed by a fourth-order polynomial as:
Figure BDA0002739358110000045
(2.2.3) let t 00, T-T1-t0Jointly solving the formulas (5) and (6) to obtain the transverse coefficient
Figure BDA0002739358110000046
Figure BDA0002739358110000047
Figure BDA0002739358110000048
(2.2.4) constructing a longitudinal running track equation of the vehicle:
Figure BDA0002739358110000049
(3) barrier decoupling
(3.1) lateral decoupling of obstacles
(3.1.1) obtaining t by using obstacle sensing module of vehicle0The lateral position, lateral velocity and lateral acceleration of the obstacle at the moment are respectively denoted as d0(t0)、
Figure BDA00027393581100000410
And
Figure BDA00027393581100000411
will t0The lateral position, lateral velocity and lateral acceleration of the obstacle at the moment are expressed by a fifth-order polynomial as:
Figure BDA00027393581100000412
wherein,
Figure BDA0002739358110000051
representing the lateral coefficient to be determined, and superscript d representing the lateral direction;
(3.1.2) obtaining t with the obstacle prediction module of the vehicle1The lateral position, lateral velocity and lateral acceleration of the obstacle at the moment are respectively denoted as d0(t1)、
Figure BDA0002739358110000052
And
Figure BDA0002739358110000053
will t1The lateral position, lateral velocity and lateral acceleration of the obstacle at the moment are expressed by a fifth-order polynomial as:
Figure BDA0002739358110000054
(3.1.3) let t 00, T-T1-t0Jointly solving the equations (9) and (10) to obtain the transverse coefficient
Figure BDA0002739358110000055
Figure BDA0002739358110000056
Figure BDA0002739358110000057
(3.1.4) constructing a transverse trajectory equation of the obstacle:
Figure BDA0002739358110000058
(3.2) longitudinal decoupling of obstacles
(3.2.1) obtaining t by using obstacle sensing module of vehicle0The longitudinal position, longitudinal velocity and longitudinal acceleration of the obstacle at the moment are respectively denoted as s0(t0)、
Figure BDA0002739358110000059
And
Figure BDA00027393581100000510
will t0The longitudinal position, longitudinal velocity and longitudinal acceleration of the obstacle at the moment are expressed by a fourth-order polynomial as:
Figure BDA00027393581100000511
wherein,
Figure BDA00027393581100000512
representing the longitudinal coefficient to be determined, the superscript s representing the longitudinal direction;
(3.2.2) obtaining t with the obstacle prediction module of the vehicle1The longitudinal velocity and longitudinal acceleration of the obstacle at that moment, respectively
Figure BDA0002739358110000061
And
Figure BDA0002739358110000062
will t1The longitudinal velocity and longitudinal acceleration of the obstacle at the moment are expressed by a fourth-order polynomial as:
Figure BDA0002739358110000063
(3.2.3) let t 00, T-T1-t0Jointly solving the formulas (13) and (14) to obtain longitudinal coefficients
Figure BDA0002739358110000064
Figure BDA0002739358110000065
Figure BDA0002739358110000066
(3.2.4) constructing a longitudinal trajectory equation of the obstacle:
Figure BDA0002739358110000067
(4) respectively carrying out collision detection on the transverse track equation and the longitudinal track equation to generate a transverse alternative track set and a longitudinal alternative track set of the vehicle;
(4.1) and the transverse running track equation d (t) of the vehicle and the transverse track equation d of the obstacle0(t) carrying out combined solution, and if a solution exists at a certain moment t, judging that the positions of the vehicle and the obstacle are overlapped in the transverse direction at the moment t; if the certain moment t is not solved, adding a transverse running track equation d (t) and a longitudinal running track equation s (t) corresponding to the moment t into the alternative track set, and then performing collision detection at the next moment;
(4.2) judging whether the solution of the time t in the step (4.1) is the longitudinal driving track equation s (t) of the vehicle and the longitudinal track equation s of the obstacle0(t), if yes, judging that the vehicle collides with the obstacle at the time t, and eliminating a transverse driving track equation d (t) and a longitudinal driving track equation s (t) which correspond to the vehicle at the time t; if not, adding a transverse running track equation d (t) and a longitudinal running track equation s (t) corresponding to the moment t into the alternative track set, and then performing collision detection at the next moment;
(5) determining the final driving track of the vehicle
(5.1) calculating the total loss value C of each time d (t) and s (t) in the alternative track set by using a loss functiontotal(t)；
Ctotal(t)＝Kd(t)Cd(t)+Ks(t)Cs(t) (17)
Wherein, Kd(t) is the transverse distance loss coefficient at time t, Cd(t) is the lateral loss value at time t,Ks(t) is the longitudinal distance loss coefficient at time t, Cs(t) is the longitudinal loss value at time t;
and (5.2) selecting the corresponding track d (t) and s (t) when the total loss value in the candidate track set is minimum, and taking the selected track d (t) and s (t) as the finally planned driving track.
The invention aims to realize the following steps:
the invention relates to a vehicle dynamic trajectory planning method combining obstacle behavior intention, which is characterized in that a sensor is used for obtaining current position state information of a vehicle, a sensing module is used for obtaining surrounding environment information of an obstacle in the current environment, a decision module is used for obtaining an expected position state of the vehicle at the next moment, and a prediction module is used for obtaining possible position state information of the obstacle at the next moment. The acquired information of the vehicle and the barrier at the current moment and the next moment is used as constraints, and the vehicle and the barrier are decoupled in the transverse and longitudinal directions by respectively establishing a quintic polynomial and a quadritic polynomial in the transverse and longitudinal directions; and then, respectively carrying out collision detection on the transverse and longitudinal tracks of the vehicle and the obstacle, carrying out loss evaluation on the tracks without collision one by one, selecting the vehicle track with the minimum loss, and outputting the vehicle track.
Meanwhile, the vehicle dynamic trajectory planning method combined with the behavior intention of the barrier has the following beneficial effects:
(1) when the vehicle track planning is carried out, the information of the vehicle and the position information of the obstacle are not only used, but also the behavior intention of the obstacle is added, so that the transverse track and the longitudinal track of the vehicle are respectively analyzed, the adaptability and the safety of the track planning are improved, a reliable track is provided for the control of the automatically-driven vehicle, and the safety of the automatically-driven vehicle is improved;
(2) the track is solved by utilizing a mode of constructing a polynomial by utilizing vehicle and obstacle information, the order of the undetermined coefficient of the polynomial is low, the solution is convenient, and the real-time performance of the whole planning system can be improved compared with other track planning modes, such as a mode of utilizing deep learning and the like, and the calculated amount in the process;
(3) the method also designs different loss functions aiming at the transverse and longitudinal tracks to evaluate the transverse and longitudinal tracks, the evaluation mechanism can further quantify the quality of the track planning result, other workers can conveniently adjust and optimize on the basis of the track planning method, and the algorithm is suitable for more different driving environments by adjusting different coefficients in the loss functions.
Drawings
FIG. 1 is a flow chart of a method for vehicle dynamic trajectory planning incorporating the intent of an obstacle in accordance with the present invention;
FIG. 2 is a flow chart of vehicle, obstacle decoupling;
Detailed Description
The following description of the embodiments of the present invention is provided in order to better understand the present invention for those skilled in the art with reference to the accompanying drawings. It is to be expressly noted that in the following description, a detailed description of known functions and designs will be omitted when it may obscure the subject matter of the present invention.
Examples
Fig. 1 is a flow chart of a vehicle dynamic trajectory planning method combining with an intention of an obstacle.
In this embodiment, as shown in fig. 1, the vehicle dynamic trajectory planning method combining the behavior intention of the obstacle of the present invention includes the following steps:
s1, acquiring a global path in the vehicle driving process by using the global path module;
s2, decoupling the vehicle, wherein the specific process is shown in FIG. 2;
s2.1, transversely decoupling the vehicle;
s2.1.1, acquiring t by using vehicle-mounted combined inertial navigation sensor0The longitude and latitude of the moment are converted into position points under a Cartesian coordinate system, and the transverse driving distance is calculated by combining a global path and is recorded as d (t)0) (ii) a Directly obtaining t through vehicle-mounted combined inertial navigation sensor0The lateral running speed and the lateral running acceleration at the time are respectively recorded as
Figure BDA0002739358110000081
And
Figure BDA0002739358110000082
will t0The lateral travel distance, the lateral travel speed, and the lateral travel acceleration of the vehicle at the time are expressed by a fifth-order polynomial as:
Figure BDA0002739358110000083
wherein,
Figure BDA0002739358110000084
representing the lateral coefficient to be determined, and superscript d representing the lateral direction;
s2.1.2, providing t by a vehicle decision node1The desired lateral travel distance of the vehicle to the global path at time, denoted as d (t)1) (ii) a And t1The desired lateral travel speed and the desired lateral travel acceleration at that moment are respectively recorded as
Figure BDA0002739358110000085
And
Figure BDA0002739358110000086
wherein subscript e represents desired;
will t1The desired lateral travel distance, the desired lateral travel speed, and the desired lateral travel acceleration of the vehicle at the time are expressed by a fifth-order polynomial as:
Figure BDA0002739358110000091
s2.1.3, let t 00, T-T1-t0T is unit time; jointly solving the formulas (1) and (2) to obtain the transverse coefficient
Figure BDA0002739358110000092
Figure BDA0002739358110000093
Figure BDA0002739358110000094
S2.1.4, constructing a transverse driving track equation of the vehicle:
Figure BDA0002739358110000095
s2.2, longitudinally decoupling the vehicle;
s2.2.1, acquiring t by using vehicle-mounted combined inertial navigation sensor0The longitude and latitude of the moment are converted into position points under a Cartesian coordinate system, and the longitudinal driving distance is calculated by combining a global path and is recorded as s (t)0) (ii) a Directly obtaining t through vehicle-mounted combined inertial navigation sensor0The longitudinal running speed and the longitudinal running acceleration at the time are respectively recorded as
Figure BDA0002739358110000096
And
Figure BDA0002739358110000097
will t0The longitudinal travel distance, the longitudinal travel speed and the longitudinal travel acceleration of the vehicle at the time are expressed by a fourth-order polynomial as:
Figure BDA0002739358110000098
wherein,
Figure BDA0002739358110000099
representing the longitudinal coefficient to be determined, the superscript s representing the longitudinal direction;
s2.2.2, providing t by a vehicle decision node1The desired longitudinal running speed and longitudinal running acceleration of the vehicle at the moment are respectively recorded as
Figure BDA00027393581100000910
And
Figure BDA00027393581100000911
will t1The longitudinal travel distance, the longitudinal travel speed and the longitudinal travel acceleration of the vehicle at the time are expressed by a fourth-order polynomial as:
Figure BDA0002739358110000101
s2.2.3, let t 00, T-T1-t0Jointly solving the formulas (5) and (6) to obtain the transverse coefficient
Figure BDA0002739358110000102
Figure BDA0002739358110000103
Figure BDA0002739358110000104
S2.2.4, constructing a longitudinal running track equation of the vehicle:
Figure BDA0002739358110000105
s3, decoupling the obstacle, wherein the specific process is shown in FIG. 2;
s3.1, transversely decoupling the barrier;
s3.1.1 obtaining t by obstacle sensing module of vehicle0The lateral position, lateral velocity and lateral acceleration of the obstacle at the moment are respectively denoted as d0(t0)、
Figure BDA0002739358110000106
And
Figure BDA0002739358110000107
will t0The lateral position, lateral velocity and lateral acceleration of the obstacle at the moment are expressed by a fifth-order polynomial as:
Figure BDA0002739358110000108
s3.1.2 obtaining t by obstacle prediction module of vehicle1The lateral position, lateral velocity and lateral acceleration of the obstacle at the moment are respectively denoted as d0(t1)、
Figure BDA0002739358110000109
And
Figure BDA00027393581100001010
will t1The lateral position, lateral velocity and lateral acceleration of the obstacle at the moment are expressed by a fifth-order polynomial as:
Figure BDA00027393581100001011
s3.1.3, let t 00, T-T1-t0Jointly solving the equations (9) and (10) to obtain the transverse coefficient
Figure BDA00027393581100001012
Figure BDA0002739358110000111
Figure BDA0002739358110000112
S3.1.4, constructing a transverse track equation of the obstacle:
Figure BDA0002739358110000113
s3.2, longitudinally decoupling the barrier;
s3.2.1 obtaining t by obstacle sensing module of vehicle0The longitudinal position, longitudinal velocity and longitudinal acceleration of the obstacle at the moment are respectively denoted as s0(t0)、
Figure BDA0002739358110000114
And
Figure BDA0002739358110000115
will t0The longitudinal position, longitudinal velocity and longitudinal acceleration of the obstacle at the moment are expressed by a fourth-order polynomial as:
Figure BDA0002739358110000116
s3.2.2 obtaining t by obstacle prediction module of vehicle1The longitudinal velocity and longitudinal acceleration of the obstacle at that moment, respectively
Figure BDA0002739358110000117
And
Figure BDA0002739358110000118
will t1The longitudinal velocity and longitudinal acceleration of the obstacle at the moment are expressed by a fourth-order polynomial as:
Figure BDA0002739358110000119
s3.2.3, let t 00, T-T1-t0Jointly solving the formulas (13) and (14) to obtain longitudinal coefficients
Figure BDA00027393581100001110
Figure BDA00027393581100001111
Figure BDA00027393581100001112
S3.2.4, constructing a longitudinal track equation of the obstacle:
Figure BDA0002739358110000121
s4, respectively carrying out collision detection on the horizontal and vertical track equations to generate horizontal and vertical alternative track sets of the vehicle;
s4.1, a vehicle transverse driving track equation d (t) and a transverse track equation d of the obstacle0(t) carrying out combined solution, and if a solution exists at a certain moment t, judging that the positions of the vehicle and the obstacle are overlapped in the transverse direction at the moment t; if the certain moment t is not solved, adding a transverse running track equation d (t) and a longitudinal running track equation s (t) corresponding to the moment t into the alternative track set, and then performing collision detection at the next moment;
s4.2, judging whether the solution of the time t in the step S4.1 is the longitudinal driving track equation S (t) of the vehicle and the longitudinal track equation S of the obstacle0(t), if yes, judging that the vehicle collides with the obstacle at the time t, and eliminating a transverse driving track equation d (t) and a longitudinal driving track equation s (t) which correspond to the vehicle at the time t; if not, adding a transverse running track equation d (t) and a longitudinal running track equation s (t) corresponding to the moment t into the alternative track set, and then performing collision detection at the next moment;
s5, determining the final driving track of the vehicle;
s5.1, calculating total loss value C of each time d (t) and S (t) in the alternative track set by using a loss functiontotal(t)；
Ctotal(t)＝Kd(t)Cd(t)+Ks(t)Cs(t) (17)
Wherein, Kd(t) is the transverse distance loss coefficient at time t, Cd(t) is the lateral loss value at time t, Ks(t) is the longitudinal distance loss coefficient at time t, Cs(t) is the longitudinal loss value at time t;
wherein the lateral loss value CdThe formula for calculation of (t) is:
Cd(t)＝Kjd(t)Ad(d(t))+Kd(t)(dl(t))2
wherein, Kjd(t) is the loss coefficient of the lateral acceleration at time t, Ad(d (t)) is a lateral acceleration change value at time t, Ad(. is a function of the absolute value of the change in lateral acceleration, Kd(t) is the coefficient of lateral distance loss at time t, dl(t) is the lateral distance at which the trajectory at time t now deviates from the global path.
Longitudinal loss value CsThe formula for calculation of (t) is:
Figure BDA0002739358110000122
wherein, Kjs(t) is the loss coefficient of longitudinal acceleration at time t, As(s (t)) is a longitudinal acceleration change value at time t, As(. is a function of the absolute value of the change in longitudinal acceleration, Ks(t) is the longitudinal distance loss coefficient at time t,
Figure BDA0002739358110000131
respectively the velocity of the planned desired end point and the velocity of the planned point.
And S5.2, selecting the corresponding track d (t) and S (t) when the total loss value in the alternative track set is minimum, and taking the selected track d (t) and S (t) as the finally planned driving track.
Although illustrative embodiments of the present invention have been described above to facilitate the understanding of the present invention by those skilled in the art, it should be understood that the present invention is not limited to the scope of the embodiments, and various changes may be made apparent to those skilled in the art as long as they are within the spirit and scope of the present invention as defined and defined by the appended claims, and all matters of the invention which utilize the inventive concepts are protected.

Claims (3)

1. A vehicle dynamic trajectory planning method combining barrier behavior intents is characterized by comprising the following steps:
(1) acquiring a global path in the vehicle driving process by using a global path module;
(2) vehicle decoupling
(2.1) vehicle lateral decoupling
(2.1.1) acquiring t by utilizing vehicle-mounted combined inertial navigation sensor of vehicle0The longitude and latitude of the moment are converted into position points under a Cartesian coordinate system, and the transverse driving distance is calculated by combining a global path and is recorded as d (t)0) (ii) a Directly obtaining t through vehicle-mounted combined inertial navigation sensor0The lateral running speed and the lateral running acceleration at the time are respectively recorded as
Figure FDA0003140441130000011
And
Figure FDA0003140441130000012
will t0The lateral travel distance, the lateral travel speed, and the lateral travel acceleration of the vehicle at the time are expressed by a fifth-order polynomial as:
Figure FDA0003140441130000013
wherein,
Figure FDA0003140441130000014
representing the lateral coefficient to be determined, and superscript d representing the lateral direction;
(2.1.2) byVehicle decision node providing t1The desired lateral travel distance of the vehicle to the global path at time, denoted as d (t)1) (ii) a And t1The desired lateral travel speed and the desired lateral travel acceleration at that moment are respectively recorded as
Figure FDA0003140441130000015
And
Figure FDA0003140441130000016
will t1The desired lateral travel distance, the desired lateral travel speed, and the desired lateral travel acceleration of the vehicle at the time are expressed by a fifth-order polynomial as:
Figure FDA0003140441130000017
(2.1.3) let t00, T-T1-t0Jointly solving the formulas (1) and (2) to obtain the transverse coefficient
Figure FDA0003140441130000018
Figure FDA0003140441130000019
Figure FDA0003140441130000021
(2.1.4) constructing a transverse driving track equation of the vehicle:
Figure FDA0003140441130000022
(2.2) vehicle longitudinal decoupling
(2.2.1) vehicle-mounted combined inertial navigation sensor using vehicleDevice acquisition t0The longitude and latitude of the moment are converted into position points under a Cartesian coordinate system, and the longitudinal driving distance is calculated by combining a global path and is recorded as s (t)0) (ii) a Directly obtaining t through vehicle-mounted combined inertial navigation sensor0The longitudinal running speed and the longitudinal running acceleration at the time are respectively recorded as
Figure FDA0003140441130000023
And
Figure FDA0003140441130000024
will t0The longitudinal travel distance, the longitudinal travel speed and the longitudinal travel acceleration of the vehicle at the time are expressed by a fourth-order polynomial as:
Figure FDA0003140441130000025
wherein,
Figure FDA0003140441130000026
representing the longitudinal coefficient to be determined, the superscript s representing the longitudinal direction;
(2.2.2) providing t by a vehicle decision node1The desired longitudinal running speed and longitudinal running acceleration of the vehicle at the moment are respectively recorded as
Figure FDA0003140441130000027
And
Figure FDA0003140441130000028
will t1The longitudinal travel distance, the longitudinal travel speed and the longitudinal travel acceleration of the vehicle at the time are expressed by a fourth-order polynomial as:
Figure FDA0003140441130000029
(2.2.3) let t00, T-T1-t0Jointly solving the formulas (5) and (6) to obtain the transverse coefficient
Figure FDA00031404411300000210
Figure FDA00031404411300000211
Figure FDA0003140441130000031
(2.2.4) constructing a longitudinal running track equation of the vehicle:
Figure FDA0003140441130000032
(3) barrier decoupling
(3.1) lateral decoupling of obstacles
(3.1.1) obtaining t by using obstacle sensing module of vehicle0The lateral position, lateral velocity and lateral acceleration of the obstacle at the moment are respectively denoted as d0(t0)、
Figure FDA0003140441130000033
And
Figure FDA0003140441130000034
will t0The lateral position, lateral velocity and lateral acceleration of the obstacle at the moment are expressed by a fifth-order polynomial as:
Figure FDA0003140441130000035
wherein,
Figure FDA0003140441130000036
representing the lateral coefficient to be determined, and superscript d representing the lateral direction;
(3.1.2) obtaining t with the obstacle prediction module of the vehicle1The lateral position, lateral velocity and lateral acceleration of the obstacle at the moment are respectively denoted as d0(t1)、
Figure FDA0003140441130000037
And
Figure FDA0003140441130000038
will t1The lateral position, lateral velocity and lateral acceleration of the obstacle at the moment are expressed by a fifth-order polynomial as:
Figure FDA0003140441130000039
(3.1.3) let t00, T-T1-t0Jointly solving the equations (9) and (10) to obtain the transverse coefficient
Figure FDA00031404411300000310
Figure FDA00031404411300000311
Figure FDA0003140441130000041
(3.1.4) constructing a transverse trajectory equation of the obstacle:
Figure FDA0003140441130000042
(3.2) longitudinal decoupling of obstacles
(3.2.1) obtaining t by using obstacle sensing module of vehicle0The longitudinal position, longitudinal velocity and longitudinal acceleration of the obstacle at the moment are respectively denoted as s0(t0)、
Figure FDA0003140441130000043
And
Figure FDA0003140441130000044
will t0The longitudinal position, longitudinal velocity and longitudinal acceleration of the obstacle at the moment are expressed by a fourth-order polynomial as:
Figure FDA0003140441130000045
wherein,
Figure FDA0003140441130000046
representing the longitudinal coefficient to be determined, the superscript s representing the longitudinal direction;
(3.2.2) obtaining t with the obstacle prediction module of the vehicle1The longitudinal velocity and longitudinal acceleration of the obstacle at that moment, respectively
Figure FDA0003140441130000047
And
Figure FDA0003140441130000048
will t1The longitudinal velocity and longitudinal acceleration of the obstacle at the moment are expressed by a fourth-order polynomial as:
Figure FDA0003140441130000049
(3.2.3) let t00, T-T1-t0Jointly solving the formulas (13) and (14) to obtain longitudinal coefficients
Figure FDA00031404411300000410
Figure FDA00031404411300000411
Figure FDA00031404411300000412
(3.2.4) constructing a longitudinal trajectory equation of the obstacle:
Figure FDA00031404411300000413
(4) respectively carrying out collision detection on the transverse track equation and the longitudinal track equation to generate a transverse alternative track set and a longitudinal alternative track set of the vehicle;
(4.1) and the transverse running track equation d (t) of the vehicle and the transverse track equation d of the obstacle0(t) carrying out combined solution, and if a solution exists at a certain moment t, judging that the positions of the vehicle and the obstacle are overlapped in the transverse direction at the moment t; if the certain moment t is not solved, adding a transverse running track equation d (t) and a longitudinal running track equation s (t) corresponding to the moment t into the alternative track set, and then performing collision detection at the next moment;
(4.2) judging whether the solution of the time t in the step (4.1) is the longitudinal driving track equation s (t) of the vehicle and the longitudinal track equation s of the obstacle0(t), if yes, judging that the vehicle collides with the obstacle at the time t, and eliminating a transverse driving track equation d (t) and a longitudinal driving track equation s (t) which correspond to the vehicle at the time t; if not, adding a transverse running track equation d (t) and a longitudinal running track equation s (t) corresponding to the moment t into the alternative track set, and then performing collision detection at the next moment;
(5) determining the final driving track of the vehicle
(5.1) calculating the total loss value C of each time d (t) and s (t) in the alternative track set by using a loss functiontotal(t)；
Ctotal(t)＝Kd(t)Cd(t)+Ks(t)Cs(t) (17)
Wherein, Kd(t) is the transverse distance loss coefficient at time t, Cd(t) is the lateral loss value at time t, Ks(t) is the longitudinal distance loss coefficient at time t, Cs(t) is the longitudinal loss value at time t;
and (5.2) selecting the corresponding track d (t) and s (t) when the total loss value in the candidate track set is minimum, and taking the selected track d (t) and s (t) as the finally planned driving track.
2. The method for vehicle dynamic trajectory planning in combination with obstacle behavior intent according to claim 1, wherein the lateral loss value C isdThe formula for calculation of (t) is:
Cd(t)＝Kjd(t)Ad(d(t))+Kd(t)(dl(t))2
wherein, Kjd(t) is the loss coefficient of the lateral acceleration at time t, Ad(d (t)) is a lateral acceleration change value at time t, Ad(. is a function of the absolute value of the change in lateral acceleration, Kd(t) is the coefficient of lateral distance loss at time t, dl(t) is the lateral distance at which the trajectory at time t now deviates from the global path.
3. The method for vehicle dynamic trajectory planning in combination with obstacle behavior intent according to claim 1, wherein the longitudinal loss value C issThe formula for calculation of (t) is:
Figure FDA0003140441130000051
wherein, Kjs(t) is the loss coefficient of longitudinal acceleration at time t, As(s (t)) is a longitudinal acceleration change value at time t,As(. is a function of the absolute value of the change in longitudinal acceleration, Ks(t) is the longitudinal distance loss coefficient at time t,
Figure FDA0003140441130000061
respectively the velocity of the planned desired end point and the velocity of the planned point.
CN202011144751.2A 2020-10-23 2020-10-23 Vehicle dynamic trajectory planning method combining obstacle behavior intention Active CN112269384B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011144751.2A CN112269384B (en) 2020-10-23 2020-10-23 Vehicle dynamic trajectory planning method combining obstacle behavior intention

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011144751.2A CN112269384B (en) 2020-10-23 2020-10-23 Vehicle dynamic trajectory planning method combining obstacle behavior intention

Publications (2)

Publication Number Publication Date
CN112269384A CN112269384A (en) 2021-01-26
CN112269384B true CN112269384B (en) 2021-09-14

Family

ID=74341553

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011144751.2A Active CN112269384B (en) 2020-10-23 2020-10-23 Vehicle dynamic trajectory planning method combining obstacle behavior intention

Country Status (1)

Country Link
CN (1) CN112269384B (en)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113359763A (en) * 2021-07-05 2021-09-07 广州蓝胖子移动科技有限公司 Wheeled robot, dynamic obstacle trajectory prediction method, device and storage medium
CN113516862A (en) * 2021-07-22 2021-10-19 中国第一汽车股份有限公司 Early warning method and device, electronic equipment and storage medium
CN113753079A (en) * 2021-08-23 2021-12-07 上海宏景智驾信息科技有限公司 Time alignment method used in vehicle state tracking process
CN116300973B (en) * 2023-05-18 2023-08-04 上海伯镭智能科技有限公司 Autonomous obstacle avoidance method for unmanned mine car in complex weather
CN117962932B (en) * 2024-04-02 2024-06-11 福瑞泰克智能系统有限公司 Obstacle travel track generation method and device, storage medium and electronic equipment

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108597223A (en) * 2018-04-28 2018-09-28 北京智行者科技有限公司 A kind of data processing method and system for intelligent vehicle behavior description
CN109131326A (en) * 2018-08-08 2019-01-04 大连理工大学 A kind of adaptive learning algorithms device and its working method with lane-change miscellaneous function

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103605368A (en) * 2013-12-04 2014-02-26 苏州大学张家港工业技术研究院 Method and device for route programming in dynamic unknown environment
CN106527452B (en) * 2016-12-30 2019-11-05 广州汽车集团股份有限公司 Movement paths planning method and system when a kind of pilotless automobile chance barrier
US20190367019A1 (en) * 2018-05-31 2019-12-05 TuSimple System and method for proximate vehicle intention prediction for autonomous vehicles
CN110263607B (en) * 2018-12-07 2022-05-20 电子科技大学 Road-level global environment map generation method for unmanned driving
CN109855639B (en) * 2019-01-15 2022-05-27 天津大学 Unmanned driving trajectory planning method based on obstacle prediction and MPC algorithm
CN109960261B (en) * 2019-03-22 2020-07-03 北京理工大学 Dynamic obstacle avoiding method based on collision detection
CN110362096B (en) * 2019-08-13 2021-05-18 东北大学 Unmanned vehicle dynamic trajectory planning method based on local optimality

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108597223A (en) * 2018-04-28 2018-09-28 北京智行者科技有限公司 A kind of data processing method and system for intelligent vehicle behavior description
CN109131326A (en) * 2018-08-08 2019-01-04 大连理工大学 A kind of adaptive learning algorithms device and its working method with lane-change miscellaneous function

Also Published As

Publication number Publication date
CN112269384A (en) 2021-01-26

Similar Documents

Publication Publication Date Title
CN112269384B (en) Vehicle dynamic trajectory planning method combining obstacle behavior intention
CN113359757B (en) Unmanned vehicle path planning and trajectory tracking method
Wang et al. Trajectory planning and safety assessment of autonomous vehicles based on motion prediction and model predictive control
CN108062094B (en) Autonomous system and method for realizing vehicle driving track planning based on processor
US11698638B2 (en) System and method for predictive path planning in autonomous vehicles
US10235881B2 (en) Autonomous operation capability configuration for a vehicle
US10688991B2 (en) Systems and methods for unprotected maneuver mitigation in autonomous vehicles
Wei et al. A behavioral planning framework for autonomous driving
US10495493B2 (en) Systems and methods for controlling sensing device field of view
CN112368662A (en) Directional adjustment actions for autonomous vehicle operation management
US10678245B2 (en) Systems and methods for predicting entity behavior
EP4030403A1 (en) Method and device for predicting exit for vehicle
CN114212110B (en) Obstacle trajectory prediction method and device, electronic equipment and storage medium
WO2022173880A9 (en) System, method, and computer program product for topological planning in autonomous driving using bounds representations
KR20220110069A (en) Sampling-based maneuver realizer
US20230005173A1 (en) Cross-modality active learning for object detection
CN113460080B (en) Vehicle control device, vehicle control method, and storage medium
WO2023147769A1 (en) Systems, methods, and computer-readable media for spatio-temporal motion planning
CN115221774A (en) Autonomous vehicle traffic simulation and road network modeling
CN117315970A (en) Lane change of autonomous vehicles involving traffic congestion at an intersection
Yijing et al. Local path planning of autonomous vehicles based on A* algorithm with equal-step sampling
CN114639267A (en) Vehicle collision avoidance early warning method in vehicle-road cooperative environment
Ren et al. Intelligent path planning and obstacle avoidance algorithms for autonomous vehicles based on enhanced rrt algorithm
US20230211802A1 (en) Motion planning using spatio-temporal convex corridors
US11827245B2 (en) Systems and methods for estimating motion of an automated vehicle for cooperative driving

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