# Sequential Convex Programming for Nonlinear Optimal Control in UAV Trajectory Planning
## 2 Dynamic Model  
Based on the given set of initial and final positions for N drones, the objective was to ensure that these drones reached their intended positions within a specified time interval, T, while keeping a minimum distance of at least R from each other during their trajectories. An optimization problem was formulated to accomplish this goal, which involved several convex and affine constraints and a non-convex collision avoidance constraint. The solution to this optimization problem guaranteed the achievement of the specified objective.  
Figure 1: Dynamic model of a quadcopter.  
The goal was to generate trajectories for N agents that do not collide, allowing a transition from an initial set of states to a final set of states (position, velocity, and acceleration) within a specified time T. The trajectories must adhere to many boundaries and avoidance constraints.  
The quadcopter was modeled as a rigid body with six degrees of freedom: it can linearly translate along the inertial \(x_{3}\) axes and rotate around these three inertial axes. The rotation is described by the frame attached to the body relative to the inertial frame, using the proper orthogonal matrix \(\mathbf{R}\). The control inputs to the system were taken as the total thrust produced, denoted as f, for simplicity, normalized by the vehicle mass, with units of acceleration. The body rates, expressed as \(\boldsymbol{\omega}=(\omega_{1},\omega_{2},\omega_{3})\), are considered in the body-fixed frame. \((x_{1},x_{2},x_{3})\) represents the position of the quadcopter in inertial space. An illustration of this model can be seen in Figure 1.  
Euler angles describe the orientation of a rigid body in space. Each angle represents the rotation of the quadrotor in a certain axis. It is used to construct the rotation matrices (\(Rx_{1}\), \(Rx_{2}\), \(Rx_{3}\)). These rotation matrices map the body frame onto the inertial frame. Multiplying the three rotation matrices in a x\({}_{3}\)-x\({}_{2}\)-x\({}_{1}\) order will yield the final orientation of the object. The final rotation matrix can be expressed as follows:  
\[\begin{array}{l}R_{x_{3}x_{2}x_{3}}(\phi,\theta,\psi)=R_{x_{3}}(\psi)R_{x_{2 }}(\theta)R_{x_{1}}(\phi)\\ c(\theta)c(\psi)\quad\quad s(\phi)s(\theta)c(\psi)-c(\phi)s(\psi)\quad c(\phi)s( \theta)c(\psi)+s(\phi)s(\psi)\\ c(\theta)s(\psi)\quad s(\phi)s(\theta)s(\psi)+c(\phi)c(\psi)\quad c(\phi)s( \theta)s(\psi)-s(\phi)c(\psi)\\ -s(\theta)\quad s(\phi)c(\theta)\quad c(\phi)c(\theta)\end{array} \tag{1}\]  
where \(\phi,\theta\), \(\psi\) are the quadrotor's roll, pitch, and yaw angles, respectively, s represents sin, and c represents cos.  
The rotational dynamics of a quadcopter can be described by [PERSON]'s equations of motion, as follows:  
\[\mathbf{I}\dot{\boldsymbol{\omega}}+\boldsymbol{\omega}\times(\mathbf{I} \boldsymbol{\omega})=\boldsymbol{\tau} \tag{2}\]  
where \(\mathbf{I}\) is the inertia matrix, \(\boldsymbol{\omega}\) is the angular velocity vector, and \(\boldsymbol{\tau}\) represents the torque vector.  
The differential equations governing the flight of the quadcopter are now those of a rigid body, as follows:  
\[\tilde{x}=\mathbf{R}\mathrm{e}_{3}f+\mathbf{g} \tag{3}\]  
\[\dot{\mathbf{R}}=\mathbf{R}[\boldsymbol{\omega}\times] \tag{4}\]  
With \(\mathrm{e}_{3}=(0,0,1)\) and \([\boldsymbol{\omega}\times]\) denoting the skew-symmetric matrix form of the vector cross product, such that:  
\[[\boldsymbol{\omega}\times]=\begin{bmatrix}0&-\omega_{3}&\omega_{2}\\ \omega_{3}&0&-\omega_{1}\\ -\omega_{2}&\omega_{1}&0\end{bmatrix} \tag{5}\]  
and \(\mathbf{g}=(0,0,-g)\) represents the acceleration due to gravity.