A phased trajectory planning method for autonomous driving vehicles

Abstract

The present invention relates to the field of autonomous driving technology, and specifically discloses a phased autonomous driving vehicle trajectory planning method. In the search phase, the present invention performs sampling in the Frenet coordinate system, uses a quintic polynomial to connect the sampling points, and dynamically searches to obtain a reference trajectory to construct a drivable channel; in the optimization phase, a vehicle kinematic model is established, a cost function and boundary constraints are constructed, and the ILQR algorithm is used to perform spatiotemporal linkage optimization to output the optimal trajectory. It combines the advantages of both sampling-based and optimization-based methods, while also taking into account the advantages of the Frenet coordinate system and the Cartesian coordinate system. It solves the problem of complex calculations and difficulty in meeting real-time requirements in spatiotemporal joint trajectory planning, makes it easier to meet the requirements of dynamic scenes, solves the problem that decoupling methods are prone to falling into local optimal solutions or even no solutions, and solves the problem that the vehicle kinematic model cannot be established in the Frenet coordinate system.

Description

Technical Field

[0001] The present invention belongs to the field of autonomous driving technology, and in particular relates to a staged autonomous driving vehicle trajectory planning method. Background Art

[0002] The planning module is a key component of autonomous driving. It comprehensively considers perception, prediction, map information, and vehicle status to output a safe and comfortable trajectory. Currently, there is a wide range of research on trajectory planning for autonomous vehicles, with different categorizations depending on their focus.

[0003] Trajectory planning can be categorized into sampling-based and optimization-based approaches based on the trajectory generation method. Sampling-based approaches generate a large number of candidate trajectories by sampling the state space and then select the optimal trajectory based on pre-defined evaluation metrics. While sampling-based approaches are simple to implement and offer high real-time performance, their planning results are susceptible to the sampling interval. Excessively large sampling intervals result in overly coarse trajectories, while excessively small intervals significantly increase the computational complexity, hindering real-time performance. In contrast, optimization-based approaches formulate a constrained minimum cost problem and solve it to obtain a trajectory that satisfies the constraints. Optimization-based approaches are not restricted by the sampling method, allowing the cost function and constraints to be adjusted based on the scenario. This provides greater flexibility and makes them more suitable for complex scenarios. However, optimization-based approaches often require solving complex nonlinear programming problems, requiring significant computational resources to obtain the optimal solution. Consequently, they struggle to meet real-time performance requirements for large-scale problems.

[0004] Depending on the coordinate system, trajectory planning can be divided into methods based on the Frenet coordinate system and methods based on the Cartesian coordinate system. The Frenet coordinate system is a curvilinear coordinate system that can effectively simplify road curvature, making the description more concise and intuitive. Because Frenet ignores road curvature, it cannot accurately describe the vehicle's kinematic model in scenes with large curvature. While the Cartesian coordinate system can accurately describe the vehicle's kinematic model and obtain a trajectory that is more consistent with the vehicle and kinematic model, the description of lane boundaries and obstacles in the Cartesian coordinate system is complex and not concise, making it difficult to solve planning constraints and significantly increasing the amount of computation. Summary of the Invention

[0005] The purpose of the embodiments of the present invention is to provide a staged autonomous driving vehicle trajectory planning method, aiming to solve the problems raised in the background technology.

[0006] To achieve the above objectives, the embodiments of the present invention provide the following technical solutions:

[0007] A phased autonomous driving vehicle trajectory planning method, the method specifically comprising the following steps:

[0008] In the search phase, sampling is performed in the Frenet coordinate system, and a quintic polynomial is used to connect the sampling points to form a directed acyclic graph. Dynamic programming is used to search for reference trajectories and construct drivable channels.

[0009] In the optimization phase, a vehicle kinematic model is established, a cost function is constructed based on the drivable channel, boundary constraints are established, and the ILQR algorithm is used for spatiotemporal linkage optimization to output the optimal trajectory.

[0010] As a further limitation of the technical solution of the embodiment of the present invention, the search phase performs sampling in the Frenet coordinate system, connects the sampling points using a quintic polynomial to form a directed acyclic graph, obtains a reference trajectory through dynamic programming search, and constructs a drivable channel, specifically comprising the following steps:

[0011] Obtaining map information of the autonomous driving vehicle, constructing a Frenet coordinate system, obtaining obstacle information and autonomous driving vehicle information, and projecting them into the Frenet coordinate system;

[0012] Sampling is performed in the Frenet coordinate system to obtain sampling points, the sampling points are connected using a quintic polynomial, and corresponding costs are calculated according to a preset cost function;

[0013] Perform dynamic programming search to obtain the optimal path in the search phase, perform expansion to obtain the drivable channel, and convert it into a Cartesian coordinate system.

[0014] As a further limitation of the technical solution of the embodiment of the present invention, the formula for connecting the sampling points using a quintic polynomial is:

[0015] s∈[0,Δs]

[0016] in, are the parameters of the quintic polynomial, is the independent variable of the quintic polynomial.

[0017] As a further limitation of the technical solution of the embodiment of the present invention, the cost function is:

[0018] c path =c ref +c smooth +c obs

[0019] Among them, c path is the path cost, c ref is the reference line offset cost, c smooth is the smoothness cost, c obsThe cost of obstacles.

[0020] As a further limitation of the technical solution of the embodiment of the present invention, the formula for the reference line offset cost is:

[0021]

[0022] Among them, w0 is the weight coefficient and l(s) is the offset degree.

[0023] As a further limitation of the technical solution of the embodiment of the present invention, the formula of the smoothness cost is:

[0024]

[0025] Among them, w1, w2, w3 are weight coefficients, are the first, second, and third derivatives of l(s) respectively.

[0026] As a further limitation of the technical solution of the embodiment of the present invention, the formula of the obstacle cost is:

[0027]

[0028] 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 kinematic model is:

[0029] v i+1 =v i +a i δ t

[0030] θ i+1 =θ i +ω i δ t

[0031]

[0032]

[0033] in, t i The state quantity at the moment, is the coordinate point in the Cartesian coordinate system, v i and θ i are vehicle speed and heading angle respectively, a i is the acceleration.

[0034] 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:

[0035] The control cost function is:

[0036]

[0037] Among them, J u Time control cost, ω acc ,ω ω are weight coefficients, a i ,ω i are acceleration and yaw rate respectively;

[0038] The state cost function is:

[0039]

[0040] Among them, (r i x ,r i y ) is a point The projection point on the reference path, v r is the reference speed of the autonomous vehicle, w ref ,w vel are weight coefficients, J x The cost of status.

[0041] As a further limitation of the technical solution of the embodiment of the present invention, the expression of the boundary constraint is:

[0042]

[0043] Compared with the prior art, the present invention has the following beneficial effects:

[0044] 1. A phased planning approach is adopted, which consists of a search phase and an optimization phase. It combines the advantages of both sampling-based and optimization-based approaches, while also taking into account the advantages of both Frenet and Cartesian coordinate systems.

[0045] 2. Solve the problem of computational complexity and difficulty in meeting real-time requirements in spatiotemporal joint trajectory planning;

[0046] 3. The optimization method is spatiotemporally coupled, so it is easier to meet the requirements of dynamic scenarios and solve the problem that decoupling methods are prone to falling into local optimal solutions or even no solutions;

[0047] 4. The optimization stage is carried out in the Cartesian coordinate system, which solves the problem that the vehicle kinematic model cannot be established in the Frenet coordinate system. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are only some embodiments of the present invention.

[0049] Figure 1 A schematic flow chart of a method provided by an embodiment of the present invention is shown.

[0050] Figure 2 A schematic diagram of a vehicle kinematic model in a method provided by an embodiment of the present invention is shown.

[0051] Figure 3 A schematic diagram of establishing boundary constraints in the method provided by an embodiment of the present invention is shown. DETAILED DESCRIPTION

[0052] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0053] It is understandable that the existing autonomous driving vehicle trajectory planning technology has the following shortcomings: (1) For sampling-based methods, the planning results are easily affected by the sampling interval. If the sampling interval is too large, the generated trajectory will be too rough. If the sampling interval is too small, the amount of calculation will increase significantly, which is not conducive to meeting the real-time requirements; (2) For optimization-based methods, it is often necessary to solve complex nonlinear programming problems, which requires more computing resources to obtain the optimal solution. Therefore, when the scale of the problem is large, it is difficult to ensure that the real-time requirements are met; (3) Trajectory planning based on the Frenet coordinate system ignores the road curvature and cannot accurately describe the vehicle's kinematic model in scenes with large curvature; (4) For trajectory planning based on the Cartesian coordinate system, the description of lane boundaries and obstacles is complex and not concise enough, which makes it difficult to solve the planning constraints and significantly increases the amount of calculation.

[0054] To address these issues, an embodiment of the present invention performs sampling in the Frenet coordinate system during the search phase, connects the sampling points using quintic polynomials, and dynamically programs the search to obtain a reference trajectory and construct a drivable channel. During the optimization phase, a vehicle kinematic model is established, along with a cost function and boundary constraints. The ILQR algorithm is used for spatiotemporal optimization to output the optimal trajectory. This method combines the advantages of both sampling-based and optimization-based approaches, while also leveraging the strengths of both the Frenet and Cartesian coordinate systems. It addresses the computational complexity and difficulty in meeting real-time requirements in spatiotemporal trajectory planning, making it easier to meet the requirements of dynamic scenarios. It also addresses the problem that decoupling methods are prone to falling into local optimal solutions or even unsolvable solutions, and it solves the inability to establish a vehicle kinematic model in the Frenet coordinate system.

[0055] Figure 1 A schematic flow chart of the method provided by an embodiment of the present invention is shown.

[0056] Specifically, in a preferred embodiment of the present invention, a staged autonomous driving vehicle trajectory planning method comprises the following steps:

[0057] Step 1: Search phase: Sampling is performed in the Frenet coordinate system. A quintic polynomial is used to connect the sampling points to form a directed acyclic graph. Dynamic programming is used to search for reference trajectories and construct a drivable channel.

[0058] In the second step, during the optimization phase, a vehicle kinematic model is established. Based on the drivable channel, a cost function is constructed, boundary constraints are established, and the ILQR algorithm is used for spatiotemporal optimization to output the optimal trajectory.

[0059] In an embodiment of the present invention, the process of trajectory planning for an autonomous vehicle is divided into two stages: a search phase and an optimization phase. The purpose of the search phase is to obtain a drivable channel. Using a sampling method, the search process is completed with minimal computational effort, providing a reference for the subsequent optimization phase. The purpose of the optimization phase is to obtain a more complete trajectory. The results obtained in the search phase are too rough, so a spatiotemporal joint planning is performed in the optimization phase to calculate a spatiotemporal smooth trajectory while satisfying various constraints. Specifically, in the search phase, the map information of the autonomous vehicle is first obtained, a Frenet coordinate system is constructed based on the target lane, and the obstacle information and the autonomous vehicle information are projected into this coordinate system (the purpose of the Frenet projection is to simplify the geometric information of the road structure and obstacle trajectory, simplify constraints, and reduce computational effort). Then, sampling is performed in the Frenet coordinate system to obtain sampling points, which are then connected using a quintic polynomial. The cost of each edge is calculated according to the preset cost function; finally, a dynamic programming search is performed to obtain the optimal path in the search phase, and the drivable channel is expanded and converted into a Cartesian coordinate system; in the optimization phase, a vehicle kinematic model is established, and the trajectory boundary and constraint conditions are established according to the drivable channel obtained in the search phase. The obstacle function is then used to transform the constrained optimization problem into an unconstrained problem, and the ILQR algorithm is then used to solve the problem, and the optimized optimal trajectory is obtained and output.

[0060] In the embodiment of the present invention, sampling is performed in the Frenet coordinate system, and a quintic polynomial is used to connect the sampling points to form a directed acyclic graph (DAG). The sampling path is obtained through dynamic programming (DP) search, and a drivable channel is constructed. The quintic polynomial is a curve commonly used in trajectory planning, and its formula is:

[0061] s∈[0,Δs]

[0062] in, are the parameters of the quintic polynomial, It is the independent variable of the quintic polynomial. The purpose of sampling is to determine the expression of these quintic polynomials. Since the quintic polynomial requires six parameters, six equations can be obtained for the initial state and the end state, that is, the boundary conditions. In this way, the parameters can be solved and the standard can be formulated for optimal evaluation. This standard is the path cost c path , offset by the reference line cost c ref , smoothness cost c smooth and obstacle cost c obs The cost function is composed of the sum of:

[0063] c path =c ref +c smooth +c obs

[0064] The formula for the reference line offset cost is:

[0065]

[0066] Among them, w0 is the weight coefficient, l(s) is the degree of offset;

[0067] The formula for the smoothness cost is:

[0068]

[0069] Among them, w1, w2, w3 are weight coefficients, are the first, second, and third derivatives of l(s);

[0070] The formula for obstacle cost is:

[0071]

[0072] It is understandable that the reference line offset cost c ref It is used to evaluate the offset between the curve and the lane centerline. The smaller the offset, the better. The autonomous driving vehicle is closer to the lane centerline. The smoothness cost c smooth It is used to evaluate the smoothness of the curve, c smooth The smaller the value of c, the smoother the curve is and the more comfortable the car is. obs It is used to evaluate the principle degree of curves and obstacles. Staying away from obstacles is beneficial to ensuring the safety of the vehicle. obs The smaller the value, the farther away from the obstacle.

[0073] In an embodiment of the present invention, after calculating the costs of all curves, it is difficult to obtain the optimal path using a heuristic search due to the absence of a clear target position. Therefore, an exhaustive search can be used to find the optimal path by adopting a dynamic programming (DP) algorithm to generate a relatively smooth, collision-free path with the lowest cost in a non-convex space. The optimal path obtained by the search is expanded to obtain a drivable channel, and then the drivable channel is converted from a Frenet coordinate system to a Cartesian coordinate system to provide a basis for boundary constraints in the optimization stage.

[0074] In the embodiment of the present invention, the drivable channel in the Frenet coordinate system is converted into the constraints in the Cartesian coordinate system, and the spatiotemporal coupled trajectory optimization is performed based on the ILQR with constraints (CILQR) in the Cartesian coordinate system, such as Figure 2 A schematic diagram of a vehicle kinematic model in a method provided by an embodiment of the present invention is shown, wherein a vehicle-bicycle motion model is used to describe vehicle motion. t i The state quantity at the moment, where is the coordinate point in the Cartesian coordinate system, v i and θ i are vehicle speed and heading angle, u i =[a i ,ω i ] T t i The control amount at the moment, a i is the acceleration, ω i The yaw rate can be used to control the vehicle's overall trajectory. Assuming that the control quantity is [t i ,t i+1 ] remains unchanged, the vehicle's motion equation can be approximately expressed as follows:

[0075] v i+1 =v i +a i δ t

[0076] θ i+1 =θ i +ω i δ t

[0077]

[0078]

[0079] in, t i The state quantity at the moment, is the coordinate point in the Cartesian coordinate system, vi and θ i are vehicle speed and heading angle respectively, a i is the acceleration, δ t =t i+1 -t i Represents a very short time. During this time, assuming that the control quantity remains unchanged, it is naturally determined by t i The state at time t can be deduced i+1 Status

[0080] In this embodiment of the present invention, the cost of the optimization process is divided into control cost and state cost. The purpose of the control cost is to make the autonomous driving vehicle easier to control. The control cost includes acceleration cost and yaw rate cost, as shown in the following formula:

[0081]

[0082] Among them, J u Time control cost, ω acc ,ω ω are weight coefficients, a i ,ω i are acceleration and yaw rate respectively;

[0083] The purpose of the state cost is to ensure that the trajectory obtained in the optimization phase does not deviate too much from the path obtained in the search phase, while also satisfying the boundary constraints of the drivable channel in the search phase as much as possible. The state cost is defined as the deviation from the search phase path and the error from the reference speed as follows:

[0084]

[0085] Among them, (r i x ,r i y ) is a point The projection point on the reference path, v r is the reference speed of the autonomous vehicle, w ref ,w vel are weight coefficients, J x is the state cost;

[0086] According to the drivable channels obtained in the search phase, the boundary constraints of the trajectory can be established, such as Figure 3 A schematic diagram of establishing boundary constraints in the method provided by an embodiment of the present invention is shown. t i Vehicle position at the moment, The vehicle's position is projected onto the boundary of the drivable lane. A constraint is established at the projection point. The function g(x,y) describes a line passing through the projection point and tangent to the boundary of the drivable lane. The following constraints are established.

[0087]

[0088] The purpose of this constraint is to keep the vehicle away from the boundary of the drivable channel. The purpose of using tangents is to simplify the form of the constraint.

[0089] It should be understood that, although the various steps in the flow chart of each embodiment of the present invention are shown in sequence according to the indication of the arrows, these steps are not necessarily performed in sequence according to the order indicated by the arrows. Unless otherwise specified herein, the execution of these steps is not strictly limited in order, and these steps can be performed in other orders. Moreover, at least a portion of the steps in each embodiment may include a plurality of sub-steps or a plurality of stages, and these sub-steps or stages are not necessarily performed at the same time, but can be performed at different times, and the execution order of these sub-steps or stages is not necessarily performed in sequence, but can be performed in turn or alternately with at least a portion of other steps or sub-steps or stages of other steps.

[0090] Those skilled in the art will appreciate that all or part of the processes in the above-mentioned embodiments can be implemented by instructing the relevant hardware through a computer program. The program can be stored in a non-volatile computer-readable storage medium. When the program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to memory, storage, database or other media used in the embodiments provided in this application 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 various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM).

[0091] The technical features of the above-mentioned embodiments can be combined arbitrarily. In order to make the description concise, not all possible combinations of the technical features in the above-mentioned embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0092] The above-described embodiments merely illustrate several implementations of the present invention, and while their descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the present invention. It should be noted that a person skilled in the art would be able to make numerous variations and improvements without departing from the spirit of the present invention, all of which fall within the scope of protection of the present invention. Therefore, the scope of protection of the present invention shall be determined by the appended claims.

[0093] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A phased autonomous driving vehicle trajectory planning method, characterized in that: The method specifically comprises the following steps: During the search phase, sampling is performed in the Frenet coordinate system, sampling points are connected using a quintic polynomial to form a directed acyclic graph, a reference trajectory is obtained through dynamic programming search, and a drivable channel is constructed. The search phase specifically includes: obtaining map information of the autonomous driving vehicle, constructing a Frenet coordinate system, obtaining obstacle information and autonomous driving vehicle information, and projecting them into the Frenet coordinate system; sampling in the Frenet coordinate system to obtain sampling points, connecting the sampling points using a quintic polynomial, and calculating the corresponding cost based on a preset cost function; performing a dynamic programming search to obtain the optimal path in the search phase, performing expansion to obtain the drivable channel, and converting it into a Cartesian coordinate system; In the optimization phase, a vehicle kinematic model is established. Based on the drivable channel, a cost function is constructed, boundary constraints are established, and the ILQR algorithm is used for spatiotemporal joint optimization to output the optimal trajectory. The boundary constraint expression is: ; The vehicle's motion state equation in the vehicle kinematic model is: in, for The state quantity at the moment, is the coordinate point in the Cartesian coordinate system, and are vehicle speed and heading angle, is the acceleration; The cost function includes a control cost function and a state cost function, specifically: The control cost function is: in, Control costs for the time being; 、 All are weight coefficients; 、 are acceleration and yaw rate respectively; The state cost function is: in, for point The projection point on the reference path, is the reference speed of the autonomous vehicle, , are weight coefficients, The cost of status.

2. The method for trajectory planning of an autonomous driving vehicle in stages according to claim 1, wherein: The formula for connecting the sampling points using a quintic polynomial is: in, are the parameters of the quintic polynomial, is the independent variable of the quintic polynomial.

3. The method for trajectory planning of an autonomous driving vehicle in stages according to claim 1, wherein: The cost function is: in, is the path cost, is the reference line offset cost, is the smoothness cost, The cost of obstacles.

4. The method for trajectory planning of an autonomous driving vehicle in stages according to claim 3, wherein: The formula for the reference line offset cost is: in, is the weight coefficient, The degree of offset.

5. The method for trajectory planning of an autonomous driving vehicle in stages according to claim 4, wherein: The formula for the smoothness cost is: Among them, w1, w2, w3 are weight coefficients, They are The first, second, and third derivatives of .

6. The method for trajectory planning of an autonomous driving vehicle in stages according to claim 5, wherein: The formula for the obstacle cost is: 。

Images