A trajectory smoothing optimization method for autonomous driving vehicles considering curvature constraints

Abstract

The present invention discloses a method for smoothing optimization of the trajectory of an autonomous driving vehicle considering curvature constraints. This method follows the general idea of ​​multi-constraint optimization problems. First, according to the goal of trajectory smoothing optimization, an objective function to be optimized related to each trajectory point is established. Then, the conjugate gradient method is used to solve the extreme value of the objective function. Finally, it is verified whether the optimized result meets the constraint conditions. The key point of this method is that when calculating the curvature and its gradient at a certain trajectory point, the point and its adjacent points before and after are not directly calculated, but a second-order Lagrange interpolation polynomial passing through these three points is fitted, and then the vertex of the fitting curve is taken to solve the curvature and its gradient.

Description

Technical Field

[0001] The present invention relates to the technical field of trajectory smoothing optimization methods, and in particular to a trajectory smoothing optimization method for an autonomous driving vehicle taking curvature constraints into consideration. Background Art

[0002] The goal of path planning in autonomous driving is to plan a safe and comfortable trajectory for the vehicle to complete the driving task. It is one of the core capabilities of autonomous vehicles and determines whether the vehicle can smoothly, accurately and safely complete various driving behaviors. There are relatively mature solutions to path planning problems in fields such as robotics, but these solutions cannot meet the needs of autonomous vehicles. Traditional methods only consider the geometric constraints of terrain space and ignore the kinematic and dynamic characteristics of the vehicle. Some points in the planned trajectory may exceed the limit tracking capability of the vehicle control system, and jitter or even collision may occur during driving. Currently, the commonly used path planning methods are mainly divided into four categories: graph search-based, sampling-based, interpolation curve-based, and numerical optimization-based. Among them, except for the method based on numerical optimization, the other three methods cannot guarantee that the planned trajectory can meet the vehicle motion constraints in principle. Smooth optimization is required on the basis of the preliminary trajectory obtained in the plan to meet the driving needs of the vehicle. In actual path planning, this "preliminary trajectory + optimized smoothing" idea is often used to solve the problem. In complex unstructured environments, this idea can often solve the problem better.

[0003] Trajectory smoothing optimization is essentially a multi-constrained nonlinear optimization problem. There are two mainstream ways to solve the problem in practice. One is to perform smoothing optimization directly; the other is to solve it as a multi-constrained optimization problem. Direct smoothing optimization methods commonly used include specific curve smoothing and sliding average smoothing. When implemented, the former uses smooth curves with good properties such as ReedsShepp, Dubins, Bezier, Spline, etc. to regenerate new trajectory points; the latter starts from the first point and slides to make a certain average of the data points in a neighborhood to replace the center value point of the neighborhood, and ends at the last point to complete the smoothing of the given trajectory point. The ideas for solving multi-constrained optimization problems are relatively consistent. Generally, it is necessary to clarify the three elements of the objective function to be optimized, the constraints, and the variables, and then select an optimization algorithm to solve the value of the variable.

[0004] However, the above trajectory smoothing optimization methods all have some defects: the direct smoothing optimization method does not consider constraints such as trajectory collision, the optimized trajectory may hit surrounding obstacles, and the adopted curve cannot ensure that the curvature is less than the maximum curvature constraint of the vehicle. As a multi-constrained optimization problem, possible constraints are taken into account and the optimization objective function is established. Generally, a certain trajectory point and its two adjacent trajectory points are directly used to calculate the curvature. When calculating, the broken line structure connected by three trajectory points is directly used, and the displacement change angle of the trajectory point is divided by the modulus of the displacement. For discrete points, this calculation method will result in the trajectory point moving away from the two adjacent points, but the calculated curvature will continue to decrease, resulting in the wrong optimization direction. The root cause of this problem is that the curvature is defined for a smooth curve, and the definition includes the assumption that the displacement limit is zero. This assumption is not valid for the broken line structure connected by three discrete trajectory points. Summary of the invention

[0005] In view of the above, an object of the present invention is to provide a trajectory smoothing optimization method for an autonomous driving vehicle taking into account curvature constraints.

[0006] To achieve the purpose of the present invention, the present invention provides a method for smoothing and optimizing the trajectory of an autonomous driving vehicle considering curvature constraints, comprising the following steps:

[0007] Firstly, according to the goal of trajectory smoothing optimization, an objective function to be optimized related to each trajectory point is established;

[0008] Then the conjugate gradient method is used to solve the extreme value of the objective function;

[0009] Finally, verify whether the optimized results meet the constraints;

[0010] Among them, when calculating the curvature and its gradient at a certain trajectory point, the point and its adjacent points are not used for direct calculation, but a second-order Lagrange interpolation polynomial passing through these three points is fitted, and then the vertices of the fitting curve are taken to solve the curvature and its gradient.

[0011] in,

[0012] The specific steps to establish the objective function are:

[0013] The objective function is designed based on the following three requirements: the trajectory point maintains a certain distance from the obstacle, the curvature at the point satisfies the maximum curvature constraint of the vehicle, and the trajectory is smooth:

[0014]

[0015]

[0016]

[0017]

[0018]

[0019]

[0020] k i =2|a|

[0021] Among them, w o is the weight coefficient corresponding to the distance to the obstacle, w k is the weight coefficient corresponding to the curvature term, w s is the weight coefficient corresponding to the smoothing term, A represents the quadratic term coefficient matrix, b T represents the coefficient vector of the first-order term; Represents trajectory points The displacement at Represents trajectory points The displacement at is the coordinate of the trajectory point obtained by planning (x i ,y i ); Indicates the coordinate point of the obstacle closest to the trajectory point, d min Indicates the shortest distance allowed to the obstacle; lag2(x) indicates the distance from the trajectory point and its adjacent and The curve obtained by second-order Lagrange interpolation, a represents its quadratic coefficient, k i represents the curvature at its vertex, k max It is the maximum permissible curvature of the track determined by the minimum turning radius of the vehicle.

[0022] in,

[0023] The conjugate gradient method is used to solve the problem:

[0024] 1) Calculate the gradient

[0025]

[0026] Track Points The smoothing term gradient is represented by the five adjacent points before and after it:

[0027]

[0028] Track Points The gradient of the distance term to the obstacle can be expressed as:

[0029]

[0030] Track Points The gradient of the curvature term is converted into the calculated interpolation curve The curvature gradient at:

[0031]

[0032] 2) Initial value substitution

[0033] For each trajectory point, the initial value is substituted in accordance with the iterative update idea; for ease of description, replace

[0034]

[0035]

[0036] k∶=0

[0037] in is the initial iteration direction, and k is the number of iterations.

[0038] 3) Iterative Update

[0039]

[0040]

[0041]

[0042]

[0043]

[0044] k∶=k+1

[0045] if If it is small enough, exit the loop early.

[0046] in,

[0047] The verification of the optimized result specifically includes, after obtaining the optimized trajectory result, calculating whether the curvature of each trajectory point is less than the maximum curvature and whether a collision occurs at the trajectory point. If not satisfied, re-optimization is performed.

[0048] Compared with the prior art, the invention has the following beneficial effects: BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1 Schematic diagram of the trajectory optimization process of this application;

[0050] Figure 2 Schematic diagram of the process of calculating curvature for this application. DETAILED DESCRIPTION

[0051] It should be noted that, in the absence of conflict, the embodiments and features in the embodiments of the present application may be combined with each other.

[0052] The present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0053] The method for smoothing and optimizing the trajectory of an autonomous driving vehicle considering curvature constraints proposed in this invention mainly includes three parts: establishing an objective function, solving it using the conjugate gradient method, and verifying the trajectory. The whole process is as follows: Figure 1 The input of smooth optimization includes the coordinates of the planned trajectory points. If the distance transformation diagram in the planning area can be obtained, the distance to the obstacle will also be included in the objective function. It is assumed here that the position of each point in the trajectory area to the nearest obstacle point can be obtained.

[0054] The specific steps are as follows:

[0055] 1. Establish the objective function

[0056] The objective function is designed based on the following three requirements: the trajectory point maintains a certain distance from the obstacle, the curvature at the point satisfies the maximum curvature constraint of the vehicle, and the trajectory is smooth:

[0057]

[0058]

[0059]

[0060]

[0061]

[0062]

[0063] k i =2|a|

[0064] Among them, w o is the weight coefficient corresponding to the distance to the obstacle, w k is the weight coefficient corresponding to the curvature term, w s is the weight coefficient corresponding to the smoothing term, A represents the quadratic term coefficient matrix, b T represents the coefficient vector of the first-order term; Represents trajectory points The displacement at Represents trajectory points The displacement at is the coordinate of the trajectory point obtained by planning (x i ,yi ); Indicates the coordinate point of the obstacle closest to the trajectory point, d min Indicates the shortest distance allowed to the obstacle; lag2(x) indicates the distance from the trajectory point and its adjacent and The curve obtained by second-order Lagrange interpolation, a represents its quadratic coefficient, k i represents the curvature at its vertex, k max is the maximum allowable curvature of the track determined according to the minimum turning radius of the vehicle. The fitting process when calculating the curvature is as follows: Figure 2 shown.

[0065] 2. Use the conjugate gradient method to solve

[0066] 2.1 Calculating Gradients

[0067]

[0068] Track Points The smoothing term gradient is represented by the five adjacent points before and after it:

[0069]

[0070] Track Points The gradient of the distance term to the obstacle can be expressed as:

[0071]

[0072] Track Points The gradient of the curvature term is converted into the calculated interpolation curve The curvature gradient at:

[0073]

[0074] 2.2 Initial value substitution

[0075] For each trajectory point, the initial value is substituted in the same way as the iterative update idea. replace

[0076]

[0077]

[0078] k∶=0

[0079] in is the initial iteration direction, and k is the number of iterations.

[0080] 3.3 Iterative Update

[0081]

[0082]

[0083]

[0084]

[0085]

[0086] k∶=k+1

[0087] if If it is small enough, the loop is exited early (assuming that the optimal solution has been found).

[0088] 3. Verify the trajectory

[0089] After obtaining the optimized trajectory results, calculate whether the curvature of each trajectory point is less than the maximum curvature and whether a collision occurs at the trajectory point. If not, re-optimize.

[0090] It should be noted that the technical solutions not described in detail in this application adopt publicly known technologies.

[0091] The above is only a preferred embodiment of the present invention. It should be pointed out that, for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principle of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. A method for smoothing and optimizing the trajectory of an autonomous driving vehicle considering curvature constraints, characterized in that: The steps include: Firstly, according to the goal of trajectory smoothing optimization, the objective function to be optimized related to each trajectory point is established; Then, the conjugate gradient method is used to solve the extreme value of the objective function; Finally, verify whether the optimized results meet the constraints; Among them, when calculating the curvature and its gradient at a certain trajectory point, instead of directly calculating the point and its adjacent points before and after it, a second-order Lagrange interpolation polynomial passing through these three points is fitted, and then the vertex of the fitting curve is taken to solve the curvature and its gradient; The specific steps to establish the objective function are: The objective function is designed based on the following three requirements: the trajectory point maintains a certain distance from the obstacle, the curvature at the point satisfies the maximum curvature constraint of the vehicle, and the trajectory is smooth: Among them, w o is the weight coefficient corresponding to the distance to the obstacle, w k is the weight coefficient corresponding to the curvature term, w s is the weight coefficient corresponding to the smoothing term, A represents the quadratic term coefficient matrix, b T represents the coefficient vector of the first-order term; Represents trajectory points The displacement at Represents trajectory points The displacement at The coordinates of the trajectory points obtained by planning Indicates the coordinate point of the obstacle closest to the trajectory point, d min Indicates the shortest distance allowed to the obstacle; lag2(x) indicates the distance from the trajectory point and its adjacent and The curve obtained by second-order Lagrange interpolation, a represents its quadratic coefficient, k i represents the curvature at its vertex, k max It is the maximum permissible curvature of the track determined by the minimum turning radius of the vehicle.

2. The method for smoothing and optimizing the trajectory of an autonomous driving vehicle considering curvature constraints according to claim 1, characterized in that: The conjugate gradient method is used to solve the problem: 1) Calculate the gradient Track Points The smoothing term gradient is represented by the five adjacent points before and after it: Track Points The gradient of the distance term to the obstacle can be expressed as: Track Points The gradient of the curvature term is converted into the calculated interpolation curve The curvature gradient at: 2) Initial value substitution For each trajectory point, the initial value is substituted in accordance with the iterative update idea; for ease of description, replace in is the initial iteration direction, k is the number of iterations; 3) Iterative Update if If it is small enough, exit the loop early.

3. The method for smoothing and optimizing the trajectory of an autonomous driving vehicle considering curvature constraints according to claim 2, characterized in that: The verification of the optimized result specifically includes, after obtaining the optimized trajectory result, calculating whether the curvature of each trajectory point is less than the maximum curvature and whether a collision occurs at the trajectory point. If not satisfied, re-optimization is performed.

Images