# 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.1 Initial Obstacle Tracking  
For robots in the roaming or collaborative search state, the expected velocity \(\mathbf{Ve_{i}}(\mathbf{t}+\mathbf{1})\) = \(\left[\dot{x}\dot{e_{i|(t+1)}},\dot{y}\dot{e_{i|(t+1)}},\dot{z}\dot{e_{i|(t+1)}}\right]\) is calculated according to the corresponding method. However, the expected velocity direction may not be tangent to the ground but may point to the air or the ground. Therefore, it is necessary to further calculate the velocity tangent to the ground. Consider the nearest obstacle and two obstacle points not collinear to the nearest obstacle. In Equation (12), the nearest obstacle point to robot \(R_{i}\) is described as \(obs_{k_{0}}\). \[drs_{i,k_{0}|t}=\min_{obs_{k}\in\mathcal{S}}\left(drs_{i,k|t}\right) \tag{12}\]  
Based on \(obs_{k_{0}}\), two other obstacle points, \(obs_{k_{1}}\) and \(obs_{k_{2}}\), are found to satisfy the conditions described in Equation (13). According to Equation (13), points \(\mathbf{obs_{k_{0}}}\), \(\mathbf{obs_{k_{1}}}\), and \(\mathbf{obs_{k_{2}}}\) are not collinear, so these three points can determine plane \(f_{i|t}\). For obstacle tracking to search for ground targets, the robot will tend to move parallel to plane \(f_{i|t}\). Figure 3: Discretization of a mountain. (**a**) Mountain, (**b**) mountain dispersion. \[\left\{ \begin{array}{l}\mathbf{obs_{k_{0}}}=\left[xs_{k_{0}},ys_{k_{0}},zs_{k_{0}} \right]\\ \mathbf{obs_{k_{1}}}=\left[xs_{k_{1}},ys_{k_{1}},zs_{k_{1}}\right]\\ \mathbf{obs_{k_{2}}}=\left[xs_{k_{2}},ys_{k_{2}},zs_{k_{2}}\right]\\ xs_{k_{1}}=xs_{k_{0}}-\Delta l.\text{sign}\Big{(}xs_{k_{0}}-xu_{i|t}\Big{)}\\ ys_{k_{1}}=ys_{k_{0}}\\ xs_{k_{2}}=xs_{k_{0}}\\ ys_{k_{2}}=ys_{k_{0}}-\Delta l.\text{sign}\Big{(}ys_{k_{0}}-yu_{i|t}\Big{)} \end{array} \right. \tag{13}\]  
At this time, plane \(f_{i|t}\) is shifted so that the resulting plane \(f_{i|t}{}^{\prime}\) passes through \(\mathbf{R}_{i}(\mathbf{t})\), and the coordinate system \(\mathbf{R}_{it}xyz\) is established with \(\mathbf{R}_{i}(\mathbf{t})\) as the origin. In this coordinate system, \(\mathbf{R}_{i}(\mathbf{t})=[0,0,0]\), and \(\mathbf{obs_{k_{0}}}\), \(\mathbf{obs_{k_{1}}}\), and \(\mathbf{obs_{k_{2}}}\) are expressed as Equation (14):  
\[\left\{ \begin{array}{l}\mathbf{obs^{\prime}_{k_{0}}}=\left[\Big{(}xs_{k_{0}}-xu_ {i|t}\Big{)},\big{(}ys_{k_{0}}-yu_{i|t}\Big{)},\big{(}zs_{k_{0}}-zu_{i|t}\Big{)} \right]\\ \mathbf{obs^{\prime}_{k_{1}}}=\left[\Big{(}xs_{k_{1}}-xu_{i|t}\Big{)},\big{(}ys_{ k_{1}}-yu_{i|t}\Big{)},\big{(}zs_{k_{1}}-zu_{i|t}\Big{)}\right]\\ \mathbf{obs^{\prime}_{k_{2}}}=\left[\Big{(}xs_{k_{2}}-xu_{i|t}\Big{)},\big{(}ys_{ k_{2}}-yu_{i|t}\Big{)},\big{(}zs_{k_{2}}-zu_{i|t}\Big{)}\right]\end{array} \right. \tag{14}\]  
Under the \(\mathbf{R}_{it}xyz\) coordinate system, plane \(f_{i|t}{}^{\prime}\) is determined. Let Equation (15) of the plane be:  
\[ax+by+cz=0 \tag{15}\]  
The vector normal to the plane for \(\mathbf{nl_{i}}_{1t}=[a,b,c]\) is a plane of two known vectors as Equation (16):  
\[\left\{ \begin{array}{l}\mathbf{p_{1,i|t}}=\left[\big{(}xs_{k_{1}}-xs_{k_{0}} \big{)},\big{(}ys_{k_{1}}-ys_{k_{0}}\big{)},\big{(}zs_{k_{1}}-zs_{k_{0}}\big{)} \right]\\ \mathbf{p_{2,i|t}}=\left[\big{(}xs_{k_{2}}-xs_{k_{0}}\big{)},\big{(}ys_{k_{2}}-ys_ {k_{0}}\big{)},\big{(}zs_{k_{2}}-zs_{k_{0}}\big{)}\right]\end{array} \right. \tag{16}\]  
The normal vector can be obtained as Equations (17) and (18):  
\[\mathbf{nl_{i|t}}=[a,b,c]=\left[ \begin{array}{ccc}[1,0,0]&[0,1,0]&[0,0,1]\\ (xs_{k_{1}}-xs_{k_{0}})&\big{(}ys_{k_{1}}-ys_{k_{0}}\big{)}&\big{(}zs_{k_{1}}- zs_{k_{0}}\big{)}\\ (xs_{k_{2}}-xs_{k_{0}})&\big{(}ys_{k_{2}}-ys_{k_{0}}\big{)}&\big{(}zs_{k_{2}}- zs_{k_{0}}\big{)}\end{array} \right] \tag{17}\]  
\[\left\{