Optimal longitudinal trajectory generation under varied lateral acceleration constraints
In one embodiment, a method, apparatus, and system for planning a trajectory for an autonomous driving vehicle (ADV) is disclosed. The operations comprise: receiving a plurality of optimization inputs, the plurality of optimization inputs comprising a trajectory time length, a time discretization resolution, an autonomous driving vehicle (ADV) starting state, a road shape function, a maximal jerk, and a maximal lateral acceleration; receiving a plurality of optimization constraints, the plurality of optimization constraints comprising constraints relating to the maximal jerk and the maximal lateral acceleration; receiving a cost function associated with an optimization objective, the cost function comprising a first term relating to cumulative jerk, a second term relating to an end longitudinal position, a third term relating to an end longitudinal speed, and a fourth term relating to an end longitudinal acceleration; generating a plurality of planned ADV states as optimization results with nonlinear optimization, wherein the optimization results minimize a value of the cost function; and generating control signals to control the ADV based on the plurality of planned ADV states.
1. A computer-implemented method, comprising:
receiving a plurality of inputs, the plurality of inputs comprising a trajectory time length, a time discretization resolution, an autonomous driving vehicle (ADV) starting state, a road shape function, a maximal jerk, and a maximal lateral acceleration;
receiving a plurality of constraints, the plurality of constraints comprising constraints relating to the maximal jerk and the maximal lateral acceleration;
receiving a cost function associated with an objective, the cost function comprising a first term relating to cumulative jerk, a second term relating to an end longitudinal position, a third term relating to an end longitudinal speed, and a fourth term relating to an end longitudinal acceleration;
generating a plurality of planned ADV states as results with nonlinear optimization, wherein the results minimize a value of the cost function;
generating control signals to control the ADV based on the plurality of planned ADV states; and
controlling the ADV, using a vehicle control system, according to the control signals.
2. The method of claim 1 , wherein an SL-coordinate system comprising a longitudinal dimension and a lateral dimension is utilized, wherein the longitudinal dimension is along a tangential direction of a reference line, and wherein the lateral dimension is perpendicular to the longitudinal dimension.
3. The method of claim 2 , wherein each of the ADV starting state and the plurality of planned ADV states comprises a longitudinal pose, a longitudinal speed, and a longitudinal acceleration.
4. The method of claim 2 , wherein the road shape function comprises a sequence of quartic spiral curves.
5. The method of claim 2 , wherein the cost function further comprises a first weight associated with the first term, a second weight associated with the second term, a third weight associated with the third term, and a fourth weight associated with the fourth term.
6. The method of claim 2 , wherein the plurality of planned ADV states correspond to time instants spaced by the time discretization resolution between a planning starting time and a planning ending time, and wherein the planning ending time is later than the planning starting time by the trajectory time length.
7. The method of claim 2 , wherein the plurality of inputs further comprise a target ADV ending state.
8. 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:
receiving a plurality of inputs, the plurality of inputs comprising a trajectory time length, a time discretization resolution, an autonomous driving vehicle (ADV) starting state, a road shape function, a maximal jerk, and a maximal lateral acceleration;
receiving a plurality of constraints, the plurality of constraints comprising constraints relating to the maximal jerk and the maximal lateral acceleration;
receiving a cost function associated with an objective, the cost function comprising a first term relating to cumulative jerk, a second term relating to an end longitudinal position, a third term relating to an end longitudinal speed, and a fourth term relating to an end longitudinal acceleration;
generating a plurality of planned ADV states as results with nonlinear optimization, wherein the results minimize a value of the cost function;
generating control signals to control the ADV based on the plurality of planned ADV states; and
controlling the ADV, using a vehicle control system, according to the control signals.
9. The non-transitory machine-readable medium of claim 8 , wherein an SL-coordinate system comprising a longitudinal dimension and a lateral dimension is utilized, wherein the longitudinal dimension is along a tangential direction of a reference line, and wherein the lateral dimension is perpendicular to the longitudinal dimension.
10. The non-transitory machine-readable medium of claim 9 , wherein each of the ADV starting state and the plurality of planned ADV states comprises a longitudinal pose, a longitudinal speed, and a longitudinal acceleration.
11. The non-transitory machine-readable medium of claim 9 , wherein the road shape function comprises a sequence of quartic spiral curves.
12. The non-transitory machine-readable medium of claim 9 , wherein the cost function further comprises a first weight associated with the first term, a second weight associated with the second term, a third weight associated with the third term, and a fourth weight associated with the fourth term.
13. The non-transitory machine-readable medium of claim 9 , wherein the plurality of planned ADV states correspond to time instants spaced by the time discretization resolution between a planning starting time and a planning ending time, and wherein the planning ending time is later than the planning starting time by the trajectory time length.
14. The non-transitory machine-readable medium of claim 9 , wherein the plurality of inputs further comprise a target ADV ending state.
15. 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
receiving a plurality of inputs, the plurality of inputs comprising a trajectory time length, a time discretization resolution, an autonomous driving vehicle (ADV) starting state, a road shape function, a maximal jerk, and a maximal lateral acceleration,
receiving a plurality of constraints, the plurality of constraints comprising constraints relating to the maximal jerk and the maximal lateral acceleration,
receiving a cost function associated with an objective, the cost function comprising a first term relating to cumulative jerk, a second term relating to an end longitudinal position, a third term relating to an end longitudinal speed, and a fourth term relating to an end longitudinal acceleration,
generating a plurality of planned ADV states as results with nonlinear optimization, wherein the results minimize a value of the cost function,
generating control signals to control the ADV based on the plurality of planned ADV states, and
controlling the ADV, using a vehicle control system, according to the control signals.
16. The data processing system of claim 15 , wherein an SL-coordinate system comprising a longitudinal dimension and a lateral dimension is utilized, wherein the longitudinal dimension is along a tangential direction of a reference line, and wherein the lateral dimension is perpendicular to the longitudinal dimension.
17. The data processing system of claim 16 , wherein each of the ADV starting state and the plurality of planned ADV states comprises a longitudinal pose, a longitudinal speed, and a longitudinal acceleration.
18. The data processing system of claim 16 , wherein the road shape function comprises a sequence of quartic spiral curves.
19. The data processing system of claim 16 , wherein the cost function further comprises a first weight associated with the first term, a second weight associated with the second term, a third weight associated with the third term, and a fourth weight associated with the fourth term.
20. The data processing system of claim 16 , wherein the plurality of planned ADV states correspond to time instants spaced by the time discretization resolution between a planning starting time and a planning ending time, and wherein the planning ending time is later than the planning starting time by the trajectory time length.
21. The data processing system of claim 16 , wherein the plurality of inputs further comprise a target ADV ending state.