# A Hierarchical Mission Planning Method for Simultaneous Arrival of Multi-UAV Coalition
## 4 Path Estimation and Simultaneous Arrival Path Planning
### Path Estimation  
In the task allocation process, according to Equation (15), each UAV needs to calculate the ETA from its current location to the target. For the UAV \(U_{i}\), if \(A^{(i)}=\varnothing\), \(L_{ij}^{e}\) needs to be estimated; otherwise, the ETA from the UAV \(U_{i}\) to the target \(T_{j}\) consists of two parts, the first part is the accurate path length of the assigned targets \(L_{im}^{before}\) (can be obtained in Section 4.3), the second part \(L_{mj}^{e}\) requires to be estimated, where \(T_{m}\) denotes the last target that has been assigned to the UAV \(U_{i}\). For both \(L_{ij}^{e}\) and \(L_{mj}^{e}\), they can be obtained directly by calculating the length of the PH curve from one point to another.  
Assuming the starting and ending points are \(P_{s}\) and \(P_{f}\), the flyable and safe path from \(P_{s}\) to \(P_{f}\) can be obtained by a constraint optimization problem as follows:  
\[\begin{array}{ll}\min&J(x)\\ s.t.&Path\
ot\equiv obstacles\\ &\kappa(Path)\leq\kappa_{\max}\end{array} \tag{24}\]  
where \(x\) denotes the decision variable, the collision avoidance and curvature constraints are shown in the first and second constraint, respectively. \(J(x)\) means the cost function to be minimized. In this paper, two criteria are selected to quantify the cost function, which are the length and the smoothness of the path as follows:  
\[J=w\times J_{length}+(1-w)\times J_{smooth} \tag{25}\]  
where \(w\in[0,1]\) means the weight coefficient; \(J_{length}\) represents the cost function of the path length. The length of the path is very important in the path planning process, the shorter path will save time and fuel. The length of the PH curve was used in this paper. \(J_{smooth}\) denotes the cost function of the smoothness of the generated path, which can be expressed by the elastic energy of the generated PH curve [29].  
To the constraint optimization problem, the penalty function for violating the constraints can be represented as:  
\[J^{\prime}(x)=P_{j}\times J \tag{26}\]  
where \(P_{j}\) is the penalty parameter for violating the collision avoidance or curvature constraint, in this paper, \(P_{j}=100\).  
As mentioned above, a PH curve can be determined by the modulus values of the direction vectors of the starting and ending points, which are represented as \(h_{0}\) and \(h_{1}\), respectively. In this paper, A PSO-based algorithm was designed to solve the constraint optimization problem.  
First, the initial position and velocity of each particle is initialized as \(z_{i}=(h_{i,0},h_{i,1})\) and \(v_{i}=(v_{i,0},v_{i,1})\), respectively. \(i=1,\ldots,M_{p}\), where \(M_{p}\) denotes the population size of the PSO algorithm. If the particle satisfies the collision avoidance and curvature constraint, its fitness can be calculated by Equation (25); otherwise, Equation (26) is used to calculate the particle's fitness. The best position of each particle is represented as \(P_{best}=(p_{i,0},p_{i,1})\), the global best position of all particles is represented as \(g_{best}=(g_{0},g_{1})\). Then, each particle updates its position and velocity as follows:  
\[\begin{array}{l}\vspace{0.2 cm}v_{i,d}^{k+1}=\omega\vspace{0.2 cm}v_{i,d}^{k }+c_{1}r_{1}(\vspace{0.2 cm}p_{i,d}^{k}-\vspace{0.2 cm}h_{i,d}^{k})+c_{2}r_{2}( \vspace{0.2 cm}g_{d}^{k}-\vspace{0.2 cm}h_{i,d}^{k})\\ h_{i,d}^{k+1}=\vspace{0.2 cm}h_{i,d}^{k}+\vspace{0.2 cm}v_{i,d}^{k+1}\end{array} \tag{27}\]  
where \(i=1,\ldots,M_{p}\); \(d=0,1\); \(k=1,\ldots,T\); \(T\) is the iteration number. \(c_{1}\) and \(c_{2}\) are positive constants, \(r_{1}\) and \(r_{2}\) are random numbers between \([0,1]\).  
After \(T\) number of iterations, the global extremum is obtained. The estimated path length between the starting and ending point can be calculated by Equation (22).