# Sequential Convex Programming for Nonlinear Optimal Control in UAV Trajectory Planning
## 3 Problem Formulation
### Sequential Convex Programming  
Sequential Convex Programming (SCP) is a local optimization method for non-convex problems that leverage convex optimization. We considered the non-convex problem as follows:  
\[\min f_{0}(x)\] \[s.t.f_{i}(x)\leq 0,\forall i=1,2,\ldots,m\] \[h_{j}(x)=0,\forall j=1,2,\ldots,p\]  
with \(x\in\mathbb{R}^{n}\), \(f_{0}(x)\) and \(f_{i}(x)\) non-convex and \(h_{j}\) non-affine. In the \(k^{th}\) iteration of SCP, we maintained the estimate \(x^{(k)}\) of the solution, and the convex trust region \(\tau^{(k)}\subset\mathbb{R}^{n}\). We constructed convex approximations \(\overline{f}_{0}\) and \(\overline{f}_{i}\) of \(f_{0}\) and \(f_{i}\), respectively, and affine approximations \(\overline{h}_{j}\) of \(h_{j}\) over the trust region \(\tau^{(k)}\). The approximate convex problem obtained is written as follows:  
\[\min\overline{f}_{0}(x)\] \[\overline{f}_{i}(x)\leq 0,\forall i=1,2,\ldots,m\] \[\overline{h}_{j}(x)=0,\forall j=1,2,\ldots,p\]  
The solution to this approximate problem is \(x^{(k+1)}\). In summary, the specific algorithm for trajectory optimization using SCP is depicted as Algorithm 1:  
```
```
1:N,R, T, \(f_{threshold}\), \(p_{\max}\), \(p_{\min}\), \(v_{\min}\), \(a_{\max}\), \(a_{\min}\), \(j_{\max}\), \(j_{\min}\)
2:\(\mathbf{X}^{0}\leftarrow\mathbf{QP}\Big{(}A_{eq},b_{eq},A_{1,feasibility},b_{1,feasibility}\Big{)}\)
3:\(f_{p}\gets f\Big{(}\mathbf{X}^{0}\Big{)}\)
4:\(n_{i}\gets 0\)
5:while\(n_{i}\leq m_{i}\)do
6:\(\mathbf{X}\leftarrow\mathbf{QP}\Big{(}A_{eq},b_{eq},A_{feasibility}b_{feasibility}\Big{)}\)
7:\(f_{c}\gets f(\mathbf{X})\)
8:if\(|f_{c}-f_{q}|<f_{threshold}\)then
9: break
10:\(f_{p}\gets f_{c}\)
11:\(n_{i}\gets n_{i}+1\)
12:end while
```

**Algorithm 1**Trajectory Optimization Using SCP

## 4 Experiment

This section presents four case studies and demonstrates the performance of the suggested strategy through simulation data. The algorithm was executed using MATLABR2022b on a PC with an Intel i9-13900 HX CPU operating at a clock cycle of 2.2 GHz and 16.0 GB RAM. Specifically, we used MATLAB's quadratic programming function, quadprop. The case study examines two, four, six, and eight quadcopters operating within a three-dimensional space of \(5\times 5\times 3\), respectively. Initially, the simulation was executed without imposing any distance constraints. Figures 2a, 3a, 4a and 5 illustrate that the algorithm produced the optimal trajectories, i.e., the shortest distances between the initial and final locations. Due to the absence of constraints on collision, the trajectories intersect at some points. For the first iteration of the optimization loop of SCP, we did not consider the collision avoidance constraint and we formulated our QP problem using only the position, velocity, acceleration, and jerk constraints; therefore, the total inequality coefficient matrices, as well as the residual matrices, for a single drone were obtained by stacking all the coefficient matrices and residual matrices vertically. In the first iteration, we did not have any previous solution to linearize our collision constraint; therefore, we did not consider the collision constraint in the first iteration. However, from the second iteration onwards, we enforced the collision avoidance constraints, since we had a previous solution around which we could linearize the collision avoidance constraint. Figures 2-10 show the simulation results of two, four, six, and eight quadcopters in the absence of collision avoidance constraints and collision-free. The parameters used for the case study were chosen as follows:

1. Number of agents = 2 Simulation length = 30 s Discretization step-size, dt = 0.2 s Cost Threshold, \(f_{threshold}\) = 0.05 Run time for the first step of the algorithm = 2.45 s Total run time = 35 s
2.