Sequential Convex Programming for Collaboration of Connected and Automated Vehicles Xiaoxue Zhang, Jun Ma, Zilong Cheng, Frank L

Home , Convex optimization

Sequential Convex Programming for Collaboration of Connected and Automated Vehicles Xiaoxue Zhang, Jun Ma, Zilong Cheng, Frank L. Lewis, Life Fellow, IEEE, and Tong Heng Lee

Abstract—This paper investigates the collaboration of multiple may naturally incur the coupling constraints that involve the connected and automated vehicles (CAVs) in different scenarios. decision variables of the connected (coupled) vehicles [13], In general, the collaboration of CAVs can be formulated as [14]. The existence of coupling constraints may result in the a nonlinear and nonconvex model predictive control (MPC) problem. Most of the existing approaches available for utiliza- nonconvexity and nonlinearity and increase the difficulty of tion to solve such an optimization problem suffer from the achieving the cooperation task. Generally, the collaboration of drawback of considerable computational burden, which hinders CAVs can be formulated as an optimal control problem, and the practical implementation in real time. This paper proposes there are many existing methods to solve the problem and the use of sequential convex programming (SCP), which is a obtain the optimal solution [15]–[18]. powerful approach to solving the nonlinear and nonconvex MPC problem in real time. To appropriately deploy the methodology, In the existing literature, convex optimization has been a as a first stage, SCP requires linearization and discretization powerful tool to solve optimal control problems [19]. For when addressing the nonlinear dynamics of the system model convex optimization problems, there is an effective class of adequately. Based on the linearization and discretization, the approaches such as linear programming (LP), quadratic pro- original MPC problem can be transformed into a quadratically gramming (QP), semi-definite programming (SDP), second- constrained quadratic programming (QCQP) problem. Besides, SCP also involves convexification to handle the associated non- order cone programming (SOCP), etc. In most cases, these convex constraints. Thus, the nonconvex QCQP can be reduced convex problems can be solved in polynomial time due to their to a quadratic programming (QP) problem that can be solved low complexity. Some of the existing algorithms, for example, rather quickly. Therefore, the computational efficiency is suitably the interior point method, determines an optimal solution with improved despite the existence of nonlinear and nonconvex deterministic stopping criterion with predefined accuracy [20]– characteristics, whereby the implementation is realized in real time. Furthermore, simulation results in three different scenarios [23]. Given suitable accuracy, the optimal solution can be of autonomous driving are presented to validate the effectiveness obtained with deterministic upper bound of the iterations. Ad- and efficiency of our proposed approach. ditionally, by a self-dual embedding technique, the primal-dual Index Terms—Autonomous driving, connected and automated interior point method can start from a self-generated feasible vehicles (CAVs), multi-agent system, model predictive control point, instead of the predefined initial guess. Therefore, these (MPC), sequential convex programming (SCP), collaboration convex optimization methods are highly promising for the control. practical applications. However, the collaboration of CAVs always leads to a nonlinear and nonconvex optimization problem, which involves I.INTRODUCTION the nonlinear constraints derived from the vehicles’ dynamics Connected and automated vehicles (CAVs) have attracted and coupling constraints with nonconvex characteristics for increasing research attention in academia and industry due to collision avoidance [24], [25]. For the nonconvex optimization their great potential to enhance the traffic safety, fuel economy, problem, the interior point based methods can be certainly and road capacity [1]–[4]. Recent advances in communication deployed to solve the nonconvex problem; however, they are arXiv:2101.00202v1 [math.OC] 1 Jan 2021 technologies and computational resources prompt the CAVs to prone to be trapped into the local optimum solution and unable be crucial elements in intelligent transportation systems [5]– to realize real-time implementation for multi-agent systems. [7]. However, the collaboration of CAVs is still challenging [26], [27] and [28] utilize the pseudo-spectral method to due to the existence of sharing information, strong nonlinear- address the nonlinear system dynamics, whereas this method ity, and nonconvexity [8]–[12]. With the cooperation of CAVs, can not be implemented in real time, because its computational the decision-making process of vehicles is influenced by each time grows dramatically with the number of agents. Another other; and thus the cooperation and interaction of vehicles effective method is to convert the the nonconvex problem into the mixed-integer linear programming (MILP) or mixed- X. Zhang, Z. Cheng, and T. H. Lee are with the NUS Graduate School for Integrative Sciences and Engineering, National University of Singapore, integer quadratic programming (MIQP) problems, which can Singapore 119077 (e-mail:[email protected]; [email protected]; be solved by using branch-and-cut method to obtain some [email protected]). feasible solutions (by decomposing the free space as multiple J. Ma is with the Department of Mechanical Engineering, University of California, Berkeley, CA 94720 USA (e-mail: [email protected]). polytopes). Nevertheless, it will take long time if an optimal F. L. Lewis is with the Automation and Robotics Research Institute, Univer- solution is required, and they also performs poorly with the sity of Texas at Arlington, Arlington, TX 76118 USA (e-mail: [email protected]). number of agents increasing, even though some heuristics can This work has been submitted to the IEEE for possible publication. Copyright may be transferred without notice, after which this version may be applied to accelerate the computation process [29]–[32]. In no longer be accessible. general, these methods have certain inevitable shortcomings to solve the nonconvex problem. Particularly, they cannot ensure operator kXk is the Euclidean norm of matrix X. ⊗ denotes or determine a known bound of the computational time, and the Kronecker product. blockdiag(X1,X2, ··· ,Xn) denotes a may not guarantee the convergence to the optimum or even a block diagonal matrix with diagonal entries X1,X2, ··· ,Xn. feasible point within the predefined iterations. Besides, most and b mean the sets of positive integers {1, 2, ··· , a} and Za Za of them rely on the quality of the user-defined initial guess {a, a+1, ··· , b} with a < b, respectively. Define {xi} n ∀i∈Z1 in most scenarios. Furthermore, these approaches cannot be n to be the concatenation of the vector xi for all i ∈ Z1 , i.e., implemented for real-time and on-board applications. Thus, it > > >> {xi}∀i∈ n = x1 x2 ··· xn , and {xi}∀i∈ n is is critical to explore the use of more effective approaches to Z1 Z1 realize the deterministic convergence to the optimum in real equivalent to (x1, x2, ··· , xn). time. Sequential convex programming (SCP) is an effective approach to solving nonlinear optimal control problem in real B. Network of CAVs time. It dissects the convex problem into a sequence of con- In order to represent the constraint or information topology vex programming problems with restricted collision-avoidance of CAVs, an undirected graph G(V, E) can be utilized. The constraints, which prevents the constraints from obstruction node set V = {1, 2, ··· ,N} denotes the vehicles, and the and allows for the use of some efficient convex optimization al- edge set E = {1, 2, ··· ,M} denotes the coupling constraints gorithms [33], [34]. In this approach, discretizing the problem (information flow) between two CAVs: and linearizing the nonlinear constraints are required. Besides, ( (i, j) ∈ E(t), d ≤ kp − p k ≤ d the convexification of the nonconvex constraints is needed to safe i j cmu (1) make the optimization problem convex. (i, j) ∈/ E(t), otherwise, This paper addresses the collaborative motion planning of where N and M are the number of vehicles and coupling CAVs by using the SCP approach. The collaboration of CAVs constraints in the CAVs, respectively, p , p are the position is first formulated as a nonconvex and nonlinear model pre- i j vectors of the ith vehicle and jth vehicle, d and d dictive control (MPC) problem in the continuous-time domain. cmu safe mean the maximum communication distance and minimum Then the problem is discretized and linearized adequately, as safe distance between two vehicles, respectively. Therefore, the SCP requires linearization of the nonlinear optimization based on the communication topology, an adjacency matrix problem around local solutions iteratively. Besides, a quadrat- of the network (denoted by D) can be obtained, which is ically constrained quadratic programming (QCQP) problem a square symmetric matrix representing the finite undirected can be reformulated and convexified to facilitate the use of graph G(V, E). The elements of D indicate whether pairs of SCP algorithm. Thus, a sequence of simplified problems is vertices are adjacent/connected or not in the graph. Therefore, recursively generated and solved. These simplified problems the neighbor nodes of the ith vehicle are the corresponding are indeed reduced to QP problems. Therefore, this approach indexes of nonzero elements of the ith row in D, which is can be applied for collaborative motion planning of CAVs denoted by ν(i) = {j|(i, j) ∈ E, ∀j ∈ V}. rather efficiently and effectively. The remainder of this paper is organized as follows. Sec- tion II defines the nonlinear and nonconvex optimization C. Problem Description in Continuous Time problem with the introduction of the dynamic model, objective 1) Dynamic Modeling: Define the state vector as x ∈ Rn function, and constraints. Section III demonstrates the problem and the input vector as u ∈ Rm for vehicles. In this paper, discretization and linearization and convexification for noncon- we assume the height of the vehicle’s gravity center to be vex constraints. Section IV proposes the numerical procedures zero, and neglect the pitch and roll dynamics of the vehicle. of the SCP algorithm to solve such an optimization problem. Also, we neglect the rolling resistance and aerodynamic drag. In Section V, three different scenarios in autonomous driving A well-known kinematic bicycle model is used to represent the tasks are used to show the effectiveness and efficiency of the vehicles’ dynamics [35]. Define the state vector of a vehicle is proposed methodology. At last, the conclusion of this work is x(t) = (px(t), py(t), ψ(t), v(t)) and the control input vector given in Section VI. is u(t) = (a(t), δ(t)). The system dynamics at time t is

p˙x(t) = v(t) cos(ψ(t) + β(t)) II.PROBLEM FORMULATION p˙y(t) = v(t) sin(ψ(t) + β(t)) A. Notation v(t) tan(δ(t)) cos(β(t)) ψ˙(t) = The following notations are used in the remaining text. lf + lr a×b R denotes the set of real matrices with a rows and b v˙(t) = a(t) a a columns, R means the set of -dimensional real column l vectors. x y and x y mean that vector x is element- β(t) = tan−1 r tan(δ(t)) , (2) l + l wisely greater and no less than the vector y. A> and x> denote f r the transpose of the matrix A and vector x. Ia represents where px(t) and py(t) are the position of the center point of the a-dimensional identity matrix; 1a and 1(a,b) denote the the vehicle in X and Y dimension in the Cartesian coordinates, a-dimensional all-one vector and the a-by-b all-one matrix, respectively, ψ(t) represents the heading angle of the vehicle respectively; 0a and 0(a,b) represent the a-dimensional all- in the positive X-dimension, β(t) denotes the side slip angle zero vector and the a-by-b all-zero matrix, respectively. The between the velocity vector and the longitudinal direction of

2 the vehicle, v(t) denotes the velocity of the vehicle, δ(t) is and the collision avoidance between each pair of vehicles, the steering angle, a(t) is the acceleration of the vehicle, and which can be formulated as Z lf and lr denote the distance from the front axis and rear axis X to the center of gravity of the vehicle, respectively. Therefore, min `i (xi(t), ui(t)) dt + `f,i(xi(tf )) (8a) xi,ui T we have i∈V s. t. x˙ i(t) = f(xi(t), ui(t)), (8b)

x˙(t) = f(x(t), u(t)), (3) ui ui(t) ui, (8c) kxp,i(t) − xp,j(t)k ≥ dsafe, (8d) where f denotes the dynamic characteristics of vehicles. Note kxp,i(t) − po(t)k ≥ dsafe, (8e) that the position vector x (t) ∈ np is included in the state p R ∀t ∈ T , ∀j ∈ ν(i), ∀o ∈ O, ∀i ∈ V, vector x, i.e., x(t) = (xp(t), ··· ). ` : n → 2) Cost Function: Given the predicted time set T = [t0, tf ) where the terminal cost function f,i R R is kx (t ) − x (t )k2 , and the matrix P is the weighting with the initial time t0 and terminal time tf , the cost function i f r,i f Pi i of the finite horizon optimal control problem for the ith vehicle matrix for the terminal state at final time tf . is defined as III.PROBLEM REFORMULATION AND CONVEXIFICATION Z In order to solve the problem (8) efficiently, the nonlinear min ì(xi(t), ui(t)) dt + `f,i(xi(tf )), (4) xi,ui T and nonconvex constraints need to be discretized and convexified such that the SCP approach can be used to determine its n m where ì : R × R → R is the cost-to-go regard- (near-)optimal solution for the collaboration problem. ing the states xi(t) and ui(t) at time t ∈ [t0, tf ), and n `f,i : R → R is the terminal cost with respect to the A. Discretization and Linearization terminal state xi(tf ) at time tf . In this paper, we define 2 2 First, we need to convert the nonlinear equality con- ì(xi(t), ui(t)) = kxi(t)k +kui(t)k where Qi and Ri are Qi Ri straint (8b) into a constraint that can be used in direct op- the weighting matrices with respect to xi(t) and ui(t). Further 2 timization by using a zero-order hold discretization approach. define `f,i(xi(tf )) = kxi(tf )k with the terminal weighting Pi To find the numerical solution, the prediction time domain matrix Pi. [t0, tf ] can be discretized into T equal time interval with T +1 3) Constraints: The restriction on the control input variable tf −t0 nodes. The time step size τs = , and the discretized should be taken into consideration; hence the box constraint T nodes are denoted as {t0, t1, ··· , tT } with tk+1 = tk + τs. of the input variable is introduced as For the sake of brevity, we express {t0, t1, ··· , tT } as the T set Z0 = {0, 1, ··· ,T − 1}. Furthermore, set T as the final u ui(t) ui, (5) i discrete time step T = tf . Define the state vector as xiτ = xi(tτ ) and the input vector u , u where i i denote the minimum value and maximum value as uiτ = ui(tτ ) for xi(t) and ui(t) at time tτ ∈ [t0, tf ). Then, of the input variables, respectively. the stacked state and control input variables can be written as Besides the box constraints, the collision avoidance con- xi = {xiτ } T and ui = {uiτ } T , respectively. ∀τ∈Z1 ∀τ∈Z1 straints for the CAVs need to be satisfied as well, which gives Since the objective of the control problem is to reduce the deviation between the real and reference trajectories as much kxp,i(t) − xp,j(t)k ≥ dsafe, ∀j ∈ ν(i), ∀t ∈ [t0, tf ], (6) as possible and maintain the satisfaction of collision avoidance constraints, the cost function in the MPC problem is given by ν(i) i where denotes the set of neighbors of the th vehicle and T −1 T −1 np xp,i(t) ∈ is the position vector of the ith vehicle at time X 2 X 2 2 R Li = kxiτ − riτ k + kuiτ k + kxiT − riT k . (9) t. Qi Ri Pi τ=1 τ=0 Additionally, the vehicle-to-obstacle distances should also In SCP, the nonlinear dynamics constraints are sequentially be considered in our paper. Thus, the collision avoidance linearized on the nominal solutions. Given the initial nominal constraints between vehicles and obstacles are defined as solution as an initial guess, the solution of the preceding iteration in SCP can be utilized for the succeeding nominal kxp,i(t) − po(t)k ≥ dsafe, ∀o ∈ O, ∀t ∈ [t0, tf ], (7) solutions. Assume the current SCP iteration is the kth iteration. (k−1) (k−1) Define xîτ = xiτ and uîτ = uiτ as the previous where po(t) is the position vector of the oth obstacle at time corresponding state vector and control input at the previous t, and the set O denotes all obstacles in the environment. (k − 1)th SCP iteration, respectively. The linearized dynamics 4) Problem Formulation: A finite-horizon optimal control constraints can be written as problem can be formulated regarding the collaboration of x = A(ˆx )x + B(û )u − C(ˆx , uˆ ), (10) CAVs. The objective of this problem is to calculate the optimal i(τ+1) iτ iτ iτ iτ iτ iτ control inputs such that every vehicle can follow its reference where trajectory without collisions. The MPC problem considers the ∂f ∂f A(ˆx ) = ,B(û ) = , dynamic model of the vehicles, the control input limitation, iτ xîτ ,uîτ iτ xîτ ,uîτ ∂xiτ ∂uiτ

3 and C(ˆxiτ , uîτ ) = f(ˆxiτ , uîτ ) − A(ˆxiτ )ˆxiτ − B(ûiτ )ûiτ . The cost function Li for the ith vehicle can be rewritten as T −1 T −1 X 2 X 2 2 Li = kxiτ − riτ k + kuiτ k + kxiT − riT k B. QCQP formulation Qi Ri Pi τ=1 τ=0 T n Define the vector xi = (xi1, xi2, ··· , xiT ) ∈ , the > > R = (xi − ri) Qî(xi − ri) + u Rîui, (14) vector x = (x , x , ··· , x , ··· , x ) ∈ NT n. Similarly, we i 1 2 i N R NT m where Qî = blockdiag Qi, ··· ,Qi,Pi and Rî = IT ⊗ Ri. have the vector u = {uiτ }∀τ∈ T ,∀i∈V ∈ R . Given the Z1 | {z } initial state vector xi0 for vehicle i ∈ V, we can rewrite (8) as T −1 Define u = {u } ∈ NT m, x = {x } ∈ X i ∀i∈V R 0 i0 ∀i∈V min L Nn NT n i R , and x = {xi}∀i∈V ∈ R . For all vehicles i ∈ V, i∈V the total cost of all vehicles is s. t. xi(τ+1) = A(ˆxiτ )xiτ + B(ûiτ )uiτ + C(ˆxiτ , uîτ ), X L = (x − r)>Q(x − r) + u>Ru, (15) ui uiτ ui, i∈V kxp,iτ − xp,jτ k ≥ dsafe, where r = {ri}∀i∈V is the concatenated reference vec- kxp,iτ − poτ k ≥ dsafe, tor for all vehicles ∀i ∈ V, and the weighting matrices T −1 ˆ ˆ ˆ NT n×NT n ∀τ ∈ Z0 , ∀j ∈ ν(i), ∀o ∈ O, ∀i ∈ V. (11) Q = blockdiag Q1, Q2, ··· , QN ∈ R and R = ˆ ˆ ˆ NT m×NT m The nonconvex collision-avoidance optimization problem blockdiag R1, R2, ··· , RN ∈ R . for CAVs can be intuitively formulated as a nonconvex QCQP, Concatenate the dynamics constraints for the ith vehi- which can be convexified by using a convex approximation cle (12) as the constraints for all vehicles ∀i ∈ V, which can and further solved via SCP. The reason why we formulate the be represented by collision-avoidance optimization problem as a QCQP primar- x = Ax0 + Bu + C, (16) ily lies in the characteristics of the cost function, which is ˆ ˆ ˆ NT n×Nn usually chosen to be quadratic. Besides, quadratic inequality where A = blockdiag(A1, A2, ··· , AN ) ∈ R , B = blockdiag(Bˆ , Bˆ , ··· , Bˆ ) ∈ NT n×NT m, and C = constraints are often used to describe collision avoidance, 1 2 N R ˆ ˆ ˆ NT n which means the distance among vehicles should be no less C1, C2, ··· , CN ∈ R . Substitute (16) into (15) for all than a predefined safety distance. vehicles, and then the total cost can be expressed as a quadratic ˆ ˆ ˆ Define A(ˆxiτ ) = Aiτ , B(ˆxiτ ) = Biτ , C(ˆxiτ , uîτ ) = Ciτ , form, denoted by the dynamics constraints can be rewritten as X > Li = u Φ0u + Ψ0u + γ0, (17) ˆ ˆ ˆ xi(τ+1) = Aiτ xiτ + Biτ uiτ + Ciτ . (12) i∈V > > where Φ0 = B QB + R, Ψ0 = 2 (Ax0 + C − r) QB, and The stacked state vector for the ith vehicle is represent by > T n γ0 = (Ax0 + C − r) Q (Ax0 + C − r). xi = {xiτ }∀τ∈ T ∈ R . Set ui = {uiτ }∀τ∈ T −1 ∈ Z1 Z0 The collision avoidance constraints in continuous time (6) T m R , and then it follows that can be rewritten in the form of discrete-time domain as ˆ ˆ ˆ > 2 xi = Aixi0 + Biui + Ci, (13) (xp,iτ − xp,jτ ) (xp,iτ − xp,jτ ) ≥ dsafe, T ˆ T n×n ∀i ∈ V, ∀j ∈ ν(i), ∀τ ∈ , (18) where xi0 is the measured state vector at time t, Ai ∈ R , Z1 T n×T m T n (τ) (τ) (τ) Bî ∈ , and Cî ∈ are given by ˆ ˆ ˆ R R where xp,jτ = Aj xj0 + Bj uj + Cj . Define the lin- NT n np     ear operator E(ij)τ : → is used to extract Aî0 Cî0 R R xiτ − xjτ from x. It can be regarded as a matrix E =  .   .  (ij)τ  .   .   τ   τ τ  0n ×T n ··· 0n ×T n Eiτ 0n ×T n ··· 0n ×T n  Q ˆ   P Q ˆ ˆ ˆ  p p p p ˆ  Aik  ˆ  AikCi(s−1) + Ciτ  | {z } | {z } Ai = k=0  , Ci =  s=1 k=s  , i−1 j−i−1  .   .   .   .  np×NT n  .   .  − Ejτ 0np×T n ··· 0np×T n ∈ R with Eiτ = T −1  T −1 T −1  | {z }  Q ˆ   P Q ˆ ˆ ˆ  N−j−1 Aik AikCi(s−1) + Ci(T −1) k=0 s=1 k=s np×T n 0np×(τ−1)n Inp 0np×((T −τ)n−np) ∈ R . Then, the   vehicles-avoidance constraints (18) can be converted to Bî0 ··· 0 ··· 0 >  ......  u Φ(ij),τ u + Ψ(ij),τ u + γ(ij),τ ≤ 0,  . . . . .   τ  ∀i ∈ V, ∀j ∈ ν(i), ∀τ ∈ T , (19)  Q  Z1 AîkBî0 ··· Bîτ ··· 0 ˆ   > > Bi = k=1  . where the matrix Φ = −Bˆ E E Bˆ, Ψ =   (ij),τ (ij)τ (ij)τ (ij),τ  . . . . .  > ...... ˆ ˆ > ˆ   −2 Ax0 + C E E(ij)τ B, and the scalar γ(ij),τ = T −1 T −1  (ij)τ  Q ˆ ˆ Q ˆ ˆ ˆ  > AikBi0 ··· AikBiτ ··· Bi(T −1) d2 − Aˆx + Cˆ E> E Aˆx + Cˆ . k=1 k=τ+1 safe 0 (ij)τ (ij)τ 0

4 The collision avoidance constraints between vehicles and The inequalities (24) are convex with locally approximating obstacles in the environment (7) in continuous time are equiv- the concave ones. Besides, the approximation is an upper alent to (20) in discrete time. bound of the original concave ones, i.e.,

> 2 (Ψ + 2˜u>Φ )u + (γ − u˜>Φ u˜) (xp,iτ − poτ ) (xp,iτ − poτ ) ≥ dsafe, (ij),τ (ij),τ (ij),τ (ij),τ T > ∀i ∈ V, ∀o ∈ O, ∀τ ∈ Z1 , (20) ≥ u Φ(ij),τ u + Ψ(ij),τ u + γ(ij),τ , > > (Ψ(io),τ + 2˜u Φ(io),τ )u + (γ(io),τ − u˜ Φ(io),τ u˜) where poτ is the position vector of the oth obstacle at time > ≥ u Φ(io),τ u + Ψ(io),τ u + γ(io),τ , stamp τ, and xp,iτ = Eiτ x. Substituting the expression of T xp,iτ into (20), we can derive ∀i ∈ V, ∀j ∈ ν(i), ∀o ∈ O, ∀τ ∈ Z1 . (25)

> Therefore, the approximated inequalities are more conser- u Φ(io),τ u + Ψ(io),τ u + γ(io),τ ≤ 0, vative, compared with the original concave ones. Then, the ∀i ∈ V, ∀o ∈ O, ∀τ ∈ T , (21) Z1 resulting problem is where Φ = −B>E> E B, Ψ = > (io),τ iτ iτ (io),τ min u Φ0u + Ψ0u + γ0 −2 (E Ax + E C − p )> E B, and γ = u iτ 0 iτ oτ iτ (io),τ > > > 2 s. t. (Ψ(ij),τ + 2˜u Φ(ij),τ )u + (γ(ij),τ − u˜ Φ(ij),τ u˜) ≤ 0 − (Eiτ Ax0 + Eiτ C − poτ ) (Eiτ Ax0 + Eiτ C − poτ ) + dsafe. > > The box constraints of control input variable for all i ∈ V (Ψ(io),τ + 2˜u Φ(io),τ )u + (γ(io),τ − u˜ Φ(io),τ u˜) ≤ 0 can be concatenated as Ψuu ulim T ∀i ∈ V, ∀j ∈ ν(i), ∀o ∈ O, ∀τ ∈ Z1 . (26) Ψuu ulim, (22) Remark 1. In an optimization problem, restriction of the 1 where Ψ = ⊗ I ∈ 2NT m×NT m, and u = constraints can result in a smaller feasible region, a subset u −1 NT m R lim of the original feasible region. Thus, the minimal cost of the {1 ⊗ u } T i ∀i∈V ∈ 2NT m. restricted optimization problem (26) is no less than that of the {1 ⊗ −u } R T i ∀i∈V original problem. Therefore, a feasible restricted optimization Thus, we can formulate this nonconvex programming prob- problem (26) can provide a solution with a higher cost, com- lem in a nonconvex QCQP form, which is represented by pared with the original nonconvex optimization problem (23), > which means the restricted problem (26) gives an upper bound min u Φ0u + Ψ0u + γ0 u for the nonconvex one. A solution with a lower cost will be > s. t. u Φ(ij),τ u + Ψ(ij),τ u + γ(ij),τ ≤ 0 generated by solving the problem (26) around the resulted u>Φ u + Ψ u + γ ≤ 0 solution iteratively for the reason that the convex inequality (io),τ (io),τ (io),τ constraints in (26) are the upper bound of the original concave Ψuu ulim inequality constraints in (23). T ∀i ∈ V, ∀j ∈ ν(i), ∀o ∈ O, ∀τ ∈ Z1 . (23)

IV. SCP ALGORITHM C. Convexification Convex optimization methods are typically used to de- In fact, the collision-avoidance constraints (19) and (21) termine the global solution with predefined accuracy rather are concave functions, due to the fact that matrices Φ efficiently. Generally, their computational time grows polyno- (ij),τ mially along with the increase of the optimization problems’ and Φ(io),τ in (19) and (21) are negative semidefinite. Thus, the optimization problem (23) is nonconvex and NP-hard. dimension and the predefined numerical accuracy for most As the dimension of the optimization problem increases, the of the convex problems. As for the nonconvex optimization computational time of most known algorithms to solve a methods, they may be capable of computing a locally optimal nonconvex QCQP (23) grows exponentially. solution as efficiently as the convex optimization methods. However, it is also possible that the global solution is deter- In this paper, an affine approximation is used to approximate mined with expensive computational cost, which means their a concave function. We know that the affine function is an computation time has exponential growth along with the opti- upper bound on the concave function, as the concave function mization problem’s dimension. Here, SCP, a local optimization lies below the derived affine function at any point. Therefore, approach, can be used to solve the nonconvex optimization the terms u>Φ u and u>Φ u in (19) and (21) can (ij),τ (io),τ problems. Particularly, SCP aims to solve a sequence of convex be convexified as a first-order Taylor series expansion at the optimization problems. The nonconvex optimization problem operating point u˜, which is given by will be converted into a sequence of convex ones, each of (Ψ + 2˜u>Φ )u + (γ − u˜>Φ u˜) ≤ 0, which can be solved using convex optimization methods to (ij),τ (ij),τ (ij),τ (ij),τ obtain the global solution efficiently. The global optimum can > > (Ψ(io),τ + 2˜u Φ(io),τ )u + (γ(io),τ − u˜ Φ(io),τ u˜) ≤ 0, then be obtained if SCP always converges to the same solution, T ∀i ∈ V, ∀j ∈ ν(i), ∀o ∈ O, ∀τ ∈ Z1 . (24) given different starting points with enough amount.

5 Since the starting point is essential for SCP, the restricted iteration k + 1. Then, ∆Lˆ can be considered as a predicted convex program (26) may be infeasible in some cases, accord- reduction of the real reduction ∆L. Since Lˆ(k+1) decreases in ing to the starting point. In order to address this issue, a slack iterations, L(k+1) also decreases. variable ω ∈ can be appended in the optimization variable R+ Remark 4. The computation time of SCP relies on its iteration u. Then, a new problem can be obtained by number to convergence. As a heuristic method, SCP may fail > u Φ0 0NT m×1 u Ψ0 to find an optimal solution, and the result of SCP depends on min + U + γ0 u,ω ω 01×NT m 0 ω ψω the quality of the starting point. However, in practice, SCP > can always obtain a feasible solution with a lower cost. (Ψ + 2˜u>Φ ) s. t. (ij),τ (ij),τ u + γ − u˜>Φ u˜ ≤ 0 −1 (ij),τ (ij),τ The pseudocode of SCP is shown in Algorithm. 1. > (Ψ + 2˜u>Φ ) (io),τ (io),τ u + γ − u˜>Φ u˜ ≤ 0 Algorithm 1 SCP Algorithm for collaboration of CAVs at tth −1 (io),τ (io),τ step. Ψ u u , ω > 0 u lim Initialization: dynamic model for all vehicles i ∈ V; com- T ∀i ∈ V, ∀j ∈ ν(i), ∀o ∈ O, ∀τ ∈ Z1 , (27) munication network G(V, E); parameters M, Pijτ ; weighting matrices Qi and Ri in the cost function; upper and lower where ψω is a scalar weighting value for the additional slack variable ω. bound of the state x and control input u, i.e., x, x and u, u, respectively; the maximum iteration number of SCP, i.e., Remark 2. A slack variable is introduced in the optimization kscp; the initial state xi,0 at initial timestamp τ = 0 for all problem (27), in order to substitute the constant trust-region vehicle i ∈ V; the prediction horizon T ; the accuracy . size. This can increase the numerical stability during solving Set the SCP iteration k = 0. by guaranteeing that the solution persists to be close to the Determine the starting point u(k). linearization operating point. while L(k) − L(k+1) > do (k) It is obvious that the problem (27) is always feasible Evaluate the cost of the nonconvex problem L based (k) with introducing ω. Actually, ω is related to the maximal on u and the constraints violations. violation of the collision-avoidance constraints under a large Approximate the nonconvex inequality constraints based (k) enough penalty ψ . The violation of the collision-avoidance on u by (24). ω (k+1) constraints occurs when no feasible solution to the original Solve the optimal solution u of the resulted convex problem (23) can be obtained. Similar to the exact penalty problem (27). (k+1) (k+1) method, (27) can be considered as Evaluate the cost L based on the computed u and the constraints violations. > min u Φ0u + Ψ0u + γ0 + ψω max 0, u k = k + 1. > end while u Φ(ij),τ u + Ψ(ij),τ u + γ(ij),τ , > u Φ(io),τ u + Ψ(io),τ u + γ(io),τ , T ∀i ∈ V, ∀j ∈ ν(i), ∀o ∈ O, ∀τ ∈ Z1 . (28) V. NUMERICAL VALIDATIONS Remark 3. Since both the cost and the constraint violations A. Simulation Setting are contained in (28), both the cost value and constraint The weighting matrices in the MPC problem are set as Q = violation can be reduced in this procedure. i In, Ri = 4000Im, and Pi = 20In. The reason why we give We can use the cost value (28) to evaluate the solving Ri a higher value is that the more penalty on the control input process for the original problem (23). Indeed, a feasible ap- will lead to better driving comfort and smoother trajectories. proximated program (27) means a feasible original nonconvex Set the prediction horizon T = 10. The length and width problem, due to the fact that the affine approximation of a of vehicles in the simulation are set to be 0.98 and 0.88 m, concave constraint makes the constraint more conservative, respectively. Set lf = lr = 0.34 m, τd = 0.03 s, and τs = 0.01 compared with the original concave one. This also indicates s. Also, set the mechanical limitation of steering angle for all that if an inequality constraint in (23) is active, the correspond- vehicles are [−3, 3]◦. The length and width of all obstacles in ing inequality constraint in (27) is also active. Nevertheless, scenario 2 are 4 and 2 m. The length and width of all the 4 an infeasible approximated problem may not reflect that the obstacles in the scenario 3 are (2, 4, 4, 2) m and (4, 2, 2, 2) original one is infeasible (actually, it may be feasible or m, respectively. The duration of the simulation is set to be 20 infeasible). In order to overcome the infeasible nonconvex s, and the MPC sampling time is 0.4 s, which means that the problem, the cost of the problem (28) is used for evalua- MPC problem will be solved every 0.4 s. Thus, the number tion. (28) provides a measurement of the violation degree of of total steps of the simulation is 50. constraints for the infeasible original problem. Define L(k) as Besides, the corresponding discrete reference points on the cost of the nonconvex program (23) at iteration k and the reference trajectories should be calculated to evaluate ∆L = L(k) − L(k+1) as the reduction at iteration k + 1. the tracking error. Here, we use the orthogonal projection Let Lˆ(k) denote the cost of the approximated program (28) of vehicle position on the reference trajectories as the first at iteration k and ∆Lˆ = L(k) − Lˆ(k+1) be the reduction at reference point. Since this point has the shortest distance to the

6 reference trajectories, it can obtain the fastest route to reach the reference trajectory. The remaining reference points are computed from the ﬁrst reference point based on the vehicle’s velocity and the sampling time of MPC. 20 10 ] m

[ 0 0 B. Scenario 1 y In this scenario, all vehicles are initially evenly distributed in a circle with a radius of 30 m. All vehicles need to go 20 10 through the origin point (0, 0) m from the initial positions to t = 0 t = 10 the goal positions. Each different color represents a different 20 0 20 10 0 10 vehicle. Set the number of vehicles N = 8. Fig. 1 demonstrates the trajectories of all vehicles passing through the origin in the 10 different timestamp t, meanwhile maintaining the safe distance 10 50 5 of 3 m. Note that t in this ﬁgure subject to t ∈ Z1 . In ]

this ﬁgure, the dotted line and solid line mean the reference m

[ 0 0

trajectories and predicted trajectories in the prediction horizon. y According to Fig. 1, it is evident that all vehicles can be 5 successfully driven from the initial position to the goal position 10 10 t = 14 while keeping the safety distance. t = 17 As required, all vehicles’ steering angle δi, ∀i ∈ V should 10 0 10 10 0 10 be conﬁned into the predeﬁned range [−3, 3]◦. For simplicity, the control input of the 1st vehicle is used to demonstrate the 20 restrictiveness of the control inputs, as shown in Fig. 2. The 20 10 black dashed lines mean the boundaries of inputs. Based on 10 ]

Fig. 2, the control input has been limited to the predeﬁned m

[ 0 0

range. Fig. 3 illustrates the distance among all vehicles by y 10 using different colors. The black broader solid line is to 10 represent the safety distance. It is straightforward to observe 20 t = 20 t = 25 that all the distances satisfy the collision avoidance constraints. 20 Therefore, the effectiveness of the proposed approach in this 20 0 20 20 0 20 scenario can be successfully validated. x [m] x [m] The efficiency of the proposed algorithm is shown in Fig. 4. The computational time per optimization and per step are Fig. 1. Trajectories of the vehicles by using SCP in scenario 1. represented by the orange and blue line in this figure. From this figure, it is straightforward to observe that the average computational time per optimization and per step are less than 0.05 s, which renders it possible for real-time implementation. 3 ] [ C. Scenario 2 3 Assume there are some moving obstacles in the environ- 0 20 40 Step ment, as shown in Fig. 5. The gray rectangles in this figure represent the obstacles moving upwards vertically. Obviously, the real position of the vehicle will maintain the safety distance Fig. 2. Steering angle of the 1st vehicle by using SCP in scenario 1. away from the moving obstacles, according to Fig. 5. Besides, the control inputs and vehicle-to-obstacle distance have also been confined into the defined ranges, as shown in Fig. 6 60 and Fig. 7, respectively. Moreover, the computational time per optimization and per step are shown in Fig. 8. 40 20 Distance [m] D. Scenario 3 0 0 20 40 In this scenario, multiple vehicles are driving in parallel Step and moving rightwards, as represented by different colors. There are several static obstacles located in the environment, Fig. 3. Distance among all vehicles by using SCP in scenario 1. (solid lines: which needs to be avoided for these vehicles. Fig. 9 presents vehicle-to-vehicle distances; gray broader solid line: safety distance.) the trajectories of 11 vehicles with avoiding the 4 obstacles

7 0.2 Comp. time per opt. 50 Comp. time per step 0.1 Distance [m] 0 Comp. time [s] 0.0 0 20 40 0 20 40 Step Step

Fig. 7. Distance between the vehicle and all obstacles by using SCP in Fig. 4. Computational time spent in optimization and ﬁnishing one step by scenario 2. (dashed lines: vehicle-to-obstacle distances; gray broader solid using SCP in scenario 1. line: safety distance.)

Comp. time per opt. 0.05 Comp. time per step 20

Comp. time [s] 0.00 ] 0 20 40 m

[ 0 Step y

20 Fig. 8. Computational time spent in optimization and ﬁnishing one step by t = 0 t = 12 using SCP in scenario 2.

at different time. Additionally, the control input of the 1st 20 vehicle, as an example to show the effectiveness of the proposed method, is successfully restricted into the deﬁned

] range, as depicted in Fig. 10. Also, the vehicle-to-vehicle m

[ 0 distances represented by the solid lines and the vehicle-to- y obstacle distances represented by dashed lines are no less than the safety distance 3 m, as shown in Fig. 11. This also reveals 20 t = 17 t = 20 the successful avoidance of the collision to other vehicles and obstacles. Similarly, the computational time per optimization and per step are displayed in Fig. 12.

20 20 ] ] m [

0 m

[ 0

y y

t = 0 t = 10 20 20 t = 25 t = 30 20 20 0 20 20 0 20 x [m] x [m] ] m

[ 0

y Fig. 5. Trajectories of the vehicle by using SCP in scenario 2. 20 t = 15 t = 20

20 3 ] m

[ 0

] y [ 20 t = 25 t = 30 3 40 20 0 20 40 40 20 0 20 40 0 20 40 x [m] x [m] Step

Fig. 6. Steering angle of the vehicle by using SCP in scenario 2. Fig. 9. Trajectories of the vehicles by using SCP in scenario 3.

8 3 1.0 ] [ 0.5 3 0 20 40 0.0

Step Comp. time per step [s]

Fig. 10. Steering angle of the 1st vehicle by using SCP in scenario 3. 1.0

0.5 60 0.0 40 Comp. time per opt. [s] 0 10 20 30 40 Step 20 Distance [m] 0 0 20 40 Fig. 13. Computational time spent in optimization and ﬁnishing one step by Step using SCP in scenario 1.

Fig. 11. Distance among all vehicles and among the vehicles and all obstacles by using SCP in scenario 3. (solid lines: vehicle-to-vehicle distances; dashed derive that the SCP approach can obtain the lower cost value, lines: vehicle-to-obstacle distances; gray broader solid line: safety distance.) compared with the result of MIQP, which is represented by the same color with different line-styles. This also reveals the E. Comparison with MIQP compelling effectiveness of SCP, as compared to the widely- used MIQP method. The commonly-used MIQP approach is deployed as a comparison to show the effectiveness and efficiency of the proposed approach. Here, taking the scenario 1 as an example, Fig. 13 exhibits the computational time per optimization and 3500 SCP MIQP per step in scenario 1 with different number of vehicles ranging 3000 from N = 3 to N = 20. In this figure, the darker line shows the result of more vehicles, and the lighter line represents that 2500 of fewer vehicles. Based on this figure, the more vehicle this 2000 scenario has, the larger the problem size is, and the more time 1500

it spend. Cost value Here, we use the scenario 1 as an example to show the 1000 effectiveness of the proposed SCP approach. Fig. 14 displays the comparison of cost values in each step with the use of 500 SCP and MIQP in scenario 1, with different vehicle numbers. 0 The results of varying vehicle numbers are clarified by using 0 20 40 different colors, and the two line-styles are used to distinguish Step the result of SCP and MIQP. In this figure, there are some relatively higher cost values from the 5th step to the 26th step, Fig. 14. Cost value by using SCP and MIQP in scenario 1. which means there are larger deviations from the reference trajectories, due to the requirement of collision avoidance. Obviously, the more vehicles existing in scenario 1 result Fig. 15 illustrates the comparison of computation time in a larger cost value. According to Fig. 14, we can also per optimization and per step by using SCP and MIQP in scenario 1 with different vehicle numbers, respectively. In this figure, the green dashed, and solid lines mean the average computational time spent in solving each optimization problem 0.4 Comp. time per opt. and each complete step in the MPC problem by using MIQP, Comp. time per step respectively. Similarly, the average computational time per 0.2 optimization and per step that the SCP costs are indicated

Comp. time [s] by the orange dashed and solid lines. From Fig. 15, the 0.0 0 20 40 computational time per optimization and per step of SCP are Step dramatically less than the computational time of MIQP, which effectively displays the efﬁciency of SCP. This is because Fig. 12. Computational time spent in optimization and ﬁnishing one step by MIQP is NP-complete and can be solved in exponential time using SCP in scenario 3. and SCP can be solved in polynomial time.

9 [8] X. Zhang, J. Ma, Z. Cheng, S. Huang, C. W. de Silva, and T. H. Lee, 1.2 Opt. time of SCP “Accelerated hierarchical ADMM for nonconvex optimization in multi- agent decision making,” arXiv preprint arXiv:2011.00463, 2020. Opt. time of MIQP [9] P. Hang, C. Lv, C. Huang, J. Cai, Z. Hu, and Y. Xing, “An integrated 1.0 Step time of SCP framework of decision making and motion planning for autonomous Step time of MIQP vehicles considering social behaviors,” arXiv preprint arXiv:2005.11059, 0.8 2020. [10] P. Hang, C. Lv, Y. Xing, C. Huang, and Z. Hu, “Human-like decision 0.6 making for autonomous driving: A noncooperative game theoretic approach,” IEEE Transactions on Intelligent Transportation Systems, 2020. [11] J. Chen, W. Zhan, and M. Tomizuka, “Autonomous driving motion plan- Comp. time [s] 0.4 ning with constrained iterative LQR,” IEEE Transactions on Intelligent Vehicles, vol. 4, no. 2, pp. 244–254, 2019. 0.2 [12] Y. Bian, S. E. Li, W. Ren, J. Wang, K. Li, and H. Liu, “Cooperation of multiple connected vehicles at unsignalized intersections: Distributed 0.0 observation, optimization, and control,” IEEE Transactions on Industrial 3 4 5 6 7 8 Electronics, 2019. the ith vehicle [13] S. Manzinger, C. Pek, and M. Althoff, “Using reachable sets for trajectory planning of automated vehicles,” IEEE Transactions on Intelligent Vehicles, 2020. Fig. 15. Computational time spent in optimization and finishing one step by [14] F. Mohseni, E. Frisk, and L. Nielsen, “Distributed cooperative mpc for using SCP and MIQP in scenario 1. autonomous driving in different traffic scenarios,” IEEE Transactions on Intelligent Vehicles, 2020. [15] X. Zhang, J. Ma, Z. Cheng, S. Huang, S. S. Ge, and T. H. Lee, “Trajectory generation by chance constrained nonlinear MPC with VI.CONCLUSION probabilistic prediction,” IEEE Transactions on Cybernetics, 2020. [16] J. Ji, A. Khajepour, W. W. Melek, and Y. Huang, “Path planning and The collaborative motion planning problem of multiple tracking for vehicle collision avoidance based on model predictive control with multiconstraints,” IEEE Transactions on Vehicular Technology, CAVs is investigated in this paper. A general nonlinear and vol. 66, no. 2, pp. 952–964, 2016. nonconvex MPC problem is formulated to describe the col- [17] X. Zhang, J. Ma, S. Huang, Z. Cheng, and T. H. Lee, “Integrated laboration of CAVs, which requires all CAVs to finish their planning and control for collision-free trajectory generation in 3D driving tasks and avoid potential collisions with each other environment with obstacles,” in 2019 19th International Conference on Control, Automation and Systems (ICCAS). IEEE, 2019, pp. 974–979. as well as obstacles. Then, a real-time SCP approach is [18] J. Ma, Z. Cheng, X. Zhang, A. A. Mamun, C. W. de Silva, and T. H. presented for the nonlinear and nonconvex MPC problem Lee, “Data-driven predictive control for multi-agent decision making to improve the performance under nonlinear and nonconvex with chance constraints,” arXiv preprint arXiv:2011.03213, 2020. [19] S. Boyd, S. P. Boyd, and L. Vandenberghe, Convex optimization. New constraints. Hence, it facilitates the practical implementation York: Cambridge University Press, 2004. in real time. In this method, linearization and discretization [20] L. E. Sokoler, A. Skajaa, G. Frison, R. Halvgaard, and J. B. Jørgensen, are necessary to deal with the nonlinear dynamics constraints. “A warm-started homogeneous and self-dual interior-point method for linear economic model predictive control,” in 52nd IEEE Conference on Also, convexification is used to address the constraints with Decision and Control. IEEE, 2013, pp. 3677–3683. nonconvex characteristics. At last, three different scenarios [21] C. Liu, S. Lee, S. Varnhagen, and H. E. Tseng, “Path planning for in autonomous driving tasks are considered to validate the autonomous vehicles using model predictive control,” in 2017 IEEE Intelligent Vehicles Symposium (IV). IEEE, 2017, pp. 174–179. effectiveness and efficiency of our proposed approach. [22] I. Necoara, D. N. Clipici, and S. Olaru, “Distributed model predictive control of leader-follower systems using an interior point method with efficient computations,” in 2013 American Control Conference. IEEE, REFERENCES 2013, pp. 1697–1702. [23] J. Liu, P. Jayakumar, J. L. Stein, and T. Ersal, “A nonlinear model [1] W. Lucia, A. Venturino et al., “A distributed model predictive control predictive control formulation for obstacle avoidance in high-speed strategy for constrained multi-vehicle systems moving in unknown autonomous ground vehicles in unstructured environments,” Vehicle environments,” IEEE Transactions on Intelligent Vehicles, 2020. System Dynamics, vol. 56, no. 6, pp. 853–882, 2018. [2] J. K. Subosits and J. C. Gerdes, “From the racetrack to the road: Real- [24] Z. Cheng, J. Ma, X. Zhang, F. L. Lewis, and T. H. Lee, “Neural net- time trajectory replanning for autonomous driving,” IEEE Transactions work iLQR: A new reinforcement learning architecture,” arXiv preprint on Intelligent Vehicles, vol. 4, no. 2, pp. 309–320, 2019. arXiv:2011.10737, 2020. [3] S. E. Li, Y. Zheng, K. Li, Y. Wu, J. K. Hedrick, F. Gao, and [25] J. Ma, Z. Cheng, X. Zhang, M. Tomizuka, and T. H. Lee, “Alternat- H. Zhang, “Dynamical modeling and distributed control of connected ing direction method of multipliers for constrained iterative LQR in and automated vehicles: Challenges and opportunities,” IEEE Intelligent autonomous driving,” arXiv preprint arXiv:2011.00462, 2020. Transportation Systems Magazine, vol. 9, no. 3, pp. 46–58, 2017. [26] F. Fahroo and I. M. Ross, “Advances in pseudospectral methods for [4] Z. Wang, Y. Bian, S. E. Shladover, G. Wu, S. E. Li, and M. J. optimal control,” in AIAA Guidance, Navigation and Control Conference Barth, “A survey on cooperative longitudinal motion control of multiple and Exhibit, 2008, p. 7309. connected and automated vehicles,” IEEE Intelligent Transportation [27] S. E. Li, S. Xu, and D. Kum, “Efficient and accurate computation of Systems Magazine, vol. 12, no. 1, pp. 4–24, 2019. model predictive control using pseudospectral discretization,” Neuro- [5] K. Li, Y. Bian, S. E. Li, B. Xu, and J. Wang, “Distributed model pre- computing, vol. 177, pp. 363–372, 2016. dictive control of multi-vehicle systems with switching communication [28] A. V. Rao, “A survey of numerical methods for optimal control,” topologies,” Transportation Research Part C: Emerging Technologies, Advances in the Astronautical Sciences, vol. 135, no. 1, pp. 497–528, vol. 118, p. 102717, 2020. 2009. [6] B. Xu, X. J. Ban, Y. Bian, W. Li, J. Wang, S. E. Li, and K. Li, [29] S. A. Fayazi and A. Vahidi, “Mixed-integer linear programming for “Cooperative method of traffic signal optimization and speed control optimal scheduling of autonomous vehicle intersection crossing,” IEEE of connected vehicles at isolated intersections,” IEEE Transactions on Transactions on Intelligent Vehicles, vol. 3, no. 3, pp. 287–299, 2018. Intelligent Transportation Systems, vol. 20, no. 4, pp. 1390–1403, 2018. [30] A. Richards and J. P. How, “Aircraft trajectory planning with collision [7] P. Hang, X. Chen, and W. Wang, “Cooperative control framework for avoidance using mixed integer linear programming,” in Proceedings of human driver and active rear steering system to advance active safety,” the 2002 American Control Conference, vol. 3. IEEE, 2002, pp. 1936– IEEE Transactions on Intelligent Vehicles, 2020. 1941.

10 [31] A. Alonso-Ayuso, L. F. Escudero, and F. J. Martín-Campo, “Collision avoidance in air trafﬁc management: A mixed-integer linear optimization approach,” IEEE Transactions on Intelligent Transportation Systems, vol. 12, no. 1, pp. 47–57, 2010. [32] C. Burger and M. Lauer, “Cooperative multiple vehicle trajectory planning using MIQP,” in 2018 21st International Conference on Intelligent Transportation Systems (ITSC). IEEE, 2018, pp. 602–607. [33] Z. Zhang, J. Li, and J. Wang, “Sequential convex programming for nonlinear optimal control problems in uav path planning,” Aerospace Science and Technology, vol. 76, pp. 280–290, 2018. [34] X. Huang and H. Peng, “Speed trajectory planning at signalized intersections using sequential convex optimization,” in 2017 American Control Conference (ACC). IEEE, 2017, pp. 2992–2997. [35] R. Rajamani, Vehicle Dynamics and Control. New York, NY, USA: Springer Science & Business Media, 2011.