## 3 Core Algorithm Analysis
### Representation of 4D Trajectory and Conflict Detection
#### 3.2.1 Distributed Dynamic Optimization Approach  
The _UAVi_ trajectory is discrete and the sampling interval is \(\Delta t\). Therefore, in the \(m\)-th moment, the related position and pitch angle of _UAVi_ are \(x_{i}(m),y_{i}(m),z_{i}(m),\varphi_{i}(m),m\in N\). The discrete kinematic model of _UAVi_ is:  
\[\left[ \begin{array}{c}x_{i}(m+1)\\ y_{i}(m+1)\\ z_{i}(m+1)\\ \varphi_{i}(m+1)\end{array} \right]=\left[ \begin{array}{c}x_{i}(m)+v_{i}\cos (\varphi_{i}(m))\cos\theta_{i}\cdot\Delta t\\ y_{i}(m)+v_{i}\cos(\varphi_{i}(m))\sin\theta_{i}\cdot\Delta t\\ z_{i}(m)+v_{i}(\sin\varphi_{i}(m))\cdot\Delta t\\ \varphi_{i}(m)+\omega_{t}^{i}\cdot\Delta t\end{array} \right] \tag{3}\]  
In order to generate an amending trajectory for CA, it is better if each UAV could adjust its track as few times as possible, execute the trajectory as shorter as possible and reach the target point as soon as possible. Therefore, at time \(m\), the objective function \(O_{i}(m)\) of _UAVi_ comprises three aspects above:  
\[O_{i}(m)=w_{1}C_{i}(m)+w_{2}D_{i}(m)+w_{3}T_{i}(m) \tag{4}\]  
In this formula, \(w_{1},w_{2},w_{3}\) are the corresponding weight coefficients of the three indexes \(C_{i}(m)\), \(D_{i}(m)\), \(T_{i}(m)\). Their values reflect different application contexts, e.g., \(w_{3}>w_{1}\) indicates that the controllers prefer shorter time to achieve the destination even if the fuel cost is higher. Before the computation, normalization processing is needed. The three indexes can be calculated using the following formulas. \[C_{i}(m)=\left\{ \begin{array}{c}|\varphi_{i}(m)-\varphi_{i}(m-1)|\quad if\ m\geq 1\\ 0\quad if\ m=0\end{array} \right. \tag{5}\]  
where \(C_{i}(m)\) expresses the track adjustment cost of _UAVi_, which is defined as the change value of pitch angle. \[D_{i}(m)=\|p_{i}(m)-p_{i}^{d}\|_{2}=\left[\left(x_{i}(m)-x_{i}^{d}\right)^{2}+ \left(y_{i}(m)-y_{i}^{d}\right)^{2}+\left(z_{i}(m)-z_{i}^{d}\right)^{2}\right] ^{1/2} \tag{6}\]  
where \(D_{i}(m)\) indicates the distance from current location to its destination \(p_{i}^{d}=\left(x_{i}^{d},y_{i}^{d},z_{i}^{d}\right)^{T}\). \[T_{i}(m)=m\cdot\Delta t+D_{i}(m)/v_{i} \tag{7}\]  
where \(T_{i}(m)\) represents the total flight time of _UAVi_. Assume that there are \(N\) intervals from time \(m\) to the destination, thus the position and corresponding pitch angle sequences of _UAVi_ in \([m,m+N-1]\) are respectively:  
\[ \begin{array}{l}P_{i}\overset{\Delta}{=}\left[p_{i}(m),p_{i}(m+1),\cdots,p_ {i}(m+r),\cdots,p_{i}(m+N-1)\right]^{T}\\ \Psi_{i}\overset{\Delta}{=}\left[\varphi_{i}(m),\varphi_{i}(m+1),\cdots,\varphi _{i}(m+r),\cdots,\varphi_{i}(m+N-1)\right]^{T}\\ \forall r\in[0,N-1]\end{array}  \tag{8}\]  
The objective function for progressive optimization is:  
\[\text{min}O_{i}(m+r)=\text{min}(\sum_{r=0}^{N-1}\left(w_{1}D_{i}(m+r)+w_{2}T_ {i}(m+r)+w_{3}C_{i}(m+r)\right)) \tag{9}\]  
Note that it should meet the following constraints in the optimization process:  
\[q(\varphi_{i}(m+r))=\left[ \begin{array}{c}1\\ -1\end{array} \right]C_{i}(m+r)-\left[ \begin{array}{c}B\\ -A\end{array} \right]=\left[ \begin{array}{c}1\\ -1\end{array} \right](\varphi_{i}(m+r)-\varphi_{i}(m+r-1))-\left[ \begin{array}[ ]{c}B\\ -A\end{array} \right]\leq 0 \tag{10}\]\[e_{i}(p_{i}(m+r))=\left[ \begin{array}{c}x_{i}(m+r)-\left[x_{i}(m+r-1)+v_{i}\cos( \varphi_{i}(m+r-1))\cos\theta_{i}\cdot\Delta t\right]\\ y_{i}(m+r)-\left[y_{i}(m+r-1)+v_{i}\cos(\varphi_{i}(m+r-1))\sin\theta_{i}\cdot \Delta t\right]\\ z_{i}(m+r)-\left[z_{i}(m+r-1)+v_{i}(\sin\varphi_{i}(m+r-1))\cdot\Delta t \right]\\ \varphi_{i}(m+r)-\varphi_{i}(m+r)\end{array}