# Multitarget Search Algorithm Using Swarm Robots in an Unknown 3D Mountain Environment
## 4 Multitarget Ground Search Algorithm for Swarm Robots in a 3D Mountain Environment
### 3D Curved Obstacle Tracking Algorithm (3D-COTA)
#### 4.3.2 Second-Obstacle Tracking  
After \(\mathbf{Vo_{i}}(t+1)\) is calculated using the corresponding method for the robot in the roaming or cooperative search state, the speed direction is adjusted according to the slope of the surrounding mountains, avoiding the movement direction of the mountain slope beyond the robot climbing ability range.  
It is assumed that, according to the perception of the slope sensor, the distance near the robot is less than \(\|\mathbf{Vo_{i}}(t+1)\|\), and in the direction of angle set \(\mathbf{\Theta}\), the slope exceeds the climbing ability of the robot; that is, the slope is greater than \(\beta\).  
Within the set \(\mathbf{\Phi}=\left\{\frac{2\pi}{n_{\phi}}n\middle|n\in\left[-\frac{n_{\phi}}{2}, \frac{n_{\phi}}{2}\right]\bigcap\mathbf{Z}\right\}\), the sensor can identify mountain slopes in the \(n_{\phi}\) angle directions, where \(\left[-\frac{n_{\phi}}{2},\frac{n_{\phi}}{2}\right]\) represents the set of numbers between \(-\frac{n_{\phi}}{2}\) and \(\frac{n_{\phi}}{2}\), and \(\mathbf{Z}\) is the set of integers.  
Suppose that the function \(F(\varphi)\) has the following expression as Equation (22):  
\[F(\varphi)=\varphi-2\pi\cdot\text{sgn}(\varphi)\cdot\delta(|\varphi|-\pi)\quad -2\pi<\varphi<2\pi \tag{22}\]  
Among them:  
\[\text{sgn}(\varphi)=\begin{cases}-1&\varphi<0\\ 0&\varphi=0\\ 1&\varphi>0\end{cases} \tag{23}\]  
\[\delta(\varphi)=\begin{cases}0&\varphi\leq 0\\ 1&\varphi>0\end{cases} \tag{24}\]  
The second obstacle tracking velocity is denoted as \(\mathbf{Vt_{i}}(\mathbf{t+1})=\left[\dot{x}\dot{t}_{i|(t+1)},\dot{y}\dot{t}_{i|(t+1)}, \right.\\ \left.\dot{z}\dot{t}_{i|(t+1)}\right]\), and \(\mathbf{Vo_{i}}(t+1)=\left[\dot{x}\dot{o}_{i|(t+1)},\dot{y}\dot{o}_{i|(t+1)},\dot {z}\dot{o}_{i|(t+1)}\right]\). Subsequently, \(\mathbf{Vt_{i}}(\mathbf{t+1})\) is calculated as:  
(1) If \(\arctan\left(\frac{\dot{y}\dot{o}_{i|(t+1)}}{\dot{x}\dot{o}_{i|(t+1)}}\right) +\delta\left(-\dot{x}\dot{o}_{i|(t+1)}\right)\cdot\text{sgn}\left(\dot{y}\dot{ o}_{i|(t+1)}\right)\cdot\pi\
otin\mathbf{\Theta}\)  
\[\mathbf{Vt_{i}}(\mathbf{t+1})=\mathbf{Vo_{i}}(\mathbf{t+1}) \tag{25}\]  
(2) If \(\arctan\left(\frac{\dot{y}\dot{o}_{i|(t+1)}}{\dot{x}\dot{o}_{i|(t+1)}}\right) +\delta\left(-\dot{x}\dot{o}_{i|(t+1)}\right)\cdot\text{sgn}\left(\dot{y}\dot{ o}_{i|(t+1)}\right)\cdot\pi\in\mathbf{\Theta}\)  
Calculation angle:  
\[\varphi_{0}=\min_{\varphi\in\mathbf{\Phi},\dot{\varphi}_{n}\in(\mathbf{\Phi}-\mathbf{ \Theta})}|F(\varphi-\varphi_{n})| \tag{26}\]  
To calculate \(\mathbf{Vt_{i}}(\mathbf{t+1})\):  
\[\begin{cases}\begin{aligned} &\dot{x}\dot{t}_{i|(t+1)}=\left( \sqrt{\dot{x}\dot{o}_{i|(t+1)}^{2}+\dot{x}\dot{o}_{i|(t+1)}^{2}}\right)cos( \varphi_{0})\\ &\dot{y}\dot{t}_{i|(t+1)}=\left(\sqrt{\dot{x}\dot{o}_{i|(t+1)}^{2}+ \dot{x}\dot{o}_{i|(t+1)}^{2}}\right)sin(\varphi_{0})\\ &\dot{z}\dot{t}_{i|(t+1)}=-\dfrac{a\ \dot{x}\dot{t}_{i|(t+1)}+b\ \dot{y}\dot{t}_{i|(t+1)}}{c} \end{aligned}\end{cases} \tag{27}\]  
\(a\), \(b\), and \(c\) are shown in Equation (18).