Automated driving control unit, trajectory planning method and machine-readable storage medium

Abstract

This invention relates to an autonomous driving control unit, a trajectory planning method, and a machine-readable storage medium capable of generating autonomous driving routes for autonomous vehicles to follow on roads. The parameters of each segment of the autonomous driving route, calculated using the interior-point method, ensure that the absolute value of curvature at each point on the trajectory is below a preset maximum curvature, and that the total mileage of the trajectory and the deviation between the trajectory endpoint and the target point are minimized. The autonomous driving route generated by the autonomous driving trajectory planning scheme according to this invention allows autonomous vehicles to reliably follow and reach a endpoint with the smallest possible deviation from the target point over the shortest possible mileage.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving, and more specifically, to an autonomous driving control unit for generating autonomous driving routes, a trajectory planning method, and a machine-readable storage medium. Background Technology

[0002] The smooth movement of autonomous vehicles relies on a planned trajectory from the starting point to the target point as a control reference. This trajectory must guarantee a certain degree of geometric continuity (point continuity, tangent continuity, curvature continuity, and even curvature derivative continuity) to avoid abrupt changes in controller input during trajectory tracking, which could affect control performance. For example, the curvature of the vehicle's trajectory is related to the vehicle's steering angle. Due to limitations in the vehicle's steering mechanism performance, the steering angle can only change continuously, making it impossible to output the steering control quantity required when the curvature of the reference trajectory changes discontinuously. This can easily lead to tracking deviations (including position deviations and heading angle deviations) when the vehicle tracks a trajectory with discontinuous curvature. Simultaneously, the trajectory must satisfy vehicle motion constraints. A typical kinematic constraint is that the steering angle of the vehicle's steering wheels can only change within a certain range, corresponding to the motion characteristic that the absolute value of curvature at any point on the trajectory is less than a certain value. This constraint requires that the curvature of points on the planned trajectory should not exceed this value to prevent the steering mechanism from being unable to output the required steering angle, thus affecting trajectory tracking performance. Therefore, it is necessary to calculate a curve that satisfies a certain degree of geometric continuity and where the curvature at any point is less than a given value.

[0003] In existing technical solutions, to obtain a sufficiently high geometrical continuity for the planned trajectory, cubic or higher-order spline curves or spirals are often used as the sole trajectory representation form. Specifically: Spline curve trajectories, using cubic or quintic polynomials to represent the horizontal and vertical coordinates, ensure the planned curve passes through the start and end points. However, the heading angle and curvature of each point on the curve cannot be linearly represented by the polynomial parameters of the horizontal and vertical coordinates, making it difficult to control the heading angle and curvature of points on the planned trajectory, thus affecting the trajectory tracking effect. Spiral trajectories, using cubic or quintic polynomials to represent the heading angle, can directly represent the heading angle, curvature, and even the rate of change of curvature of each point on the planned trajectory. However, cubic spirals, due to their lower degree and fewer curve parameters, struggle to guarantee the rate of change of curvature or even curvature continuity at the splicing points when splicing multiple trajectory segments to ensure they pass through the given start and end points. Quintic polynomial curves have more parameters and better flexibility, but also result in a higher degree of curvature expression for points on the trajectory, making it difficult to control the maximum curvature of the trajectory.

[0004] Therefore, there is an urgent need in this field for a technical solution that can ensure the continuity of the trajectory and control the maximum curvature of the trajectory. Summary of the Invention

[0005] To address the problems in the prior art, this invention proposes an autonomous driving trajectory planning scheme. This scheme includes a control unit for autonomous vehicles and an autonomous driving trajectory planning method. The control unit is capable of generating an autonomous driving route for the autonomous vehicle to follow on the road, configured as follows:

[0006] The road is divided into n road segments, and the autonomous driving route includes n trajectory segments Γ located on the n road segments; and

[0007] Determine the trajectory Γ on the i-th road segment i Where 1≤i≤n, the trajectory Γ i It has set starting point information and target point information, wherein the starting point information includes the starting point heading angle θ. io , initial curvature κ io and the starting point coordinates x io y io The target point information includes the target point heading angle θ. if Target point curvature κ if and the target point coordinates x if y if ;

[0008] Among them, the trajectory Γ is determined. i include:

[0009] The trajectory Γ i Expressed as: θ i (s)=a i *s 5 +b i *s 4 +c i *s 3 +d i *s 2 +κ io *s+θ io , where θ i (s) is the trajectory Γ i The heading angle at a point s distance from the starting point;

[0010] Define the objective function T and the constraint function C. i ,in,

[0011]

[0012] Where, α i and β i It is a set coefficient, sf i It is the trajectory Γ i Total mileage, X i (sf i ), Yi (sf i ) is through θ i (s) The total distance sf from the starting point is calculated. i The coordinates of the endpoint, κ max It is the set maximum curvature, κ i (s) is θ i The derivative of (s), and the set {s′}={s′1,s′2,s′3}, where s′1,s′2,s′3 are such that κ i Mileage s for which the derivative of (s) is zero;

[0013] Set c i d i sf i The initial value of c is obtained by using the interior point method. i d i sf i Perform iterations to compute the results under constraint function C. j The c that minimizes the objective function T when ≤0 i d i sf i ;as well as

[0014] Based on the calculated c i d i sf i Calculate a i and b i And determine θ i (s).

[0015] Under the above configuration, the trajectory Γ on the i-th road segment generated by the control unit according to the present invention i The curvature at any point on it must not exceed the set maximum curvature κ. max Under these circumstances, the total mileage sf i And the deviation between the endpoint and the target point should be as small as possible.

[0016] According to an optional embodiment of the present invention, the trajectory Γ is determined. i Also includes:

[0017] Store s′4 and s′5 into the set {s′} such that {s′} = {s′1, s′2, s′3, s′4, s′5}, where s′4 = 0 and s′5 = sf. i The constraint function C j Set as:

[0018]

[0019] Under the above configuration, the trajectory Γ on the i-th road segment generated by the control unit according to the present inventioni Not only does the curvature at any point on it not exceed the set maximum curvature κ max Furthermore, the curvature at its starting and ending points does not exceed the set maximum curvature κ. max .

[0020] According to an optional embodiment of the present invention, the trajectory Γ is determined. i Also includes:

[0021] If set {s′} contains duplicate elements, then the set {s′} is deduplicated to generate set {s″}, which contains m elements, and the constraint function C is applied. j Set as:

[0022]

[0023] With the above configuration, duplicate calculations of repetitive elements in set {s′} can be avoided when performing the interior point method, thereby speeding up the generation of trajectory Γ. i Speed ​​and improve trajectory generation Γ i The success rate.

[0024] According to an optional embodiment of the present invention, the trajectory Γ is determined. i Also includes:

[0025] If the set {s″} contains both real and imaginary elements, then the constraint function C will be... j Set as:

[0026]

[0027] Here, set {r} is a set of real elements from set {s″} and includes u elements, and set {t} is a set of imaginary elements from set {s″} and includes v elements.

[0028] With the above configuration, the calculation of imaginary elements in set {s′} can be avoided when performing the interior point method, thereby speeding up the generation of trajectory Γ. i Speed ​​and improve trajectory generation Γ i The success rate.

[0029] According to an optional embodiment of the present invention, the trajectory Γ is determined. i Also includes:

[0030] Calculate the objective function T and the constraint function C. j Regarding c i d i sf i The partial derivatives, and based on the calculated partial derivatives, the interior point method is set for c.i d i sf i The direction of iteration.

[0031] With the above configuration, a directional search can be performed within the feasible region instead of a random search when executing the interior point method, thereby speeding up the generation of trajectory Γ. i Speed ​​and improve trajectory generation Γ i The success rate.

[0032] According to an optional embodiment of the present invention, the trajectory Γ is determined. i Also includes:

[0033] α i and β i Set as:

[0034] With the above configuration, the trajectory Γ can be considered in a relatively balanced way when performing the interior point method. i Total mileage sf i And the deviation between the endpoint and the target point, thus generating the trajectory Γ i Such that the curvature at any point on it does not exceed the set maximum curvature κ. max Under the condition of total mileage sf i And the deviation between the endpoint and the target point should be as small as possible.

[0035] According to an optional embodiment of the present invention, the trajectory Γ is determined. i Also includes:

[0036] c i d i sf i The initial values ​​are set as follows:

[0037]

[0038] Under the above configuration, parameter c i d i sf i The initial value of Γ is usually very close to its final value, which can speed up the generation of the trajectory Γ. i Speed ​​and improve trajectory generation Γ i The success rate.

[0039] According to an optional embodiment of the present invention, the control unit is configured to:

[0040] The trajectory Γ on each road segment is determined in the order from the first road segment to the nth road segment, where the trajectory Γ is determined. i It also includes: based on the determined θ i (s) Calculate the trajectory Γi The endpoint information includes the endpoint heading angle θ. i (sf i ), terminal curvature κ i (sf i and the endpoint coordinates X i (sf i ), Y i (sf i And the endpoint information is used as the next trajectory segment. i+1 The starting point information.

[0041] With the above configuration, the trajectory of each segment of the autonomous driving route can be generated segment by segment starting from the first trajectory Γ1. The autonomous driving route generated in this way will not only ensure that the curvature at any point does not exceed the set maximum curvature κ. max Moreover, it can increase the total mileage Minimize it as much as possible, and also make it consistent with the set target points (X). i (sf i ),Y i (sf i The deviations between i = 1, ..., n should be minimized, thereby enabling the tracking of each target point (X). i (sf i ),Y i (sf i The effect of i = 1, ..., n.

[0042] The autonomous driving trajectory planning method can generate an autonomous driving route for autonomous vehicles to follow on the road, and includes the following steps:

[0043] S100: Divide the road into n road segments, and the autonomous driving route includes n trajectory segments Γ located on the n road segments; and

[0044] S200: Determine the trajectory Γ on the i-th road segment. i Where 1≤i≤n, the trajectory Γ i It has set starting point information and target point information, wherein the starting point information includes the starting point heading angle θ. io , initial curvature κ io and the starting point coordinates x io y io The target point information includes the target point heading angle θ. if Target point curvature κ if and the target point coordinates x if y if ;

[0045] Step S200 includes the following sub-steps:

[0046] S210: The trajectory Γ i Expressed as: θ i (s)=a i *s 5 +b i *s 4 +c i *s 3 +d i *s 2 +κ io *s+θ io , where θ i (s) is the trajectory Γ i The heading angle at a point s distance from the starting point;

[0047] S220: Define the objective function T and constraint functions C. i ,in,

[0048]

[0049] Where, α i and β i It is a set coefficient, sf i It is the trajectory Γ i Total mileage, X i (sf i ), Y i (sf i ) is through θ i (s) The total distance sf from the starting point is calculated. i The coordinates of the endpoint, κ max It is the set maximum curvature, κ i (s) is θ i The derivative of (s), and the set {s′}={s′1,s′2,s′3}, where s′1,s′2,s′3 are such that κ i Mileage s for which the derivative of (s) is zero;

[0050] S230: Setting c i d i sf i The initial value of c is obtained by using the interior point method. i d i sf i Perform iterations to compute the results under constraint function C. j The c that minimizes the objective function T when ≤0 i d i sf i ;as well as

[0051] S240: Based on the calculated c i d isf i Calculate a i and b i And determine θ i (s);

[0052] Optionally, the method includes various features in the control unit as described above.

[0053] This autonomous driving trajectory planning scheme also includes a machine-readable storage medium storing executable instructions, which, when executed by a processor, implement the autonomous driving trajectory planning method as described above.

[0054] The present invention may be embodied in the illustrative embodiments shown in the accompanying drawings. However, it should be noted that the drawings are merely illustrative, and any variations contemplated under the teachings of this invention should be considered to be included within the scope of this invention. Attached Figure Description

[0055] The accompanying drawings illustrate exemplary embodiments of the invention. These drawings should not be construed as necessarily limiting the scope of the invention, wherein:

[0056] Figure 1 It is a schematic plan view of an autonomous driving route generated on a road using the autonomous driving trajectory planning scheme according to the present invention; and

[0057] Figures 2-7 This is a schematic flowchart of various optional embodiments of the autonomous driving trajectory planning scheme according to the present invention. Detailed Implementation

[0058] Further features and advantages of the invention will become more apparent from the following description with reference to the accompanying drawings. Exemplary embodiments of the invention are illustrated in the drawings, and the figures are not necessarily drawn to scale. However, the invention can be implemented in many different forms and should not be construed as necessarily limited to the exemplary embodiments shown herein. Rather, these exemplary embodiments are provided merely to illustrate the invention and to convey its spirit and essence to those skilled in the art.

[0059] This invention aims to propose an improved autonomous driving trajectory planning scheme. This scheme can plan a smooth trajectory from the starting point to the destination segment by segment for an autonomous vehicle, and then stitch the segments together to form an autonomous driving route designed for the autonomous vehicle to follow. According to the invention, the autonomous driving trajectory planning scheme can limit the curvature of each segment of the autonomous driving route at any point to below a preset maximum curvature. Because vehicles have kinematic constraints determined by their structure (e.g., the maximum turning angle that the steering wheels can present), the curvature of the trajectory that the vehicle can reliably follow is limited. That is, if the curvature of the trajectory is too large (graphically represented by excessively sharp turns), the vehicle cannot reliably follow the trajectory and will deviate from it. Therefore, by reasonably setting the aforementioned maximum curvature, it can be ensured that the autonomous vehicle can reliably and stably follow the trajectory in each segment. Specifically, the autonomous driving trajectory planning scheme according to the present invention can also limit the curvature of each trajectory segment at the splicing point to below a preset maximum curvature. By reasonably setting this maximum curvature, it can be ensured that the splicing point also satisfies the maximum curvature constraint. Therefore, this method can be extended to splicing multiple trajectory segments, and the autonomous driving route formed by splicing the trajectories still satisfies the maximum curvature constraint. More specifically, the autonomous driving trajectory planning scheme according to the present invention can also ensure the continuity of parameters such as heading angle, curvature, and coordinate position of each trajectory segment at the splicing point. This can prevent the changes in parameters such as heading angle, curvature, and coordinate position of adjacent trajectories at the splicing point from exceeding the vehicle kinematic constraints of the autonomous driving vehicle (e.g., the fastest response speed of the vehicle's power system). Moreover, it can splice each trajectory segment into an autonomous driving route with continuous parameters such as heading angle, curvature, and coordinate position, thereby enabling the autonomous driving vehicle to stably track the entire autonomous driving route so as to smoothly drive from the starting point to the destination.

[0060] The following will describe in detail, with reference to the accompanying drawings, optional but non-limiting embodiments of the autonomous driving trajectory planning scheme according to the present invention. It should be noted that although the following description uses an autonomous driving route on road 100 as an example to illustrate the implementation of the autonomous driving trajectory planning scheme according to the present invention, this is intended to more intuitively explain the teachings of the present invention. Those skilled in the art will understand that the autonomous driving trajectory planning scheme according to the present invention can obviously also be used to plan autonomous driving routes on roads of various types and shapes; that is, the specific road on which this autonomous driving trajectory planning scheme is applied should not constitute a limitation on the scope of protection of the present invention.

[0061] refer to Figure 1 The diagram shows a schematic plan view of a road where an autonomous driving trajectory planning scheme according to the present invention is applied. Figure 1As shown, road 100 is defined by road boundaries 101 and 102, formed by, for example, road barriers, curbs, or marked by directional lines. Specifically, road boundaries 101 and 102 are spaced apart from each other along the road width W, thus forming a road 100 for vehicles to travel between them. Road 100 extends from its starting point Po to its ending point Pf along the road length L and has a road centerline 103 that divides road 100 into two halves in the direction of road width W. That is, the road centerline 103 is formed by connecting the midpoints of road 100 in the direction of road width W. It should be noted that the road width W is not necessarily constant. For example, when there is an obstacle 104 on road 100, the road width W will decrease at obstacle 104. Therefore, road width W can also be understood as the passage width of road 100 that allows vehicles to pass. The obstacle 104 may be, for example, a vehicle in front that will be overtaken by this autonomous vehicle. Of course, road 100 does not necessarily extend straight along the road length L. Figure 1 In the case shown, road 100 tilts to the right as it extends toward the endpoint Pf.

[0062] The following is for reference. Figures 1-7 The description describes the generation of an autonomous driving route Γ from the starting point Po to the ending point Pf of road 100 using the autonomous driving trajectory planning scheme according to the present invention, wherein... Figures 2-7 A schematic flowchart of an autonomous driving trajectory planning scheme according to the present invention is shown. It is known that if the road length L of road 100 is long and / or the shape of road 100 is complex, directly generating the entire autonomous driving route Γ, or in other words, the autonomous driving route Γ consisting of only one trajectory segment, is impractical because it requires a huge amount of computation, and the probability of successfully generating the autonomous driving route Γ is very low. Therefore, in this case, it is usually necessary to first divide road 100 into multiple road segments, then generate a trajectory segment for each road segment, and finally stitch the trajectories together to form the autonomous driving route Γ. That is, the autonomous driving route Γ can be composed of trajectories from each road segment. Of course, if the road length L of road 100 is short and / or the shape of road 100 is simple, then the entire autonomous driving route Γ can also be generated directly, i.e., the autonomous driving route Γ consists of only one trajectory segment.

[0063] like Figure 1As shown, multiple points are set at certain intervals on the road centerline 103. Multiple dividing lines d1 to dn, extending through these points and perpendicular to the road centerline 103, divide the road 100 into n road segments: road segment 1, ..., road segment i, ..., and road segment n. Specifically, these points can be set at constant intervals so that the length of the road centerline 103 between any two adjacent points is equal. Alternatively, these points can be set at variable intervals; for example, larger intervals can be used at locations far from the obstacle 104, and smaller intervals at locations closer to the obstacle 104. This configuration is advantageous because the autonomous driving route Γ is typically more complex near the obstacle 104. Therefore, by more densely segmenting near the obstacle 104, more complex parts of the autonomous driving route Γ can be formed by combining more simple trajectories, which helps improve the speed and success rate of trajectory generation.

[0064] like Figure 1 As shown, the autonomous driving route Γ includes n trajectories located in n road segments, namely, trajectory Γ1, ..., trajectory Γ2. i , ..., and trajectory Γ n In other words, the autonomous driving route Γ is formed by stitching together these n trajectory segments end to end.

[0065] The following describes the generation of trajectory Γ using the autonomous driving trajectory planning scheme according to the present invention. i The principle. The trajectory Γ i Given a starting point io and a target point if, and with given starting point information (i.e., the starting point heading angle θ). io , initial curvature κ io Starting point x-coordinate io and the y-coordinate of the starting point io ) and the given target point information (i.e., the target point heading angle θ) if Target point curvature κ if x-coordinate of the target point if and the target point's y-coordinate if The trajectory Γ is represented by a quintic helix. i Expressed as:

[0066] The equation for the heading angle is: θ i (s)=a i *s 5 +b i *s 4 +c i *s 3 +d i *s 2 +e i *s+f i

[0067] Where, θ i (s) is the trajectory Γ i The heading angle at a point s distance from the starting point io, a i b i c i d i e i f i For trajectory Γ i The specific coefficients. Therefore, the autonomous driving trajectory planning scheme according to the present invention aims to control the heading angle of the autonomous vehicle at a point mileage s away from the starting point io according to the above-mentioned heading angle equation. Compared with the scheme of controlling the vehicle's lateral and longitudinal coordinates, the autonomous driving trajectory planning scheme according to the present invention can more directly characterize and plan the heading angle, curvature, and rate of change of curvature at each point on the trajectory through the above-mentioned heading angle equation, which makes it possible to control the maximum curvature at each point on the trajectory, and thus enables the autonomous vehicle to reliably track the trajectory.

[0068] Additionally, the trajectory Γ i The x-coordinate of the point s that is mileage s away from the starting point io. i (s) and ordinate Y i (s) can be obtained through the heading angle θ i (s) is obtained or converted to:

[0069] Coordinate equations:

[0070] Since curvature is defined as the rate of rotation of the tangent angle at a point on a curve with respect to the arc length, and the above heading angle equation characterizes the heading angle θ... i The relationship between the mileage s and the heading angle θ is determined by solving for the mileage s. i The derivative with respect to mileage s, i.e., taking the derivative of the above heading angle equation, yields:

[0071] Curvature equation: κ i (s)=5*a i *s 4 +4*b i *s 3 +3*c i *s 2 +2*d i *s+e i

[0072] Among them, κ i (s) trajectory Γ i The curvature at a point mileage s away from the starting point i0.

[0073] Therefore, if the coefficient a can be calculated i bi c i d i e i f i Then the equations for the heading angle, coordinates, and curvature can all be determined, thus making the trajectory Γ... i It can be determined. Therefore, the trajectory Γ i Given the starting point information (i.e., the starting point heading angle θ) io , initial curvature κ io Starting point x-coordinate io Starting point y-coordinate io Substituting the heading angle equation and curvature equation, these starting point information correspond to the time when the mileage s is zero, thus we can derive:

[0074] f i =θ io

[0075] e i =κ io

[0076] Furthermore, assuming the trajectory Γ i The total mileage is sf i And the given target point information (i.e., the target point heading angle θ) if Target point curvature κ if x-coordinate of the target point if and the target point's y-coordinate if Substituting further into the heading angle equation and curvature equation, this target point information and the mileage s are sf i Corresponding to the time, we can conclude that:

[0077]

[0078]

[0079] Based on the above conversion, a i b i c i d i e i f i The five coefficients were reduced to c i d i sf i Three coefficients. If we can solve for c... i d i sf i With three coefficients, the equations for the heading angle, coordinates, and curvature can all be determined, thus enabling the trajectory Γ to be determined. i It can be determined.

[0080] To calculate c i d i sf i Three coefficients are used as variables to solve for the objective function using the interior-point method. The interior-point method is a known optimization algorithm that optimizes the objective function within the feasible region constructed by the constraint functions. For the sake of brevity, this paper only describes how to establish the objective function and constraint functions to solve for the variables using the interior-point method, without elaborating on the specific principles of the interior-point method.

[0081] The endpoint coordinates (X) are calculated using coordinate equations. i (sf i ), Y i (sf i The target point coordinates (x) in the given target point information are compared with those of the target point. if y if The deviation between ) and the trajectory Γ i Total mileage sf i Establish the following objective function T(c) for the parameters. i ,d i ,sf i The objective function T(c) i ,d i ,sf i ) with c i d i sf i For variables.

[0082] Objective function:

[0083] Where, α i With β i Given a scaling factor, and the objective function T(c i ,d i ,sf i The first item It reflects the trajectory Γ i Total mileage sf i The size of the second item, and the second item This reflects the calculated endpoint coordinates (X). i (sf i ), Y i (sf i )) and the given target point coordinates (x if y if The magnitude of the deviation between ) is determined by adjusting the coefficient α of the first term. i The coefficient β of the second term i The weights of the first and second terms can be adjusted. For example, if the coefficient α of the first term... i The coefficient β of the second term is relatively large.i If the value is relatively small, then the objective function T(c) i ,d i ,sf i It will place more emphasis on reflecting the trajectory. i Total mileage sf i Conversely, if the coefficient α of the first term... i The coefficient β of the second term is smaller. i If the objective function T(c) is large, then... i ,d i ,sf i It will place more emphasis on reflecting the calculated endpoint coordinates (X). i (sf i ), Y i (sf i )) and the given target point coordinates (x if y if The deviation between α and β. Of course, α can also be... i With β i One of them is set to zero so that the objective function only reflects the trajectory Γ. i Total mileage sf i Or the aforementioned deviation between the endpoint and the target point.

[0084] Specifically, α can be i With β i Set them as follows:

[0085]

[0086] The above settings take into account the numerical difference between the first and second terms of the objective function, thus allowing the objective function to reflect the trajectory Γ in a roughly balanced (or equally weighted) manner. i Total mileage sf i And the aforementioned deviation between the endpoint and the target point.

[0087] As mentioned above, the autonomous driving trajectory planning scheme according to the present invention constrains the maximum curvature of the trajectory to allow the autonomous vehicle to stably track the trajectory. Therefore, with trajectory Γ i The maximum curvature value (the largest absolute value of curvature at each point) does not exceed the given maximum curvature κ. max Construct constraint functions for the conditions. Since the trajectory Γ i The curvature extrema will appear in the curvature derivative κ′ i For points where (s) is zero, therefore, differentiating the above curvature equation yields the curvature derivative equation:

[0088] Curvature derivative equation: κ′ i (s)=20*a i *s 3+12*b i *s 2 +6*c i *s+2*d i

[0089] The extreme points where the derivative of curvature is zero can be found using known methods such as Cardan's formula, Lagrange's method, trigonometric methods, and Shengjin's formula, i.e., where κ′ is the point where the derivative of curvature is zero. i The root of (s) = 0. Because κ′ i (s) = 0 is a cubic equation in one variable, therefore three roots can be calculated. These three roots are denoted as s′1, s′2, and s′3, and stored in a set {s′}, i.e., {s′} = {s′1, s′2, s′3}. Therefore, the trajectory Γ... i The curvature extrema will occur at three points where the mileage s equals the three roots. Based on this, a constraint function C is constructed. j (c i ,d i ,sf i The constraint function C j (c i ,d i ,sf i Also with c i d i sf i For variables.

[0090] Constraint functions:

[0091] After establishing the objective function and constraint functions described above, we set c... i d i sf i The initial value of the objective function T(c) is given, and the objective function T(c) is set to the target function T(c i ,d i ,sf i The minimization of ) is set as an optimization problem using the interior point method, and the constraint function C is defined. j (c i ,d i ,sf i )≤0 is set as the constraint for the optimization problem of the interior point method. The interior point method is used to optimize c. i d i sf i Perform iterations to compute c i d i sf i In this configuration, the interior-point method will start from c. i d i sf i The iteration begins with the initial value of C, and is performed by C. j (c i ,di ,sf i The convergence of T(c) within the feasible region defined by ≤0 to a value that makes T(c) converge. i ,d i ,sf i Minimize c i d i sf i Using the coefficient α in the objective function i =1 and β i =(x io -x if ) 2 +(y io -y if ) 2 For example, c is calculated using the interior point method. i d i sf i This will make the trajectory Γ i Total mileage sf i and the calculated endpoint coordinates (X) i (sf i ), Y i (sf i )) and the given target point coordinates (x if y if The deviations between them are minimized as much as possible, and the trajectory Γ is made smaller. i The curvature extrema on the curve do not exceed the given maximum curvature κ. max This ensures the trajectory Γ i The absolute value of the curvature at any point on the curve does not exceed the given maximum curvature κ. max With this configuration, autonomous vehicles can reliably and stably track the trajectory. i So that starting from the starting point io, the total distance sf is minimized. i Reach the given target point (x) if y if The deviation between the endpoints (X) should be minimized. i (sf i ), Y i (sf i )).

[0092] Specifically, c can be i d i sf i The initial values ​​are set as follows:

[0093]

[0094] If the trajectory Γ i If the shape is not too complex, then the above initial value will be relatively close to c. i di sf i The final value after iteration; therefore, this configuration helps reduce computational load, speed up computation, and improve the success rate of computation.

[0095] Specifically, if there are duplicate roots in the set {s′}, only one of the duplicate roots is retained in the set {s′} and the extra duplicate roots are deleted so that there are no duplicate roots in the set {s′}.

[0096] Under this configuration, even κ′ i The roots s′1, s′2, and s′3 of (s) = 0 contain duplicate roots, and the interior point method will not perform duplicate calculations on duplicate roots. This helps to reduce the amount of computation, speed up the calculation, and improve the success rate of the calculation.

[0097] Specifically, if imaginary roots exist in the set {s′}, then all imaginary roots are deleted so that the set {s′} contains only real roots, and the constraint function C is... i (c i ,d i ,sf i Limited to:

[0098]

[0099] In this configuration, if κ′ i If there exists an imaginary root among the roots s′1, s′2, and s′3 of (s) = 0, then the objective function C targeted by this imaginary root... j (c i ,d i ,sf i ) will be directly set to and It must be less than or equal to zero, therefore this configuration no longer considers the trajectory Γ. i The curvature at the point where the mileage s equals the imaginary root is because the point where the mileage s equals the imaginary root only exists in calculation and has no actual physical meaning. In other words, the point where the mileage s equals the imaginary root will not actually appear on the trajectory Γ. i Therefore, the above configuration helps to further reduce the amount of computation, speed up the computation, and improve the success rate of the computation. It is worth mentioning that the reason for setting the objective function C to which the imaginary root is located is... j (c i ,d i ,sf i Set directly to Instead of setting it to other negative constants, this is to ensure that it corresponds to the objective function C targeted by the real roots. j (c i ,d i ,sfi In interior point operations, they have relatively consistent weights to avoid adverse effects on the operations.

[0100] Specifically, s′4=0 and s′5=sf i Two elements are stored in the set {s′} such that {s′} = {s′1, s′2, s′3, s′4, s′5}.

[0101] Under this configuration, trajectory Γ i The curvature at the starting and ending points is also taken into account; that is, the curvature c calculated by the interior point method is... i d i sf i Not only can it ensure the trajectory Γ i The absolute value of curvature at any intermediate point does not exceed the given maximum curvature κ. max Moreover, it can ensure the trajectory Γ i The absolute values ​​of curvature at the starting and ending points do not exceed the given maximum curvature κ. max This further ensures that autonomous vehicles can reliably and stably track their trajectories. i .

[0102] Specifically, the objective function T(c) can be solved. i ,d i ,sf i Regarding variable c i d i sf i The gradient, i.e., and constraint function C j (c i ,d i ,sf i Regarding variable c i d i sf i The gradient, i.e., And based on the gradient setting interior point method, c i d i sf i The iteration direction. Under this configuration, the objective function T(c i ,d i ,sf i and constraint function C j (c i ,d i ,sf i Regarding variable c i d i sf i The gradient can help determine c i d isf i The iterative direction can significantly accelerate the computation speed and improve the success rate of the computation.

[0103] The above describes the trajectory generation scheme of the autonomous driving trajectory planning according to the present invention. i Based on these principles, the autonomous driving trajectory planning scheme according to the present invention generates an autonomous driving route for autonomous vehicles to follow on road 100, including the following processes: Figures 2-7 As shown:

[0104] S100: Divide road 100 into n road segments, and the autonomous driving route includes n trajectory segments Γ located on the n road segments;

[0105] S200: Determine the trajectory Γ on the i-th road segment. i Where 1≤i≤n, the trajectory Γ i It has a set starting point io and associated starting point information, and a set target point if and associated target point information, wherein the starting point information includes the starting point heading angle θ. io , initial curvature κ io Starting point x-coordinate io and the y-coordinate of the starting point io The target point information includes the target point heading angle θ. if Target point curvature κ if x-coordinate of the target point if and the target point's y-coordinate if .

[0106] Step S200 includes the following sub-steps:

[0107] S210: The trajectory Γ i Expressed in the form of a quintic helix: θ i (s)=a i *s 5 +b i *s 4 +c i *s 3 +d i *s 2 +κ io *s+θ io , where θ i (s) is the trajectory Γ i The heading angle at a point mile s away from the starting point io;

[0108] S220: Define the objective function T and constraint functions C. i ,in,

[0109]

[0110] Where, α i and β i It is for the trajectory Γ i The set coefficient, sf i It is the trajectory Γ i Total mileage, X i (sf i ) and Y i (sf i ) is through θ i (s) The total distance sf from the starting point io is calculated. i The endpoint (trajectory Γ) i The coordinates of the endpoint, κ max It is the maximum curvature set according to the kinematic constraints of autonomous vehicles, κ i (s) is θ i The derivative of (s) thus represents the trajectory Γ i Let κ be the curvature at a point s away from the starting point, and let {s′} = {s′1, s′2, s′3}, where s′1, s′2, s′3 are such that κ i The mileage s for which the derivative of (s) is zero. As mentioned earlier, the heading angle θ i (s) can be converted into coordinates (X). i (s), Y i (s)), and after substituting the starting point information and the target point information, we can obtain:

[0111]

[0112] Based on the above system of equations, the objective function T and constraint function C are established in sub-step S220. jj It can be expressed as c i d i sf i A function with three coefficients as variables.

[0113] S230: Setting c i d i sf i The initial value of c is obtained by using the interior point method. i d i sf i Perform iterations to compute c i d i sf i The optimization problem is to minimize the objective function T and to use the constraint function C. j ≤0 is a constraint. As mentioned earlier, c is calculated under the above configuration. i d i sfi In constraint function C j To minimize the objective function T when ≤0, i.e., on the trajectory Γ i The absolute value of the curvature at any point on the curve is lower than the maximum curvature κ. max In the case of Γ, make the trajectory Γ i Total mileage sf i and trajectory Γ i The endpoint (X) i (sf i ), Y i (sf i )) and the given target point (x) if y if The deviations between them should be as small as possible.

[0114] S240: Based on the calculated c i d i sf i Calculate a i and b i And determine θ i (s). Specifically, based on c calculated in sub-step S230 i d i sf i In addition to the starting point information and the target point information, a can be calculated using the system of equations involved in sub-step S220. i and b i Furthermore, the heading angle θ can be determined. i The functional relationship between (s) and mileage s, that is, the heading angle θ i (s) is expressed as a quintic function of the distance s in one variable. Based on this determined functional relationship, the trajectory Γ can be plotted. i Furthermore, the heading angle θ of the autonomous vehicle at mileage S can be controlled. i (s) to enable the autonomous vehicle to follow the trajectory. i As mentioned above, the trajectory Γ i The absolute value of the curvature at any point on the curve is lower than the maximum curvature κ. max Therefore, autonomous vehicles can reliably track trajectories. i With this configuration, autonomous vehicles can reliably and stably track the trajectory. i So that starting from the starting point io, the total distance sf is minimized. i Reach the given target point (x) if y if The deviation between the endpoints (X) should be minimized. i (sf i ), Y i (sf i)).

[0115] In particular, such as Figure 2 As shown, step S200 includes sub-step S221 after sub-step S220: store s′4 and s′5 into the set {s′} such that {s′} = {s′1, s′2, s′3, s′4, s′5}, where s′4 = 0 and s′5 = sf i The constraint function C j Further specified as: Under this configuration, not only the trajectory Γ i The absolute value of the curvature at each intermediate point is constrained to the maximum curvature κ. max Below, and trajectory Γ i The absolute values ​​of the curvature at the starting point io and the ending point are also limited to the maximum curvature κ. max This further ensures that autonomous vehicles can reliably track their trajectories. i .

[0116] In particular, such as Figure 3 As shown, step S200 includes sub-step S222 after sub-step S221: If there are identical elements in set {s′}, then the set {s′} is deduplicated to generate set {s″}, which contains m elements, and the constraint function C is applied. j Further specified as: In this configuration, since redundant duplicate elements are removed, there are no duplicate elements in the set {s″}. Therefore, duplicate elements can be avoided in the interior point method, which can speed up the iteration and improve the convergence success rate.

[0117] In particular, such as Figure 4 As shown, step S200 includes sub-step S223 after sub-step S222: If the set {s″} contains both real and imaginary elements, then the constraint function C... j Further specified as:

[0118]

[0119] Here, {r} is the set of real elements in {s″}, including u elements, and {t} is the set of imaginary elements in {s″}, including v elements. Under this configuration, it is equivalent to no longer considering curvature κ. i (s) The curvature at the imaginary mileage s where the extreme value appears is because the imaginary mileage does not actually appear in the actual trajectory and does not have actual physical meaning. This can also speed up the iteration speed and improve the convergence success rate.

[0120] In particular, such as Figure 5 As shown, step S200 includes sub-step S224 after sub-step S220: calculating the objective function T and the constraint function C. j Regarding c i d i sf i The partial derivative of, i.e., Furthermore, sub-step S230 also involves: setting the interior point method for c based on the calculated partial derivatives. i d i sf i The iteration direction is determined by the direction of the iteration. Compared to randomly searching for variable values ​​within the feasible region, the above configuration can accelerate the iteration speed and improve the convergence success rate by setting the iteration direction.

[0121] In particular, sub-step S220 also involves α i and β i Set as:

[0122]

[0123] Under this configuration, minimizing the objective function T can uniformly represent the trajectory Γ. i Total mileage sf i Minimize the endpoint and the deviation between the endpoint and the target point.

[0124] Specifically, sub-step S230 also involves c i d i sf i The initial values ​​are set as follows:

[0125]

[0126] Under this configuration, c i d i sf i The initial values ​​are close to their final values, which can speed up the iteration and improve the convergence success rate.

[0127] In particular, such as Figure 6 As shown, the autonomous driving trajectory planning scheme according to the present invention further comprises: executing step S200 for each of the n road segments until the n segments of the autonomous driving route are determined. i ,i=1,…,n.

[0128] More specifically, the autonomous driving trajectory planning scheme according to the present invention further comprises: executing step S200 in the order from the first road segment to the nth road segment, and step S200 including sub-step S250 after sub-step S240: according to the determined θ i(s) Calculate the trajectory Γ i The total distance sf from the starting point io i The endpoint information includes the endpoint heading angle θ. i (sf i ), terminal curvature κ i (sf i ), endpoint x-coordinate i (sf i ) and the endpoint ordinate Y i (sf i And the endpoint information is used as the next trajectory segment. i+1 The starting point information. Under this configuration, the previous trajectory Γ i The endpoint information will be used as the next trajectory segment. i+1 The endpoint information is then applied to the next segment of the trajectory. i+1 In the calculation, this ensures that each trajectory Γ i The heading angle, curvature, coordinates, etc., are all continuous, which makes it possible to stitch together each trajectory segment Γ i It can generate autonomous driving routes with continuous heading angle, curvature, coordinates, etc., and the absolute value of curvature at each point is below the maximum curvature. Autonomous vehicles can reliably follow the autonomous driving route and travel through road 100.

[0129] In particular, such as Figure 1 As shown, the road centerline 103 intersects with the dividing lines d1 to dn of each road segment. Therefore, the trajectory Γ of each segment can be set based on the information (e.g., heading angle, curvature, coordinates, etc.) at the points where the road centerline 103 intersects with each dividing line. i The starting point information and / or target point information. More specifically, the starting point information of the first trajectory Γ1 can be determined by the actual situation when the autonomous vehicle enters road 100, and the starting point information of the second trajectory Γ2 to the nth trajectory Γ... n The starting point information of each trajectory is determined by the ending point information of the preceding trajectory segment, while the target point information of all trajectories is determined by the information at the points where the road centerline 103 intersects with each dividing line. For example, the target point information of the first trajectory Γ1 is determined by the information at the point where the road centerline 103 intersects with the first dividing line d1. Under this configuration, the generated autonomous driving route will follow the road centerline 103 while satisfying the aforementioned curvature requirements, allowing the autonomous vehicle to travel along the road centerline 103 as close as possible to the center of road 100. Figure 1 One advantage of the present invention is described using the special case of the presence of obstacle 104 as an example. Due to the presence of obstacle 104, the road width W suddenly decreases, causing the road centerline 103 to jump towards the side away from obstacle 104. However, as with trajectory Γ1 and trajectory Γ 1+1As shown, due to the limitation of maximum curvature, the autonomous driving route does not jump with the road centerline 103, but gradually tracks the road centerline 103 with a controlled curvature. This allows the autonomous vehicle to reliably track the autonomous driving route and cross the obstacle 104. Of course, this is only one of the advantages of the present invention. In practical applications, the present invention can generate more advantages through its technical solution.

[0130] The autonomous driving trajectory planning scheme of the present invention includes a corresponding control unit and an autonomous driving trajectory planning method. The features described above apply to the control unit and autonomous driving trajectory planning method of this application. The autonomous driving trajectory planning scheme of the present invention also includes a machine-readable storage medium storing executable instructions, which, when executed by a processor, implement the autonomous driving trajectory planning method of this application.

[0131] The optional but non-limiting embodiments of the autonomous driving trajectory planning scheme according to the present invention have been described in detail above with reference to the accompanying drawings. For those skilled in the art, modifications and additions to the technology and structure, as well as recombinations of features in the various embodiments, should obviously be considered within the scope of the present invention without departing from the spirit and substance of this disclosure. Therefore, such modifications and additions conceivable under the teachings of this invention should be considered part of this invention. The scope of this invention includes equivalent technologies known at the time of filing and equivalent technologies not yet foreseen.

Claims

1. A control unit for an autonomous vehicle, the control unit being capable of generating an autonomous driving route for the autonomous vehicle to follow on a road, configured to: The road is divided into n road segments, and the autonomous driving route includes n trajectory segments Γ located on the n road segments; as well as Determine the trajectory Γi on the i-th road segment, where, 1≤i≤n, the trajectory Γi has set starting point information and target point information. The starting point information includes the starting point heading angle θio, the starting point curvature κio, and the starting point coordinates xio and yio. The target point information includes the target point heading angle θif, the target point curvature κif, and the target point coordinates xif and yif. Determining the trajectory Γi includes: The trajectory Γi is expressed as: θi(s)=ai*s5+bi*s4+ci*s3+di*s2+κio*s+θio, where θ(s) is the heading angle of a point on trajectory Γi that is mileage s away from the starting point; Define the objective function T and the constraint function Cj, where, ， Where αi and βi are set coefficients, sfi is the total mileage of trajectory Γi, Xi(sfi) and Yi(sfi) are the coordinates of the endpoints that are a total distance sfi from the starting point, calculated by θi(s), κmax is the set maximum curvature, κi(s) is the derivative of θi(s), and the set {s′} = {s′1, s′2, s′3}, s′1, s′2, s′3 are the mileages s that make the derivative of κi(s) zero; Initialize ci, di, and sfi, and iterate through ci, di, and sfi using the interior-point method to calculate ci, di, and sfi that minimize the objective function T while constraining the function Cj≤0; and Calculate ai and bi based on the calculated ci, di, and sfi, and determine θi(s).

2. The control unit according to claim 1, wherein, Determining the trajectory Γi also includes: Store s′4 and s′5 in the set {s′} such that {s′} = {s′1, s′2, s′3, s′4, s′5}, where s′4 = 0 and s′5 = sfi. Set the constraint function Cj as follows:

3. The control unit according to claim 2, wherein, Determining the trajectory Γi also includes: If there are duplicate elements in set {s′}, then the set {s′} is deduplicated to generate set {s″}, which contains m elements, and the constraint function Cj is set as follows:

4. The control unit according to claim 3, wherein, Determining the trajectory Γi also includes: If the set {s″} contains both real and imaginary elements, then the constraint function Cj is set as follows: ， Here, set {r} is a set of real elements from set {s″} and includes u elements, and set {t} is a set of imaginary elements from set {s″} and includes v elements.

5. The control unit according to any one of claims 1 to 4, wherein, Determining the trajectory Γi also includes: Calculate the partial derivatives of the objective function T and the constraint function Cj with respect to ci, di, and sfi, and set the iteration direction of the interior point method for ci, di, and sfi based on the calculated partial derivatives.

6. The control unit according to claim 1, wherein, Determining the trajectory Γi also includes: Set αi and βi as 7. The control unit according to claim 1, wherein, Determining the trajectory Γi also includes: setting the initial values ​​of ci, di, and sfi as follows:

8. The control unit according to claim 1, wherein, The control unit is configured to: The trajectory Γ on each road segment is determined in the order from the first road segment to the nth road segment. The determination of trajectory Γi also includes: calculating the endpoint information of trajectory Γi at the endpoint based on the determined θi(s). The endpoint information includes the endpoint heading angle θi(sfi), the endpoint curvature κi(sfi), and the endpoint coordinates Xi(sfi) and Yi(sfi). The endpoint information is used as the starting point information of the next trajectory Γi+1.

9. An autonomous driving trajectory planning method, which generates an autonomous driving route for autonomous vehicles to follow on roads, and includes the following steps: S100: Divide the road into n road segments, and the autonomous driving route includes n trajectory segments Γ located on the n road segments; and S200: Determine the trajectory Γi on the i-th road segment, where, 1≤i≤n, the trajectory Γi has set starting point information and target point information. The starting point information includes the starting point heading angle θio, the starting point curvature κio, and the starting point coordinates xio and yio. The target point information includes the target point heading angle θif, the target point curvature κif, and the target point coordinates xif and yif. Step S200 includes the following sub-steps: S210: Express the trajectory Γi as: θi(s)=ai*s5+bi*s4+ci*s3+di*s2+κio*s+θio, where θi(s) is the heading angle at a point on trajectory Γi that is mileage s away from the starting point; S220: Define the objective function T and the constraint function Cj, where... ， Where αi and βi are set coefficients, sfi is the total mileage of trajectory Γi, Xi(sfi) and Yi(sfi) are the coordinates of the endpoints that are a total distance sfi from the starting point, calculated by θi(s), κmax is the set maximum curvature, κi(s) is the derivative of θi(s), and the set {s′} = {s′1, s′2, s′3}, s′1, s′2, s′3 are the mileages s that make the derivative of κi(s) zero; S230: Set initial values ​​for ci, di, and sfi, and iterate through ci, di, and sfi using the interior point method to calculate ci, di, and sfi that minimize the objective function T under the constraint function Cj≤0; and S240: Calculate ai and bi based on the calculated ci, di, and sfi, and determine θi(s); The method includes various features in the control unit according to any one of claims 1 to 8.

10. A machine-readable storage medium storing executable instructions that, when executed by a processor, implement the autonomous driving trajectory planning method according to claim 9.

Images