Trajectory optimization of non-holonomic vehicles is challenging owing to the highly non-linear motion model. Moreover, incorporation of hard bounds on curvature further increases the complexity of the optimization. In fact, we remark that it is the presence of hard curvature bounds which acts as a key bottleneck of the problem. To understand this further, consider the following two equivalent discrete time representations of a non-holonomic vehicle with curvature bound constraints.
Where, are the position and heading of the vehicle at time, while are the control inputs at some instant . The terms, represent the maximum and minimum curvature bounds respectively. Thus, the inequalities in both the representations define the curvature bound constraints. The representation (1) is the classic non-holonomic model while (2) is obtained via feedback linearization . In (1), the curvature constraints are affine functions of control inputs. Thus, a trajectory optimization which uses (1) as the motion model is made difficult by the non-linear relationship between the position and the control variables. Conversely, in (2), the position variables are simply affine functions of the control inputs. Consequently, a trajectory optimization based on (2) is difficult due to the presence of non-linear curvature bounds. Thus, it can be seen that irrespective of which representation is used for the motion model of the non-holonomic vehicle, the trajectory optimization invariably takes the form of a difficult nonlinear and non-convex optimization problem. It is also interesting to note that if the curvature bounds are absent (e.g in case of differential drive robot), then using (2), the problem of connecting a pair of given positions/orientations in obstacle free space can be posed as a convex optimization problem. Further, if the obstacles (both static and dynamic) are approximated as circles, then the trajectory optimization with model (2) can be shown to have a difference of convex (DC) structure (e.g refer the derivations presented in , , ). Specially designed sequential convex programming (SCP) routines exist for solving optimization problems with DC structure . However, inclusion of curvature constraints in (2) destroys the differnece of convex structure and consequently a more general non-linear optimization technique needs to be adopted .
In this paper, we present a trajectory optimization which tries to exploit as much as possible the problem structure. We advocate the use of representation (1) as the model of the non-holonomic vehicle for the simple reason that herein, the curvature constraints are affine functions of control inputs. Thus, it is straightforward to ensure that the output of the trajectory optimization is always kinematically feasible. The central idea of the proposed work hinges on two inter related observations on nonlinear non-convex trajectory optimization. Firstly, the computational performance of such a trajectory optimization depends on how big the trust region is, which is the region where the convex approximation of the problem holds. Secondly, on a related context, the reason why trajectory optimization with linear motion models and circular obstacles are shown to be easily solvable, atleast to local optimality is because they have infinitely big trust region owing to the DC structure of the problem ,, , ). The proposed trajectory optimization leverages both these observations by using better linearization for non-holonomic motion model and by reformulating collision avoidance between two convex polygons into that between a point and a circle.
In particular, the algorithmic contributions of the proposed work are two fold. Firstly, we present an alternating minimization routine which alternately operates in the space of angular accelerations and linear velocities. More precisely, the trajectory optimization has 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 at the values obtained at the first layer and so on. Secondly, we use the concept of Minkowski sum and minimum bounding circle to reduce collision avoidance between two convex polygons to that between a point and a circle. These two contributions in in conjunction provides the proposed trajectory optimization with following key advantages over the current state of the art.
Firstly, for a given angular acceleration profile, the non-holonomic motion model is affine with respect to linear velocity. Further, fixing angular acceleration also ensures that the posture profile of the vehicle does not change in the velocity optimization layer. Since the Minkowski sum only depends on the relative posture of the vehicle and obstacle and not on their relative distance, the velocity optimization has the same DC structure which has been reported for linear systems with circular vehicles/obstacles , , ). We are not aware of any work which has highlighted such structure in non-holonomic trajectory optimization with convex polygon shapes.
We empirically show through extensive simulations that the proposed alternating optimization can afford larger trust regions as compared to joint optimizations which simultaneously optimizes in the space of angular acceleration and linear velocity. Consequently, the alternating optimization provides some gain in computation time over the joint formulations.
The proposed circle approximation using the concept of Minkowski sum and minimum bounding circle leads to significantly less conservative approximation than representing each polygon individually through a circumscribing circle. Further, this is also an improvement over the approach presented in  where polygons are represented as multiple overlapping circles. In particular, as compared to these cited works, the proposed approach leads to reduced number of constraints in the trajectory optimization. For example, if three overlapping circles are used to represent just the vehicle polygon, then the trajectory optimization in  would have three times more number of constraints than the proposed approach.
Ii Problem Formulation
The trajectory optimization considered in this paper can be described by the following set of cost functions and constraints.
Where, and represents the state of the system at time . The individual cost terms can be represented in the following manner.
The objective function (3a) consists of smoothness and terminal cost terms. As can be seen from (4), smoothness cost penalizes high value of angular accelerations and jerk modeled as second order finite difference of linear velocity. The terminal cost ensures that the optimal trajectory terminates as close as possible to the goal position . The equality (3b) constrains the control variables and states to be compatible with the motion model of the robot. The inequality (3c)-(3d) represents bounds on linear and angular velocities while (3e)-(3f) can be thought as actuator constraints bounding the linear and angular acceleration magnitudes. The inequalities (3g) are the curvature bound constraints. Inequalities (3h) models the collision avoidance constraints and has the following algebraic form.
Where, are the position and size of the obstacle. For static obstacles, the position would be independent of . The form of (6) assumes that the vehicle and the obstacles are both modeled as circular disks. As we show later, the same form can be leveraged to model collision avoidance between polygonal shapes as well.
The inequalities (6) are purely concave in terms of position variables or in other words has the so called difference of convex form. Thus, as shown in , it can be upper bounded by the following affine inequality obtained by linearizing (6) by some initial guess trajectory . Satisfaction of (7) ensures that the inequalities (6) are satisfied.
The core complexity of optimization (3a)-(3h) stems from the non-linear motion model, (3b) since we have already constructed an affine approximation for collision avoidance constraints. If the motion model would have been affine, then the optimization could be efficiently solved to (local) optimality through a specially designed sequential convex programming routine . In the case of non-linear motion model, the most common approach has been to adopt general non-linear optimization techniques wherein at each iteration, the non-linear motion model is approximated by an affine expression. In the next subsequent sections, we describe how the affine approximation, (7) can be leveraged for modeling collision avoidance between polygonal shapes as well. We follow that by the presentation of our alternating minimization routine.
Iii Collision Avoidance Between Convex Polygons
The proposed modeling approach for collision avoidance between convex polygons hinges on two basic ingredients namely Minkowski sum and circumscribing circle of an arbitrary polygon.
Iii-a Minkowski Sum
Minkowski sum of two sets can be defined as
In our case, are polygons in . Minkowski sum boundary is similar to contact space, which means robot is placed in contact with obstacles but with out collision . The boundary of the Minkowski polygon can be represented as
Thus, using one can replace one of the polygons to a point and the other to an arbitrary polygon. The Minkowski sum of two convex polygons with sides has a computational complexity of and thus can be computed with relative ease for shapes like rectangles, squares.
Iii-B Minimum Bounding Circle
Given a set of vertex points, of the Minkowski polygon , we compute the smallest circle containing the polygon through the concept of minimum bounding circle. Various algorithms e.g based on randomization and quadratic programming are generally used to find such circle fitting into vertices of . The computational complexity is linear with respect to number of vertices, . We use the open source implementation  in our work.
Iv Alternating Optimization
Algorithm 1 summarizes the proposed alternating optimization routine. It starts with (line 1) choosing an initial guess for linear velocity, , angular acceleration, and a counter along with two positive weights, . The function (line 3) computes an initial guess for position and heading trajectory, based on the guess for linear velocity and acceleration. Lines 6 and 12 represent the angular acceleration and linear velocity optimization layer respectively. Both the layers continue till the change in the cost function 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 linear velocities (lines 11 and 17).
Iv-a Angular Acceleration Layer
Firstly, 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 and consequently angular acceleration, .
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 serve this purpose. The trust region is modified at each iteration of based on constraint violations 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. Consequently, we also incorporate a penalty on the slack variables in the cost function. The weights of the penalty is sequentially increased as long as .
Iv-B Linear Velocity Layer
Note, the motion model, which has been obtained from (1) by fixing the based on the angular acceleration, obtained at the previous layer. Consequently, is affine with respect to linear velocity.
The naturally affine motion model means that there is no need to incorporate trust region constraints in this 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.
Iv-C A Note on Structure
The linear velocity optimization layer in algorithm (1) has the same DC structure as that reported in , , ). Consequently, the computational performance of algorithm (1) depends on how big the trust region at the angular acceleration optimization layer is.
V Implementation and Results
To compare alternating minimization with an more general approach where linear velocity and angular acceleration are optimized at the same time (Joint Minimization), we have prototyped both of them in CVX . Then, comparisons were made on runtime, number of iterations taken to converge, arc-length of path generated, velocity profile smoothness, acceleration profile smoothness, angular acceleration profile smoothness. In subsequent subsection we discuss details of these comparisons and results obtained by implementing our approach in MPC framework. By testing our approach on some typical urban overtaking and merging scenarios with dynamic obstacles we show robustness of our approach.
The comparison between runtime and iterations of alternating and joint minimization in Fig. 3-3 show that our approach has half the number of iterations to that of joint minimization. We also notice an improvement in runtime by . Absence of trust region in velocity layer has provided some advantage. We can expect a further improvement in runtime by only iterating over one layer once change in other layer is less than threshold.
Fig. 4-4 show comparison between different smoothness terms. Smoothness of a control variable is defined as sum of square of rate of change of that variable. In our comparison joint minimization has better smoothness due to its search in angular acceleration space and linear velocity space at the same time. However, the changes in control variables are strictly under the limits provided in inequalities in line 6 and 12 of Algorithm 1 and hence are allowed with in the dynamic limits of model.
V-B Model Predictive Control
To evaluate our approach, we have used cvxgen  which generates appropriate C code to be mexed with MATLAB to speed up the process of optimization. During implementation into MPC framework we have taken a planning horizon of 5 seconds, for 50 steps with a of 0.1 seconds. We replan at a frequency of 5hz and also limited to a maximum speed of vehicle to 10m/s. Below is box plot of runtime of cvxgen, Though worst case comes out to be around 800ms which is a scenario with parking lot. This is being further improved by including only active sets of constraints.
In addition, we have implemented few maneuvers in GAZEBO  and ROS . The control points input for the MPC framework comes from cruise control with an average velocity of 2.7m/s equally spaced along the lane. The red lines in Fig.5 indicate maximum and minimum bounds provided to control variables during optimization.
Initially, vehicle starts from rest and accelerates up to 2.7m/s to track the way points on the path. Later during overtaking maneuver, we observed an increase in velocity and gain in angular velocity (between 7s to 12s). We also notice control variables with in their dynamic limits provided at the same time completing overtaking maneuver. In Fig 6-6, we can observe a sequence of overtaking maneuver. Figures 6 and 6 shows vehicle location and obstacle location every 100ms with colour change from light blue to dark blue.
In a more general scenario, where our ego vehicle is trying to overtake slow moving vehicle on our lane. However, we find another slow moving vehicle obstructing overtaking maneuver on other lane. In such case, we observe our vehicle slowing down on current lane and then speeding up as soon as vehicle on other lane vehicle passes by.
Here vehicle encircled in red is ego vehicle and we are trying to overtake slow moving red car on our lane and other lane has white car that obstructs our maneuver.Figures 7,7 and 6 shows vehicle location and obstacle location every 100ms with colour change from light blue to dark blue.
Similarly we have simulated merging maneuver in gazebo, in which our vehicle(marked with red box) merges into other vehicle at an intersection. During this scenario, the car moving ahead obstructs our simple cruise control path. Then it is observed that ego vehicle reduces its speed and merged in between red and green cars marked in 8, 8 and 8.
In this paper, we have also tested our MPC framework in an unstructured environments such as parking lot. Figures 9 - 9 show one such scenario in a parking lot with space between yellow taxis parked in.
-  M. Rufli, J. Alonso-Mora, and R. Siegwart, “Reciprocal collision avoidance with motion continuity constraints,” IEEE Transactions on Robotics, vol. 29, no. 4, pp. 899–912, 2013.
-  F. Gao and S. Shen, “Quadrotor trajectory generation in dynamic environments using semi-definite relaxation on nonconvex qcqp,” in Robotics and Automation (ICRA), 2017 IEEE International Conference on. IEEE, 2017, pp. 6354–6361.
-  A. K. Singh, S. Berman, and I. Nisky, “Stochastic optimal control for modeling reaching movements in the presence of obstacles: Theory and simulation,” arXiv preprint arXiv:1701.01547, 2017.
-  F. Augugliaro, A. P. Schoellig, and R. D’Andrea, “Generation of collision-free trajectories for a quadrocopter fleet: A sequential convex programming approach,” in Intelligent Robots and Systems (IROS), 2012 IEEE/RSJ International Conference on. IEEE, 2012, pp. 1917–1922.
-  S. Boyd, “Sequential convex programming,” Lecture Notes, Stanford University, 2008.
-  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.
-  J.-M. Lien, “Hybrid motion planning using minkowski sums,” Proceedings of robotics: science and systems IV, 2008.
-  John D’Errico, https://in.mathworks.com/matlabcentral/fileexchange/34767-a-suite-of-minimal-bounding-objects?focused=3820656&tab=function, 2015, online; accessed October 2017.
-  M. Grant and S. Boyd, “CVX: Matlab software for disciplined convex programming, version 2.1,” http://cvxr.com/cvx, Mar. 2014.
-  J. Mattingley and S. Boyd, “Cvxgen: A code generator for embedded convex optimization,” Optimization and Engineering, vol. 13, no. 1, pp. 1–27, 2012.
-  Open Source Robotics Foundation, “Gazebo,” http://gazebosim.org/, 2017, online; accessed 2017.
-  M. Quigley, K. Conley, B. Gerkey, J. Faust, T. Foote, J. Leibs, R. Wheeler, and A. Y. Ng, “Ros: an open-source robot operating system,” in ICRA workshop on open source software, vol. 3, no. 3.2. Kobe, Japan, 2009, p. 5.