CN114167860B - Automatic driving optimal track generation method and device - Google Patents
Automatic driving optimal track generation method and device Download PDFInfo
- Publication number
- CN114167860B CN114167860B CN202111406002.7A CN202111406002A CN114167860B CN 114167860 B CN114167860 B CN 114167860B CN 202111406002 A CN202111406002 A CN 202111406002A CN 114167860 B CN114167860 B CN 114167860B
- Authority
- CN
- China
- Prior art keywords
- cost
- candidate
- track
- weight
- vehicle
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 36
- 238000004364 calculation method Methods 0.000 claims abstract description 29
- 238000010606 normalization Methods 0.000 claims abstract description 19
- 238000012545 processing Methods 0.000 claims abstract description 18
- 230000008859 change Effects 0.000 claims description 26
- 238000005070 sampling Methods 0.000 claims description 13
- 230000001186 cumulative effect Effects 0.000 claims description 10
- 230000036461 convulsion Effects 0.000 claims description 7
- 230000003542 behavioural effect Effects 0.000 claims description 6
- 238000006243 chemical reaction Methods 0.000 claims description 6
- 230000003068 static effect Effects 0.000 claims description 6
- 238000010276 construction Methods 0.000 claims description 3
- 230000006870 function Effects 0.000 description 34
- 230000006399 behavior Effects 0.000 description 30
- 238000004422 calculation algorithm Methods 0.000 description 6
- 238000010586 diagram Methods 0.000 description 6
- 238000004590 computer program Methods 0.000 description 5
- 238000012216 screening Methods 0.000 description 4
- 230000003287 optical effect Effects 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 238000005065 mining Methods 0.000 description 2
- 230000000644 propagated effect Effects 0.000 description 2
- 238000005452 bending Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 239000000835 fiber Substances 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- 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
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Abstract
The invention discloses a method and a device for generating an optimal automatic driving track, and relates to the technical field of automatic driving systems. Constructing a cost function of the candidate track according to the cost influence weights of the candidate track and the weight coefficients of the cost influence weights; based on the arctangent function, carrying out normalization processing on each cost influence weight of the candidate track, and converting the cost influence weight into a dimensionless numerical value; configuring weight coefficients of the cost impact weights of the candidate tracks based on the road curvature, the vehicle speed and the automatic driving behavior instructions; and (3) based on the constructed cost function, combining the configured weight coefficient and the converted cost influence weight, performing cost calculation on each candidate track, and taking the candidate track with the minimum cost as the optimal track. The method and the device can realize accurate generation of the optimal track.
Description
Technical Field
The invention relates to the technical field of automatic driving systems, in particular to a method and a device for generating an optimal track of automatic driving.
Background
Trajectory planning is a key technology of automatic driving, and optimal trajectory generation is an important link of trajectory planning. The optimal track generation is mainly to determine an optimal track according to a series of candidate tracks generated in a drivable area. And (3) calculating the cost of each candidate track, and screening out the track with the minimum cost as the optimal track according to the scene in the mining area, the speed of the vehicle and the shape of the road. The cost of each candidate trajectory consists of 5 factors: 1. the degree of track curvature; 2. trace bending degree change rate; 3. track lateral offset; 4. upper level instruction-behavior; 5. distance from the obstacle. Since each factor varies in magnitude, range and unit, it needs to be normalized, while the weight coefficient is affected by the road shape and the current speed of the vehicle and the relative distance to the relevant object.
Currently, the following method is generally adopted in the process of generating an optimal track: step one, track planning is carried out according to a corresponding algorithm to obtain a plurality of expected tracks; step two, calculating potential field peak values and accumulated values of all tracks according to the artificial potential fields, wherein the total potential field consists of a lane line potential field, a road boundary line potential field and an obstacle potential field, the potential field peak value is the potential field value of the maximum point on the track, and the potential field accumulated value is the sum of potential field values of all points on the track; thirdly, calculating the cost of each track by adopting a track cost function; and step four, selecting an optimal track according to the potential field peak value and the accumulated value of each expected track and combining a track screening function. The optimal track generation method is that the road is rasterized, the track planning is carried out by adopting a corresponding algorithm, the effective searching algorithm for solving the shortest path is adopted, but the time complexity is high, the time consumption is long, meanwhile, when the cost is calculated by adopting a manual potential field method, when an obstacle exists near the target point, the repulsive force is very large, the attractive force is relatively small, and the object is difficult to reach the target point, so that the optimal track is difficult to accurately generate.
Disclosure of Invention
Aiming at the defects in the prior art, the invention aims to provide an automatic driving optimal track generation method and device, which can realize accurate generation of an optimal track.
In order to achieve the above object, the present invention provides a method for generating an optimal trajectory for automatic driving, comprising the following steps:
generating candidate tracks based on the road form, pose information and speed information of the vehicle;
constructing a cost function of the candidate track according to the cost influence weights of the candidate track and the weight coefficients of the cost influence weights;
based on the arctangent function, carrying out normalization processing on each cost influence weight of the candidate track, and converting the cost influence weight into a dimensionless numerical value;
configuring weight coefficients of the cost impact weights of the candidate tracks based on the road curvature, the vehicle speed and the automatic driving behavior instructions;
and (3) based on the constructed cost function, combining the configured weight coefficient and the converted cost influence weight, performing cost calculation on each candidate track, and taking the candidate track with the minimum cost as the optimal track.
On the basis of the technical scheme, the generation of the candidate track is performed based on the road form, the pose information and the speed information of the vehicle, and the specific steps include:
based on the road form, pose information and speed information of the vehicle, sampling longitudinal distance and transverse distance of the road, and taking a sampling point as a target point;
carrying out state parameter configuration on a starting point and a target point, wherein the starting point is the current position of the vehicle, and the state parameters comprise a course angle, curvature and vehicle transverse and longitudinal coordinates;
and generating candidate tracks by adopting a fifth-order polynomial according to the configured state parameters.
On the basis of the technical scheme, the candidate track is generated by adopting a fifth-order polynomial, wherein the expression of the generated candidate track is as follows:
L i (s)＝a i5 *s 5 +a i4 *s 4 +a i3 *s 3 +a i2 *s 2 +a i1 *s+a i0
wherein L is i (s) represents candidate trajectories, a i5 Fifth order coefficients representing fifth order polynomials, a i4 Fourth order coefficient representing fifth order polynomial, a i3 Third order term coefficients representing a fifth order polynomial, a i2 A, a represents the quadratic coefficient of the fifth order polynomial i1 First order term coefficients representing a fifth order polynomial, a i0 The zero term coefficient representing the fifth order polynomial, s representing the longitudinal distance of the vehicle from the starting point in the candidate trajectory.
On the basis of the technical scheme, the cost influence weight comprises an average curvature change rate, an average curvature, an average lateral offset, a cumulative lateral offset, a behavior instruction and a minimum distance cost from an obstacle.
Based on the technical scheme, the cost function of the constructed candidate track is specifically as follows:
Cost i ＝W i1 *Jerk i +W i2 *Cur i +W i3 *Averg_Lat i +W i4 *Acumlt_Lat i +W i5 *Bhv i +W i6 *Obs_MinDist i
wherein, cost i Representing the cost of candidate trajectories, jerk i Represents the average curvature change rate, W i1 Weight coefficient indicating average curvature change rate, cur i Represents the average curvature, W i2 Weighting coefficients representing average curvature, averg_lat i Represents the average lateral offset, W i3 Weighting coefficient representing average lateral offset, acumlt_Lat i Represents the cumulative lateral offset, W i4 Weight coefficient representing cumulative lateral offset Bhv i Representing behavioural instructions, W i5 Weight coefficient representing behavior instruction, obs_mindist i Representing the minimum distance cost from the obstacle, W i6 A weight coefficient representing a minimum distance cost from the obstacle.
On the basis of the technical proposal, the method comprises the following steps,
the calculation formula of the average curvature change rate is as follows:
the calculation formula of the average curvature is as follows:
the calculation formula of the average lateral offset is as follows:
the calculation formula of the accumulated lateral offset is as follows:
wherein s is 0 Representing the longitudinal distance s of the vehicle at the starting point i1 Representing a vehicleThe longitudinal distance at the target point, n represents the equal division of the candidate trajectory,
represents L i (s) second derivative of s, < ->
Represents L i (s) deriving s three times.
On the basis of the technical proposal, the method comprises the following steps,
the method is characterized in that the weight coefficient of each cost influence weight of the candidate track is configured based on the road curvature, the vehicle speed and the automatic driving behavior instruction, and specifically comprises the following steps: based on road curvature, vehicle speed and automatic driving behavior instructions, configuring weight coefficients of cost influence weights of candidate tracks according to different driving conditions;
the driving working conditions comprise straight-way cruising, turning, static obstacle avoidance, meeting and lane changing.
On the basis of the technical scheme, the weight coefficient of each cost influence weight of the candidate track is configured based on the road curvature, the vehicle speed and the automatic driving behavior instruction, and the configuration process specifically comprises:
under the straight-way cruising driving condition, W i1 、W i2 、W i3 Proportional to vehicle speed, W i4 Inversely proportional to vehicle speed, W i3 、W i4 Proportional to the curvature of the road;
under the turning driving condition, W i4 Inversely proportional to vehicle speed, W i3 、W i4 Proportional to the curvature of the road;
under the static obstacle avoidance driving working condition, W i4 Inversely proportional to the collision time, which is calculated based on the relative distance and the relative speed;
under the driving condition of meeting the vehicles, W i4 Inversely proportional to the collision time;
under the driving condition of lane change, W i1 、W i2 、W i3 Proportional to vehicle speed, W i4 Inversely proportional to vehicle speed, W i4 Proportional to the lane change completion time.
On the basis of the above technical solution, before constructing the cost function of the candidate track according to the cost impact weight of the candidate track and the weight coefficient of each cost impact weight, the method further includes: and removing candidate tracks beyond the road boundary from the generated candidate tracks.
Based on the technical scheme, the normalization processing is carried out on each cost influence weight of the candidate track based on the arctangent function, and the cost influence weight is converted into a dimensionless numerical value, wherein the calculation formula for carrying out the normalization processing is as follows:
y＝tan -1 x*2/P1
where y represents the cost impact weight after conversion, x represents the cost impact weight, and P1 represents the circumference ratio.
The invention provides an automatic driving optimal track generating device, which comprises:
the generation module is used for generating candidate tracks based on the road form, the pose information and the speed information of the vehicle;
the construction module is used for constructing a cost function of the candidate track according to the cost influence weights of the candidate track and the weight coefficients of the cost influence weights;
the conversion module is used for carrying out normalization processing on each cost influence weight of the candidate track based on the arctangent function and converting the cost influence weight into a dimensionless numerical value;
the configuration module is used for configuring weight coefficients of the cost impact weights of the candidate tracks based on the road curvature, the vehicle speed and the automatic driving behavior instructions;
the selection module is used for carrying out cost calculation on each candidate track based on the constructed cost function and combining the configured weight coefficient and the converted cost influence weight, and taking the candidate track with the minimum cost as the optimal track.
Compared with the prior art, the invention has the advantages that: according to the method, a candidate track is generated by combining vehicle information in an actual road scene through a polynomial fitting-based algorithm, meanwhile, according to different shapes and different behavior instructions of a road, the cost influence weight is considered, the cost influence weight is subjected to normalization processing, unit limitation of the cost influence weight is removed, meanwhile, the weight coefficient of each cost influence weight of the candidate track is configured based on the road curvature, the vehicle speed and an automatic driving behavior instruction, so that the weight coefficient of the cost influence weight can be changed along with different scenes, the optimal track is screened out, the operation time is effectively shortened, the timeliness is improved, and the accurate generation of the optimal track is realized.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings that are needed in the description of the embodiments will be briefly introduced below, and it is obvious that the drawings in the following description are only some embodiments of the present application, and that other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
FIG. 1 is a flowchart of a method for generating an optimal trajectory for automatic driving according to an embodiment of the present invention;
FIG. 2 is a diagram illustrating road sampling according to an embodiment of the present invention;
fig. 3 is a schematic diagram of candidate trajectories generated in an embodiment of the invention.
Detailed Description
The embodiment of the invention provides an automatic driving optimal track generation method, which combines vehicle information in an actual road scene, adopts a polynomial fitting-based algorithm to generate candidate tracks, simultaneously carries out normalization processing on cost influence weights according to different shapes and different behavior instructions of roads and considering the composition of the cost influence weights, removes unit limitations of the cost influence weights, and configures weight coefficients of all the cost influence weights of the candidate tracks based on road curvature, vehicle speed and automatic driving behavior instructions, so that the weight coefficients of the cost influence weights can be changed along with different scenes, thereby screening out optimal tracks, effectively reducing operation time length, improving timeliness and realizing accurate generation of the optimal tracks. The embodiment of the invention correspondingly provides a device for generating the optimal track of the automatic driving.
For the purposes of making the objects, technical solutions and advantages of the embodiments of the present application more clear, the technical solutions of the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application, and it is apparent that the described embodiments are some embodiments of the present application, but not all embodiments.
Referring to fig. 1, the method for generating an optimal trajectory for automatic driving according to the embodiment of the present invention specifically includes the following steps:
s1: generating candidate tracks based on the road form, pose information and speed information of the vehicle; namely, generating candidate tracks by adopting a quintic polynomial according to the shape of the road where the vehicle is currently located, and the current pose information and the current speed information of the vehicle, wherein the generated candidate tracks comprise a plurality of pieces.
In the embodiment of the invention, the generation of the candidate track is performed based on the road form, the pose information and the speed information of the vehicle, and the specific steps include:
s101: based on the road form, pose information and speed information of the vehicle, sampling longitudinal distance and transverse distance of the road, and taking a sampling point as a target point; referring to fig. 2, a current position of the vehicle is taken as a starting point, and then sampling is performed along a transverse and longitudinal direction of the road to obtain a plurality of sampling points, wherein the dots in fig. 2 represent the sampling points, and the rectangular boxes represent the vehicle. The 24 sample points are included in fig. 2, so that 24 candidate tracks are generated later.
S102: carrying out state parameter configuration on a starting point and a target point, wherein the starting point is the current position of the vehicle, and the state parameters comprise a course angle, curvature and vehicle transverse and longitudinal coordinates; i.e. the vehicle's lateral and longitudinal coordinates when the vehicle is at the start point or target point, and the heading angle and curvature of the candidate trajectory at the start point or target point.
S103: and generating candidate tracks by adopting a fifth-order polynomial according to the configured state parameters. I.e. configuring parameters required for candidate trajectory generation according to the state parameters of the starting point or the target point, the planned candidate trajectory is represented by a fifth order polynomial.
In the embodiment of the invention, a candidate track is generated by adopting a fifth-order polynomial, wherein the expression of the generated candidate track is as follows:
L i (s)＝a i5 *s 5 +a i4 *s 4 +a i3 *s 3 +a i2 *s 2 +a i1 *s+a i0
wherein L is i (s) represents a candidate track, in particular the ith candidate track, a i5 Fifth order coefficients representing fifth order polynomials, a i4 Fourth order coefficient representing fifth order polynomial, a i3 Third order term coefficients representing a fifth order polynomial, a i2 A, a represents the quadratic coefficient of the fifth order polynomial i1 First order term coefficients representing a fifth order polynomial, a i0 The zero term coefficient representing the fifth order polynomial, s representing the longitudinal distance of the vehicle from the starting point in the candidate trajectory. For example, referring to fig. 3, each curve in fig. 3 represents one candidate track, a dot represents a sampling point, a rectangular box represents a vehicle, and each candidate track in fig. 3 can be represented by the above expression.
From the above expression, the ith fifth order polynomial candidate trajectory has 6 parameters, so 6 constraint parameters are required for configuration. When the vehicle is at the starting point, the longitudinal distance is s 0 When the lateral distance of the vehicle from the reference line is L 0 The heading angle of the vehicle and the reference line is L. 0 The projected point curvature of the vehicle on the reference line is l. 0 The reference line is a lane center line; the vehicle is at the target point with a longitudinal distance s i1 When the lateral distance of the vehicle from the reference line is L i1 The heading angle of the vehicle and the reference line is L. i1 The projected point curvature of the vehicle on the reference line is l. i1 From this, L can be calculated i (s) and (d) a polynomial coefficient matrix therebetween, as follows:
thereby calculating a model of the candidate trajectory.
S2: constructing a cost function of the candidate track according to the cost influence weights of the candidate track and the weight coefficients of the cost influence weights; in an embodiment of the present invention, the cost impact weight includes an average curvature change rate, an average curvature, an average lateral offset, a cumulative lateral offset, a behavioral command, and a minimum distance cost from the obstacle.
In the embodiment of the present invention, before constructing the cost function of the candidate track according to the cost impact weight of the candidate track and the weight coefficient of each cost impact weight, the method further includes: and removing candidate tracks beyond the road boundary from the generated candidate tracks.
S3: based on the arctangent function, carrying out normalization processing on each cost influence weight of the candidate track, and converting the cost influence weight into a dimensionless numerical value;
s4: configuring weight coefficients of the cost impact weights of the candidate tracks based on the road curvature, the vehicle speed and the automatic driving behavior instructions;
s5: and (3) based on the constructed cost function, combining the configured weight coefficient and the converted cost influence weight, performing cost calculation on each candidate track, and taking the candidate track with the minimum cost as the optimal track. Namely, for the cost function constructed in the step S2, the cost influence weight of the cost function in the step S2 is converted into the converted cost influence weight, the weight coefficient is converted into the reconfigured weight coefficient, then cost calculation is carried out to obtain the cost of each candidate track, and the candidate track with the minimum cost is used as the optimal track. And outputting the optimal track to a transverse control layer of the automatic driving after the optimal track is obtained.
In the embodiment of the invention, one candidate track is screened out from a plurality of generated candidate tracks and used as an optimal track, the candidate track exceeding the road boundary is firstly removed, the cost of each remaining candidate track is calculated, the cost function of each candidate track consists of 6 weights, and specifically, the cost function of the constructed candidate track is as follows:
Cost i ＝W i1 *Jerk i +W i2 *Cur i +W i3 *Averg_Lat i +W i4 *Acumlt_Lat i +W i5 *Bhv i +W i6 *Obs_MinDist i
wherein, cost i Representing the cost of candidate trajectories, jerk i Represents the average curvature change rate, W i1 Weight coefficient indicating average curvature change rate, cur i Represents the average curvature, W i2 Weighting coefficients representing average curvature, averg_lat i Represents the average lateral offset, W i3 Weighting coefficient representing average lateral offset, acumlt_Lat i Represents the cumulative lateral offset, W i4 Weight coefficient representing cumulative lateral offset Bhv i Representing behavioural instructions, W i5 Weight coefficient representing behavior instruction, obs_mindist i Representing the minimum distance cost from the obstacle, W i6 A weight coefficient representing a minimum distance cost from the obstacle.
Wherein the average curvature change rate corresponds to continuity and smoothness of the candidate track; the average curvature corresponds to the degree of curvature of the candidate track; accumulating the completion time of the vehicle joint change channel corresponding to the transverse offset; in the behavior instructions, if the behaviors are different, selecting different candidate tracks; for minimum distance costs from an obstacle, the smaller the minimum distance, the more dangerous the vehicle.
In the embodiment of the invention, the calculation formula of the average curvature change rate is as follows:
the average curvature change rate is used to represent the smoothness of the candidate trajectory, and Jerk i The smaller the value, the smoother the candidate trajectory.
The calculation formula of the average curvature is as follows:
the average curvature is used to represent the change in the candidate trajectory yaw angle, and Cur i The smaller the value, the smaller the yaw angle change is relatively.
The calculation formula of the average lateral offset is as follows:
the average lateral offset is used to represent the average lateral offset distance of the candidate track;
the calculation formula of the accumulated lateral offset is as follows:
the accumulated lateral offset is used for the accumulated lateral offset distance of the candidate track, and the greater the candidate track longitudinal distance, the greater the accumulated lateral offset.
Wherein s is 0 Representing the longitudinal distance s of the vehicle at the starting point i1 Represents the longitudinal distance of the vehicle at the target point, n represents the equal division of the candidate track, the value of n can be 100 or 500,
represents L i (s) second derivative of s, < ->
Represents L i (s) deriving s three times.
The behavior instruction is used for expressing the instruction cost sent by the automatic driving behavior layer, and if the behavior is cruising and turning, the closer the target point is to the center line of the current running road, the lower the instruction cost is; if the behavior is obstacle avoidance and vehicle meeting, the farther the target point is from the vehicle meeting and the obstacle, the lower the instruction cost.
The minimum distance cost from the obstacle is used to represent the cost of the distance closest to the obstacle in the candidate trajectory, and the closer the distance is, the higher the cost is.
In the embodiment of the invention, based on an arctangent function, normalization processing is carried out on each cost influence weight of the candidate track, and the cost influence weight is converted into a dimensionless numerical value, wherein the calculation formula for carrying out normalization processing is as follows:
y＝tan -1 x*2/P1
where y represents the cost impact weight after conversion, x represents the cost impact weight, P1 represents the circumference ratio, and tan represents the tangent calculation.
Because the cost function has different units and different orders of magnitude of the cost influence weights, normalization processing is needed for the cost influence weights, and the data normalization is realized by adopting an arctangent function, wherein the mapped interval is [0,1].
The unit constraint of the data is removed and converted into a dimensionless pure number. Meanwhile, since the numerical value of each cost impact weight is positive, and the maximum value and the minimum value of each cost impact weight cannot be determined, an arctangent function is adopted, and the normalization results are positive values.
In the embodiment of the invention, based on road curvature, vehicle speed and automatic driving behavior instructions, the weight coefficient of each cost influence weight of the candidate track is configured, and the method specifically comprises the following steps: based on road curvature, vehicle speed and automatic driving behavior instructions, configuring weight coefficients of cost influence weights of candidate tracks according to different driving conditions; driving conditions include straight road cruising, turning, static obstacle avoidance, meeting and lane changing. Different behaviors can be determined for different target lanes when the weight coefficients are configured, and Bhv when configured i Should be at maximum, i.e. W i5 Should be maximized.
In the embodiment of the invention, the weight coefficient of each cost influence weight of the candidate track is configured based on the road curvature, the vehicle speed and the automatic driving behavior instruction, and the configuration process specifically comprises the following steps:
under the straight-way cruising driving condition, W i1 、W i2 、W i3 Proportional to vehicle speed, W i4 Inversely proportional to vehicle speed, W i3 、W i4 Proportional to the curvature of the road;
under the turning driving condition, W i4 Inversely proportional to vehicle speed, W i3 、W i4 Proportional to the curvature of the road;
under the static obstacle avoidance driving working condition, W i4 Inversely proportional to the collision time, which is calculated based on the relative distance and the relative speed;
under the driving condition of meeting the vehicles, W i4 Inversely proportional to the collision time;
under the driving condition of lane change, W i1 、W i2 、W i3 Proportional to vehicle speed, W i4 Inversely proportional to vehicle speed, W i4 Proportional to the lane change completion time.
According to the automatic driving optimal track generation method, an automatic driving vehicle generates candidate tracks according to the pose form and the road shape of the current vehicle; the method comprises the steps of forming a cost function by considering five aspects of planning track smoothness, transverse offset, experience of a driver, upper-layer behavior instructions and relative distance between the upper-layer behavior instructions and obstacles, wherein each aspect is called cost influence weight, normalizing each cost influence weight according to different units of the cost influence weight, removing unit limitation, and converting the cost influence weight into a dimensionless numerical value; the optimal track is screened according to the cost function, the decision is an important component of autonomous driving, whether the vehicle can stably and accurately finish various driving functions is determined, the track planning is an important research field of a decision layer, the track screening is an important link of the track planning, the execution condition of a transverse control layer is directly influenced, and the safety of the autonomous driving vehicle and the experience of the comfort of a driver are influenced.
The current position point and the target point of the vehicle are subjected to polynomial curve fitting for five times, so that the time consumption is reduced, and the smoothness and timeliness of the planned track are improved; aiming at special automatic driving scenes, such as mining area scenes, the speed of an automatic driving vehicle is generally slower, and the vehicle kinematic limit, lane changing and meeting working conditions, upper-layer behavior instructions and time limit of the vehicle reaching the target point position from the current position are considered, so that the weight of the cost function is correspondingly changed, and the weight coefficient is correspondingly changed to meet the automatic driving condition.
In a possible implementation manner, the embodiment of the present invention further provides a readable storage medium, where the readable storage medium is located in the PLC controller, and the readable storage medium stores a computer program, where the program is executed by the processor to implement the following steps of the automatic driving optimal trajectory generation method:
generating candidate tracks based on the road form, pose information and speed information of the vehicle;
constructing a cost function of the candidate track according to the cost influence weights of the candidate track and the weight coefficients of the cost influence weights;
based on the arctangent function, carrying out normalization processing on each cost influence weight of the candidate track, and converting the cost influence weight into a dimensionless numerical value;
configuring weight coefficients of the cost impact weights of the candidate tracks based on the road curvature, the vehicle speed and the automatic driving behavior instructions;
and (3) based on the constructed cost function, combining the configured weight coefficient and the converted cost influence weight, performing cost calculation on each candidate track, and taking the candidate track with the minimum cost as the optimal track.
The storage media may take the form of any combination of one or more computer-readable media. The computer readable medium may be a computer readable signal medium or a computer readable storage medium. The computer readable storage medium may be, for example, but not limited to: an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or a combination of any of the foregoing. More specific examples (a non-exhaustive list) of the computer-readable storage medium would include the following: an electrical connection having one or more wires, a portable computer diskette, a hard disk, a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing. In this document, a computer readable storage medium may be any tangible medium that can contain, or store a program for use by or in connection with an instruction execution system, apparatus, or device.
The computer readable signal medium may include a propagated data signal with computer readable program code embodied therein, either in baseband or as part of a carrier wave. Such a propagated data signal may take any of a variety of forms, including, but not limited to, electro-magnetic, optical, or any suitable combination of the foregoing. A computer readable signal medium may also be any computer readable medium that is not a computer readable storage medium and that can communicate, propagate, or transport a program for use by or in connection with an instruction execution system, apparatus, or device. Program code embodied on a computer readable medium may be transmitted using any appropriate medium, including but not limited to: wireless, wire, fiber optic cable, RF, etc., or any suitable combination of the foregoing.
Computer program code for carrying out operations of the present invention may be written in one or more programming languages, including an object oriented programming language such as Java, smalltalk, C ++ and conventional procedural programming languages, such as the "C" programming language or similar programming languages. The program code may execute entirely on the user's computer, partly on the user's computer, as a stand-alone software package, partly on the user's computer and partly on a remote computer or entirely on the remote computer or server. In the case of a remote computer, the remote computer may be connected to the user's computer through any kind of network, including a Local Area Network (LAN) or a Wide Area Network (WAN), or may be connected to an external computer (for example, through the Internet using an Internet service provider).
The device for generating the optimal track of the automatic driving provided by the embodiment of the invention comprises a generating module, a constructing module, a converting module, a configuring module and a selecting module.
The generation module is used for generating candidate tracks based on the road form, the pose information and the speed information of the vehicle; the construction module is used for constructing a cost function of the candidate track according to the cost influence weights of the candidate track and the weight coefficients of the cost influence weights; the conversion module is used for carrying out normalization processing on each cost influence weight of the candidate track based on the arctangent function and converting the cost influence weight into a dimensionless numerical value; the configuration module is used for configuring weight coefficients of the cost influence weights of the candidate tracks based on the road curvature, the vehicle speed and the automatic driving behavior instruction; the selection module is used for carrying out cost calculation on each candidate track based on the constructed cost function and combining the configured weight coefficient and the converted cost influence weight, and taking the candidate track with the minimum cost as the optimal track.
According to the automatic driving optimal track generation device, the candidate tracks are generated by adopting a polynomial fitting-based algorithm in combination with vehicle information in an actual road scene, meanwhile, according to different shapes and different behavior instructions of a road, the cost influence weight is considered, normalization processing is carried out on the cost influence weight, unit limitation of the cost influence weight is removed, and meanwhile, the weight coefficient of each cost influence weight of the candidate tracks is configured on the basis of the curvature of the road, the speed of the vehicle and the automatic driving behavior instruction, so that the weight coefficient of the cost influence weight can be changed along with different scenes, the optimal track is screened out, the operation time is effectively shortened, the timeliness is improved, and the accurate generation of the optimal track is realized.
The foregoing is merely a specific embodiment of the application to enable one skilled in the art to understand or practice the application. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the generic principles defined herein may be applied to other embodiments without departing from the spirit or scope of the application. Thus, the present application is not intended to be limited to the embodiments shown herein but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.
The present invention is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems) and computer program products according to embodiments of the invention. It will be understood that each flow and/or block of the flowchart illustrations and/or block diagrams, and combinations of flows and/or blocks in the flowchart illustrations and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
Claims (6)
1. The method for generating the optimal track of the automatic driving is characterized by comprising the following steps of:
generating candidate tracks based on the road form, pose information and speed information of the vehicle;
constructing a cost function of the candidate track according to the cost influence weights of the candidate track and the weight coefficients of the cost influence weights;
based on the arctangent function, carrying out normalization processing on each cost influence weight of the candidate track, and converting the cost influence weight into a dimensionless numerical value;
configuring weight coefficients of the cost impact weights of the candidate tracks based on the road curvature, the vehicle speed and the automatic driving behavior instructions;
based on the constructed cost function, combining the configured weight coefficient and the converted cost influence weight, performing cost calculation on each candidate track, and taking the candidate track with the minimum cost as an optimal track;
the method for generating the candidate track based on the road form, the pose information and the speed information of the vehicle comprises the following specific steps:
based on the road form, pose information and speed information of the vehicle, sampling longitudinal distance and transverse distance of the road, and taking a sampling point as a target point;
carrying out state parameter configuration on a starting point and a target point, wherein the starting point is the current position of the vehicle, and the state parameters comprise a course angle, curvature and vehicle transverse and longitudinal coordinates;
generating candidate tracks by adopting a fifth-order polynomial according to the configured state parameters;
the candidate track is generated by adopting a five-degree polynomial, wherein the expression of the generated candidate track is as follows:
L i (s)＝a i5 *s 5 +a i4 *s 4 +a i3 *s 3 +a i2 *s 2 +a i1 *s+a i0
wherein L is i (s) represents candidate trajectories, a i5 Fifth order coefficients representing fifth order polynomials, a i4 Fourth order coefficient representing fifth order polynomial, a i3 Third order term coefficients representing a fifth order polynomial, a i2 A, a represents the quadratic coefficient of the fifth order polynomial i1 First order term coefficients representing a fifth order polynomial, a i0 A zero term coefficient representing a fifth order polynomial, s representing a longitudinal distance between the vehicle and the starting point in the candidate trajectory;
wherein the cost impact weight includes an average curvature change rate, an average curvature, an average lateral offset, a cumulative lateral offset, a behavioral command, and a minimum distance cost from the obstacle;
wherein, the calculation formula of the average curvature change rate is as follows:
the calculation formula of the average curvature is as follows:
the calculation formula of the average lateral offset is as follows:
the calculation formula of the accumulated lateral offset is as follows:
wherein s is 0 Representing the longitudinal distance s of the vehicle at the starting point i1 Represents the longitudinal distance of the vehicle at the target point, n represents the division of the candidate trajectory into equal parts,
represents L i (s) second derivative of s, < ->
Represents L i (s) deriving s three times.
2. The method for generating an optimal trajectory for automatic driving according to claim 1, wherein the cost function of the constructed candidate trajectory is specifically:
Cost i ＝W i1 *Jerk i +W i2 *Cur i +W i3 *Averg_Lat i +
W i4 *Acumlt_Lat i +W i5 *Bhv i +W i6 *Obs_MinDist i
wherein, cost i Representing the cost of candidate trajectories, jerk i Represents the average curvature change rate, W i1 Weight coefficient indicating average curvature change rate, cur i Represents the average curvature, W i2 Weighting coefficients representing average curvature, averg_lat i Represents the average lateral offset, W i3 Weighting coefficient representing average lateral offset, acumlt_Lat i Represents the cumulative lateral offset, W i4 Weight coefficient representing cumulative lateral offset Bhv i Representing behavioural instructions, W i5 Weight coefficient representing behavior instruction, obs_mindist i Representing the minimum distance cost from the obstacle, W i6 A weight coefficient representing a minimum distance cost from the obstacle.
3. The automatic driving optimal trajectory generation method according to claim 1, wherein:
the method is characterized in that the weight coefficient of each cost influence weight of the candidate track is configured based on the road curvature, the vehicle speed and the automatic driving behavior instruction, and specifically comprises the following steps: based on road curvature, vehicle speed and automatic driving behavior instructions, configuring weight coefficients of cost influence weights of candidate tracks according to different driving conditions;
the driving working conditions comprise straight-way cruising, turning, static obstacle avoidance, meeting and lane changing.
4. The method for generating an optimal trajectory for automatic driving according to claim 3, wherein the configuring of the weighting coefficients of the cost-impact weights of the candidate trajectories based on the road curvature, the vehicle speed and the automatic driving behavior command comprises the following steps:
under the straight-way cruising driving condition, W i1 、W i2 、W i3 Proportional to vehicle speed, W i4 Inversely proportional to vehicle speed, W i3 、W i4 Proportional to the curvature of the road;
under the turning driving condition, W i4 Inversely proportional to vehicle speed, W i3 、W i4 Proportional to the curvature of the road;
under the static obstacle avoidance driving working condition, W i4 Inversely proportional to the collision time, which is calculated based on the relative distance and the relative speed;
under the driving condition of meeting the vehicles, W i4 Inversely proportional to the collision time;
under the driving condition of lane change, W i1 、W i2 、W i3 Proportional to vehicle speed, W i4 Inversely proportional to vehicle speed, W i4 Proportional to the lane change completion time.
5. The automatic driving optimal trajectory generation method according to claim 1, wherein each cost impact weight of the candidate trajectory is normalized based on an arctangent function and converted into a dimensionless value, and wherein a calculation formula for performing the normalization is:
y＝tan -1 x*2/P1
where y represents the cost impact weight after conversion, x represents the cost impact weight, and P1 represents the circumference ratio.
6. An automatic driving optimum trajectory generation device, characterized by comprising:
the generation module is used for generating candidate tracks based on the road form, the pose information and the speed information of the vehicle;
the construction module is used for constructing a cost function of the candidate track according to the cost influence weights of the candidate track and the weight coefficients of the cost influence weights;
the conversion module is used for carrying out normalization processing on each cost influence weight of the candidate track based on the arctangent function and converting the cost influence weight into a dimensionless numerical value;
the configuration module is used for configuring weight coefficients of the cost impact weights of the candidate tracks based on the road curvature, the vehicle speed and the automatic driving behavior instructions;
the selection module is used for carrying out cost calculation on each candidate track based on the constructed cost function and combining the configured weight coefficient and the converted cost influence weight, and taking the candidate track with the minimum cost as the optimal track;
the method for generating the candidate track based on the road form, the pose information and the speed information of the vehicle comprises the following specific steps:
based on the road form, pose information and speed information of the vehicle, sampling longitudinal distance and transverse distance of the road, and taking a sampling point as a target point;
carrying out state parameter configuration on a starting point and a target point, wherein the starting point is the current position of the vehicle, and the state parameters comprise a course angle, curvature and vehicle transverse and longitudinal coordinates;
generating candidate tracks by adopting a fifth-order polynomial according to the configured state parameters;
the candidate track is generated by adopting a five-degree polynomial, wherein the expression of the generated candidate track is as follows:
L i (s)＝a i5 *s 5 +a i4 *s 4 +a i3 *s 3 +a i2 *s 2 +a i1 *s+a i0
wherein L is i (s) represents candidate trajectories, a i5 Fifth order coefficients representing fifth order polynomials, a i4 Fourth order coefficient representing fifth order polynomial, a i3 Third order term coefficients representing a fifth order polynomial, a i2 A, a represents the quadratic coefficient of the fifth order polynomial i1 First order term coefficients representing a fifth order polynomial, a i0 A zero term coefficient representing a fifth order polynomial, s representing a longitudinal distance between the vehicle and the starting point in the candidate trajectory;
wherein the cost impact weight includes an average curvature change rate, an average curvature, an average lateral offset, a cumulative lateral offset, a behavioral command, and a minimum distance cost from the obstacle;
wherein, the calculation formula of the average curvature change rate is as follows:
the calculation formula of the average curvature is as follows:
the calculation formula of the average lateral offset is as follows:
the calculation formula of the accumulated lateral offset is as follows:
wherein the method comprises the steps of，s 0 Representing the longitudinal distance s of the vehicle at the starting point i1 Represents the longitudinal distance of the vehicle at the target point, n represents the division of the candidate trajectory into equal parts,
represents L i (s) second derivative of s, < ->
Represents L i (s) deriving s three times.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111406002.7A CN114167860B (en) | 2021-11-24 | 2021-11-24 | Automatic driving optimal track generation method and device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111406002.7A CN114167860B (en) | 2021-11-24 | 2021-11-24 | Automatic driving optimal track generation method and device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114167860A CN114167860A (en) | 2022-03-11 |
| CN114167860B true CN114167860B (en) | 2023-07-07 |
Family
ID=80480722
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111406002.7A Active CN114167860B (en) | 2021-11-24 | 2021-11-24 | Automatic driving optimal track generation method and device |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114167860B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114964288A (en) * | 2022-05-16 | 2022-08-30 | 北京京东乾石科技有限公司 | Path planning method and device and unmanned vehicle |
Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2021175313A1 (en) * | 2020-03-05 | 2021-09-10 | 中国第一汽车股份有限公司 | Automatic driving control method and device, vehicle, and storage medium |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10515321B2 (en) * | 2017-09-11 | 2019-12-24 | Baidu Usa Llc | Cost based path planning for autonomous driving vehicles |
| US20190204842A1 (en) * | 2018-01-02 | 2019-07-04 | GM Global Technology Operations LLC | Trajectory planner with dynamic cost learning for autonomous driving |
| US11378961B2 (en) * | 2018-04-17 | 2022-07-05 | Baidu Usa Llc | Method for generating prediction trajectories of obstacles for autonomous driving vehicles |
| US11199847B2 (en) * | 2018-09-26 | 2021-12-14 | Baidu Usa Llc | Curvature corrected path sampling system for autonomous driving vehicles |
| CN109375632B (en) * | 2018-12-17 | 2020-03-20 | 清华大学 | Real-time trajectory planning method for automatic driving vehicle |
| CN112824198B (en) * | 2019-11-21 | 2023-05-02 | 北京京东乾石科技有限公司 | Track decision method, device, equipment and storage medium |
-
2021
- 2021-11-24 CN CN202111406002.7A patent/CN114167860B/en active Active
Patent Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2021175313A1 (en) * | 2020-03-05 | 2021-09-10 | 中国第一汽车股份有限公司 | Automatic driving control method and device, vehicle, and storage medium |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114167860A (en) | 2022-03-11 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112677995B (en) | Vehicle track planning method and device, storage medium and equipment | |
| CN109960261B (en) | Dynamic obstacle avoiding method based on collision detection | |
| CN110834644B (en) | Vehicle control method and device, vehicle to be controlled and storage medium | |
| Sadat et al. | Jointly learnable behavior and trajectory planning for self-driving vehicles | |
| CN106371439B (en) | Unified automatic driving transverse planning method and system | |
| WO2018176593A1 (en) | Local obstacle avoidance path planning method for unmanned bicycle | |
| CN112810630B (en) | Method and system for planning track of automatic driving vehicle | |
| CN111238521B (en) | Path planning method and system for unmanned vehicle | |
| CN112389427A (en) | Vehicle track optimization method and device, electronic equipment and storage medium | |
| CN112394725B (en) | Prediction and reaction field of view based planning for autopilot | |
| CN111896004A (en) | Narrow passage vehicle track planning method and system | |
| CN114167860B (en) | Automatic driving optimal track generation method and device | |
| CN113015981A (en) | System and method for efficient, continuous and safe learning using first principles and constraints | |
| Piacentini et al. | A macroscopic model for platooning in highway traffic | |
| Liu et al. | Impact of sharing driving attitude information: A quantitative study on lane changing | |
| CN114715193A (en) | Real-time trajectory planning method and system | |
| Gu et al. | Safe-state enhancement method for autonomous driving via direct hierarchical reinforcement learning | |
| Zheng et al. | Learning-based safe control for robot and autonomous vehicle using efficient safety certificate | |
| Yu et al. | Hierarchical framework integrating rapidly-exploring random tree with deep reinforcement learning for autonomous vehicle | |
| CN115973179A (en) | Model training method, vehicle control method, device, electronic equipment and vehicle | |
| CN114117944B (en) | Model updating method, device, equipment and readable storage medium | |
| Zeng et al. | A steerable curvature approach for efficient executable path planning for on-road autonomous vehicle | |
| CN114670837A (en) | Intelligent automobile human-like bend track dual planning system | |
| CN114537435A (en) | Real-time whole vehicle track planning method in automatic driving | |
| Peng et al. | Hierarchical motion planning system with consideration of the dynamic lane-changing behaviour |
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 |