Multi-Profile Quadratic Programming (MPQP) for Optimal Gap Selection and Speed Planning of Autonomous Driving

TL;DR

This work addresses the challenge of simultaneous path-space and time synchronization for speed planning in dynamic traffic. It introduces Multi-Profile Quadratic Programming (MPQP), which uses a space-time graph to explore multiple timing profiles via BFS and integrates these profiles as hard/soft bounds into a convex quadratic program that optimizes a longitudinal speed profile with realistic dynamics. Key contributions include (i) a complete ST-graph construction and viable-cell generation, (ii) a BFS-based profile search with parallelizable processing, (iii) a unified QP formulation with hard and soft constraints, and (iv) a funnel technique to enforce lateral acceleration limits. Validation in CARLA across multiple scenarios and a real-world urban demonstration in Joso city show MPQP achieves real-time performance (~20 ms per decision) and robust, safe behavior, highlighting its potential for practical deployment in autonomous driving while noting future work on uncertainty handling and multi-modal predictions.

Abstract

Smooth and safe speed planning is imperative for the successful deployment of autonomous vehicles. This paper presents a mathematical formulation for the optimal speed planning of autonomous driving, which has been validated in high-fidelity simulations and real-road demonstrations with practical constraints. The algorithm explores the inter-traffic gaps in the time and space domain using a breadth-first search. For each gap, quadratic programming finds an optimal speed profile, synchronizing the time and space pair along with dynamic obstacles. Qualitative and quantitative analysis in Carla is reported to discuss the smoothness and robustness of the proposed algorithm. Finally, we present a road demonstration result for urban city driving.

Figures (6)

• Figure 1: Motivational example: there exists multiple timing options of crossing the intersection for the ego vehicle (in green) along with the crossing timings of cars 1, 2, and 3. Precisely synchronizing the timing and distance as well as choosing the best gaps is the key for a safe maneuver.
• Figure 2: T-junction scenario (top left), corresponding ST-graph (top center) and ST Cell Planner (top right). ST Cell Planner shows also the possible paths found using the Breadth First Search Algorithm (BFS). Bottom images shows the feasible and infeasible paths found in the ST-graph.
• Figure 3: T junction scenario with 2 agents (Car 1966 in grey, Car 1967 in dark red). The hard constraints can be seen in a solid color fill, the soft constraints can be seen in a semi-transparent color fill.
• Figure 4: Testing scenarios (from left): (i) highway merging, (ii) T-junction, (iii) four-way intersection, and (iv) lane changing in dense traffic.
• Figure 5: Testing scenarios (from left): (i) highway merging, (ii) T-junction, (iii) four-way intersection, and (iv) lane changing in dense traffic.