Non-linear reference line optimization method using piecewise quintic polynomial spiral paths for operating autonomous driving vehicles
A first reference line representing a trajectory from a first location to a second location associated with an autonomous driving vehicle (ADV) is received. The first reference line is segmented into a number of reference line segments. For each of the reference line segments, a quintic polynomial function is defined to represent the reference line segment. An objective function is determined based on the quintic polynomial functions of the reference line segments. An optimization is performed on coefficients of the quintic polynomial functions in view of a set of constraints associated with the reference line segments, such that an output of the objective function reaches minimum while the constraints are satisfied. A second reference line is then generated based on the optimized parameters or coefficients of the quintic polynomial functions of the objective function. The second reference line is then utilized to plan and control the ADV.
1. A computer-implemented method operating an autonomous driving vehicle, the method comprising:
in response to a first reference line representing a route from a first location to a second location associated with an autonomous driving vehicle (ADV), segmenting the first reference line into a plurality of reference line segments;
for each of the reference line segments, determining a quintic polynomial function to represent the reference line segment;
determining an objective function based on the quintic polynomial functions of the reference line segments;
performing an optimization on parameters of the quintic polynomial functions in view of a set of constraints associated with the reference line segments, such that an output of the objective function reaches minimum while the set of constraints are satisfied; and
generating a second reference line based on the optimized parameters of the quintic polynomial functions of the objective function, wherein the second reference line is utilized as a reference line of the route to control the ADV.
2. The method of claim 1 , wherein the objective function represents a sum of at least a portion of derivatives of the quintic polynomial functions of the reference line segments.
3. The method of claim 1 , wherein coefficients of a quintic polynomial function of each reference line segment are determined based on a location of the ADV, a direction of the ADV, curvature of the ADV, and curvature change rate of the ADV associated with the reference line segment of the first reference line.
4. The method of claim 3 , wherein performing an optimization on the parameters of the quintic polynomial functions comprises optimizing coefficients of each quintic polynomial function, such that an output of the quintic polynomial function at a starting point of the reference line segment is similar to a direction of the ADV at the starting point of the reference line segment and an output of the quintic polynomial function at an ending point of the reference line segment is similar to a direction of the ADV at a starting point of a next reference line segment.
5. The method of claim 4 , wherein the set of constraints further comprises a condition in which a first order derivative of the quintic polynomial function at the starting point of the reference line segment is similar to a curvature at the starting point of the reference line segment, and wherein a first order derivative of the quintic polynomial function at the ending point of the reference line segment is similar to a curvature at the starting point of the next reference line segment.
6. The method of claim 4 , wherein the set of constraints further comprises a condition in which a second order derivative of the quintic polynomial function at the starting point of the reference line segment is similar to a curvature change rate at the starting point of the reference line segment, and wherein a second order derivative of the quintic polynomial function at the ending point of the reference line segment is similar to a curvature change rate at the starting point of the next reference line segment.
7. The method of claim 4 , wherein the set of constraints comprises a condition in which a difference between a location of the ADV on the first reference line and a corresponding location of the ADV derived from the quintic polynomial function is below a predetermined threshold.
8. The method of claim 1 , wherein the optimization is iteratively performed on the objective function until a difference between an output of the objective function of a current iteration and an output of the objective function of a previous iteration is below a predetermined threshold.
9. The method of claim 1 , wherein performing an optimization comprises:
segmenting a first reference line segment of the plurality of reference line segments into a plurality of sub-segments;
for each of the second sub-segments, performing a second optimization using a first quintic polynomial function associated with the first reference line segment; and
summing outputs of the first quintic polynomial functions corresponding to the sub-segments to represent the first reference line segment.
10. A non-transitory machine-readable medium having instructions stored therein, which when executed by a processor, cause the processor to perform operations, the operations comprising:
in response to a first reference line representing a route from a first location to a second location associated with an autonomous driving vehicle (ADV), segmenting the first reference line into a plurality of reference line segments;
for each of the reference line segments, determining a quintic polynomial function to represent the reference line segment;
determining an objective function based on the quintic polynomial functions of the reference line segments;
performing an optimization on parameters of the quintic polynomial functions in view of a set of constraints associated with the reference line segments, such that an output of the objective function reaches minimum while the set of constraints are satisfied; and
generating a second reference line based on the optimized parameters of the quintic polynomial functions of the objective function, wherein the second reference line is utilized as a reference line of the route to control the ADV.
11. The machine-readable medium of claim 10 , wherein the objective function represents a sum of at least a portion of derivatives of the quintic polynomial functions of the reference line segments.
12. The machine-readable medium of claim 10 , wherein coefficients of a quintic polynomial function of each reference line segment are determined based on a location of the ADV, a direction of the ADV, curvature of the ADV, and curvature change rate of the ADV associated with the reference line segment of the first reference line.
13. The machine-readable medium of claim 12 , wherein performing an optimization on the parameters of the quintic polynomial functions comprises optimizing coefficients of each quintic polynomial function, such that an output of the quintic polynomial function at a starting point of the reference line segment is similar to a direction of the ADV at the starting point of the reference line segment and an output of the quintic polynomial function at an ending point of the reference line segment is similar to a direction of the ADV at a starting point of a next reference line segment.
14. The machine-readable medium of claim 13 , wherein the set of constraints further comprises a condition in which a first order derivative of the quintic polynomial function at the starting point of the reference line segment is similar to a curvature at the starting point of the reference line segment, and wherein a first order derivative of the quintic polynomial function at the ending point of the reference line segment is similar to a curvature at the starting point of the next reference line segment.
15. The machine-readable medium of claim 13 , wherein the set of constraints further comprises a condition in which a second order derivative of the quintic polynomial function at the starting point of the reference line segment is similar to a curvature change rate at the starting point of the reference line segment, and wherein a second order derivative of the quintic polynomial function at the ending point of the reference line segment is similar to a curvature change rate at the starting point of the next reference line segment.
16. The machine-readable medium of claim 13 , wherein the set of constraints comprises a condition in which a difference between a location of the ADV on the first reference line and a corresponding location of the ADV derived from the quintic polynomial function is below a predetermined threshold.
17. The machine-readable medium of claim 10 , wherein the optimization is iteratively performed on the objective function until a difference between an output of the objective function of a current iteration and an output of the objective function of a previous iteration is below a predetermined threshold.
18. The machine-readable medium of claim 10 , wherein performing an optimization comprises:
segmenting a first reference line segment of the plurality of reference line segments into a plurality of sub-segments;
for each of the second sub-segments, performing a second optimization using a first quintic polynomial function associated with the first reference line segment; and
summing outputs of the first quintic polynomial functions corresponding to the sub-segments to represent the first reference line segment.
19. A data processing system, comprising:
a processor; and
a memory coupled to the processor to store instructions, which when executed by the processor, cause the processor to perform operations, the operations including
in response to a first reference line representing a route from a first location to a second location associated with an autonomous driving vehicle (ADV), segmenting the first reference line into a plurality of reference line segments,
for each of the reference line segments, determining a quintic polynomial function to represent the reference line segment,
determining an objective function based on the quintic polynomial functions of the reference line segments,
performing an optimization on parameters of the quintic polynomial functions in view of a set of constraints associated with the reference line segments, such that an output of the objective function reaches minimum while the set of constraints are satisfied, and
generating a second reference line based on the optimized parameters of the quintic polynomial functions of the objective function, wherein the second reference line is utilized as a reference line of the route to control the ADV.
20. The system of claim 19 , wherein the objective function represents a sum of at least a portion of derivatives of the quintic polynomial functions of the reference line segments.
21. The system of claim 19 , wherein coefficients of a quintic polynomial function of each reference line segment are determined based on a location of the ADV, a direction of the ADV, curvature of the ADV, and curvature change rate of the ADV associated with the reference line segment of the first reference line.
22. The system of claim 21 , wherein performing an optimization on the parameters of the quintic polynomial functions comprises optimizing coefficients of each quintic polynomial function, such that an output of the quintic polynomial function at a starting point of the reference line segment is similar to a direction of the ADV at the starting point of the reference line segment and an output of the quintic polynomial function at an ending point of the reference line segment is similar to a direction of the ADV at a starting point of a next reference line segment.