Autonomous driving represents an interesting research problem which is at the intersection of many different fields including robotics and control. Motion planning is a core component of any autonomous driving set-up. Off late, there has been an upsurge in applying model predictive control (MPC) based formulation for navigation of autonomous vehicles , , . The motivation behind this is clear. MPC provides a unified optimization based approach which can handle arbitrary constraints on state and control while minimizing a user-defined cost function. In spite of the recent success of MPC on autonomous driving, some key bottlenecks still remain. Among these include improving the computational efficiency of the underlying optimization and improving the predictive component of MPC to better reflect the actual behavior of the vehicle. In this paper, we make contributions towards both these problems.
On the optimization front, we make a case for adopting the alternating minimization (AM) approach which alternately searches in the space of angular accelerations and linear velocities. More precisely, the optimization operates in two separate layers wherein at the first layer, the angular accelerations are optimized while fixing the linear velocities. Subsequently, at the second layer, the linear velocities are optimized while the angular accelerations are fixed to the values obtained at the first layer and so on. We show that AM approach has a very distinct advantage. For a given angular acceleration profile, the non-holonomic motion model reduces to an affine form with respect to linear velocity. This in turn leads to a difference of convex structure in the velocity optimization layer which allows us to solve it efficiently using the convex-concave procedure , 
. Although AM has been extensively used in robotics and machine learning applications, we are not aware of any existing works which applies it to simplify trajectory optimization with non-holonomic motion model.
Our second contribution lies in incorporating actuator dynamics in the underlying optimization of our MPC. To be precise, we incorporate the time dependent non-linear mapping between the commanded velocity and the actual body velocity attained by the vehicle. The idea of MPC with actuator dynamics is not new. However, in the context of autonomous vehicles, existing works around this idea have been mostly restricted to either only path tracking control  or have considered collision avoidance only along the longitudinal direction of motion . In contrast, the novelty of our approach stems from how we relate the actuator dynamics to the collision avoidance constraints through the non-holonomic motion model.
On the implementation side, we show the usefulness of our AM approach by showing reduction in computation time over the joint formulation that simultaneously optimizes linear velocities and angular accelerations. Furthermore, we use a high fidelity simulator to compare our actuator dynamics augmented MPC with a more conventional formulation with no-actuator dynamics and piece-wise constant acceleration input. The specific metrics used for comparison include margin of inter-vehicle distance and velocity oscillation during standard maneuvers like lane-change, overtaking and lane following. The rest of the paper is organized as follows: Section II gives an overview of the current work in this field, Section III describes the actuator dynamics in detail along with the formulation of cost function and constraints. Section IV, describes the alternating minimization routine and section V gives a detail evaluation of our AM approach.
Ii Related Works
In this section, we present a review of MPC based autonomous driving, especially focusing on the underlying optimization of the MPC and whether or not it considers actuator dynamics.
for some excellent surveys. For better comparison with our proposed work, we classify the existing MPC formulations into two categories. Those in the first category like, 
formulate the underlying optimization of the MPC as a rigorous non-linear programming problem and then use iterative techniques like sequential convex programming for the solution. The advantage of these approaches is that they consider the exact motion model of the vehicle and thus, are guaranteed to produce a kinematically feasible trajectory. The so called warm-start or using trajectories obtained at the past iteration to initialize the optimization at the current iteration has been shown to speed up computation in practice. The formulations in the second category directly works with an affine approximation of the vehicle motion model , . Consequently, the optimization becomes simpler although at the expense of obtaining trajectories that may not be kinematically feasible or even collision free. Our AM based approach falls into the first category. But in contrast to existing works, we try to exploit the inherent structure in the optimization leading to tangible computational improvements.
All the above cited works employ a sophisticated trajectory optimization but do not consider the actuator dynamics within the optimization. Most of the current MPC formulations with actuator dynamics either have not considered collision avoidance or have worked with simplified collision benchmarks. For example,  claims improved tracking performance by incorporating actuator dynamics within the MPC. Authors in  formulate cooperative adaptive cruise control for a fleet of vehicles and have considered collision avoidance but only along the longitudinal direction of motion. Along the same line,  builds a MPC with actuator dynamics but does not consider collision avoidance.
Iii Problem Formulation
In this section, we formulate the underlying optimization associated with our MPC formulation. We begin by introducing the motion model and actuator dynamics followed by systematic description of cost function and constraints of our optimization problem.
Iii-a Motion model
The discrete time model of a non-holonomic autonomous vehicle is given by the following set of equations.
Where, , and are respectively the position and heading of the vehicle. The term represents the linear velocity of the vehicle while corresponds to the angular velocity at time . As shown the motion model is controlled by angular accelerations, and velocity commands, . Furthermore, we assume that commanded velocity gets translated to actual body velocity ()of the vehicle through a non-linear time dependent function, .
Iii-B Actuator Dynamics
In the context of the proposed work, the role of actuator dynamics, is to model how actual body velocity varies with respect to the commanded velocity over time. As a simple example, consider a which relates and linearly over time. Thus, the relationship between and takes the following form:
It is clear that (2) is equivalent to non-holonomic motion model with piece-wise constant acceleration as the control input. In our work, we adopt a more expressive which accounts for the inherent relationship between and .
First order system: We conducted several experiments with our autonomous vehicle hardware  and a high-fidelity simulator CARSIM  to analyze the response to a step velocity input. Some sample results are presented in Fig. 2-2. Predominantly, our autonomous vehicle shows actuator dynamics similar to a first order system (2). However, during de-accelerations (Fig. 2), we observed occasional velocity overshoots indicating that the actual dynamics could have multiple modes. The response from our simulator (2) is more typical of a second order system.
Motivated by the results from our autonomous vehicle hardware, we model our actuator dynamics as a first order system (3), where is the velocity acquired by the system at instant and is time varying response for a given step input. Model parameter is called time constant of the system. Finding
analytically is difficult as many components of our vehicle actuation mechanism are difficult to model. So, we adopted a data-driven approach and estimated
following a linear regression based on the response to different velocity step inputs. We note that our first order actuator dynamics is a simplified model and it leads to a simpler optimization problem (see equation (4)). Moreover, our extensive simulations show that even with a simulator which predominantly has second order actuator dynamics, our first order model could ensure collision avoidance with high fidelity. Finally, a first order model has only one parameter and thus require simpler system identification as compared to a second order actuator dynamics.
Expression for vehicle velocity: If the system is subjected to a set of commanded velocity inputs, then vehicle velocity after time-steps each of duration, can be represented as
The underlying optimization of our MPC can be described in the following manner:
and represents the state of the system. The vector valued functionis just a compact representation of (1), where we have used as mentioned in 1. The cost function (6a) is a summation of smoothness and goal reaching costs (7)-(8). As can be seen from (7), smoothness cost penalizes high value of angular accelerations and jerk modeled as a second order finite difference of linear velocity. The terminal cost ensures that the obtained trajectory terminates as close as possible to the goal position . The equality (6b) constrains the control variables and states to be compatible with the motion model of the robot. The inequalities (6c)-(6d) represent the bounds on angular velocities and accelerations respectively. Inequality (6e) ensures that the commanded velocity is less than the physical limit of the vehicle. In (6f) we have modeled acceleration as a finite difference between the vehicle velocity at and the next commanded velocity. Inequalities, (6g) represent the curvature bounds for the vehicles. Note how these have been written with respect to the actual body velocity and not the commanded velocity. Inequalities (6h) models the collision avoidance constraints and has the following algebraic form:
Where, and describe the position of obstacle. Following , , we model our ego-vehicle and the neighboring obstacles as an overlap of circles. Consequently, represents the combined radius of each circle of the ego-vehicle and obstacle. For static obstacles, the position would be independent of . The form of (9) assumes that the vehicle and the obstacles are both modeled as circles. It is convenient to obtain an affine approximation for (9) by linearizing it around a guess trajectory . In (10), , stands for partial derivative of with respect to and respectively. Further, is obtained by evaluating right hand side of (9) at .
The core complexity of optimization (6a)-(6h) stems from the non-linear motion model (6b). This is because, for an affine motion model, (10) turns out to be globally valid convex approximation of the original collision avoidance constraints (9). , . In other words, satisfaction of (10) ensures satisfaction of (9) as well. However, the non-linearity of non-holonomic motion model destroys this structure and makes the optimization more difficult.
In the next section, we propose an optimization routine which tries to explore as much as possible the inherent structure of the problem. The core idea hinges on the observation that if the heading trajectory of the vehicle is fixed, the motion model becomes affine with respect to linear velocities.
Iv Alternating Optimization
Algorithm 1 summarizes the proposed alternating optimization routine. It starts with (line 1) choosing an initial guess for commanded velocity , angular acceleration and a counter along with two positive weights . The function (line 3) computes an initial guess for position and heading trajectory, using and in the motion model (1). Lines 6 and 12 represent the angular acceleration and linear velocity optimization layer respectively. Both the layers continue till the change in the cost functions between subsequent iteration is greater than the threshold and the collision avoidance constraints are not satisfied. The optimal solution obtained after each layer is used to update the initial guesses of angular accelerations and commanded velocities (lines 11 and 17).
Iv-a Angular Acceleration Layer
First, note the motion model which is obtained by first order Taylor series expansion of the first two equations in (1) around . Consequently, we obtain a motion model which is affine with respect to heading angle .
The affine approximation holds only in the vicinity of . Thus, a trust region needs to be incorporated to ensure that and are sufficiently close to each other. The last inequality in line 6 of Algorithm 1 which puts a box constraints on serves this purpose. The trust region is modified and guess point is updated at each iteration as discussed in .
The collision avoidance constraints have been augmented with a non-negative slack variable . This is to ensure that the Algorithm 1 continues to make progress towards the optimal solution even if the initial guess trajectory renders infeasible (or in other words is not collision free). Consequently, we also incorporate a penalty on the slack variables in the cost function. The weights of the penalty is sequentially increased using a positive factor till .
Iv-B Linear Velocity Layer
Note, the motion model, which has been obtained from (1) by using obtained by the previous angular acceleration layer.
The collision avoidance constraints are augmented with non-negative slacks similar to angular acceleration layer. The penalty on the slacks also follows the same reasoning.
Note that this layer does not have any trust region constraints. This is because in this layer we do not make any linearization which in turn is due to the motion model being already affine in terms of forward velocity. The physical implication of this is that the velocity optimization layer can take as large as possible step towards the optimal solution . In contrast, the progress of the angular acceleration layer is limited by the size of the trust region.
Algorithm 1 is solved in a receding horizon manner to serve as our MPC. At the start of the MPC, the Algorithm 1 is intialized with the output of a high-level planner such as . In the subsequent iteration of the MPC, we adopt the warm-start approach where we initialize with the trajectory and controls obtained in the previous iteration.
V Results and Discussion
V-a Benchmarking Alternating Minimization (AM)
Here we compare our proposed AM with the more conventional formulation where angular accelerations and linear velocities are simultaneously obtained. We prototyped both the approaches in MATLAB using CVX . These simulations are ran in a Intel core i5-3230M @ 2.6 GHz CPU with 6GB RAM. The results are summarized in Fig.3-3 and 4-4.
Fig.5-5 presents the comparison of the computational aspects in terms of run time and number of iterations. As shown, on an average, our proposed AM approach takes around less time and less iterations. As shown in Fig.4-4, both our AM and joint formulation results in low smoothness cost. However, smoothness cost obtained with joint formulation is significantly lower than that obtained with our AM. It can be thus concluded that our proposed AM approach sacrifices a bit of optimality in the pursuit of computing reasonably smooth, kinematically feasible collision avoiding trajectories in quick time. We believe such a feature can be useful for autonomous driving.
V-B Effect of Actuator Dynamics
To analyze the effect of actuator dynamics, we integrated our MPC with a state of the art vehicle simulator called CARSIM. The simulator provides state feedback for the ego-vehicle along with the information about the state of the obstacle through a virtual LIDAR with a sensing range of . For this implementation, we prototyped Algorithm 1 in C++ using Gurobi solver  obtaining significant speed up. We were able to iterate our MPC at 10Hz with a planning horizon of 50 steps each of duration 0.1s. To ensure that the ego vehicle respects the road geometry, the boundaries of the road are modeled as imaginary obstacles and included into Algorithm 1. The bounds on velocity (), acceleration(), and deceleration() were kept at , , and respectively to make simulations closer to real-world scenarios. The detailed videos of simulations in several safety critical situations described in Section V-B1 are provided at https://researchweb.iiit.ac.in/~mithun.babu/acc_2019.mp4
V-B1 Benchmark Scenarios
Occlusion during overtaking: In this benchmark, the ego-vehicle traveling at initiates an overtaking maneuver. Midway during the maneuver, it notices another vehicle (which was previously occluded from its field of view) also performing a similar maneuver. In such a situation, the ego-vehicle should decelerate quickly to to avoid collision. Fig. 5-5 shows the occlusion scenario, wherein the ego-vehicle is shown in blue. The vehicle it is overtaking is the yellow van and the occluded vehicle is marked with a black circle. Fig.5-5 repeats the same benchmark with an ego-vehicle (shown in red) that uses MPC with actuator dynamics (2) to compute its motion. Fig.6,6 shows the plot of inter-vehicle distance and vehicle velocity for a specific portion of time. It can be seen that the ego-vehicle that incorporates actuator dynamics (3) within MPC can pro-actively anticipate the transient velocity response and thus de-accelerate faster resulting in improved inter-vehicle distance. The complete velocity profile is shown in Fig.6 and as shown it exhibits less overshoot and oscillation with the incorporation of the actuator dynamics.
Lane change : In this benchmark, the ego-vehicle moving at a speed of performs a lane change maneuver to an adjacent high speed lane (). In this situation, a delay in achieving the increased velocity would bring the ego-vehicle in collision course with the vehicle approaching from the rear. Fig.7-7 show the lane change scenario. The ego-vehicle is shown in blue while the rear vehicle is marked with a black circle. Similar to the previous benchmark, in Fig.7-7, we repeat the simulation with a different ego-vehicle (shown in red) that has actuator dynamics modeled as eqn.(2). It can be seen from Fig.8 that use of actuator dynamics (3) allows the vehicle to accelerate faster to the required velocity and thus maintain higher clearance from the vehicle approaching from behind (Fig.8). The complete vehicle velocity shown in Fig.8 shows improved transient response with reduced overshoot and oscillation when using actuator dynamics (3) within the MPC.
Sudden Braking: This benchmark is shown in Fig.9-9. Here, the ego-vehicle (shown in blue) is following a yellow vehicle (marked by a black circle) in the front at a speed of . Suddenly, the front vehicle de-accelerates bringing our ego-vehicle in collision course. The ego-vehicle responds by reducing its speed. Fig.9-9 repeats the same simulation with an ego-vehicle (shown in red) with actuator dynamics (2). As shown in Fig.10, bringing actuator dynamics explicitly within the MPC allowed the ego-vehicle to achieve significantly faster de-acceleration. This in turn improves the inter-vehicle distance (Fig.10). Similar to previous benchmarks, in Fig.10, we observe velocity profile with improved transient response with actuator dynamics (3) incorporated within the MPC.
Quantitative Comparison: The bar-graph shown in Fig. 11 summarizes the results on inter vehicle distance observed during previous benchmarks. Here, we also present an additional comparison that is based on actuator dynamics (eqn.3) but with a which varies with vehicle velocity
. We used a function approximator based on radial basis function to to learn themapping. As shownn in Fig.11, we noticed only marginal benefit of using a complicated dependent in our first order actuator dynamics (3).
Vi Conclusions and Future Work
In this work, we built an MPC formulation, on a novel alternating minimization based optimization routine coupled with a non-holonomic motion model with a first order actuator dynamics. We performed extensive simulations to show how incorporation of actuator dynamics within the MPC improves autonomous driving. In particular, we showed improved inter-vehicle distance during different maneuvers like overtaking, lane-change and vehicle-following. We also benchmarked our alternating minimization and showed that it can compute feasible collision avoiding trajectories faster while still achieving quite low smoothness cost.
There are several directions where our current formulation can be extended to remove some of the existing limitations. For example, one of our primary future focus is on incorporating steering actuator dynamics within our MPC and analyzing the resulting benefits in the context of autonomous driving. We are also working on implementing the proposed formulation on our autonomous car prototype.
-  W. Schwarting, J. Alonso-Mora, L. Pauli, S. Karaman, and D. Rus, “Parallel autonomy in automated vehicles: Safe motion generation with minimal intervention,” in Robotics and Automation (ICRA), 2017 IEEE International Conference on. IEEE, 2017, pp. 1928–1935.
-  D. Kim, J. Kang, and K. Yi, “Control strategy for high-speed autonomous driving in structured road,” in Intelligent Transportation Systems (ITSC), 2011 14th International IEEE Conference on. IEEE, 2011, pp. 186–191.
-  C. Katrakazas, M. Quddus, W.-H. Chen, and L. Deka, “Real-time motion planning methods for autonomous on-road driving: State-of-the-art and future research directions,” Transportation Research Part C: Emerging Technologies, vol. 60, pp. 416–442, 2015.
-  X. Shen, S. Diamond, Y. Gu, and S. Boyd, “Disciplined convex-concave programming,” in Decision and Control (CDC), 2016 IEEE 55th Conference on. IEEE, 2016, pp. 1009–1014.
-  S. Boyd, “Sequential convex programming,” Lecture Notes, Stanford University, 2008.
-  E. Kim, J. Kim, and M. Sunwoo, “Model predictive control strategy for smooth path tracking of autonomous vehicles with steering actuator dynamics,” International Journal of Automotive Technology, vol. 15, no. 7, pp. 1155–1164, 2014.
-  N. Murgovski, B. Egardt, and M. Nilsson, “Cooperative energy management of automated vehicles,” Control Engineering Practice, vol. 57, pp. 84–98, 2016.
-  B. Paden, M. Čáp, S. Z. Yong, D. Yershov, and E. Frazzoli, “A survey of motion planning and control techniques for self-driving urban vehicles,” IEEE Transactions on Intelligent Vehicles, vol. 1, no. 1, pp. 33–55, 2016.
-  A. Carvalho, S. Lefévre, G. Schildbach, J. Kong, and F. Borrelli, “Automated driving: The role of forecasts and uncertainty—a control perspective,” European Journal of Control, vol. 24, pp. 14–32, 2015.
-  A. Carvalho, Y. Gao, A. Gray, H. E. Tseng, and F. Borrelli, “Predictive control of an autonomous ground vehicle using an iterative linearization approach,” in Intelligent Transportation Systems-(ITSC), 2013 16th International IEEE Conference on. IEEE, 2013, pp. 2335–2340.
-  S. Boyd, “Sequential convex programming,” Lecture Notes, Stanford University, 2008.
-  J. Nilsson, P. Falcone, M. Ali, and J. Sjöberg, “Receding horizon maneuver generation for automated highway driving,” Control Engineering Practice, vol. 41, pp. 124–133, 2015.
-  J. Nilsson, M. Brännström, J. Fredriksson, and E. Coelingh, “Longitudinal and lateral control for automated yielding maneuvers,” IEEE Transactions on Intelligent Transportation Systems, vol. 17, no. 5, pp. 1404–1414, 2016.
-  Y. Luo, A. Serrani, S. Yurkovich, D. B. Doman, and M. W. Oppenheimer, “Model predictive dynamic control allocation with actuator dynamics,” in American Control Conference, 2004. Proceedings of the 2004, vol. 2. IEEE, 2004, pp. 1695–1700.
-  A. Modh, S. Singh, A. Kumar, K. M. Krishna et al., “Gradient aware-shrinking domain based control design for reactive planning frameworks used in autonomous vehicles,” arXiv preprint arXiv:1804.08679, 2018.
-  M. S. Corporation, “Mechanical simulation,” https://www.carsim.com/, Jan. 2016.
-  A. Chakravarthy and D. Ghose, “Obstacle avoidance in a dynamic environment: A collision cone approach,” IEEE Transactions on Systems, Man, and Cybernetics-Part A: Systems and Humans, vol. 28, no. 5, pp. 562–574, 1998.
-  S. Sharma and M. E. Taylor, “Autonomous waypoint generation strategy for on-line navigation in unknown environments,” environment, vol. 2, p. 3D, 2012.
-  M. Grant and S. Boyd, “CVX: Matlab software for disciplined convex programming, version 2.1,” http://cvxr.com/cvx, Mar. 2014.
-  I. Gurobi Optimization, “Gurobi optimizer reference manual,” 2016. [Online]. Available: http://www.gurobi.com