Dynamic programming and quadratic programming based decision and planning for autonomous driving vehicles
According to some embodiments, a system calculates a first trajectory based on a map and a route information. The system generates a path profile based on the first trajectory, traffic rules, and an obstacle information describing one or more obstacles perceived by the ADV. The system generates a speed profile based on the path profile, where the speed profile includes, for each of the obstacles, a decision to yield or overtake the obstacle. The system performs a quadratic programming optimization on the path profile and the speed profile to identify an optimal path with optimal speeds. The system generates a second trajectory based on the optimal path and optimal speeds to control the ADV autonomously according to the second trajectory.
1. A computer-implemented method to generate a driving trajectory for an autonomous driving vehicle (ADV), the method comprising:
calculating a first trajectory based on a map and a route information;
generating a path profile based on the first trajectory, traffic rules, and an obstacle information describing one or more obstacles perceived by the ADV;
generating a speed profile based on the path profile, wherein the speed profile includes, for each of the obstacles, a decision to yield or overtake the obstacle;
performing a quadratic programming optimization on the path profile and the speed profile to identify an optimal path with optimal speeds, including optimizing a path cost function using quadratic programming to generate a station-lateral map based on the path profile and optimizing a speed cost function using quadratic programming to generate a two-dimensional station-time graph indicative of a distance travelled with respect to time based on the speed profile;
generating a second trajectory based on the optimal path profile and the optimal speeds; and
driving the ADV autonomously according to the second trajectory.
2. The method of claim 1 , wherein the path profile is generated iteratively and the speed profile is generated iteratively using dynamic programming.
3. The method of claim 1 , wherein the path profile comprises, for each encountered obstacle decisions, a decision to yield to or ignore the encountered obstacle to a left or a right side of the encountered obstacle.
4. The method of claim 1 , wherein the station-lateral map is generated by forming one or more barriers based on one or more obstacle decisions.
5. The method of claim 1 , wherein the path cost function comprises a heading, a curvature, and a distance costs.
6. The method of claim 1 , wherein the speed cost function comprises an acceleration, a jerk, and a distance costs.
7. The method of claim 1 , further comprising interpolating a plurality of points, based on points on the station-lateral map and the station-time graph, to the second trajectory that are absent from the first trajectory.
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:
calculating a first trajectory based on a map and a route information;
generating a path profile based on the first trajectory, traffic rules, and an obstacle information describing one or more obstacles perceived by an autonomous driving vehicle (ADV);
generating a speed profile based on the path profile, wherein the speed profile includes, for each of the obstacles, a decision to yield or overtake the obstacle;
performing a quadratic programming optimization on the path profile and the speed profile to identify an optimal path with optimal speeds, including optimizing a path cost function using quadratic programming to generate a station-lateral map based on the path profile and optimizing a speed cost function using quadratic programming to generate a two-dimensional station-time graph indicative of a distance travelled with respect to time based on the speed profile;
generating a second trajectory based on the optimal path profile and the optimal speeds; and
driving the ADV autonomously according to the second trajectory.
9. The non-transitory machine-readable medium of claim 8 , wherein the path profile and the speed profile are generated iteratively using dynamic programming.
10. The non-transitory machine-readable medium of claim 8 , wherein the path profile comprises, for each encountered obstacle decisions, a decision to yield to or ignore the encountered obstacle to a left or a right side of the encountered obstacle.
11. The non-transitory machine-readable medium of claim 8 , wherein the station-lateral map is generated by forming one or more barriers based on one or more obstacle decisions.
12. The non-transitory machine-readable medium of claim 8 , wherein the path cost function comprises a heading, a curvature, and a distance costs.
13. The non-transitory machine-readable medium of claim 8 , wherein the speed cost function comprises an acceleration, a jerk, and a distance costs.
14. The non-transitory machine-readable medium of claim 8 , further comprising interpolating a plurality of points, based on points on the station-lateral map and the station-time graph, to the second trajectory that are absent from the first trajectory.
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
calculating a first trajectory based on a map and a route information;
generating a path profile based on the first trajectory, traffic rules, and an obstacle information describing one or more obstacles perceived by an autonomous driving vehicle (ADV);
generating a speed profile based on the path profile, wherein the speed profile includes, for each of the obstacles, a decision to yield or overtake the obstacle;
performing a quadratic programming optimization on the path profile and the speed profile to identify an optimal path with optimal speeds, including optimizing a path cost function using quadratic programming to generate a station-lateral map based on the path profile and optimizing a speed cost function using quadratic programming to generate a two-dimensional station-time graph indicative of a distance travelled with respect to time based on the speed profile;
generating a second trajectory based on the optimal path profile and the optimal speeds; and
driving the ADV autonomously according to the second trajectory.
16. The system of claim 15 , wherein the path profile and the speed profile are generated iteratively using dynamic programming.
17. The system of claim 15 , wherein the path profile comprises, for each encountered obstacle decisions, a decision to yield to or ignore the encountered obstacle to a left or a right side of the encountered obstacle.
18. The system of claim 15 , wherein the station-lateral map is generated by forming one or more barriers based on one or more obstacle decisions.