CN115730756A - Staged automatic driving vehicle track planning method - Google Patents

Staged automatic driving vehicle track planning method Download PDF

Info

Publication number
CN115730756A
CN115730756A CN202211523129.1A CN202211523129A CN115730756A CN 115730756 A CN115730756 A CN 115730756A CN 202211523129 A CN202211523129 A CN 202211523129A CN 115730756 A CN115730756 A CN 115730756A
Authority
CN
China
Prior art keywords
coordinate system
cost
vehicle
sampling
planning method
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.)
Pending
Application number
CN202211523129.1A
Other languages
Chinese (zh)
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.)
Jilin University
Original Assignee
Jilin University
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 Jilin University filed Critical Jilin University
Priority to CN202211523129.1A priority Critical patent/CN115730756A/en
Publication of CN115730756A publication Critical patent/CN115730756A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • YGENERAL 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
    • Y02TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02TCLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00Road transport of goods or passengers
    • Y02T10/10Internal combustion engine [ICE] based vehicles
    • Y02T10/40Engine management systems

Abstract

The invention relates to the technical field of automatic driving, and particularly discloses a staged automatic driving vehicle trajectory planning method. In the searching stage, sampling under a Frenet coordinate system is carried out, a fifth-order polynomial is used for connecting sampling points, a reference track is obtained through dynamic planning and searching, and a travelable channel is constructed; in the optimization stage, a vehicle kinematics model is established, a cost function and boundary constraint are constructed, space-time linkage optimization is carried out by using an ILQR algorithm, and an optimal track is output. The advantages of the two methods based on sampling and optimization are combined, the advantages of a Frenet coordinate system and the advantages of a Cartesian coordinate system are taken into consideration, the problems that calculation is complex and real-time performance is difficult to meet in space-time joint trajectory planning are solved, the requirements of a dynamic scene are easily met, the problems that decoupling methods are prone to fall into local optimal solutions or even do not have solutions are solved, and the problem that a vehicle kinematics model cannot be established in the Frenet coordinate system is also solved.

Description

Staged automatic driving vehicle track planning method
Technical Field
The invention belongs to the technical field of automatic driving, and particularly relates to a staged automatic driving vehicle trajectory planning method.
Background
The planning module is a key module for automatic driving, and needs to comprehensively consider perception, prediction, map information and vehicle states and output a safe and comfortable track. At present, there are many research works on trajectory planning of an autonomous vehicle, and different classification methods are available according to different emphasis points.
According to different track generation modes, track planning can be divided into a sampling-based method and an optimization-based method, the sampling-based method generates a large number of candidate tracks by sampling a state space, and then an optimal track is selected according to a pre-designed evaluation index. The sampling-based method is simple to implement and high in real-time performance, but the planning result is easily influenced by the sampling interval, the generated track is too rough if the sampling interval is too large, and the calculated amount is remarkably increased if the sampling interval is too small, so that the real-time performance requirement is not favorably met. Corresponding to this is an optimization-based method that obtains trajectories that satisfy constraints by building a minimum cost problem with the constraints and solving the problem. The optimization-based method is not limited by a sampling mode, the cost function and the constraint condition can be adjusted according to different scenes, and the method has higher flexibility and is more suitable for coping with complex scenes. However, the optimization-based method often needs to solve a complex nonlinear programming problem, and needs more computing resources to obtain an optimal solution, so that when the scale of the problem is large, it is difficult to ensure that the requirement of real-time performance is met.
Trajectory planning can be divided into methods based on the Frenet coordinate system and methods based on the cartesian coordinate system, depending on the coordinate system. The Frenet coordinate system is a curved coordinate system, and can well simplify the curvature of the road, so that the expression is more brief and visual. Since Frenet ignores the curvature of the road, the kinematic model of the vehicle cannot be accurately described in a scene with a large curvature. Although the kinematic model of the vehicle can be accurately described in the cartesian coordinate system to obtain a track more conforming to the vehicle and the kinematic model, the description of the lane boundary and the obstacle in the cartesian coordinate system is complex and not concise, so that the constraint solution of the planning is difficult, and the calculation amount is significantly increased.
Disclosure of Invention
The embodiment of the invention aims to provide a staged automatic driving vehicle track planning method, aiming at solving the problems in the background technology.
In order to achieve the above purpose, the embodiments of the present invention provide the following technical solutions:
a staged automatic driving vehicle trajectory planning method specifically comprises the following steps:
in the searching stage, sampling is carried out under a Frenet coordinate system, a quintic polynomial is used for connecting sampling points to form a directed acyclic graph, a reference track is obtained through dynamic planning and searching, and a travelable channel is constructed;
and in the optimization stage, a vehicle kinematics model is established, a cost function is established according to the travelable channel, boundary constraint is established, space-time linkage optimization is performed by using an ILQR algorithm, and an optimal track is output.
As a further limitation of the technical solution of the embodiment of the present invention, in the search stage, sampling is performed in a Frenet coordinate system, a quintic polynomial is used to connect sampling points to form a directed acyclic graph, a reference trajectory is obtained by dynamic planning and searching, and the construction of a travelable channel specifically includes the following steps:
obtaining map information of an automatic driving vehicle, constructing a Frenet coordinate system, obtaining obstacle information and automatic driving vehicle information, and projecting the obstacle information and the automatic driving vehicle information to the Frenet coordinate system;
sampling in the Frenet coordinate system to obtain sampling points, connecting the sampling points by using a fifth-order polynomial, and calculating corresponding cost according to a preset cost function;
and performing dynamic planning and searching to obtain an optimal path in a searching stage, performing expansion to obtain a travelable channel, and converting the travelable channel into a Cartesian coordinate system.
As a further limitation of the technical solution of the embodiment of the present invention, the formula using a fifth-order polynomial to connect sampling points is:
Figure BDA0003972113200000021
s∈[0,Δs]
wherein the content of the first and second substances,
Figure BDA0003972113200000031
is a parameter of a fifth-order polynomial,
Figure BDA0003972113200000032
is an argument of a fifth order polynomial.
As a further limitation of the technical solution of the embodiment of the present invention, the cost function is:
c path =c ref +c smooth +c obs
wherein, c path As a path cost, c ref As a reference line offset cost, c smooth At the cost of smoothness, c obs At the expense of the obstacle.
As a further limitation of the technical solution of the embodiment of the present invention, the formula of the offset cost of the reference line is as follows:
Figure BDA0003972113200000033
wherein, w 0 As a weight coefficient, l(s) is a degree of offset.
As a further limitation of the technical solution of the embodiment of the present invention, the smoothness cost formula is:
Figure BDA0003972113200000034
wherein, w 1 ,w 2 ,w 3 All of which are weight coefficients, are obtained,
Figure BDA0003972113200000035
the first, second and third derivatives of l(s), respectively.
As a further limitation of the technical solution of the embodiment of the present invention, the formula of the barrier cost is as follows:
Figure BDA0003972113200000036
as a further limitation of the technical solution of the embodiment of the present invention, the motion state equation of the vehicle in the vehicle kinematics model is:
v i+1 =v i +a i δ t
θ i+1 =θ ii δ t
Figure BDA0003972113200000037
Figure BDA0003972113200000038
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0003972113200000039
is t i The amount of state at the moment in time,
Figure BDA00039721132000000310
is a coordinate point in a Cartesian coordinate system, v i And theta i Respectively the vehicle speed and the course angle, a i Is the acceleration.
As a further limitation of the technical solution of the embodiment of the present invention, the cost function includes a control cost function and a state cost function, specifically:
the control cost function is:
Figure BDA0003972113200000041
wherein, J u To control the cost, ω accω Are all weight coefficients, a ii Acceleration and yaw rate, respectively;
the state cost function is:
Figure BDA0003972113200000042
wherein (r) i x ,r i y ) Is a point
Figure BDA0003972113200000043
Projected point on the reference path, v r Reference speed for autonomous driving of the vehicle, w ref ,w vel Are all weight coefficients, J x Is the state cost.
As a further limitation of the technical solution of the embodiment of the present invention, the expression of the boundary constraint is as follows:
Figure BDA0003972113200000044
compared with the prior art, the invention has the beneficial effects that:
1. a staged planning method is adopted and divided into a searching stage and an optimizing stage, the advantages of two methods based on sampling and optimizing are combined, and the advantages of a Frenet coordinate system and a Cartesian coordinate system are also considered;
2. the problems that the calculation is complex and the real-time performance is difficult to meet in the space-time joint trajectory planning are solved;
3. the optimization mode is space-time coupled, so that the requirements of a dynamic scene are more easily met, and the problem that a decoupling method is easily trapped into a local optimal solution or even has no solution is solved;
4. the optimization stage is carried out under a Cartesian coordinate system, and the problem that a vehicle kinematic model cannot be established under a Frenet coordinate system is solved.
Drawings
In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the drawings used in the description of the embodiments or the prior art will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present invention.
Fig. 1 shows a schematic flow chart of a method provided by an embodiment of the present invention.
Fig. 2 is a schematic diagram illustrating a vehicle kinematics model in a method provided by an embodiment of the invention.
Fig. 3 is a schematic diagram illustrating establishment of a boundary constraint in the method according to the embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.
It can be understood that the existing automatic driving vehicle trajectory planning technology has the following disadvantages: (1) Based on the sampling method, the planning result is easily influenced by the sampling interval, if the sampling interval is too large, the generated track is too rough, and if the sampling interval is too small, the calculated amount is obviously increased, which is not beneficial to meeting the real-time requirement; (2) Based on an optimization method, a complex nonlinear programming problem is often solved, and more computing resources are needed to obtain an optimal solution, so that when the problem scale is large, the requirement of real-time performance is difficult to meet; (3) The method comprises the following steps that (1) track planning based on a Frenet coordinate system ignores road curvature, and cannot accurately describe a vehicle kinematic model under a scene with large curvature; (4) The trajectory planning based on the Cartesian coordinate system has complex description of lane boundaries and obstacles, and is not concise enough, so that the planning constraint solving is difficult, and the calculation amount is obviously increased.
In order to solve the problems, in the searching stage, sampling under a Frenet coordinate system is carried out, a fifth-order polynomial is used for connecting sampling points, a reference track is obtained through dynamic planning and searching, and a travelable channel is constructed; in the optimization stage, a vehicle kinematics model is established, a cost function and boundary constraint are established, space-time linkage optimization is performed by using an ILQR algorithm, and an optimal track is output. The advantages of the two methods based on sampling and optimization are combined, the advantages of a Frenet coordinate system and the advantages of a Cartesian coordinate system are taken into consideration, the problems that calculation is complex and real-time performance is difficult to meet in space-time joint trajectory planning are solved, the requirements of a dynamic scene are easily met, the problems that decoupling methods are prone to falling into local optimal solutions or even do not have solutions are solved, and the problem that a vehicle kinematics model cannot be established in the Frenet coordinate system is solved.
Fig. 1 shows a schematic flow chart of a method provided by an embodiment of the present invention.
Specifically, in a preferred embodiment provided by the present invention, a staged automatic driving vehicle trajectory planning method specifically includes the following steps:
step one, a searching stage, namely sampling under a Frenet coordinate system, connecting sampling points by using a fifth-order polynomial to form a directed acyclic graph, dynamically planning and searching to obtain a reference track, and constructing a travelable channel;
and step two, in an optimization stage, a vehicle kinematic model is established, a cost function is established according to the travelable channel, boundary constraint is established, space-time linkage optimization is carried out by using an ILQR algorithm, and an optimal track is output.
In the embodiment of the invention, the process of planning the track of the automatic driving vehicle is divided into a searching stage and an optimizing stage, wherein the searching stage aims to obtain a drivable channel, a sampling method is used, the searching process is completed under the condition of extremely small calculation amount, and a reference basis is provided for the next optimizing stage; the purpose of the optimization stage is to obtain a more perfect track, the result obtained in the search stage is too coarse, a space-time joint plan is made in the optimization stage, and a space-time smooth track is calculated under the condition that various constraints are met. Specifically, the method comprises the following steps: in the searching stage, map information where the automatic driving vehicle is located is obtained firstly, a Frenet coordinate system is built according to a target lane, and obstacle information and automatic driving vehicle information are projected under the coordinate system (so that the obstacle information and the automatic driving vehicle information are projected to the Frenet, the purpose is to simplify geometrical information of a road structure and an obstacle track, simplify constraint and reduce calculated amount); then sampling is carried out under a Frenet coordinate system to obtain sampling points, and the sampling points are connected by a fifth-order polynomial. Calculating the cost according to a preset cost function to obtain the cost of each edge; finally, dynamic planning and searching are carried out to obtain the optimal path in the searching stage, the feasible driving channel is obtained through expansion, and the feasible driving channel is converted into a Cartesian coordinate system; in the optimization stage, a vehicle kinematics model is established, a track boundary establishing limiting condition is established according to a travelable channel obtained in the searching stage, then a barrier function is used for converting the constrained optimization problem into an unconstrained problem, and then an ILQR algorithm is used for solving the problem to obtain an optimized optimal track and outputting the optimized optimal track.
In the embodiment of the invention, sampling is carried out under a Frenet coordinate system, a fifth-order polynomial is used for connecting sampling points to form a Directed Acyclic Graph (DAG), a sampling path is obtained through Dynamic Programming (DP) search, a travelable channel is constructed, the fifth-order polynomial is a common curve for trajectory planning, and the formula is as follows:
Figure BDA0003972113200000071
s∈[0,Δs]
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0003972113200000072
is a parameter of a fifth-order polynomial,
Figure BDA0003972113200000073
is independent variable of quintic polynomial, the sampling is to determine the expression form of these quintic polynomials, since the quintic polynomial needs six parameters, six equations, namely boundary conditions, can be obtained for the initial state and the end state, so that the parameters can be solved, and the optimal evaluation is performed by determining the standard, which is the path cost c path Offsetting the cost c from the reference line ref Smoothness cost c smooth And obstacle cost c obs And (3) the sum of the two, and the formed expression of the cost function is as follows:
c path =c ref +c smooth +c obs
the formula for the reference line offset cost is:
Figure BDA0003972113200000074
wherein, w 0 Is the weight coefficient, l(s) is the degree of offset;
the smoothness cost is formulated as:
Figure BDA0003972113200000075
wherein w 1 ,w 2 ,w 3 Are all the weight coefficients of the weight coefficient,
Figure BDA0003972113200000076
first, second, third derivatives of l(s), respectively;
the formula for the obstacle cost is:
Figure BDA0003972113200000077
it can be understood that the reference line offset cost c ref The method is used for evaluating the deviation of a curve and a lane central line, and the smaller the deviation is, the better the deviation is, the closer an automatically-driven vehicle is to the lane central line; cost of smoothness c smooth Is used to evaluate the smoothness of the curve, c smooth The smaller the value of (A), the smoother the curve, and the more comfortable the automobile is; cost of obstacle c obs Is used for evaluating the principle degree of the curve and the barrier, is beneficial to ensuring the safety of the vehicle when being far away from the barrier, c obs A smaller value of (c) indicates a greater distance from the obstacle.
In the embodiment of the invention, after the cost of all curves is calculated, because a clear target position does not exist, the heuristic search is difficult to obtain the most available path, so the exhaustive search can be used, the optimal path is searched by adopting a Dynamic Programming (DP) algorithm, the path with the minimum cost of relative smoothness and no collision is generated in a non-convex space, the optimal path obtained by the search is expanded, and thus the travelable channel is obtained, and then the travelable channel is converted from a Frenet coordinate system to a Cartesian coordinate system, thereby providing a basis for the boundary limit of the optimized stage.
In the embodiment of the invention, a travelable channel in a Frenet coordinate system is converted into a constraint limit in a Cartesian coordinate system, and space-time coupling trajectory optimization is carried out in the Cartesian coordinate system based on ILQR (CILQR) with the constraint limit, as shown in FIG. 2, a schematic diagram of a vehicle kinematics model in the method provided by the embodiment of the invention is shown, vehicle motion is described by using a vehicle-bicycle kinematics model,
Figure BDA0003972113200000081
is t i State quantity of time of day wherein
Figure BDA0003972113200000082
Is a coordinate point in a Cartesian coordinate system, v i And theta i Respectively the vehicle speed and course angle u i =[a ii ] T Is t i Control amount of time, a i As acceleration, ω i For the yaw rate, the control of the overall motion trajectory of the vehicle can be accomplished by controlling the acceleration and yaw rate of the vehicle, assuming that the controlled variable is [ t ] i ,t i+1 ]If the vehicle is kept constant, the equation of state of motion of the vehicle can be expressed approximately as:
v i+1 =v i +a i δ t
θ i+1 =θ ii δ t
Figure BDA0003972113200000083
Figure BDA0003972113200000084
wherein the content of the first and second substances,
Figure BDA0003972113200000085
is t i The amount of state at the moment of time,
Figure BDA0003972113200000086
is a coordinate point in a Cartesian coordinate system, v i And theta i Respectively the vehicle speed and the course angle, a i Is the acceleration, δ t =t i+1 -t i Represents an extremely short time during which the control quantity is assumed to be constant, naturally represented by t i The state of the moment can be deduced to t i+1 State of (1)
Figure BDA0003972113200000087
In the embodiment of the invention, the cost of the optimization process is divided into control cost and state cost, and the control cost aims to facilitate control of the automatic driving vehicle. The control cost includes an acceleration cost and a yaw rate cost, as follows:
Figure BDA0003972113200000091
wherein, J u To control cost, ω accω Are all weight coefficients, a ii Acceleration and yaw rate, respectively;
the objective of the state cost is to make the trajectory obtained in the optimization stage not deviate too much from the path obtained in the search stage, and to satisfy the boundary constraint of the travelable path in the search stage as much as possible, and the state cost is defined as the deviation from the path in the search stage and the error from the reference speed as follows:
Figure BDA0003972113200000092
wherein (r) i x ,r i y ) Is a point
Figure BDA0003972113200000093
Projection point on the reference path, v r Reference speed for autonomous driving of the vehicle, w ref ,w vel Are all weight systemsNumber, J x Is the state cost;
boundary constraints of the trajectory can be established according to the travelable path obtained in the search stage, and as shown in fig. 3, a schematic diagram and points for establishing the boundary constraints in the method provided by the embodiment of the present invention
Figure BDA0003972113200000094
Is t i At the time of the day the vehicle position,
Figure BDA0003972113200000095
and the projected point of the vehicle position on the boundary of the travelable passage. Constraints are established at the projection points and the function g (x, y) describes a straight line passing through the projection points and making it possible to tangent the channel boundaries, establishing the following constraints.
Figure BDA0003972113200000096
The meaning of the constraint is to keep the vehicle away from the boundaries of the feasible access, and the purpose of using tangent lines is to simplify the form of the constraint.
It should be understood that, although the steps in the flowcharts of the embodiments of the present invention are shown in sequence as indicated by the arrows, the steps are not necessarily performed in sequence as indicated by the arrows. The steps are not performed in the exact order shown and described, and may be performed in other orders, unless explicitly stated otherwise. Moreover, at least a portion of steps in various embodiments may include multiple sub-steps or multiple stages that are not necessarily performed at the same time, but may be performed at different times, and the order of performance of the sub-steps or stages is not necessarily sequential, but may be performed alternately or alternatingly with other steps or at least a portion of sub-steps or stages of other steps.
It will be understood by those skilled in the art that all or part of the processes of the methods of the embodiments described above can be implemented by a computer program, which can be stored in a non-volatile computer-readable storage medium, and can include the processes of the embodiments of the methods described above when the program is executed. Any reference to memory, storage, database or other medium used in the embodiments provided herein can include non-volatile and/or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically Programmable ROM (EPROM), electrically Erasable Programmable ROM (EEPROM), or flash memory. Volatile memory can include Random Access Memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in a variety of forms such as Static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double Data Rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous Link DRAM (SLDRAM), rambus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM).
The technical features of the embodiments described above may be arbitrarily combined, and for the sake of brevity, all possible combinations of the technical features in the embodiments described above are not described, but should be considered as being within the scope of the present specification as long as there is no contradiction between the combinations of the technical features.
The above-mentioned embodiments only express several embodiments of the present invention, and the description thereof is specific and detailed, but not to be understood as limiting the scope of the present invention. It should be noted that, for a person skilled in the art, several variations and modifications can be made without departing from the inventive concept, which falls within the scope of the present invention. Therefore, the protection scope of the present patent should be subject to the appended claims.
The above description is intended to be illustrative of the preferred embodiment of the present invention and should not be taken as limiting the invention, but rather, the intention is to cover all modifications, equivalents, and alternatives falling within the spirit and scope of the invention.

Claims (10)

1. A staged automatic driving vehicle trajectory planning method is characterized by specifically comprising the following steps:
in the searching stage, sampling is carried out under a Frenet coordinate system, a quintic polynomial is used for connecting sampling points to form a directed acyclic graph, a reference track is obtained through dynamic planning and searching, and a travelable channel is constructed;
and in the optimization stage, a vehicle kinematic model is established, a cost function is established according to the travelable channel, boundary constraint is established, space-time joint optimization is carried out by using an ILQR algorithm, and an optimal track is output.
2. The phased method for planning the trajectory of an autonomous vehicle according to claim 1, characterized in that in the search phase, sampling is performed in a Frenet coordinate system, a quintic polynomial is used to connect sampling points to form a directed acyclic graph, a reference trajectory is obtained by dynamic planning and searching, and the construction of a travelable channel specifically comprises the following steps:
obtaining map information of an automatic driving vehicle, constructing a Frenet coordinate system, obtaining obstacle information and automatic driving vehicle information, and projecting the obstacle information and the automatic driving vehicle information to the Frenet coordinate system;
sampling in the Frenet coordinate system to obtain sampling points, connecting the sampling points by using a fifth-order polynomial, and calculating corresponding cost according to a preset cost function;
and performing dynamic planning and searching to obtain an optimal path in a searching stage, performing expansion to obtain a travelable channel, and converting the travelable channel into a Cartesian coordinate system.
3. The phased autonomous vehicle trajectory planning method of claim 2, wherein the formula using a fifth order polynomial to tie the sample points is:
Figure FDA0003972113190000011
wherein the content of the first and second substances,
Figure FDA0003972113190000012
Figure FDA0003972113190000013
is a parameter of a fifth-order polynomial,
Figure FDA0003972113190000014
is an argument of a fifth order polynomial.
4. The phased autonomous vehicle trajectory planning method of claim 2, wherein the cost function is:
c path =c ref +c smooth +c obs
wherein, c path As a path cost, c ref As a reference line offset cost, c smooth At the cost of smoothness, c obs At the expense of the obstacle.
5. The phased autonomous vehicle trajectory planning method of claim 4, wherein the reference line offset cost is formulated as:
Figure FDA0003972113190000021
wherein, w 0 As a weight coefficient, l(s) is a degree of offset.
6. The phased autonomous vehicle trajectory planning method of claim 5, wherein the smoothness cost is formulated as:
Figure FDA0003972113190000022
wherein, w 1 ,w 2 ,w 3 Are all the weight coefficients of the weight coefficient,
Figure FDA0003972113190000023
the first, second, and third derivatives of l(s), respectively.
7. The phased autonomous vehicle trajectory planning method of claim 6, characterized in that the formula of the obstacle cost is:
Figure FDA0003972113190000024
8. the phased autonomous vehicle trajectory planning method of claim 1, wherein the equations of motion states of the vehicles in the vehicle kinematics model are:
v i+1 =v i +a i δ t
θ i+1 =θ ii δ t
Figure FDA0003972113190000025
Figure FDA0003972113190000026
wherein the content of the first and second substances,
Figure FDA0003972113190000027
is t i The amount of state at the moment of time,
Figure FDA0003972113190000028
is a coordinate point in a Cartesian coordinate system, v i And theta i Respectively the vehicle speed and the course angle, a i Is the acceleration.
9. The phased automated driven vehicle trajectory planning method of claim 8, in which the cost function comprises a control cost function and a state cost function, specifically:
the control cost function is:
Figure FDA0003972113190000031
wherein, J u To control cost, ω accω Are all weight coefficients, a ii Acceleration and yaw angular velocity, respectively;
the state cost function is:
Figure FDA0003972113190000032
wherein, the first and the second end of the pipe are connected with each other,
Figure FDA0003972113190000033
is a point
Figure FDA0003972113190000034
Projected point on the reference path, v r Reference speed for autonomous driving of the vehicle, w ref ,w vel Are all weight coefficients, J x Is a state cost.
10. The phased autonomous vehicle trajectory planning method of claim 9, wherein the boundary constraint is expressed as:
Figure FDA0003972113190000035
CN202211523129.1A 2022-11-30 2022-11-30 Staged automatic driving vehicle track planning method Pending CN115730756A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211523129.1A CN115730756A (en) 2022-11-30 2022-11-30 Staged automatic driving vehicle track planning method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211523129.1A CN115730756A (en) 2022-11-30 2022-11-30 Staged automatic driving vehicle track planning method

Publications (1)

Publication Number Publication Date
CN115730756A true CN115730756A (en) 2023-03-03

Family

ID=85299797

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211523129.1A Pending CN115730756A (en) 2022-11-30 2022-11-30 Staged automatic driving vehicle track planning method

Country Status (1)

Country Link
CN (1) CN115730756A (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116069031A (en) * 2023-01-28 2023-05-05 武汉理工大学 Underground unmanned mine car path optimization method and system based on car body sweep model

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116069031A (en) * 2023-01-28 2023-05-05 武汉理工大学 Underground unmanned mine car path optimization method and system based on car body sweep model

Similar Documents

Publication Publication Date Title
CN113276848B (en) Intelligent driving lane changing and obstacle avoiding track planning and tracking control method and system
CN109765887B (en) Automatic driving control method
CN112810630B (en) Method and system for planning track of automatic driving vehicle
CN109270933A (en) Unmanned barrier-avoiding method, device, equipment and medium based on conic section
CN115730756A (en) Staged automatic driving vehicle track planning method
CN112965476A (en) High-speed unmanned vehicle trajectory planning system and method based on multi-window sampling
US20220144265A1 (en) Moving Track Prediction Method and Apparatus
CN113830079A (en) Online planning method and system for continuous curvature parking path with any initial pose
CN116185014A (en) Intelligent vehicle global optimal track planning method and system based on dynamic planning
CN115077553A (en) Method, system, automobile, equipment and medium for planning track based on grid search
CN112124314A (en) Method and system for planning transverse path of vehicle for automatic lane change, vehicle and storage medium
CN115140096A (en) Spline curve and polynomial curve-based automatic driving track planning method
CN111634293A (en) Automatic lane changing method of automatic driving vehicle based on traffic clearance
CN116182884A (en) Intelligent vehicle local path planning method based on transverse and longitudinal decoupling of frenet coordinate system
CN114852085A (en) Automatic vehicle driving track planning method based on road right invasion degree
CN115366914A (en) Method, apparatus, and medium for controlling autonomous vehicle based on model predictive control
CN114442630A (en) Intelligent vehicle planning control method based on reinforcement learning and model prediction
Tang et al. Integrated decision making and planning framework for autonomous vehicle considering uncertain prediction of surrounding vehicles
CN116465427B (en) Intelligent vehicle lane changing obstacle avoidance path planning method based on space-time risk quantification
Na et al. Interaction-aware trajectory prediction of surrounding vehicles based on hierarchical framework in highway scenarios
CN114506340A (en) Intelligent driving vehicle lane-changing transverse path planning method and system and vehicle
CN114537380A (en) Method and system for planning obstacle avoidance and track changing tracks of internet-connected automobile
CN113674529A (en) Autonomous overtaking method and system
Hou et al. Tracking Control of Intelligent Vehicle Lane Change Based on RLMPC
Hesse et al. Motion planning for passenger vehicles-force field trajectory optimization for automated driving

Legal Events

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