Speed Planning for Autonomous Driving in Static and Dynamic Environments


Speed Planning along a Fixed Path.

Speed planning plays an important role in guaranteeing the ride comfort and safety in autonomous driving applications. All different kind of scenarios together raises distinct requirements and consequently different constraint types for speed planning problem formulations, which makes it challenging to solve.

We analyze the requirements of various driving scenarios for speed planning along the fixed path, summarize, and categorize constraints need to be addressed by speed planners as follows:

We describe reasons why every constraint is needed to address speed planning problems in detail in our paper [1], [2]. We view these constraints as requirements for speed planner design and metrics to measure the capacity of the existing speed planners for autonomous driving.

In light of this, we review the state-of-the-art speed planning methods and compare them with others in terms of constraints coverage, optimality, safety, flexibility, and capacity without revealing details, as seen in Table 2. Most of the existing methods just provided a workable speed profile rather than an optimal one for autonomous driving. None of them covered all the constraints we list in the Table 1.

Our Solution and Highlights

By taking some additional steps beyond the seminal work done by [3], [4], we present a general speed planning framework specifically for autonomous driving that is able to handle a wide range of different scenarios using convex optimization subject to a large collection of relevant constraints. Our contributions are as follows:

  • We summarize the most common constraints raised in various autonomous driving scenarios as the requirements for speed planner design and metrics to measure the capacity of the existing speed planners roughly for autonomous driving. We clarify which constraints need to be addressed by speed planners to guarantee safety in general.

  • We present a more general, flexible and complete speed planning mathematical model including friction circle , dynamics, smoothness, time efficiency, time window, ride comfort, IoD, path and boundary conditions constraints compared to similar methods explained in [4], [5]. We addressed the limitations of the method of Lipp et al. [4] by introducing a pseudo jerk objective in longitudinal dimension to improve smoothness, adding time window constraints at certain point of the path to avoid dynamics obstacles, capping a path constraint (most-likely non-smooth) on speed decision variables to deal with task constraints like speed limits, imposing a boundary condition at the end point of the path to guarantee safety for precise stop or merging scenarios. Compared to the approach of Liu et al. [5], our formulation optimizes the time efficiency directly while still staying inside of the friction circle, which ensures our method exploits the full acceleration capacity of the vehicle when necessary.

  • We introduce a semi-hard constraint concept to describe unique characters of the comfort box constraints and implement this kind of constraints using slack variables and penalty functions, which emphasizes comfort while guaranteeing fundamental motion safety without sacrificing the mobility of cars. To the best of our knowledge, none of the existing methods handle these constraints like ours. In contrast, Refs [6], [7], [8], [9], [5] regarded comfort box constraints as hard constraints, which dramatically reduces the solution space and in consequence limits the mobility of cars.

  • We demonstrate that our problem still preserves convexity with the added constraints, and hence, that the global optimality is guaranteed. This means our problem can be solved using state-of-the-art convex optimization solvers efficiently as well. We also provide some evidence to prove that our solution is able to keep consistent when the boundary conditions encounter some disturbances, which means only the part of results needed to be adjusted will be regulated due to the global optimality. This may benefit the track performance of speed controllers by providing a relative stable reference. It is not the case for these methods that solve the speed planning problem using local optimization techniques like [5]. A small change of boundary conditions or initial guess may result in a totally different solution due to local minimas in their problem.

  • We showcase how our formulation can be used in various autonomous driving scenarios by providing several challenging case studies solved in our framework, such as safe stop on a curvy road with different entry speeds, dealing with jaywalking in two different ways and merging from a freeway entrance ramp to expressways with safety guaranteed.

Abilities of Our Method

  • maintain the smoothness of the speed profile
  • driving within the limits of the friction cricle
  • achieve the time efficiency
  • cap a user-defined path constraint on the speed profile
  • control the arrival time at a certain point on the path (time window)
  • determine the end boundary condition of the state
  • align well with the user-defined speed profile while maintian smoothness and safety
  • dynamic obstacle avoidance
  • precise safe stop


  1. Yu Zhang, Huiyan Chen, Steven L. Waslander, Tian Yang, Sheng Zhang, Guangming Xiong, and Kai Liu, “Toward a More Complete, Flexible, and Safer Speed Planning for Autonomous Driving via Convex Optimization,” Sensors (Switzerland), vol. 18, no. 7, p. 2185, 2018.
  2. Yu Zhang, Huiyan Chen, Steven L. Waslander, Tian Yang, Sheng Zhang, Guangming Xiong, and Kai Liu, “Speed Planning for Autonomous Driving via Convex Optimization,” in IEEE International Conference on Intelligent Transportation Systems (ITSC), 2018, pp. 1–6.
  3. Diederik Verscheure, Bram Demeulenaere, Jan Swevers, Joris De Schutter, and Moritz Diehl, “Time-optimal path tracking for robots: A convex optimization approach,” IEEE Transactions on Automatic Control, vol. 54, no. 10, pp. 2318–2327, 2009.
  4. Thomas Lipp and Stephen Boyd, “Minimum-time speed optimisation over a fixed path,” International Journal of Control, vol. 87, no. 6, pp. 1297–1311, 2014.
  5. Changliu Liu, Wei Zhan, and Masayoshi Tomizuka, “Speed profile planning in dynamic environments via temporal optimization,” in IEEE Intelligent Vehicles Symposium (IV), 2017, pp. 154–159.
  6. Xiaohui Li, Zhenping Sun, Dongpu Cao, Zhen He, and Qi Zhu, “Real-time trajectory planning for autonomous urban driving: Framework, Algorithms, and Verifications,” IEEE/ASME Transactions on Mechatronics, vol. 21, no. 2, pp. 740–753, 2016.
  7. Tianyu Gu, Jarrod Snider, John M Dolan, and Jin-woo Lee, “Focused trajectory planning for autonomous on-road driving,” in IEEE Intelligent Vehicles Symposium (IV), 2013, pp. 547–552.
  8. Tianyu Gu, Jason Atwood, Chiyu Dong, John M Dolan, and Jin-Woo Lee, “Tunable and stable real-time trajectory planning for urban autonomous driving,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2015, pp. 250–256.
  9. Tianyu Gu, “Improved Trajectory Planning for On-Road Self-Driving Vehicles Via Combined Graph Search, Optimization & Topology Analysis,” PhD thesis, Carnegie Mellon University, 2017.