# Ellipsoidal Path Planning for Unmanned Aerial Vehicles
## 3 Teaching-Learning Based Optimization
### Fitness Function  
We designed a fitness function that is composed of four terms:  
\[f(X_{i})=d_{t}+c+h*(d_{t}+1)+s*(d_{t}+1), \tag{7}\]  
where \(d_{t}\) is the Euclidean distance between a learner \(X_{i}\) and the target point \(\theta\) (composed of only by \(x\), \(y\) and \(z\) values); by itself this term helps to attract the population towards the target. \(c\) is the obstacle collision indicator:  
\[c=\begin{cases}\infty&\text{if any }o_{j}\leq 0,\\ 0&\text{otherwise}\end{cases} \tag{8}\]  
where \(o_{j}\) is the distance between the leaner \(X_{i}\) and the obstacle \(j\), which is obtained using the neural network described in Section 2. The collision indicator's function is to heavily penalize collisions, since it is of utmost importance to guarantee the UAV's safety.  
\(h\) represents a heat factor that indicates the proximity of \(X_{i}\) to a set of obstacles \(o_{d}\in o_{j}:0<o_{d}\leq r_{1}\), where \(r_{1}\) is a user defined range of proximity:  
\[h=\sum(-log_{2}(0.001*o_{d})+log_{2}(r_{1})). \tag{9}\]  
To give the drone the capability to avoid large convex obstacles inside a room (usually obstacles that go from floor to ceiling), a stuck factor \(s\) is added. To obtain \(s\) it is necessary to create a set of stuck zones \(s_{z}\); when the euclidean distance of the \((x,y,z)\) values of teachers \(\tau_{t-1}\) and \(\tau_{t}\) (where \(\tau\) is the last teacher obtained from a TLBO run and \(t\) is the current run) is less than a user defined threshold \(\alpha\), a zone is added to \(s_{z}\). If the condition is met the \((x,y,z)\) values from \(\tau_{t}\) are queued to \(s_{z}\). The stuck factor is obtained as follows:  
\[s=\sum(-log_{2}(0.001*s_{k})+log_{2}(r_{2})), \tag{10}\]where \(s_{k}\) is the set of Euclidean distances from a learner \(X_{i}\) to any point in \(s_{z}\), where \(0<s_{k}\leq r_{2}\), and \(r_{2}\) represents a user defined range of proximity. The pseudocode for path planning is presented in Algorithm 1.  
```
1:procedureOptimize
2:\(actual\_pos\leftarrow\) starting position
3:\(X_{i}\leftarrow\) create learner population
4:\(X^{\prime}_{i}\leftarrow\) initialize to zero
5:\(X^{\prime}_{T}\leftarrow\) obtain teacher from population as a separate value
6:\(ngens\leftarrow\) number of generations
7:\(stop\leftarrow\) stopping value
8:\(tqueue\leftarrow\) teachers queue
9:\(S_{z}\leftarrow\) set stuck zones list to empty
10:\(X_{ffitness}\gets fitness(X_{i},S_{z})\)
11:\(\alpha\gets\) 0.1\(\triangleright\) User defined threshold
12:while\(X_{ffitness}>stop\)do
13:for\(gen\gets 1\) to \(ngens\)do
14:for every \(X_{i}\)do
15:\(X^{\prime}_{i}\leftarrow\)\(teacher\_phase(X_{i})\)\(\triangleright\) This step corresponds to (4)
16:\(X^{\prime}_{i}\gets bound\_increments(X_{i},actual\_pos)\)
17:\(X^{\prime}_{ffitness}\gets fitness(X^{\prime}_{i},S_{z})\)\(\triangleright\) Evaluates (7)
18:\(X_{i}\leftarrow\)\(select\_best(X_{i},X^{\prime}_{i})\)\(\triangleright\) Selects the best candidate between \(X_{i}\) and \(X^{\prime}_{i}\)
19:for every \(X_{i}\)do
20:\(X^{\prime}_{i}\leftarrow\)\(learner\_phase(X_{i})\)\(\triangleright\) This step corresponds to (5) and (6)
21:\(X^{\prime}_{i}\gets bound\_increments(X_{i},actual\_pos)\)\(\triangleright\) Described in Section 4
22:\(X^{\prime}_{ffitness}\gets fitness(X^{\prime}_{i},S_{z})\)
23:\((X_{i},X_{ffitness})\leftarrow select\_best\_learners((X_{i},X_{iftness}),(X^{ \prime}_{i},X^{\prime}_{ffitness}))\)
24:\((X_{T},X_{Tfitness})\gets update\_teacher(X_{i},X_{iftness})\)
25:\(tqueue\gets append(X_{T},tqueue)\)\(\triangleright\) Add \(X_{T}\) to queue
26:\(dist=norm(X_{T}[0:3]-actual\_pos[0:3])\)\(\triangleright\) Euclidean norm
27:\(actual\_pos\gets X_{T}\)
28:\(X_{i}\leftarrow initialize()\)\(\triangleright\) Initialize and obtain fitness values
29:\(X^{\prime}_{i}\leftarrow\)\(set\_to\_zero()\)
30:\(mt\gets max\_index(X_{iftness})\)
31:if\(self<\alpha\)then
32:\(S_{z}\gets append(X_{T})\)
33:else
34:\(X_{mi}\gets X_{T}\)return\(tqueue\)
```  
**Algorithm 1** Path Planning Algorithm.