CN115309170A - Method, device and system for planning track by considering comfort constraint - Google Patents

Method, device and system for planning track by considering comfort constraint Download PDF

Info

Publication number
CN115309170A
CN115309170A CN202211243619.6A CN202211243619A CN115309170A CN 115309170 A CN115309170 A CN 115309170A CN 202211243619 A CN202211243619 A CN 202211243619A CN 115309170 A CN115309170 A CN 115309170A
Authority
CN
China
Prior art keywords
speed
path
constraint
vehicle
comfort
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.)
Granted
Application number
CN202211243619.6A
Other languages
Chinese (zh)
Other versions
CN115309170B (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.)
Zhejiang Lab
Original Assignee
Zhejiang Lab
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 Zhejiang Lab filed Critical Zhejiang Lab
Priority to CN202211243619.6A priority Critical patent/CN115309170B/en
Publication of CN115309170A publication Critical patent/CN115309170A/en
Application granted granted Critical
Publication of CN115309170B publication Critical patent/CN115309170B/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 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/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • 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/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
    • 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, a device and a system for planning a track in consideration of comfort constraint, wherein the method comprises the following steps: calculating the coordinates of projection points projected from the self vehicle to a reference line according to the global position of the self vehicle and the information of the reference line, and sampling to obtain discrete original reference points; step two, obtaining a path optimization result by constructing and calculating a cost function and constraint of vehicle driving path planning, and calculating an orientation angle and a curvature corresponding to the path planning according to the path optimization result to obtain a smoothed path; thirdly, obtaining a speed optimization result by constructing and calculating a cost function and a constraint of the vehicle running speed plan, and calculating an orientation angle and a curvature corresponding to the speed plan according to the speed optimization result to obtain a smoothed speed; and step four, fusing the smoothed speed and the path to obtain a smooth track with speed information. The invention can carry out constraint optimization on the planned path and speed of the automatic driving vehicle and realize the riding comfort experience of automatic driving.

Description

Track planning method, device and system considering comfort constraint
Technical Field
The invention belongs to the field of trajectory planning of automatic driving, and relates to a method, a device and a system for trajectory planning considering comfort constraint.
Background
In the field of trajectory planning of automatic driving, path planning and speed planning are generally decoupled and solved, and trajectory planning is taken as an important ring in automatic driving, so that not only is the safety of a final vehicle driving trajectory determined, but also the comfort of automatic driving and riding is reflected. Most of the existing trajectory planning algorithms are solved by constructing an optimization problem, and a comfort item is put into a cost function (soft constraint) of the optimization problem, so that the planning result in some scenes cannot completely meet the comfort requirement of riding.
Disclosure of Invention
In order to solve the technical problems in the prior art, the invention provides a track planning method, a device and a system considering comfort constraint, which can carry out constraint optimization on a planned path and speed of an automatic driving vehicle and realize riding comfort experience of automatic driving, and the specific technical scheme is as follows:
a trajectory planning method taking comfort constraints into account, comprising the steps of:
calculating a projection point coordinate projected to a reference line by the self-vehicle according to the positioned self-vehicle global position and the information of the reference line, and sampling the projection point coordinate to a fixed length at equal intervals to obtain a plurality of discrete original reference points;
step two, optimizing the path plan by constructing and calculating a cost function and constraint of the vehicle driving path plan to obtain a path optimization result, and calculating an orientation angle and a curvature corresponding to the path plan according to the path optimization result to obtain a smoothed path;
thirdly, optimizing the planned speed by constructing and calculating a cost function and constraint of the vehicle running speed plan to obtain a speed optimization result, and calculating an orientation angle and a curvature corresponding to the speed plan to obtain a smoothed speed;
and step four, fusing the smoothed speed and the smoothed path to obtain a smooth track with speed information.
Further, the step one specifically includes the following sub-steps:
step 1.1: according to the global position of the vehicle given by the positioning module of the vehicle
Figure 100002_DEST_PATH_IMAGE001
And purpose of decision givingThe information of the marking lane is used for calculating the coordinate of the reference point of the projection of the self-vehicle on the original lane central line as the reference line
Figure 528164DEST_PATH_IMAGE002
) The coordinate of the closest point of the bicycle from the reference line;
step 1.2: for the input reference line information, from the projection point coordinates of the matched reference line
Figure 914146DEST_PATH_IMAGE002
) To begin with
Figure 100002_DEST_PATH_IMAGE003
For sampling intervals that are sampled equidistantly to a fixed length, comprising n discrete reference points (
Figure 490621DEST_PATH_IMAGE004
）、(
Figure 100002_DEST_PATH_IMAGE005
), …，(
Figure 948147DEST_PATH_IMAGE006
) In which
Figure 100002_DEST_PATH_IMAGE007
Which represents the coordinates of a rectangular coordinate system,
Figure 621574DEST_PATH_IMAGE008
representing a vector
Figure 100002_DEST_PATH_IMAGE009
The included angle between the X axis and the X axis in the rectangular coordinate system,
Figure 506353DEST_PATH_IMAGE010
represented as the curvature of the reference line at discrete points.
Further, the second step specifically includes the following sub-steps:
step 2.1: constructing a cost function for path planning
Figure 100002_DEST_PATH_IMAGE011
The path cost function comprises two parts, wherein the first part is similar cost of the variable to be optimized and the reference point of the original reference line
Figure 570124DEST_PATH_IMAGE012
The second part is the smoothing cost
Figure 100002_DEST_PATH_IMAGE013
As follows:
Figure 100002_DEST_PATH_IMAGE015
wherein the content of the first and second substances,
Figure 96920DEST_PATH_IMAGE016
for variables to be optimized: (
Figure 100002_DEST_PATH_IMAGE017
）、（
Figure 828116DEST_PATH_IMAGE018
），…，（
Figure 100002_DEST_PATH_IMAGE019
）；
Figure 618218DEST_PATH_IMAGE020
As the original reference line reference point (
Figure 100002_DEST_PATH_IMAGE021
）、（
Figure 434864DEST_PATH_IMAGE022
），…，（
Figure 100002_DEST_PATH_IMAGE023
）；
Figure 906296DEST_PATH_IMAGE024
The similar cost weight is in negative correlation with the wheel base L value of the vehicle and in positive correlation with the maximum steering capacity of the vehicle;
Figure 100002_DEST_PATH_IMAGE025
smoothing the cost weight, which is a fixed value weight;
the similar cost
Figure 226419DEST_PATH_IMAGE026
Setting the Euclidean distance between a variable to be optimized and an original reference point, wherein the expression is as follows:
Figure 249739DEST_PATH_IMAGE028
the smoothing cost
Figure 960206DEST_PATH_IMAGE013
Setting the degree of similarity of straight lines between three points, wherein the expression is as follows:
Figure 100002_DEST_PATH_IMAGE029
；
step 2.2: constructing safety constraint of path planning to obtain each variable to be optimized (
Figure 94384DEST_PATH_IMAGE030
) And (3) deviating from the transverse and longitudinal constraints of the original reference point, wherein the expression is as follows:
Figure 206697DEST_PATH_IMAGE032
Figure 135338DEST_PATH_IMAGE034
wherein, the first and the second end of the pipe are connected with each other,
Figure 100002_DEST_PATH_IMAGE035
、
Figure 926577DEST_PATH_IMAGE036
、
Figure 100002_DEST_PATH_IMAGE037
、
Figure 802129DEST_PATH_IMAGE038
to be arranged at
Figure 768948DEST_PATH_IMAGE016
、
Figure 100002_DEST_PATH_IMAGE039
Upper and lower bounds in direction;
step 2.3: constructing curvature constraint of path planning according to vehicle type parameters, wherein the curvature relation between discrete points meets the minimum turning radius constraint of a vehicle, and the expression is as follows:
Figure 399650DEST_PATH_IMAGE040
，
can be converted into:
Figure 100002_DEST_PATH_IMAGE041
wherein, the first and the second end of the pipe are connected with each other,
Figure 412605DEST_PATH_IMAGE042
is the distance between the two points and is,
Figure 100002_DEST_PATH_IMAGE043
setting the minimum turning radius as the constraint of the vehicle parameters;
step 2.4: according to the path cost function obtained in the step 2.1 and the constraint conditions obtained in the step 2.2 and the step 2.3, the final path optimization result is obtained by solving the nonlinear optimization path containing the constraint conditions
Figure 91848DEST_PATH_IMAGE044
I.e. the smoothed roadRadial point coordinate information;
step 2.5: using the path optimization results of step 2.4
Figure 913173DEST_PATH_IMAGE044
Calculating the orientation angle and curvature corresponding to the path point, and finally outputting a series of smoothed path point information (
Figure 100002_DEST_PATH_IMAGE045
)，（
Figure 714776DEST_PATH_IMAGE046
），…，(
Figure 100002_DEST_PATH_IMAGE047
) I.e. the output smooth path.
Further, the third step specifically includes the following substeps:
step 3.1: constructing a cost function and a constraint of a speed plan, wherein the speed cost function comprises three parts: the first part is the similar cost of the speed variable to be optimized and the expected speed of the current road section
Figure 215028DEST_PATH_IMAGE048
The second part is the longitudinal acceleration cost of the variable to be optimized
Figure 100002_DEST_PATH_IMAGE049
The third part is the longitudinal acceleration cost of the variable to be optimized
Figure 432382DEST_PATH_IMAGE050
The specific expression is as follows:
Figure 373794DEST_PATH_IMAGE052
Figure 752822DEST_PATH_IMAGE054
Figure 271528DEST_PATH_IMAGE056
Figure 100002_DEST_PATH_IMAGE057
，
wherein the content of the first and second substances,
Figure 292574DEST_PATH_IMAGE058
for variables to be optimized
Figure 100002_DEST_PATH_IMAGE059
，
Figure 416388DEST_PATH_IMAGE060
For the desired speed value in the planned route section
Figure 100002_DEST_PATH_IMAGE061
，
Figure 435159DEST_PATH_IMAGE062
For a cost weight similar to the desired speed,
Figure 100002_DEST_PATH_IMAGE063
in order to be the acceleration cost weight,
Figure 910003DEST_PATH_IMAGE064
in order to be the acceleration cost weight,
Figure 100002_DEST_PATH_IMAGE065
is a preset fixed sampling time length;
step 3.2: constructing a first comfort constraint of the speed plan, wherein the expression is as follows:
Figure 100002_DEST_PATH_IMAGE067
Figure 100002_DEST_PATH_IMAGE069
wherein the content of the first and second substances,
Figure 797056DEST_PATH_IMAGE070
in order to ensure the maximum speed limit for comfort,
Figure 100002_DEST_PATH_IMAGE071
in order to meet the maximum value of centripetal acceleration for human comfort,
Figure 837694DEST_PATH_IMAGE072
a curvature corresponding to each path point obtained in step 2.5;
step 3.3: constructing a second comfort constraint for the speed plan, the expression being:
Figure 496208DEST_PATH_IMAGE074
wherein, the first and the second end of the pipe are connected with each other,
Figure 100002_DEST_PATH_IMAGE075
，
Figure 192769DEST_PATH_IMAGE076
a maximum longitudinal acceleration value to meet human comfort;
step 3.4: constructing a continuity constraint of the speed plan, wherein the expression is as follows:
Figure 352355DEST_PATH_IMAGE078
step 3.5: obtaining a final speed optimization result by solving the optimization speed containing the constraint condition according to the speed cost function obtained in the step 3.1 and the constraint condition obtained in the step 3.2 to the step 3.4
Figure 100002_DEST_PATH_IMAGE079
I.e. smoothed speed information.
Further, the fourth step is specifically: according to the steps3.5 speed optimization results
Figure 388444DEST_PATH_IMAGE079
Combining the smoothed path points with the smoothed path points output in step 2.5 to obtain final planning track points (a) meeting comfort constraint
Figure 342493DEST_PATH_IMAGE080
)，（
Figure 100002_DEST_PATH_IMAGE081
），…，(
Figure 791929DEST_PATH_IMAGE082
)。
A track planning system considering comfort constraint comprises a path smoothing planning module and a speed planning module, wherein the path smoothing planning module inputs set reference line information, vehicle global positioning information and vehicle parameters, then applies curvature constraint of the maximum steering capacity of a vehicle to a generated path, and outputs a smooth path meeting the constraint of the minimum turning radius of the vehicle; and the speed planning module calculates the maximum speed limit meeting the riding comfort aiming at the curvature value of the smoothed path point, takes the speed limit as the constraint condition of speed optimization, thereby solving the value of the smoothed speed, finally fuses the smoothed speed and the path and outputs a series of track points with speed information.
A trajectory planning device considering comfort constraints comprises one or more processors and is used for realizing the trajectory planning method considering the comfort constraints.
A computer-readable storage medium, having stored thereon a program which, when executed by a processor, implements a trajectory planning method as described which takes into account comfort constraints.
Has the advantages that:
the invention applies curvature constraint of the maximum steering capacity of the vehicle to the planned path, so that the smoothed path can meet the control execution capacity of different vehicles; and then speed planning is carried out, the maximum speed limit meeting the riding comfort is calculated according to the curvature value of the path point after the last step of smoothing, and the speed limit is put into a constraint of a speed optimization problem, so that the solved speed value can meet the riding comfort experience.
Drawings
FIG. 1 is a general flow diagram of a method for trajectory planning in consideration of comfort constraints according to the present invention;
FIG. 2 is a detailed flow chart of the trajectory planning method of the present invention that considers comfort constraints;
fig. 3 is a schematic structural diagram of a trajectory planning device considering comfort constraints according to an embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and technical effects of the present invention more apparent, the present invention is further described in detail below with reference to the accompanying drawings and embodiments of the specification.
As shown in fig. 1 and fig. 2, a trajectory planning method considering comfort constraints of the present invention includes the following steps:
step one, calculating a projection point coordinate projected to a reference line from a vehicle according to the positioned global position of the vehicle and the information of the reference line, and sampling the projection point coordinate to a fixed length at equal intervals to obtain a plurality of discrete original reference points, wherein the method specifically comprises the following substeps:
step 1.1: according to the global position of the vehicle given by the positioning module of the vehicle
Figure 364993DEST_PATH_IMAGE001
And determining the given target lane information, and calculating the projected reference point coordinates of the vehicle on a given reference line (such as the original road center line serving as the reference line)
Figure 849064DEST_PATH_IMAGE002
) The coordinate of the closest point of the vehicle to the reference line;
step 1.2: for the input reference line information, from the projection point coordinates of the matched reference line
Figure 114960DEST_PATH_IMAGE002
) To begin with
Figure 317271DEST_PATH_IMAGE003
For sampling at equal intervals to a fixed length, the present embodiment takes 0.25m as the sampling interval to sample corresponding reference lines at equal intervals to 50m, and includes n discrete reference points(s) ((
Figure 756343DEST_PATH_IMAGE004
）、(
Figure 235866DEST_PATH_IMAGE005
), …，(
Figure 797297DEST_PATH_IMAGE006
) Wherein
Figure 96691DEST_PATH_IMAGE007
Which represents the coordinates of a rectangular coordinate system,
Figure 401771DEST_PATH_IMAGE008
representing a vector
Figure 735800DEST_PATH_IMAGE009
The included angle between the X axis and the X axis in the rectangular coordinate system,
Figure 468133DEST_PATH_IMAGE010
represented as the curvature of the reference line at discrete points.
Step two, solving a path planning optimization problem by constructing and calculating a cost function and a constraint of vehicle driving path planning to obtain a path optimization result, namely optimizing a planned path, and calculating an orientation angle and a curvature corresponding to the path planning according to the path optimization result to obtain a smoothed path, wherein the method specifically comprises the following substeps:
step 2.1: constructing a cost function of path planning, wherein the path cost function comprises two parts, and the first part is similar cost of a variable to be optimized and an original reference line reference point
Figure 520402DEST_PATH_IMAGE012
The second part is the smoothing cost
Figure 35697DEST_PATH_IMAGE013
As follows:
Figure 614446DEST_PATH_IMAGE015
wherein the content of the first and second substances,
Figure 127467DEST_PATH_IMAGE016
for variables to be optimized: (
Figure 791667DEST_PATH_IMAGE017
）、（
Figure 313915DEST_PATH_IMAGE018
），…，（
Figure 747170DEST_PATH_IMAGE019
）；
Figure 696672DEST_PATH_IMAGE020
As the original reference line reference point (
Figure 582588DEST_PATH_IMAGE021
）、（
Figure 705265DEST_PATH_IMAGE022
），…，（
Figure 868393DEST_PATH_IMAGE023
）；
Figure 113430DEST_PATH_IMAGE024
The weight is similar to the cost weight, and can be designed to be in negative correlation with the wheel base L value of the vehicle and in positive correlation with the maximum steering capacity of the vehicle;
Figure 362008DEST_PATH_IMAGE025
a smooth cost weight, which can be set to be fixedA fixed value weight;
the similar cost
Figure 350693DEST_PATH_IMAGE026
The Euclidean distance between the variable to be optimized and the original reference point can be set, and the expression is as follows:
Figure 368328DEST_PATH_IMAGE028
the smoothing cost
Figure 49845DEST_PATH_IMAGE013
Can be set as the degree of similarity of straight lines between three points, and the expression is as follows:
Figure 785719DEST_PATH_IMAGE029
；
step 2.2: constructing safety constraint of path planning to obtain each variable to be optimized (
Figure 250199DEST_PATH_IMAGE030
) And (3) deviating from the transverse and longitudinal constraints of the original reference point, wherein the expression is as follows:
Figure 246974DEST_PATH_IMAGE032
Figure 974758DEST_PATH_IMAGE034
wherein, the first and the second end of the pipe are connected with each other,
Figure 322563DEST_PATH_IMAGE035
、
Figure 262837DEST_PATH_IMAGE036
、
Figure 379698DEST_PATH_IMAGE037
、
Figure 278384DEST_PATH_IMAGE038
is arranged at
Figure 520009DEST_PATH_IMAGE016
、
Figure 654187DEST_PATH_IMAGE039
The upper and lower bounds in the direction are constrained to ensure that the constrained optimization variable is in a safe range;
step 2.3: constructing curvature constraint of path planning according to vehicle type parameters, wherein the curvature relation between discrete points meets the minimum turning radius constraint of a vehicle, and the expression is as follows:
Figure 766500DEST_PATH_IMAGE040
，
can be converted into:
Figure 695141DEST_PATH_IMAGE041
wherein, the first and the second end of the pipe are connected with each other,
Figure 627325DEST_PATH_IMAGE042
is the distance between the two points and is,
Figure 299615DEST_PATH_IMAGE043
the minimum turning radius is restricted, and the minimum turning radius is set by the parameters of the vehicle;
step 2.4: according to the cost function obtained in the step 2.1 and the constraint conditions obtained in the step 2.2 and the step 2.3, the nonlinear optimization path containing the constraint is solved to obtain a final path optimization result
Figure 266434DEST_PATH_IMAGE044
The coordinate information of the smoothed path point is obtained;
step 2.5: using the path optimization results of step 2.4
Figure 365977DEST_PATH_IMAGE044
Calculating the orientation angle and curvature corresponding to the path point, and finally outputting a series of smoothed path point information (
Figure 847774DEST_PATH_IMAGE045
)，（
Figure 199121DEST_PATH_IMAGE046
），…，(
Figure 145080DEST_PATH_IMAGE047
) I.e. the output smooth path.
Step three, solving a speed planning optimization problem by constructing and calculating a cost function and constraint of the vehicle running speed planning to obtain a speed optimization result, namely optimizing the planned speed, and calculating an orientation angle and a curvature corresponding to the speed planning according to the speed optimization result to obtain a smoothed speed, wherein the method specifically comprises the following substeps:
step 3.1: constructing a cost function and a constraint of a speed plan, wherein the speed cost function comprises three parts: the first part is the similar cost of the speed variable to be optimized and the expected speed of the current road section
Figure 290891DEST_PATH_IMAGE048
The second part is the longitudinal acceleration cost of the variable to be optimized
Figure 322301DEST_PATH_IMAGE049
The third part is the longitudinal acceleration cost of the variable to be optimized
Figure 211759DEST_PATH_IMAGE050
The specific expression is as follows:
Figure 277804DEST_PATH_IMAGE052
Figure DEST_PATH_IMAGE083
Figure 860095DEST_PATH_IMAGE084
Figure 378801DEST_PATH_IMAGE057
，
wherein the content of the first and second substances,
Figure 806372DEST_PATH_IMAGE058
for variables to be optimized
Figure 562861DEST_PATH_IMAGE059
，
Figure 316053DEST_PATH_IMAGE060
For the desired speed value in the planned route section
Figure 322055DEST_PATH_IMAGE061
，
Figure 350054DEST_PATH_IMAGE062
For a cost weight similar to the desired speed,
Figure 478DEST_PATH_IMAGE063
in order to be the acceleration cost weight,
Figure 49206DEST_PATH_IMAGE064
in order to be the acceleration cost weight,
Figure 152291DEST_PATH_IMAGE065
is a preset fixed sampling time length;
step 3.2: constructing a first comfort constraint of the speed plan, wherein the expression is as follows:
Figure DEST_PATH_IMAGE085
Figure 780718DEST_PATH_IMAGE086
wherein, the first and the second end of the pipe are connected with each other,
Figure 410283DEST_PATH_IMAGE070
in order to ensure the maximum speed limit for comfort,
Figure 302015DEST_PATH_IMAGE071
in order to meet the maximum value of centripetal acceleration for human comfort,
Figure 157976DEST_PATH_IMAGE072
a curvature corresponding to each path point obtained in step 2.5;
step 3.3: constructing a second comfort constraint for the speed plan, the expression being:
Figure 855674DEST_PATH_IMAGE074
wherein the content of the first and second substances,
Figure 480690DEST_PATH_IMAGE075
，
Figure 605641DEST_PATH_IMAGE076
a maximum longitudinal acceleration value to meet human comfort;
step 3.4: constructing a continuity constraint of the speed plan, wherein the expression is as follows:
Figure 683318DEST_PATH_IMAGE078
step 3.5: obtaining a final speed optimization result by solving the optimized speed containing the constraint according to the speed cost function obtained in the step 3.1 and the constraint conditions obtained in the steps 3.2 to 3.4
Figure 450286DEST_PATH_IMAGE079
I.e. smoothed speed information.
And step four, finally, fusing the smoothed speed and the smoothed path to obtain a smooth track with speed information.
Specifically, the speed optimization result obtained according to step 3.5
Figure 929809DEST_PATH_IMAGE079
Combining the path points with the smoothed path points output in step 2.5 to obtain final planning trajectory points (b) meeting comfort constraints
Figure 163344DEST_PATH_IMAGE080
)，（
Figure 852951DEST_PATH_IMAGE081
），…，(
Figure 767818DEST_PATH_IMAGE082
)。
A trajectory planning system taking comfort constraints into consideration by adopting the method comprises the following steps: a path smooth planning module and a speed planning module. The path smooth planning module firstly inputs set reference line information, vehicle global positioning information and vehicle parameters, and then applies curvature constraint of the maximum steering capacity of the vehicle to the generated path, thereby outputting a smooth path meeting the constraint of the minimum turning radius of the vehicle and enabling the smooth path to meet the control execution capacities of different vehicles; the speed planning module calculates the maximum speed limit meeting the riding comfort aiming at the curvature value of the path point after smoothing, and puts the speed limit into the constraint of the speed optimization problem, thereby ensuring that the solved smooth speed value can meet the riding comfort experience, finally fusing the smoothed speed with the path, and outputting a series of track points with speed information.
Corresponding to the embodiment of the track planning method considering the comfort constraint, the invention also provides an embodiment of a track planning device considering the comfort constraint.
Referring to fig. 3, a trajectory planning apparatus considering comfort constraints according to an embodiment of the present invention includes one or more processors, and is configured to implement a trajectory planning method considering comfort constraints in the foregoing embodiment.
The embodiment of the trajectory planning device considering the comfort constraint can be applied to any equipment with data processing capability, such as computers and other equipment or devices. The device embodiments may be implemented by software, or by hardware, or by a combination of hardware and software. The software implementation is taken as an example, and as a logical device, the device is formed by reading corresponding computer program instructions in the nonvolatile memory into the memory for running through the processor of any device with data processing capability. In terms of hardware, as shown in fig. 3, a hardware structure diagram of any device with data processing capability where the trajectory planning device considering comfort constraints is located according to the present invention is shown, where in addition to the processor, the memory, the network interface, and the nonvolatile memory shown in fig. 3, any device with data processing capability where the device is located in the embodiment may also include other hardware according to the actual function of the any device with data processing capability, which is not described again.
The implementation process of the functions and actions of each unit in the above device is specifically described in the implementation process of the corresponding step in the above method, and is not described herein again.
For the device embodiments, since they substantially correspond to the method embodiments, reference may be made to the partial description of the method embodiments for relevant points. The above-described embodiments of the apparatus are merely illustrative, and the units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the solution of the present invention. One of ordinary skill in the art can understand and implement it without inventive effort.
An embodiment of the present invention further provides a computer-readable storage medium, on which a program is stored, and when the program is executed by a processor, the method for trajectory planning considering comfort constraints in the foregoing embodiments is implemented.
The computer readable storage medium may be an internal storage unit, such as a hard disk or a memory, of any data processing capability device described in any of the foregoing embodiments. The computer readable storage medium may also be an external storage device such as a plug-in hard disk, a Smart Media Card (SMC), an SD Card, a Flash memory Card (Flash Card), etc. provided on the device. Further, the computer readable storage medium may include both internal storage units and external storage devices of any data processing capable device. The computer-readable storage medium is used for storing the computer program and other programs and data required by the arbitrary data processing-capable device, and may also be used for temporarily storing data that has been output or is to be output.
The above description is only a preferred embodiment of the present invention, and is not intended to limit the present invention in any way. Although the foregoing has described the practice of the present invention in detail, it will be apparent to those skilled in the art that modifications may be made to the practice of the invention as described in the foregoing examples, or that certain features may be substituted in the practice of the invention. All changes, equivalents and the like which come within the spirit and principles of the invention are desired to be protected.

Claims (8)

1. A trajectory planning method taking into account comfort constraints, characterized by comprising the steps of:
calculating a projection point coordinate projected to a reference line by the self-vehicle according to the positioned self-vehicle global position and the information of the reference line, and sampling the projection point coordinate to a fixed length at equal intervals to obtain a plurality of discrete original reference points;
step two, optimizing the path plan by constructing and calculating a cost function and constraint of the vehicle driving path plan to obtain a path optimization result, and calculating an orientation angle and a curvature corresponding to the path plan according to the path optimization result to obtain a smoothed path;
thirdly, optimizing the planned speed by constructing and calculating a cost function and constraint of the vehicle running speed plan to obtain a speed optimization result, and calculating an orientation angle and curvature corresponding to the speed plan to obtain a smoothed speed;
and step four, fusing the smoothed speed and the smoothed path to obtain a smooth track with speed information.
2. A trajectory planning method taking into account comfort constraints, as claimed in claim 1, characterized in that said first step specifically comprises the following substeps:
step 1.1: according to the global position of the vehicle given by the positioning module of the vehicle
Figure DEST_PATH_IMAGE001
And determining given target lane information, and calculating the coordinate of the reference point of the vehicle projected by taking the central line of the original road as the reference line
Figure 775980DEST_PATH_IMAGE002
) The coordinate of the closest point of the vehicle to the reference line;
step 1.2: for the input reference line information, from the projection point coordinates of the matched reference line
Figure 383679DEST_PATH_IMAGE002
) To begin with
Figure DEST_PATH_IMAGE003
For sampling at intervals equidistant to a fixed length, n discrete reference points are included (
Figure 29424DEST_PATH_IMAGE004
）、(
Figure DEST_PATH_IMAGE005
), …，(
Figure 934932DEST_PATH_IMAGE006
) Wherein
Figure DEST_PATH_IMAGE007
Which represents the coordinates of a rectangular coordinate system,
Figure 982523DEST_PATH_IMAGE008
representing a vector
Figure DEST_PATH_IMAGE009
The included angle between the X axis and the X axis in the rectangular coordinate system,
Figure 354598DEST_PATH_IMAGE010
represented as the curvature of the reference line at discrete points.
3. A trajectory planning method considering comfort constraints according to claim 2, wherein the second step specifically comprises the following substeps:
step 2.1: constructing a cost function for path planning
Figure DEST_PATH_IMAGE011
The path cost function comprises two parts, wherein the first part is similar cost of the variable to be optimized and the reference point of the original reference line
Figure 159743DEST_PATH_IMAGE012
The second part is the smoothing cost
Figure DEST_PATH_IMAGE013
As follows:
Figure DEST_PATH_IMAGE015
wherein the content of the first and second substances,
Figure 603363DEST_PATH_IMAGE016
for variables to be optimized: (
Figure DEST_PATH_IMAGE017
）、（
Figure 505460DEST_PATH_IMAGE018
），…，（
Figure DEST_PATH_IMAGE019
）；
Figure 48437DEST_PATH_IMAGE020
Is an original reference line reference point (
Figure DEST_PATH_IMAGE021
）、（
Figure 75298DEST_PATH_IMAGE022
），…，（
Figure DEST_PATH_IMAGE023
）；
Figure 729134DEST_PATH_IMAGE024
The weight is similar to the cost weight, and is in negative correlation with the vehicle wheel base L value and in positive correlation with the maximum steering capacity of the vehicle;
Figure DEST_PATH_IMAGE025
smoothing the cost weight, which is a fixed value weight;
the similar cost
Figure 485737DEST_PATH_IMAGE026
Setting the Euclidean distance between the variable to be optimized and the original reference point, wherein the expression is as follows:
Figure 730774DEST_PATH_IMAGE028
the smoothing cost
Figure 41669DEST_PATH_IMAGE013
Setting the degree of similarity of straight lines between three points, wherein the expression is as follows:
Figure DEST_PATH_IMAGE029
；
step 2.2: constructing safety constraint of path planning to obtain each variable to be optimized (
Figure 499195DEST_PATH_IMAGE030
) And (3) deviating from the transverse and longitudinal constraints of the original reference point, wherein the expression is as follows:
Figure 516830DEST_PATH_IMAGE032
Figure 198347DEST_PATH_IMAGE034
wherein, the first and the second end of the pipe are connected with each other,
Figure DEST_PATH_IMAGE035
、
Figure 262118DEST_PATH_IMAGE036
、
Figure DEST_PATH_IMAGE037
、
Figure 257756DEST_PATH_IMAGE038
to be arranged at
Figure 395476DEST_PATH_IMAGE016
、
Figure DEST_PATH_IMAGE039
Upper and lower bounds in direction;
step 2.3: constructing curvature constraint of path planning according to vehicle type parameters, wherein the curvature relation between discrete points meets the minimum turning radius constraint of a vehicle, and the expression is as follows:
Figure 513474DEST_PATH_IMAGE040
，
can be converted into:
Figure DEST_PATH_IMAGE041
wherein the content of the first and second substances,
Figure 798961DEST_PATH_IMAGE042
is the distance between the two points and is,
Figure DEST_PATH_IMAGE043
setting the minimum turning radius as the constraint of the vehicle parameters;
step 2.4: according to the path cost function obtained in the step 2.1 and the constraint conditions obtained in the step 2.2 and the step 2.3, the final path optimization result is obtained by solving the nonlinear optimization path containing the constraint conditions
Figure 863869DEST_PATH_IMAGE044
The coordinate information of the smoothed path point is obtained;
step 2.5: using the path optimization results of step 2.4
Figure 652834DEST_PATH_IMAGE044
Calculating the orientation angle and curvature corresponding to the path point, and finally outputting a series of smoothed path point information (
Figure DEST_PATH_IMAGE045
)，（
Figure 879416DEST_PATH_IMAGE046
），…，(
Figure DEST_PATH_IMAGE047
) I.e. output smooth path.
4. A trajectory planning method taking into account comfort constraints, as claimed in claim 3, characterized in that said step three specifically comprises the following substeps:
step 3.1: constructing a cost function and a constraint of a speed plan, wherein the speed cost function comprises three parts: the first part is the similar cost of the speed variable to be optimized and the expected speed of the road section
Figure 917779DEST_PATH_IMAGE048
The second part is the longitudinal acceleration cost of the variable to be optimized
Figure DEST_PATH_IMAGE049
The third part is the longitudinal acceleration cost of the variable to be optimized
Figure 520799DEST_PATH_IMAGE050
The specific expression is as follows:
Figure 164270DEST_PATH_IMAGE052
Figure 233857DEST_PATH_IMAGE054
Figure 290675DEST_PATH_IMAGE056
Figure DEST_PATH_IMAGE057
，
wherein the content of the first and second substances,
Figure 431806DEST_PATH_IMAGE058
for variables to be optimized
Figure DEST_PATH_IMAGE059
，
Figure 992100DEST_PATH_IMAGE060
For the desired speed value in the planned route section
Figure DEST_PATH_IMAGE061
，
Figure 232589DEST_PATH_IMAGE062
For a cost weight similar to the desired speed,
Figure DEST_PATH_IMAGE063
in order to be the acceleration cost weight,
Figure 245544DEST_PATH_IMAGE064
in order to be the acceleration cost weight,
Figure DEST_PATH_IMAGE065
is a preset fixed sampling time length;
step 3.2: constructing a first comfort constraint of the speed plan, wherein the expression is as follows:
Figure DEST_PATH_IMAGE067
Figure DEST_PATH_IMAGE069
wherein the content of the first and second substances,
Figure 49421DEST_PATH_IMAGE070
in order to ensure the maximum speed limit for comfort,
Figure DEST_PATH_IMAGE071
in order to meet the maximum value of centripetal acceleration for human comfort,
Figure 464222DEST_PATH_IMAGE072
curvature corresponding to each path point obtained in step 2.5;
step 3.3: constructing a second comfort constraint for the speed plan, the expression being:
Figure 610032DEST_PATH_IMAGE074
wherein the content of the first and second substances,
Figure DEST_PATH_IMAGE075
，
Figure 844704DEST_PATH_IMAGE076
maximum longitudinal acceleration value to meet human comfort;
step 3.4: constructing a continuity constraint of the speed plan, wherein the expression is as follows:
Figure 124376DEST_PATH_IMAGE078
step 3.5: obtaining a final speed optimization result by solving the optimization speed containing the constraint condition according to the speed cost function obtained in the step 3.1 and the constraint condition obtained in the step 3.2 to the step 3.4
Figure DEST_PATH_IMAGE079
I.e. smoothed speed information.
5. Consideration as set forth in claim 4The comfort-constrained trajectory planning method is characterized in that the fourth step specifically comprises the following steps: speed optimization result obtained according to step 3.5
Figure 65787DEST_PATH_IMAGE079
Combining the path points with the smoothed path points output in the step 2.5 to obtain final planning track points (A)
Figure 772712DEST_PATH_IMAGE080
)，（
Figure DEST_PATH_IMAGE081
），…，(
Figure 760260DEST_PATH_IMAGE082
)。
6. A trajectory planning system adopting the trajectory planning method considering the comfort constraint according to any one of claims 1 to 5, comprising a path smoothing planning module and a speed planning module, wherein the path smoothing planning module inputs the set reference line information, the vehicle global positioning information and the vehicle parameters, then applies the curvature constraint of the maximum steering capacity of the vehicle to the generated path, and outputs a smooth path meeting the minimum turning radius constraint of the vehicle; and the speed planning module calculates the maximum speed limit according to the curvature value of the smoothed path point, takes the speed limit as a constraint condition of speed optimization, thereby solving the value of the smoothed speed, and finally fuses the smoothed speed and the path to output a series of track points with speed information.
7. A trajectory planning device considering comfort constraints, which is characterized by comprising one or more processors and is used for realizing the trajectory planning method considering comfort constraints, wherein the trajectory planning method considering comfort constraints is claimed in any one of claims 1 to 5.
8. A computer-readable storage medium, having stored thereon a program which, when executed by a processor, implements a trajectory planning method taking into account comfort constraints as claimed in any one of claims 1 to 5.
CN202211243619.6A 2022-10-12 2022-10-12 Track planning method, device and system considering comfort constraint Active CN115309170B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211243619.6A CN115309170B (en) 2022-10-12 2022-10-12 Track planning method, device and system considering comfort constraint

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211243619.6A CN115309170B (en) 2022-10-12 2022-10-12 Track planning method, device and system considering comfort constraint

Publications (2)

Publication Number Publication Date
CN115309170A true CN115309170A (en) 2022-11-08
CN115309170B CN115309170B (en) 2023-03-24

Family

ID=83868178

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211243619.6A Active CN115309170B (en) 2022-10-12 2022-10-12 Track planning method, device and system considering comfort constraint

Country Status (1)

Country Link
CN (1) CN115309170B (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116166061A (en) * 2023-04-26 2023-05-26 中国农业大学 Unmanned speed control method and device, unmanned plane and electronic equipment
CN117406756A (en) * 2023-12-06 2024-01-16 苏州元脑智能科技有限公司 Method, device, equipment and storage medium for determining motion trail parameters

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111413966A (en) * 2020-03-12 2020-07-14 天津大学 Progressive model prediction unmanned planning tracking cooperative control method
CN111806467A (en) * 2020-07-27 2020-10-23 福州大学 Variable speed dynamic track changing planning method based on vehicle driving rule
CN112389427A (en) * 2021-01-19 2021-02-23 腾讯科技（深圳）有限公司 Vehicle track optimization method and device, electronic equipment and storage medium
CN112677995A (en) * 2021-01-07 2021-04-20 腾讯科技（深圳）有限公司 Vehicle track planning method and device, storage medium and equipment
US20210200231A1 (en) * 2019-12-30 2021-07-01 Baidu Usa Llc Speed planning guidance line for mild slow down
CN114312848A (en) * 2022-01-17 2022-04-12 吉林大学 Intelligent driving automobile track planning and tracking control method based on double-layer MPC
CN114676939A (en) * 2022-05-26 2022-06-28 之江实验室 Multi-vehicle-type parameter self-adaptive reference line smoothing method and system
CN115079699A (en) * 2022-06-30 2022-09-20 北京理工大学 Model prediction control-based motion planning method for man-machine co-driving automobile
WO2022193584A1 (en) * 2021-03-15 2022-09-22 西安交通大学 Multi-scenario-oriented autonomous driving planning method and system

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20210200231A1 (en) * 2019-12-30 2021-07-01 Baidu Usa Llc Speed planning guidance line for mild slow down
CN111413966A (en) * 2020-03-12 2020-07-14 天津大学 Progressive model prediction unmanned planning tracking cooperative control method
CN111806467A (en) * 2020-07-27 2020-10-23 福州大学 Variable speed dynamic track changing planning method based on vehicle driving rule
CN112677995A (en) * 2021-01-07 2021-04-20 腾讯科技（深圳）有限公司 Vehicle track planning method and device, storage medium and equipment
WO2022148282A1 (en) * 2021-01-07 2022-07-14 腾讯科技（深圳）有限公司 Method and apparatus for planning trajectory of vehicle, storage medium, and device
CN112389427A (en) * 2021-01-19 2021-02-23 腾讯科技（深圳）有限公司 Vehicle track optimization method and device, electronic equipment and storage medium
WO2022193584A1 (en) * 2021-03-15 2022-09-22 西安交通大学 Multi-scenario-oriented autonomous driving planning method and system
CN114312848A (en) * 2022-01-17 2022-04-12 吉林大学 Intelligent driving automobile track planning and tracking control method based on double-layer MPC
CN114676939A (en) * 2022-05-26 2022-06-28 之江实验室 Multi-vehicle-type parameter self-adaptive reference line smoothing method and system
CN115079699A (en) * 2022-06-30 2022-09-20 北京理工大学 Model prediction control-based motion planning method for man-machine co-driving automobile

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
张培志等: "非结构化道路环境下的无人车运动规划算法研究", 《上海汽车》 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116166061A (en) * 2023-04-26 2023-05-26 中国农业大学 Unmanned speed control method and device, unmanned plane and electronic equipment
CN117406756A (en) * 2023-12-06 2024-01-16 苏州元脑智能科技有限公司 Method, device, equipment and storage medium for determining motion trail parameters
CN117406756B (en) * 2023-12-06 2024-03-08 苏州元脑智能科技有限公司 Method, device, equipment and storage medium for determining motion trail parameters

Also Published As

Publication number Publication date
CN115309170B (en) 2023-03-24

Similar Documents

Publication Publication Date Title
CN115309170B (en) Track planning method, device and system considering comfort constraint
JP6667686B2 (en) Travel trajectory generation method and system for self-driving vehicle and machine-readable medium
US20210373161A1 (en) Lidar localization using 3d cnn network for solution inference in autonomous driving vehicles
US20210365712A1 (en) Deep learning-based feature extraction for lidar localization of autonomous driving vehicles
WO2022142849A1 (en) Generation method and apparatus and reminding method and apparatus of navigation prompt information, and device
Ye et al. Monocular 3d object detection via feature domain adaptation
US20210354718A1 (en) Lidar localization using rnn and lstm for temporal smoothness in autonomous driving vehicles
JP2020001678A (en) Reference line smoothing method using piecewise spiral curves with weighted geometry costs
CN114676939B (en) Multi-vehicle-type parameter self-adaptive reference line smoothing method and system
CN113033925B (en) Apparatus, electronic device, and medium for controlling travel of autonomous vehicle
CN115339453B (en) Vehicle lane change decision information generation method, device, equipment and computer medium
KR20230018352A (en) Method and apparatus for determining a driving route of a vehicle
CN114194217A (en) Vehicle automatic driving method, device, electronic equipment and storage medium
CN114170826A (en) Automatic driving control method and device, electronic device and storage medium
CN113552888A (en) Driving track control method, device, equipment and medium applied to unmanned vehicle
CN110962858A (en) Target identification method and device
CN112009460A (en) Vehicle control method, device, equipment and storage medium
CN110850878A (en) Intelligent vehicle control method, device, equipment and medium
CN115542899A (en) Method and device for tracking vehicle path, vehicle, electronic equipment and medium
CN115489544A (en) Intersection turning path reference line generation method, equipment, vehicle and storage medium
CN115657675A (en) Vehicle motion path generation method and system and storage medium
CN115014380A (en) Parking path planning method and device, electronic device and storage medium
Salzmann et al. Online Path Generation from Sensor Data for Highly Automated Driving Functions
CN113432618A (en) Trajectory generation method and apparatus, computing device and computer-readable storage medium
CN114037977A (en) Road vanishing point detection method, device, equipment and storage medium

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